🎨 Updates and simplifies command handling

esp32/include/gait/bezier_state.h

@@ -36,7 +36,7 @@ class BezierState : public GaitState {
   public:
     const char *name() const override { return "Bezier"; }
 
-    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         this->mapCommand(command);
         step_length = std::hypot(gait_state.step_x, gait_state.step_z);
         if (gait_state.step_x < 0.0f) {
@@ -78,8 +78,8 @@ class BezierState : public GaitState {
         float delta_pos[3] = {0};
         float delta_rot[3] = {0};
 
-        float length = step_length / 2.0f;
-        float angle = std::atan2(gait_state.step_z, step_length) * 2;
+        float length = step_length * 0.5f;
+        float angle = std::atan2(gait_state.step_z, step_length);
         curve(length, angle, arg, phase, delta_pos);
 
         length = gait_state.step_angle * 2.0f;
@@ -107,9 +107,10 @@ class BezierState : public GaitState {
         const float X_POLAR = std::cos(angle);
         const float Z_POLAR = std::sin(angle);
 
+        const float t = std::clamp(phase, 1e-4f, 1.f - 1e-4f);
         float phase_power = 1.0f;
-        float inv_phase_power = std::pow(1.0f - phase, 11);
-        const float one_minus_phase = 1.0f - phase;
+        float inv_phase_power = std::pow(1.0f - t, 11);
+        const float one_minus_phase = 1.0f - t;
 
         for (int i = 0; i < 12; i++) {
             float b = COMBINATORIAL_VALUES[i] * phase_power * inv_phase_power;
@@ -122,7 +123,7 @@ class BezierState : public GaitState {
         }
     }
 
-    static float yawArc(const float feet_pos[4], const float *current_pos) {
+    static float yawArc(const float feet_pos[3], const float *current_pos) {
         const float foot_mag = std::hypot(feet_pos[0], feet_pos[2]);
         const float foot_dir = std::atan2(feet_pos[2], feet_pos[0]);
         const float offsets[] = {current_pos[0] - feet_pos[0], current_pos[1] - feet_pos[1],

esp32/include/gait/crawl_state.h

@@ -27,7 +27,7 @@ class EightPhaseWalkState : public PhaseGaitState {
         }
     }
 
-    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         return PhaseGaitState::step(body_state, command, dt);
     }
 };

esp32/include/gait/four_phase_trot_state.h

@@ -22,7 +22,7 @@ class FourPhaseWalkState : public PhaseGaitState {
         }
     }
 
-    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         return PhaseGaitState::step(body_state, command, dt);
     }
 };

esp32/include/gait/phase_state_base.h

@@ -14,7 +14,7 @@ class PhaseGaitState : public GaitState {
     uint8_t contact_phases[4][8];
     float shifts[4][3];
 
-    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         mapCommand(command);
         this->dt = dt;
         updatePhase();

esp32/include/gait/rest_state.h

@@ -6,7 +6,7 @@ class RestState : public GaitState {
   protected:
     const char *name() const override { return "Rest"; }
 
-    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         body_state.omega = 0;
         body_state.phi = 0;
         body_state.psi = 0;

esp32/include/gait/stand_state.h

@@ -6,7 +6,7 @@ class StandState : public GaitState {
   protected:
     const char *name() const override { return "Stand"; }
 
-    void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
+    void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
         body_state.omega = 0;
         body_state.phi = command.rx / 8;
         body_state.psi = command.ry / 8;

esp32/include/gait/state.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <kinematics.h>
+#include <message_types.h>
 
 struct gait_state_t {
     float step_height;
@@ -11,23 +12,18 @@ struct gait_state_t {
     float step_depth;
 };
 
-struct ControllerCommand {
-    int stop;
-    float lx, ly, rx, ry, h, s, s1;
-};
-
 class GaitState {
   protected:
     virtual const char *name() const = 0;
-    float default_feet_pos[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}};
+    static constexpr const float (&default_feet_pos)[4][4] = Kinematics::default_feet_positions;
     gait_state_t gait_state = {0.4, 0, 0, 0, 1, 0.002};
 
-    void mapCommand(ControllerCommand command) {
-        this->gait_state.step_height = 0.4 + (command.s1 / 128 + 1) / 2;
-        this->gait_state.step_x = command.ly / 128;
-        this->gait_state.step_z = -command.lx / 128;
-        this->gait_state.step_velocity = command.s / 128 + 1;
-        this->gait_state.step_angle = command.rx / 128;
+    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;
     }
 
@@ -38,7 +34,5 @@ class GaitState {
 
     virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
 
-    virtual void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) {
-        this->mapCommand(command);
-    }
+    virtual void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) { this->mapCommand(command); }
 };