implement remaining commands

CommandParser.hpp

@@ -11,7 +11,7 @@ class CommandParser
             enum class TYPE : uint8_t {
                 MOVE_ABSOLUTE   = 0x00,
                 MOVE_RELATIVE   = 0x01,
-                RESET_POSITIONS = 0x02,
+                RESET_POSITIONS = 0x02, // Sends all to virtual 0
             } instruction;
             int16_t args[2];
         };

MainLoop.cpp

@@ -80,9 +80,28 @@ void MainLoop::update() noexcept
         uart.getNextPacket(packet);
         CommandParser::Command command = CommandParser::getInstance().parsePacket(packet);
 
-        if (command.instruction == CommandParser::Command::TYPE::MOVE_ABSOLUTE) {
+        switch (command.instruction) {
-            const float moveTarget = (float)command.args[1] / 10.0f;
+            // Scoping to keep variables local
-            motors[command.args[0]].moveTo(moveTarget);
+            case CommandParser::Command::TYPE::MOVE_ABSOLUTE: {
+                AdafruitServoMotor& motor = motors[command.args[0]];
+                const float moveTarget = (float)command.args[1] / 10.0f;
+                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;
         }
+
     }
 }