17 lines
546 B
C++
17 lines
546 B
C++
 
|
#pragma once
|
||
|
|
|
||
|
|
enum class RobotState { DISARMED, SIT, STAND, WALKING };
|
||
|
|
|
||
|
|
class StateMachine {
|
||
|
|
public:
|
||
|
|
void begin() { state_ = RobotState::DISARMED; blend_ = 0; }
|
||
|
|
void update(bool armed_request, bool stand_request, bool walk_active_request, float dt);
|
||
|
|
|
||
|
|
RobotState state() const { return state_; }
|
||
|
|
float sitStandBlend() const { return blend_; } // 0 = sit pose, 1 = stand pose
|
||
|
|
|
||
|
|
private:
|
||
|
|
RobotState state_ = RobotState::DISARMED;
|
||
|
|
float blend_ = 0;
|
||
|
|
static constexpr float BLEND_RATE = 1.0f; // per second - full sit->stand in ~1s
|
||
|
|
};
|