22 lines
824 B
C++
22 lines
824 B
C++
 
|
#include "StateMachine.h"
|
||
|
|
#include <Arduino.h>
|
||
|
|
|
||
|
|
void StateMachine::update(bool armed, bool stand_request, bool walk_active, float dt) {
|
||
|
|
if (!armed) {
|
||
|
|
state_ = RobotState::DISARMED;
|
||
|
|
blend_ = 0;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if (state_ == RobotState::DISARMED) {
|
||
|
|
state_ = RobotState::SIT; // arming always lands in sit first, never straight to stand
|
||
|
|
}
|
||
|
|
|
||
|
|
float target_blend = stand_request ? 1.0f : 0.0f;
|
||
|
|
float step = BLEND_RATE * dt;
|
||
|
|
if (blend_ < target_blend) blend_ = min(blend_ + step, target_blend);
|
||
|
|
else if (blend_ > target_blend) blend_ = max(blend_ - step, target_blend);
|
||
|
|
|
||
|
|
if (blend_ <= 0.001f) state_ = RobotState::SIT;
|
||
|
|
else if (blend_ >= 0.999f) state_ = walk_active ? RobotState::WALKING : RobotState::STAND;
|
||
|
|
else state_ = RobotState::STAND; // mid-transition
|
||
|
|
}
|