mangavalkandClaude Opus 5.5
ad77b06c36
Robot-side firmware (Teensy 4.0) imported unchanged as extracted. platformio.ini defines [env:robot] (teensy40) and [env:remote] (teensy41); scripts/select_src_dir.py points each env at its own sketch folder so robot/ and remote/ stay Arduino-style sketches. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
23 lines
824 B
C++
23 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
|
|
}
|