35 lines
1.1 KiB
C++
35 lines
1.1 KiB
C++
 
|
#pragma once
|
||
|
|
#include "Kinematics.h"
|
||
|
|
#include "Config.h"
|
||
|
|
|
||
|
|
struct GaitPattern {
|
||
|
|
float duty_factor; // fraction of the cycle each leg spends in stance
|
||
|
|
float leg_phase_offset[6]; // each leg's phase offset within the cycle, 0..1
|
||
|
|
};
|
||
|
|
|
||
|
|
extern const GaitPattern GAIT_TRIPOD;
|
||
|
|
extern const GaitPattern GAIT_WAVE;
|
||
|
|
|
||
|
|
struct Twist {
|
||
|
|
float vx = 0, vy = 0; // mm/s, body frame
|
||
|
|
float omega = 0; // rad/s, body frame
|
||
|
|
};
|
||
|
|
|
||
|
|
class GaitEngine {
|
||
|
|
public:
|
||
|
|
void begin();
|
||
|
|
void setRequestedPattern(const GaitPattern* p) { requested_pattern_ = p; }
|
||
|
|
void setCycleDuration(float sec) { cycle_sec_ = sec; }
|
||
|
|
|
||
|
|
// Advances every leg by dt under the current twist and pattern, writing each
|
||
|
|
// leg's target foot position (body frame) into out[6].
|
||
|
|
void update(float dt, const Twist& twist, FootTarget out[6]);
|
||
|
|
|
||
|
|
bool legInStance(int i) const { return leg_phase_[i] < legs_pattern_[i]->duty_factor; }
|
||
|
|
|
||
|
|
private:
|
||
|
|
float leg_phase_[6];
|
||
|
|
const GaitPattern* legs_pattern_[6];
|
||
|
|
const GaitPattern* requested_pattern_ = &GAIT_TRIPOD;
|
||
|
|
float cycle_sec_ = DEFAULT_CYCLE_SEC;
|
||
|
|
};
|