add other motors

MainLoop.cpp

@@ -4,7 +4,8 @@
 
 MainLoop::MainLoop() noexcept:
     pwmBoard(0x40, 60),
-    m0(
+    motors({
+        AdafruitServoMotor(
             pwmBoard,
             0,
             1.0,
@@ -16,7 +17,34 @@ MainLoop::MainLoop() noexcept:
             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);
         }
     }
 }

MainLoop.hpp

@@ -20,5 +20,5 @@ class MainLoop
         ~MainLoop() noexcept;
 
         AdafruitPWMBoard pwmBoard;
-        AdafruitServoMotor m0;
+        AdafruitServoMotor motors[3];
 };