From 95bd2a37aec072c465cdec70606e3b13f42aef26 Mon Sep 17 00:00:00 2001
From: Leonetienne <leon@etiennes.de>
Date: Sun, 14 Dec 2025 19:15:36 +0100
Subject: [PATCH] add other motors

---
 MainLoop.cpp | 60 +++++++++++++++++++++++++++++++++++++++-------------
 MainLoop.hpp |  2 +-
 2 files changed, 46 insertions(+), 16 deletions(-)

diff --git a/MainLoop.cpp b/MainLoop.cpp
index 11e3cbb..a4aa34c 100755
--- a/MainLoop.cpp
+++ b/MainLoop.cpp
@@ -4,19 +4,47 @@
 
 MainLoop::MainLoop() noexcept:
     pwmBoard(0x40, 60),
-    m0(
-        pwmBoard,
-        0,
-        1.0,
-        1.0,
-        135.0,
-        -135.0,
-        45.0,
-        180.0,
-        0,
-        172,
-        565
-    )
+    motors({
+        AdafruitServoMotor(
+            pwmBoard,
+            0,
+            1.0,
+            1.0,
+            135.0,
+            -135.0,
+            45.0,
+            180.0,
+            0,
+            172,
+            565
+        ),
+        AdafruitServoMotor(
+            pwmBoard,
+            1,
+            1.0,
+            1.0,
+            38.0,
+            -38.0,
+            85.0,
+            180.0,
+            50,
+            172,
+            565
+        ),
+        AdafruitServoMotor(
+            pwmBoard,
+            2,
+            1.0,
+            1.0,
+            123.0,
+            -123.0,
+            57.0,
+            180.0,
+            -123,
+            172,
+            565
+        ),
+    })
 {
     // Initialize state machine singleton
     StateMachine::getInstance();
@@ -37,7 +65,9 @@ MainLoop& MainLoop::getInstance() noexcept
 void MainLoop::setup() noexcept
 {
     UartHandler::getInstance().init(9600);
-    m0.commitMoveImmediately();
+    for (uint8_t i = 0; i < 3; i++) {
+        motors[i].commitMoveImmediately();
+    }
 }
 
 void MainLoop::update() noexcept
@@ -52,7 +82,7 @@ void MainLoop::update() noexcept
 
         if (command.instruction == CommandParser::Command::TYPE::MOVE_ABSOLUTE) {
             const float moveTarget = (float)command.args[1] / 10.0f;
-            m0.moveTo(moveTarget);
+            motors[command.args[0]].moveTo(moveTarget);
         }
     }
 }
diff --git a/MainLoop.hpp b/MainLoop.hpp
index afdcf74..aa9a1e1 100755
--- a/MainLoop.hpp
+++ b/MainLoop.hpp
@@ -20,5 +20,5 @@ class MainLoop
         ~MainLoop() noexcept;
 
         AdafruitPWMBoard pwmBoard;
-        AdafruitServoMotor m0;
+        AdafruitServoMotor motors[3];
 };