Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ Introduces motion as a state machine

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-04 23:11:59 +02:00
committed by Rune Harlyk
parent c85ac41ebc
commit b66ddc3e81
6 changed files with 118 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/state.h
+20 -20
View File
@@ -4,35 +4,35 @@
#include <gait/kinematic_constraints.h>
#include <message_types.h>
struct gait_state_t {
float step_height {KinConfig::default_step_height};
float step_x {0};
float step_z {0};
float step_angle {0};
float step_velocity {0.5};
float step_depth {KinConfig::default_step_depth};
};
class GaitState {
class MotionState {
protected:
virtual const char *name() const = 0;
virtual const char* name() const = 0;
static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions;
body_state_t target_body_state;
static constexpr float default_smoothing_factor = 0.06f;
gait_state_t gait_state;
void lerpToBody(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
body_state.xm = lerp(body_state.xm, target_body_state.xm, smoothing_factor);
body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor);
body_state.phi = lerp(body_state.phi, target_body_state.phi, smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi, smoothing_factor);
}
virtual void mapCommand(CommandMsg command) {
this->gait_state.step_height = command.s1 / 2;
this->gait_state.step_x = command.ly;
this->gait_state.step_z = -command.lx;
this->gait_state.step_velocity = command.s;
this->gait_state.step_angle = command.rx;
this->gait_state.step_depth = 0.002;
void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
if (target_body_state.feet != body_state.feet) {
body_state.updateFeet(target_body_state.feet);
}
}
public:
virtual ~MotionState() {}
virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
virtual void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) { this->mapCommand(command); }
virtual void handleCommand(const CommandMsg& cmd) {}
virtual void step(body_state_t& body_state, float dt = 0.02f) {}
};