✨ Makes stand imu compensating

esp32/include/motion.h

@@ -107,6 +107,7 @@ class MotionService {
 
     bool updateMotion() {
         if (!state) return false;
+        state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX());
         state->step(body_state);
         kinematics.calculate_inverse_kinematics(body_state, new_angles);
 

esp32/include/motion_states/stand_state.h

@@ -26,7 +26,7 @@ class StandState : public MotionState {
     }
 
     void step(body_state_t &body_state, float dt = 0.02f) override {
-        lerpToBody(body_state);
+        lerpToBody(body_state, true);
         updateFeet(body_state);
     }
 };

esp32/include/motion_states/state.h

@@ -9,13 +9,17 @@ class MotionState {
     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;
+    float omega_offset = 0, psi_offset = 0;
 
-    void lerpToBody(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
+    void lerpToBody(body_state_t& body_state, const bool imuCompensate = false,
+                    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);
+        body_state.psi = lerp(body_state.psi, target_body_state.psi - imuCompensate * psi_offset, smoothing_factor);
+        body_state.omega =
+            lerp(body_state.omega, target_body_state.omega - imuCompensate * omega_offset, smoothing_factor);
     }
 
     void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
@@ -25,6 +29,10 @@ class MotionState {
     }
 
   public:
+    void updateImuOffsets(const float omega_offset, const float psi_offset) {
+        this->omega_offset = omega_offset * RAD2DEG_F;
+        this->psi_offset = psi_offset * RAD2DEG_F;
+    }
     virtual ~MotionState() {}
 
     virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }

esp32/include/motion_states/walk_state.h

@@ -23,7 +23,7 @@ class WalkState : public MotionState {
     float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f};
     float stand_offset = 0.6f;
     float step_length = 0.0f;
-    float speed_factor = 1;
+    float speed_factor = 2;
     gait_state_t gait_state;
     gait_state_t target_gait_state;