From 0b7cc1ac50b9fd23dba44180be1ad551467bba44 Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Mon, 15 Dec 2025 22:52:39 +0100 Subject: [PATCH] feat: implement state machine --- MotionCore/Arm.cpp | 9 +-- MotionCore/Arm.h | 1 + MotionCore/ArmSegment.cpp | 3 +- MotionCore/ArmSegment.h | 1 + MotionCore/MainLoop.cpp | 74 ++++++++++++++-------- MotionCore/MotionCore.vcxproj | 2 + MotionCore/MotionCore.vcxproj.filters | 6 ++ MotionCore/Motor.cpp | 21 ++++++- MotionCore/Motor.h | 2 +- MotionCore/StateMachine.cpp | 89 +++++++++++++++++++++++++++ MotionCore/StateMachine.h | 31 ++++++++++ 11 files changed, 206 insertions(+), 33 deletions(-) create mode 100755 MotionCore/StateMachine.cpp create mode 100755 MotionCore/StateMachine.h diff --git a/MotionCore/Arm.cpp b/MotionCore/Arm.cpp index 1f846f3..588a33a 100755 --- a/MotionCore/Arm.cpp +++ b/MotionCore/Arm.cpp @@ -3,10 +3,6 @@ #include #include -#undef min -#undef max -#undef abs - Arm::Arm(const std::wstring& comPort) noexcept: oSerialBus(comPort), j0( @@ -97,6 +93,11 @@ void Arm::rollbackPose() noexcept } } +bool Arm::isMoving() const noexcept +{ + return j0.isMoving() || j1.isMoving() || j2.isMoving(); +} + bool Arm::isEFPositionValid(std::span deadzones) const noexcept { // Otherwise: check for endeffector position diff --git a/MotionCore/Arm.h b/MotionCore/Arm.h index fa79f16..21b7e5f 100755 --- a/MotionCore/Arm.h +++ b/MotionCore/Arm.h @@ -18,6 +18,7 @@ public: void stagePose(const double theta_j0, const double theta_j1, const double theta_j2) noexcept; void commitPose() noexcept; void rollbackPose() noexcept; + bool isMoving() const noexcept; // Will test if the end effectors current target (or staged) position is not within a collider bool isEFPositionValid(std::span deadzones) const noexcept; diff --git a/MotionCore/ArmSegment.cpp b/MotionCore/ArmSegment.cpp index 530718b..ae5c4ae 100755 --- a/MotionCore/ArmSegment.cpp +++ b/MotionCore/ArmSegment.cpp @@ -26,7 +26,8 @@ ArmSegment::ArmSegment( safeMaxAngle, 1.0 ), - parent(parent) + parent(parent), + stagedAngle(0) { } diff --git a/MotionCore/ArmSegment.h b/MotionCore/ArmSegment.h index afd72f5..54d83b3 100755 --- a/MotionCore/ArmSegment.h +++ b/MotionCore/ArmSegment.h @@ -51,6 +51,7 @@ public: double getCurrentPosition() const noexcept { return motor.getCurrentPosition(); }; double getTargetPosition() const noexcept { return isAngleStaged ? stagedAngle : motor.getTargetPosition(); }; bool isStaged() const noexcept { return isAngleStaged; }; + bool isMoving() const noexcept { return motor.isCurrentlyMoving(); }; void update(const double frametime); diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp index 6763086..bccd837 100755 --- a/MotionCore/MainLoop.cpp +++ b/MotionCore/MainLoop.cpp @@ -4,6 +4,7 @@ #include #include #include +#include "StateMachine.h" #include "yz_plotter_win.h" MainLoop::MainLoop() noexcept : @@ -16,6 +17,8 @@ MainLoop::MainLoop() noexcept : }), leftVKB(L" VKBsim Gladiator EVO L ") { + StateMachine::getInstance(); + if (!arm.assumeHomePoseImmediate()) { std::cerr << "Unable to move arm to home!" << std::endl; exit(-1); @@ -25,6 +28,8 @@ MainLoop::MainLoop() noexcept : std::cerr << "Unable to connect to left VKB controller!" << std::endl; exit(-1); } + + StateMachine::getInstance().setState(StateMachine::STATE::IDLING); } MainLoop& MainLoop::getInstance() @@ -64,37 +69,54 @@ void MainLoop::run() isRunning = false; } - constexpr double speed = 5000; - VkbInputs vkbi = leftVKB.get(); + switch (StateMachine::getInstance().getState()) { + case StateMachine::STATE::IDLING: + case StateMachine::STATE::MOVING: { + constexpr double speed = 5000; + VkbInputs vkbi = leftVKB.get(); - // Calculate new joint positions - j0Pos += -vkbi.roll * speed * frametime; - j1Pos += -vkbi.pitch * speed * frametime; - j2Pos += -vkbi.yaw * speed * frametime; + // Calculate new joint positions + j0Pos += -vkbi.roll * speed * frametime; + j1Pos += -vkbi.pitch * speed * frametime; + j2Pos += -vkbi.yaw * speed * frametime; - // Stage the new pose and keep it if it is good - arm.stagePose(j0Pos, j1Pos, j2Pos); - if (arm.isEFPositionValid(deadzones)) { - // Roll back individual joints if their angles are out of range - arm.rollbackJointsBasedOnValidRange(); + // Stage the new pose and keep it if it is good + arm.stagePose(j0Pos, j1Pos, j2Pos); + if (arm.isEFPositionValid(deadzones)) { + // Roll back individual joints if their angles are out of range + arm.rollbackJointsBasedOnValidRange(); - std::cout << "Pose good!!" << std::endl; - arm.commitPose(); + std::cout << "Pose good!!" << std::endl; + arm.commitPose(); - j0Pos = arm.getJ0().getTargetPosition(); - j1Pos = arm.getJ1().getTargetPosition(); - j2Pos = arm.getJ2().getTargetPosition(); + j0Pos = arm.getJ0().getTargetPosition(); + j1Pos = arm.getJ1().getTargetPosition(); + j2Pos = arm.getJ2().getTargetPosition(); + } + else { + arm.rollbackPose(); + std::cout << "Hit deadzone!!" << std::endl; + } + + const Vector3d ef = arm.getJ2().getCurrentGlobalEndpoint(); + //std::cout << ef << std::endl; + //plot.draw(ef); + + arm.update(frametime); + + // Unset MOVING to IDLING state if no arm is moving + if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) { + StateMachine::getInstance().setState(StateMachine::STATE::IDLING); + } + + } + break; + case StateMachine::STATE::FAULT: + default: + // Errorous states: Halt the machine + std::this_thread::yield(); + break; } - else { - arm.rollbackPose(); - std::cout << "Hit deadzone!!" << std::endl; - } - - const Vector3d ef = arm.getJ2().getCurrentGlobalEndpoint(); - //std::cout << ef << std::endl; - //plot.draw(ef); - - arm.update(frametime); lastBegin = std::chrono::high_resolution_clock::now(); } diff --git a/MotionCore/MotionCore.vcxproj b/MotionCore/MotionCore.vcxproj index 21f4c08..8f8f2eb 100755 --- a/MotionCore/MotionCore.vcxproj +++ b/MotionCore/MotionCore.vcxproj @@ -151,6 +151,7 @@ + @@ -162,6 +163,7 @@ + diff --git a/MotionCore/MotionCore.vcxproj.filters b/MotionCore/MotionCore.vcxproj.filters index f178124..4703f20 100755 --- a/MotionCore/MotionCore.vcxproj.filters +++ b/MotionCore/MotionCore.vcxproj.filters @@ -45,6 +45,9 @@ Quelldateien + + Quelldateien + @@ -77,5 +80,8 @@ Headerdateien + + Headerdateien + \ No newline at end of file diff --git a/MotionCore/Motor.cpp b/MotionCore/Motor.cpp index ba96937..3fc7fe5 100755 --- a/MotionCore/Motor.cpp +++ b/MotionCore/Motor.cpp @@ -3,6 +3,10 @@ #include #include +// This can set the MOVING state without any problem, because it should be true if at least +// one motor is moving. But this class cannot unset it, as for IDLE, NO motor has to be moving! +#include "StateMachine.h" + constexpr double RAMPUP_REDUCTION_FAC = 0.1; constexpr double RAMPUP_RECOVERY_SPEED = 0.1; constexpr double SPEED_CORRECTION_FAC = 100000.0; @@ -60,6 +64,8 @@ bool Motor::moveTo(const double theta, const std::optional speed) noexce targetPosition = theta; moveStartPosition = currentPosition; + StateMachine::getInstance().setState(StateMachine::STATE::MOVING); + return true; } @@ -80,6 +86,9 @@ bool Motor::moveToImmediate(const double theta) noexcept targetPosition = theta; moveStartPosition = theta; } + + StateMachine::getInstance().setState(StateMachine::STATE::MOVING); + return result; } @@ -104,6 +113,8 @@ bool Motor::moveBy(const double theta, const std::optional speed) noexce targetPosition = acuteTarget; moveStartPosition = currentPosition; + StateMachine::getInstance().setState(StateMachine::STATE::MOVING); + return true; } @@ -126,6 +137,9 @@ bool Motor::moveByImmediate(const double theta) noexcept currentPosition = acuteTarget; targetPosition = acuteTarget; } + + StateMachine::getInstance().setState(StateMachine::STATE::MOVING); + return result; } @@ -156,6 +170,11 @@ void Motor::resumeMoving() noexcept return; } + // Transition to moving if we were paused previously and if we have not reached our target yet + if (paused && std::abs(currentPosition - targetPosition) > MOVEMENT_COMPARISON_EPSILON) { + StateMachine::getInstance().setState(StateMachine::STATE::MOVING); + } + paused = false; } @@ -276,5 +295,5 @@ double Motor::getCurrentMovementProgress() const noexcept bool Motor::isCurrentlyMoving() const noexcept { - return std::abs(currentPosition - targetPosition) > MOVEMENT_COMPARISON_EPSILON; + return !paused && std::abs(currentPosition - targetPosition) > MOVEMENT_COMPARISON_EPSILON; } diff --git a/MotionCore/Motor.h b/MotionCore/Motor.h index a860f7b..9299642 100755 --- a/MotionCore/Motor.h +++ b/MotionCore/Motor.h @@ -42,6 +42,7 @@ public: double getSafeMaxPos() const noexcept { return safeMaxPos; }; double getPaused() const noexcept { return paused; }; bool isPositionInRange(const double pos) const noexcept; + bool isCurrentlyMoving() const noexcept; private: // Update-handler for moving @@ -51,7 +52,6 @@ private: // Will calculate the current speed for rampup based on currentPosition, moveStartPosition, and targetPosition double calculateCurrentSpeed() const noexcept; double getCurrentMovementProgress() const noexcept; - bool isCurrentlyMoving() const noexcept; const std::uint8_t index; OSerialBus& serialBus; diff --git a/MotionCore/StateMachine.cpp b/MotionCore/StateMachine.cpp new file mode 100755 index 0000000..11e475c --- /dev/null +++ b/MotionCore/StateMachine.cpp @@ -0,0 +1,89 @@ +#include "StateMachine.h" +#include + +StateMachine::StateMachine() noexcept +{ } + +StateMachine& StateMachine::getInstance() noexcept +{ + static StateMachine instance; + return instance; +} + +bool StateMachine::setState(const STATE newState) noexcept +{ + if (canSwitchToState(newState)) { + state = newState; + return true; + } + + return false; +} + +bool StateMachine::canSwitchToState(const STATE newState) const noexcept +{ + // Error state is final + if (state == STATE::FAULT) { + return false; + } + + // Error state only settable via raiseError() + if (newState == STATE::FAULT) { + return false; + } + + // Allow same-to-same transition + if (state == newState) { + return true; + } + + switch (state) { + case StateMachine::STATE::INITIALIZING: + switch (newState) { + // Can go to idle + case STATE::IDLING: + return true; + // Can't go straight to moving + case STATE::MOVING: + return false; + + default: + return false; + } + break; + case StateMachine::STATE::IDLING: + switch (newState) { + // Can't initialize again + case STATE::INITIALIZING: + return false; + // Can enter move state + case STATE::MOVING: + return true; + + default: + return false; + } + break; + case StateMachine::STATE::MOVING: + switch (newState) { + // Can't initialize again + case STATE::INITIALIZING: + return false; + // Can go into idling + case STATE::IDLING: + return true; + + default: + return false; + } + break; + } + return false; +} + +bool StateMachine::raiseError(const std::string& reason) noexcept +{ + state = STATE::FAULT; + lastError = reason; + std::cerr << "Error: " << reason << std::endl; +} diff --git a/MotionCore/StateMachine.h b/MotionCore/StateMachine.h new file mode 100755 index 0000000..e27742e --- /dev/null +++ b/MotionCore/StateMachine.h @@ -0,0 +1,31 @@ +#pragma once +#include + +// Singleton-instance +class StateMachine +{ +public: + enum class STATE { + INITIALIZING, + IDLING, + MOVING, + FAULT, + }; + + StateMachine(const StateMachine&) = delete; + StateMachine(StateMachine&&) = delete; + static StateMachine& getInstance() noexcept; + + STATE getState() const noexcept { return state; }; + std::string getLastError() const noexcept { return lastError; }; + // May fail if a state transition from state a to b is not valid + bool setState(const STATE newState) noexcept; + bool raiseError(const std::string& reason) noexcept; + bool canSwitchToState(const STATE newState) const noexcept; + +private: + StateMachine() noexcept; + + STATE state = STATE::INITIALIZING; + std::string lastError = ""; +};