feat: main loop now listens to state

MainLoop.cpp

@@ -75,33 +75,44 @@ void MainLoop::update() noexcept
     UartHandler& uart = UartHandler::getInstance();
     uart.poll();
 
-    while(uart.getNumAvailablePackets()) {
+    switch (StateMachine::getInstance().getState()) {
-        uint8_t packet[16];
+        case StateMachine::STATE::IDLING:
-        uart.getNextPacket(packet);
+        case StateMachine::STATE::MOVING:
-        CommandParser::Command command = CommandParser::getInstance().parsePacket(packet);
+            while(uart.getNumAvailablePackets()) {
+            uint8_t packet[16];
+            uart.getNextPacket(packet);
+            CommandParser::Command command = CommandParser::getInstance().parsePacket(packet);
 
-        switch (command.instruction) {
+            switch (command.instruction) {
-            // Scoping to keep variables local
+                // Scoping to keep variables local
-            case CommandParser::Command::TYPE::MOVE_ABSOLUTE: {
+                case CommandParser::Command::TYPE::MOVE_ABSOLUTE: {
-                AdafruitServoMotor& motor = motors[command.args[0]];
+                    AdafruitServoMotor& motor = motors[command.args[0]];
-                const float moveTarget = (float)command.args[1] / 10.0f;
+                    const float moveTarget = (float)command.args[1] / 10.0f;
-                motor.moveTo(moveTarget);
+                    motor.moveTo(moveTarget);
-            }
-            break;
-            case CommandParser::Command::TYPE::MOVE_RELATIVE: {
-                AdafruitServoMotor& motor = motors[command.args[0]];
-                const float moveDelta = (float)command.args[1] / 10.0f;
-                motor.moveTo(motor.getPosition() + moveDelta);
-            }
-            break;
-            case CommandParser::Command::TYPE::RESET_POSITIONS: {
-                for (uint8_t i = 0; i < 3; i++) {
-                    motors[i].moveTo(0);
-                    motors[i].commitMoveImmediately();
                 }
+                break;
+                case CommandParser::Command::TYPE::MOVE_RELATIVE: {
+                    AdafruitServoMotor& motor = motors[command.args[0]];
+                    const float moveDelta = (float)command.args[1] / 10.0f;
+                    motor.moveTo(motor.getPosition() + moveDelta);
+                }
+                break;
+                case CommandParser::Command::TYPE::RESET_POSITIONS: {
+                    for (uint8_t i = 0; i < 3; i++) {
+                        motors[i].moveTo(0);
+                        motors[i].commitMoveImmediately();
+                    }
+                }
+                break;
             }
-            break;
-        }
 
+        }
+    break;
+    case StateMachine::STATE::ERROR:
+    default:
+        // Freeze in place until error clears or device reset
+        yield();
+        break;
     }
+
 }

StateMachine.cpp

@@ -14,7 +14,7 @@ StateMachine& StateMachine::getInstance() noexcept
     return instance;
 }
 
-bool StateMachine::setState(const State newState) noexcept
+bool StateMachine::setState(const STATE newState) noexcept
 {
     if (canSwitchToState(newState)) {
         state = newState;
@@ -24,15 +24,15 @@ bool StateMachine::setState(const State newState) noexcept
     return false;
 }
 
-bool StateMachine::canSwitchToState(const State newState) const noexcept
+bool StateMachine::canSwitchToState(const STATE newState) const noexcept
 {
     // Error state is final
-    if (state == State::ERROR) {
+    if (state == STATE::ERROR) {
         return false;
     }
 
     // Error state only settable via raiseError()
-    if (newState == State::ERROR) {
+    if (newState == STATE::ERROR) {
         return false;
     }
 
@@ -42,39 +42,39 @@ bool StateMachine::canSwitchToState(const State newState) const noexcept
     }
 
     switch (state) {
-        case StateMachine::State::INITIALIZING:
+        case StateMachine::STATE::INITIALIZING:
             switch (newState) {
                 // Can go to idle
-                case State::IDLING:
+                case STATE::IDLING:
                     return true;
                 // Can't go straight to moving
-                case State::MOVING:
+                case STATE::MOVING:
                     return false;
 
                 default:
                     return false;
             }
         break;
-        case StateMachine::State::IDLING:
+        case StateMachine::STATE::IDLING:
             switch (newState) {
                 // Can't initialize again
-                case State::INITIALIZING:
+                case STATE::INITIALIZING:
                     return false;
                 // Can enter move state
-                case State::MOVING:
+                case STATE::MOVING:
                     return true;
 
                 default:
                     return false;
             }
         break;
-        case StateMachine::State::MOVING:
+        case StateMachine::STATE::MOVING:
             switch (newState) {
                 // Can't initialize again
-                case State::INITIALIZING:
+                case STATE::INITIALIZING:
                     return false;
                 // Can go into idling
-                case State::IDLING:
+                case STATE::IDLING:
                     return true;
 
                 default:
@@ -87,7 +87,7 @@ bool StateMachine::canSwitchToState(const State newState) const noexcept
 
 bool StateMachine::raiseError(const String &reason) noexcept
 {
-    state = State::ERROR;
+    state = STATE::ERROR;
     lastError = reason;
     digitalWrite(ERROR_INDICATOR_LED_PIN, HIGH);
     if (Serial) {

StateMachine.hpp

@@ -4,28 +4,28 @@
 // Singleton-instance
 class StateMachine
 {
-     enum class State {
+    public:
+      enum class STATE {
         INITIALIZING,
         IDLING,
         MOVING,
         ERROR
     };
  
-    public:
     StateMachine(const StateMachine&) = delete;
     StateMachine(StateMachine&&) = delete;
     static StateMachine& getInstance() noexcept;
 
-    State getState() const noexcept { return state; };
+    STATE getState() const noexcept { return state; };
     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 setState(const STATE newState) noexcept;
     bool raiseError(const String& reason) noexcept;
-    bool canSwitchToState(const State newState) const noexcept;
+    bool canSwitchToState(const STATE newState) const noexcept;
 
     private:
     StateMachine() noexcept;
 
-    State state = State::INITIALIZING;
+    STATE state = STATE::INITIALIZING;
     String lastError = "";
 };