From fd218f03584129a0ca1c57df54d32748e4cdc144 Mon Sep 17 00:00:00 2001
From: Leonetienne <leon@etiennes.de>
Date: Sun, 14 Dec 2025 19:27:21 +0100
Subject: [PATCH] implement remaining commands

---
 CommandParser.hpp |  2 +-
 MainLoop.cpp      | 25 ++++++++++++++++++++++---
 2 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/CommandParser.hpp b/CommandParser.hpp
index cd218d3..e563476 100755
--- a/CommandParser.hpp
+++ b/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];
         };
diff --git a/MainLoop.cpp b/MainLoop.cpp
index a4aa34c..6afcfbb 100755
--- a/MainLoop.cpp
+++ b/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) {
-            const float moveTarget = (float)command.args[1] / 10.0f;
-            motors[command.args[0]].moveTo(moveTarget);
+        switch (command.instruction) {
+            // Scoping to keep variables local
+            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;
         }
+
     }
 }