got motor running

MainLoop.cpp

@@ -1,8 +1,26 @@
 #include "MainLoop.hpp"
+#include "StateMachine.hpp"
+#include "CommandParser.hpp"
 
-MainLoop::MainLoop() noexcept
+MainLoop::MainLoop() noexcept:
+    pwmBoard(0x40, 60),
+    m0(
+        pwmBoard,
+        0,
+        1.0,
+        1.0,
+        135.0,
+        -135.0,
+        45.0,
+        180.0,
+        0,
+        172,
+        565
+    )
 {
-    UartHandler::getInstance().init(9600);
+    // Initialize state machine singleton
+    StateMachine::getInstance();
+    //m0.setScaleCalibration(RangedMotor::calculateScaleCalibration(90, 95));
 }
 
 MainLoop::~MainLoop() noexcept
@@ -18,9 +36,23 @@ MainLoop& MainLoop::getInstance() noexcept
 
 void MainLoop::setup() noexcept
 {
+    UartHandler::getInstance().init(9600);
+    m0.commitMoveImmediately();
 }
 
 void MainLoop::update() noexcept
 {
-    UartHandler::getInstance().poll();
+    UartHandler& uart = UartHandler::getInstance();
+    uart.poll();
+
+    while(uart.getNumAvailablePackets()) {
+        uint8_t packet[16];
+        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;
+            m0.moveTo(moveTarget);
+        }
+    }
 }