leonetienne/MinumelArmMCU
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: main loop now listens to state

Browse Source
This commit is contained in:
Leonetienne
2025-12-15 20:13:05 +01:00
parent 0405df14fe
commit ace1b9ef66
3 changed files with 56 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MainLoop.cpp
+35 -24
View File
@@ -75,33 +75,44 @@ 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);
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();
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;
}
}
break;
case StateMachine::STATE::ERROR:
default:
// Freeze in place until error clears or device reset
yield();
break;
}
}
StateMachine.cpp
+14 -14
View File
@@ -14,7 +14,7 @@ StateMachine& StateMachine::getInstance() noexcept
return instance;
}
bool StateMachine::setState(const State newState) noexcept
bool StateMachine::setState(const STATE newState) noexcept
{
if (canSwitchToState(newState)) {
state = newState;
@@ -24,15 +24,15 @@ bool StateMachine::setState(const State newState) noexcept
return false;
}
bool StateMachine::canSwitchToState(const State newState) const noexcept
bool StateMachine::canSwitchToState(const STATE newState) const noexcept
{
// Error state is final
if (state == State::ERROR) {
if (state == STATE::ERROR) {
return false;
}
// Error state only settable via raiseError()
if (newState == State::ERROR) {
if (newState == STATE::ERROR) {
return false;
}
@@ -42,39 +42,39 @@ bool StateMachine::canSwitchToState(const State newState) const noexcept
}
switch (state) {
case StateMachine::State::INITIALIZING:
case StateMachine::STATE::INITIALIZING:
switch (newState) {
// Can go to idle
case State::IDLING:
case STATE::IDLING:
return true;
// Can't go straight to moving
case State::MOVING:
case STATE::MOVING:
return false;
default:
return false;
}
break;
case StateMachine::State::IDLING:
case StateMachine::STATE::IDLING:
switch (newState) {
// Can't initialize again
case State::INITIALIZING:
case STATE::INITIALIZING:
return false;
// Can enter move state
case State::MOVING:
case STATE::MOVING:
return true;
default:
return false;
}
break;
case StateMachine::State::MOVING:
case StateMachine::STATE::MOVING:
switch (newState) {
// Can't initialize again
case State::INITIALIZING:
case STATE::INITIALIZING:
return false;
// Can go into idling
case State::IDLING:
case STATE::IDLING:
return true;
default:
@@ -87,7 +87,7 @@ bool StateMachine::canSwitchToState(const State newState) const noexcept
bool StateMachine::raiseError(const String &reason) noexcept
{
state = State::ERROR;
state = STATE::ERROR;
lastError = reason;
digitalWrite(ERROR_INDICATOR_LED_PIN, HIGH);
if (Serial) {
StateMachine.hpp
+7 -7
View File
@@ -4,28 +4,28 @@
// Singleton-instance
class StateMachine
{
enum class State {
public:
enum class STATE {
INITIALIZING,
IDLING,
MOVING,
ERROR
};
public:
StateMachine(const StateMachine&) = delete;
StateMachine(StateMachine&&) = delete;
static StateMachine& getInstance() noexcept;
State getState() const noexcept { return state; };
STATE getState() const noexcept { return state; };
String getLastError() const noexcept { return lastError; };
// May fail if a state transition from state a to b is not valid
bool setState(const State newState) noexcept;
bool setState(const STATE newState) noexcept;
bool raiseError(const String& reason) noexcept;
bool canSwitchToState(const State newState) const noexcept;
bool canSwitchToState(const STATE newState) const noexcept;
private:
StateMachine() noexcept;
State state = State::INITIALIZING;
STATE state = STATE::INITIALIZING;
String lastError = "";
};