feat: implement state machine

MotionCore/Arm.cpp

@@ -3,10 +3,6 @@
 #include <numbers>
 #include <algorithm>
 
-#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<const AxisAlignedBoundingBox> deadzones) const noexcept
 {
 	// Otherwise: check for endeffector position

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<const AxisAlignedBoundingBox> deadzones) const noexcept;

MotionCore/ArmSegment.cpp

@@ -26,7 +26,8 @@ ArmSegment::ArmSegment(
 		safeMaxAngle,
 		1.0
 	),
-	parent(parent)
+	parent(parent),
+	stagedAngle(0)
 {
 	
 }

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);
 

MotionCore/MainLoop.cpp

@@ -4,6 +4,7 @@
 #include <chrono>
 #include <algorithm>
 #include <numbers>
+#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;
+        switch (StateMachine::getInstance().getState()) {
-        VkbInputs vkbi = leftVKB.get();
+            case StateMachine::STATE::IDLING:
+            case StateMachine::STATE::MOVING: {
+                constexpr double speed = 5000;
+                VkbInputs vkbi = leftVKB.get();
 
-        // Calculate new joint positions
+                // Calculate new joint positions
-        j0Pos += -vkbi.roll * speed * frametime;
+                j0Pos += -vkbi.roll * speed * frametime;
-        j1Pos += -vkbi.pitch * speed * frametime;
+                j1Pos += -vkbi.pitch * speed * frametime;
-        j2Pos += -vkbi.yaw * speed * frametime;
+                j2Pos += -vkbi.yaw * speed * frametime;
 
-        // Stage the new pose and keep it if it is good
+                // Stage the new pose and keep it if it is good
-        arm.stagePose(j0Pos, j1Pos, j2Pos);
+                arm.stagePose(j0Pos, j1Pos, j2Pos);
-        if (arm.isEFPositionValid(deadzones)) {
+                if (arm.isEFPositionValid(deadzones)) {
-            // Roll back individual joints if their angles are out of range
+                    // Roll back individual joints if their angles are out of range
-            arm.rollbackJointsBasedOnValidRange();
+                    arm.rollbackJointsBasedOnValidRange();
 
-            std::cout << "Pose good!!" << std::endl;
+                    std::cout << "Pose good!!" << std::endl;
-            arm.commitPose();
+                    arm.commitPose();
 
-            j0Pos = arm.getJ0().getTargetPosition();
+                    j0Pos = arm.getJ0().getTargetPosition();
-            j1Pos = arm.getJ1().getTargetPosition();
+                    j1Pos = arm.getJ1().getTargetPosition();
-            j2Pos = arm.getJ2().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();
     }

MotionCore/MotionCore.vcxproj

@@ -151,6 +151,7 @@
     <ClCompile Include="Matrix3x3.cpp" />
     <ClCompile Include="Motor.cpp" />
     <ClCompile Include="OSerialBus.cpp" />
+    <ClCompile Include="StateMachine.cpp" />
     <ClCompile Include="Vector3d.cpp" />
     <ClCompile Include="vkb_controllerh.cpp" />
   </ItemGroup>
@@ -162,6 +163,7 @@
     <ClInclude Include="Matrix3x3.h" />
     <ClInclude Include="Motor.h" />
     <ClInclude Include="OSerialBus.h" />
+    <ClInclude Include="StateMachine.h" />
     <ClInclude Include="Vector3d.h" />
     <ClInclude Include="vkb_controller.h" />
     <ClInclude Include="yz_plotter_win.h" />

MotionCore/MotionCore.vcxproj.filters

@@ -45,6 +45,9 @@
     <ClCompile Include="vkb_controllerh.cpp">
       <Filter>Quelldateien</Filter>
     </ClCompile>
+    <ClCompile Include="StateMachine.cpp">
+      <Filter>Quelldateien</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="MainLoop.h">
@@ -77,5 +80,8 @@
     <ClInclude Include="yz_plotter_win.h">
       <Filter>Headerdateien</Filter>
     </ClInclude>
+    <ClInclude Include="StateMachine.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

MotionCore/Motor.cpp

@@ -3,6 +3,10 @@
 #include <algorithm>
 #include <array>
 
+// 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<double> 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<double> 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;
 }

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;

MotionCore/StateMachine.cpp

@@ -0,0 +1,89 @@
+#include "StateMachine.h"
+#include <iostream>
+
+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;
+}

MotionCore/StateMachine.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <string>
+
+// 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 = "";
+};