MinumelArmMCU/MainLoop.cpp

#include "MainLoop.hpp"
#include "StateMachine.hpp"
#include "CommandParser.hpp"

MainLoop::MainLoop() noexcept:
    pwmBoard(0x40, 60),
    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();
    //m0.setScaleCalibration(RangedMotor::calculateScaleCalibration(90, 95));
}

MainLoop::~MainLoop() noexcept
{
    UartHandler::getInstance().close();
}

MainLoop& MainLoop::getInstance() noexcept
{
    static MainLoop instance;
    return instance;
}

void MainLoop::setup() noexcept
{
    UartHandler::getInstance().init(9600);
    for (uint8_t i = 0; i < 3; i++) {
        motors[i].commitMoveImmediately();
    }
}

void MainLoop::update() noexcept
{
    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;
            motors[command.args[0]].moveTo(moveTarget);
        }
    }
}