add other motors

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