From 774c546487f2eb978b624e48d4f2225a805022a7 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Wed, 3 Sep 2025 16:18:36 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Cleanup=20crawl?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/include/gait/walk_state.h | 35 +++++++++++++++++++++++----------
 1 file changed, 25 insertions(+), 10 deletions(-)

diff --git a/esp32/include/gait/walk_state.h b/esp32/include/gait/walk_state.h
index 411c0c0..ba245af 100644
--- a/esp32/include/gait/walk_state.h
+++ b/esp32/include/gait/walk_state.h
@@ -69,25 +69,30 @@ class WalkState : public GaitState {
     void updatePhase(float dt) { phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * 2, 1.0f); }
 
     void updateBodyPosition(body_state_t &body_state, float dt) {
+        if (mode != WALK_GAIT::CRAWL) return;
         const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f;
         if (!moving) return;
         const auto c = dynamicStanceCentroid(body_state);
-        const float k = mode == WALK_GAIT::CRAWL ? 16.f : 10.f;
-        const float a = 1.f - std::exp(-k * dt);
+        const float a = 1.f - std::exp(-16.f * dt);
         body_state.xm += (c[0] - body_state.xm) * a;
         body_state.zm += (c[2] - body_state.zm) * a;
     }
 
     std::array<float, 3> dynamicStanceCentroid(const body_state_t &body_state) const {
-        float sx = 0.f, sz = 0.f, sw = 0.f;
+        if (mode != WALK_GAIT::CRAWL) return {body_state.xm, 0.f, body_state.zm};
+        float sx = 0.f, sz = 0.f;
+        int n = 0;
         for (int i = 0; i < 4; ++i) {
-            const float w = stanceWeight(i);
-            sx += body_state.feet[i][0] * w;
-            sz += body_state.feet[i][2] * w;
-            sw += w;
+            float p = std::fmod(phase_time + phase_offset[i], 1.f);
+            if (p < 0.f) p += 1.f;
+            if (p <= stand_offset) {
+                sx += default_feet_pos[i][0];
+                sz += default_feet_pos[i][2];
+                ++n;
+            }
         }
-        if (sw == 0.f) return {body_state.xm, 0.f, body_state.zm};
-        return {sx / sw, 0.f, sz / sw};
+        if (n == 0) return {body_state.xm, 0.f, body_state.zm};
+        return {sx / n, 0.f, sz / n};
     }
 
     static float smoothstep01(float t) {
@@ -127,13 +132,23 @@ class WalkState : public GaitState {
     }
 
     void standController(body_state_t &body_state, const int index, const float phase) {
-        controller(index, body_state, phase, stanceCurve, &gait_state.step_depth);
+        if (mode == WALK_GAIT::CRAWL)
+            controller(index, body_state, phase, stanceCurveFlat, &gait_state.step_depth);
+        else
+            controller(index, body_state, phase, stanceCurve, &gait_state.step_depth);
     }
 
     void swingController(body_state_t &body_state, const int index, const float phase) {
         controller(index, body_state, phase, bezierCurve, &gait_state.step_height);
     }
 
+    static void stanceCurveFlat(const float length, const float angle, const float *, const float phase, float *point) {
+        float step = length * (1.0f - 2.0f * phase);
+        point[0] += step * std::cos(angle);
+        point[2] += step * std::sin(angle);
+        point[1] += 0.f;
+    }
+
     void controller(const int index, body_state_t &body_state, const float phase,
                     std::function<void(float, float, float *, float, float *)> curve, float *arg) {
         float delta_pos[3] = {0};