119 lines
3.1 KiB
C++
Executable File
119 lines
3.1 KiB
C++
Executable File
#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();
|
|
|
|
switch (StateMachine::getInstance().getState()) {
|
|
case StateMachine::STATE::IDLING:
|
|
case StateMachine::STATE::MOVING:
|
|
while(uart.getNumAvailablePackets()) {
|
|
uint8_t packet[16];
|
|
uart.getNextPacket(packet);
|
|
CommandParser::Command command = CommandParser::getInstance().parsePacket(packet);
|
|
|
|
switch (command.instruction) {
|
|
// Scoping to keep variables local
|
|
case CommandParser::Command::TYPE::MOVE_ABSOLUTE: {
|
|
AdafruitServoMotor& motor = motors[command.args[0]];
|
|
const float moveTarget = (float)command.args[1] / 10.0f;
|
|
motor.moveTo(moveTarget);
|
|
}
|
|
break;
|
|
case CommandParser::Command::TYPE::MOVE_RELATIVE: {
|
|
AdafruitServoMotor& motor = motors[command.args[0]];
|
|
const float moveDelta = (float)command.args[1] / 10.0f;
|
|
motor.moveTo(motor.getPosition() + moveDelta);
|
|
}
|
|
break;
|
|
case CommandParser::Command::TYPE::RESET_POSITIONS: {
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
motors[i].moveTo(0);
|
|
motors[i].commitMoveImmediately();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
}
|
|
break;
|
|
case StateMachine::STATE::ERROR:
|
|
default:
|
|
// Freeze in place until error clears or device reset
|
|
yield();
|
|
break;
|
|
}
|
|
|
|
}
|