From e5bf10cdb007d552409a5bb0c4495123d484b2fa Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 1 Sep 2025 18:41:59 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Updates=20and=20simplifies=20com?=
 =?UTF-8?q?mand=20handling?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/components/Visualization.svelte | 19 ++++++-----
 app/src/lib/gait.ts                         | 19 ++++++-----
 app/src/routes/controller/Controls.svelte   | 31 ++++++++++--------
 esp32/include/gait/bezier_state.h           | 13 ++++----
 esp32/include/gait/crawl_state.h            |  2 +-
 esp32/include/gait/four_phase_trot_state.h  |  2 +-
 esp32/include/gait/phase_state_base.h       |  2 +-
 esp32/include/gait/rest_state.h             |  2 +-
 esp32/include/gait/stand_state.h            |  2 +-
 esp32/include/gait/state.h                  | 24 ++++++--------
 esp32/include/kinematics.h                  |  4 +--
 esp32/include/message_types.h               | 28 +++++++++++++++++
 esp32/include/motion.h                      | 35 ++++++++++++---------
 esp32/test/gait_performance.cpp             |  5 +--
 14 files changed, 111 insertions(+), 77 deletions(-)
 create mode 100644 esp32/include/message_types.h

diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte
index f48c9d1..02f54de 100644
--- a/app/src/lib/components/Visualization.svelte
+++ b/app/src/lib/components/Visualization.svelte
@@ -33,7 +33,7 @@
   import SceneBuilder from '$lib/sceneBuilder'
   import { lerp, degToRad } from 'three/src/math/MathUtils'
   import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
-  import Kinematic, { type body_state_t } from '$lib/kinematic'
+  import { type body_state_t } from '$lib/kinematic'
   import {
     BezierState,
     CalibrationState,
@@ -264,16 +264,15 @@
     if (sceneManager.isDragging || !settings['Internal kinematic']) return
     const controlData = get(outControllerData)
     const data = {
-      stop: controlData[0],
-      lx: controlData[1],
-      ly: controlData[2],
-      rx: controlData[3],
-      ry: controlData[4],
-      h: controlData[5],
-      s: controlData[6],
-      s1: controlData[7]
+      lx: controlData[0],
+      ly: controlData[1],
+      rx: controlData[2],
+      ry: controlData[3],
+      h: controlData[4],
+      s: controlData[5],
+      s1: controlData[6]
     }
-    body_state.ym = ((data.h + 127) * 0.35) / 100
+    body_state.ym = data.h
 
     let planner = planners[get(mode)]
     const delta = performance.now() - lastTick
diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts
index 021f523..df8e504 100644
--- a/app/src/lib/gait.ts
+++ b/app/src/lib/gait.ts
@@ -14,7 +14,6 @@ export interface gait_state_t {
 }
 
 export interface ControllerCommand {
-  stop: number
   lx: number
   ly: number
   rx: number
@@ -61,11 +60,11 @@ export abstract class GaitState {
 
   map_command(command: ControllerCommand) {
     const newCommand = {
-      step_height: 0.4 + (command.s1 / 128 + 1) / 2,
-      step_x: command.ly / 128,
-      step_z: -command.lx / 128,
-      step_velocity: command.s / 128 + 1,
-      step_angle: command.rx / 128,
+      step_height: 0.4 + (command.s1 + 1) / 2,
+      step_x: command.ly,
+      step_z: -command.lx,
+      step_velocity: command.s,
+      step_angle: command.rx,
       step_depth: 0.002
     }
 
@@ -112,10 +111,10 @@ export class StandState extends GaitState {
 
   step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
     body_state.omega = 0
-    body_state.phi = command.rx / 8
-    body_state.psi = command.ry / 8
-    body_state.xm = command.ly / 2 / 100
-    body_state.zm = command.lx / 2 / 100
+    body_state.phi = command.rx
+    body_state.psi = command.ry
+    body_state.xm = command.ly / 4
+    body_state.zm = command.lx / 4
     body_state.feet = this.default_feet_pos
     return body_state
   }
diff --git a/app/src/routes/controller/Controls.svelte b/app/src/routes/controller/Controls.svelte
index f71b96a..0b1ae66 100644
--- a/app/src/routes/controller/Controls.svelte
+++ b/app/src/routes/controller/Controls.svelte
@@ -1,11 +1,11 @@
 <script lang="ts">
   import nipplejs from 'nipplejs'
   import { onMount } from 'svelte'
-  import { capitalize, throttler, toInt8 } from '$lib/utilities'
+  import { capitalize, throttler } from '$lib/utilities'
   import { input, outControllerData, mode, modes, type Modes, ModesEnum } from '$lib/stores'
   import type { vector } from '$lib/types/models'
   import { VerticalSlider } from '$lib/components/input'
-  import { gamepadAxes, gamepadButtons, hasGamepad } from '$lib/stores/gamepad'
+  import { gamepadAxes, hasGamepad } from '$lib/stores/gamepad'
   import { notifications } from '$lib/components/toasts/notifications'
 
   let throttle = new throttler()
@@ -64,14 +64,13 @@
   }
 
   const updateData = () => {
-    data[0] = 0
-    data[1] = toInt8($input.left.x, -1, 1)
-    data[2] = toInt8($input.left.y, -1, 1)
-    data[3] = toInt8($input.right.x, -1, 1)
-    data[4] = toInt8($input.right.y, -1, 1)
-    data[5] = toInt8($input.height, 0, 100)
-    data[6] = toInt8($input.speed, 0, 100)
-    data[7] = toInt8($input.s1, 0, 100)
+    data[0] = $input.left.x
+    data[1] = $input.left.y
+    data[2] = $input.right.x
+    data[3] = $input.right.y
+    data[4] = $input.height
+    data[5] = $input.speed
+    data[6] = $input.s1
 
     outControllerData.set(data)
   }
@@ -122,7 +121,11 @@
   </div>
   <div class="absolute bottom-0 z-10 flex items-end">
     <div class="flex items-center flex-col bg-base-300 bg-opacity-50 p-3 pb-2 gap-2 rounded-tr-xl">
-      <VerticalSlider min={0} max={100} oninput={(e: Event) => handleRange(e, 'height')} />
+      <VerticalSlider
+        min={0}
+        max={1}
+        step={0.01}
+        oninput={(e: Event) => handleRange(e, 'height')} />
       <label for="height">Ht</label>
     </div>
     <div
@@ -146,7 +149,8 @@
               type="range"
               name="s1"
               min="0"
-              max="25"
+              step="0.01"
+              max="1"
               oninput={e => handleRange(e, 's1')}
               class="range range-sm range-primary" />
           </div>
@@ -156,7 +160,8 @@
               type="range"
               name="speed"
               min="0"
-              max="25"
+              step="0.01"
+              max="1"
               oninput={e => handleRange(e, 'speed')}
               class="range range-sm range-primary" />
           </div>
diff --git a/esp32/include/gait/bezier_state.h b/esp32/include/gait/bezier_state.h
index d882db0..66fbcb0 100644
--- a/esp32/include/gait/bezier_state.h
+++ b/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],
diff --git a/esp32/include/gait/crawl_state.h b/esp32/include/gait/crawl_state.h
index 96ae998..2b341e8 100644
--- a/esp32/include/gait/crawl_state.h
+++ b/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);
     }
 };
\ No newline at end of file
diff --git a/esp32/include/gait/four_phase_trot_state.h b/esp32/include/gait/four_phase_trot_state.h
index d4489de..7332656 100644
--- a/esp32/include/gait/four_phase_trot_state.h
+++ b/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);
     }
 };
\ No newline at end of file
diff --git a/esp32/include/gait/phase_state_base.h b/esp32/include/gait/phase_state_base.h
index 9b9c8ca..b03042c 100644
--- a/esp32/include/gait/phase_state_base.h
+++ b/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();
diff --git a/esp32/include/gait/rest_state.h b/esp32/include/gait/rest_state.h
index f857af2..03a9fb5 100644
--- a/esp32/include/gait/rest_state.h
+++ b/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;
diff --git a/esp32/include/gait/stand_state.h b/esp32/include/gait/stand_state.h
index d39789f..4631e1a 100644
--- a/esp32/include/gait/stand_state.h
+++ b/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;
diff --git a/esp32/include/gait/state.h b/esp32/include/gait/state.h
index 1deb95a..09c4a87 100644
--- a/esp32/include/gait/state.h
+++ b/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); }
 };
\ No newline at end of file
diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h
index cea8cb8..c280d15 100644
--- a/esp32/include/kinematics.h
+++ b/esp32/include/kinematics.h
@@ -30,8 +30,8 @@ class Kinematics {
     static constexpr float L = 207.5f / 100.0f;
     static constexpr float W = 78.0f / 100.0f;
 #elif defined(SPOTMICRO_ESP32_MINI)
-    static constexpr float l1 = 0.0f / 100.0f;
-    static constexpr float l2 = 0.0f / 100.0f;
+    static constexpr float l1 = 0.01f / 100.0f;
+    static constexpr float l2 = 0.01f / 100.0f;
     static constexpr float l3 = 52.0f / 100.0f;
     static constexpr float l4 = 65.0f / 100.0f;
 
diff --git a/esp32/include/message_types.h b/esp32/include/message_types.h
new file mode 100644
index 0000000..8d505f3
--- /dev/null
+++ b/esp32/include/message_types.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <ArduinoJson.h>
+
+struct CommandMsg {
+    float lx, ly, rx, ry, h, s, s1;
+    friend void toJson(JsonVariant v, CommandMsg const &c) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(c.lx);
+        arr.add(c.ly);
+        arr.add(c.rx);
+        arr.add(c.ry);
+        arr.add(c.h);
+        arr.add(c.s);
+        arr.add(c.s1);
+    }
+
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        lx = arr[0].as<float>();
+        ly = arr[1].as<float>();
+        rx = arr[2].as<float>();
+        ry = arr[3].as<float>();
+        h = arr[4].as<float>();
+        s = arr[5].as<float>();
+        s1 = arr[6].as<float>();
+    }
+};
\ No newline at end of file
diff --git a/esp32/include/motion.h b/esp32/include/motion.h
index c71b636..5c6e363 100644
--- a/esp32/include/motion.h
+++ b/esp32/include/motion.h
@@ -10,6 +10,7 @@
 #include <gait/state.h>
 #include <gait/crawl_state.h>
 #include <gait/bezier_state.h>
+#include <message_types.h>
 
 #define DEFAULT_STATE false
 #define ANGLES_EVENT "angles"
@@ -57,26 +58,29 @@ class MotionService {
     }
 
     void handleInput(JsonVariant &root, int originId) {
-        JsonArray array = root.as<JsonArray>();
-        command.lx = array[1];
-        command.ly = array[2];
-        command.rx = array[3];
-        command.ry = array[4];
-        command.h = array[5];
-        command.s = array[6];
-        command.s1 = array[7];
+        command.fromJson(root);
 
-        body_state.ym = (command.h + 127.f) * 0.35f / 100;
+        body_state.ym = command.h * 1.f - 0.5f;
 
         switch (motionState) {
             case MOTION_STATE::STAND: {
-                body_state.phi = command.rx / 8;
-                body_state.psi = command.ry / 8;
-                body_state.xm = command.ly / 2 / 100;
-                body_state.zm = command.lx / 2 / 100;
+                body_state.phi = command.rx; // * 0.254f;
+                body_state.psi = command.ry; // * 0.254f;
+                body_state.xm = command.ly / 4;
+                body_state.zm = command.lx / 4;
                 body_state.updateFeet(kinematics.default_feet_positions);
                 break;
             }
+            case MOTION_STATE::CRAWL:
+            case MOTION_STATE::WALK: {
+                gait_state.step_height = 0.4 + (command.s1 + 1) / 2;
+                gait_state.step_x = command.ly;
+                gait_state.step_z = -command.lx;
+                gait_state.step_velocity = command.s;
+                gait_state.step_angle = command.rx;
+                gait_state.step_depth = 0.002;
+                break;
+            }
         }
     }
 
@@ -139,7 +143,7 @@ class MotionService {
   private:
     ServoController *_servoController;
     Kinematics kinematics;
-    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
+    CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
 
     friend class GaitState;
 
@@ -149,7 +153,10 @@ class MotionService {
     MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
     unsigned long _lastUpdate;
 
+    body_state_t target_body_state = {0, 0, 0, 0, 0, 0};
     body_state_t body_state = {0, 0, 0, 0, 0, 0};
+    gait_state_t gait_state = {12, 0, 0, 0, 1, 0.002};
+
     float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
     float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 
diff --git a/esp32/test/gait_performance.cpp b/esp32/test/gait_performance.cpp
index a429964..5bd7085 100644
--- a/esp32/test/gait_performance.cpp
+++ b/esp32/test/gait_performance.cpp
@@ -7,7 +7,7 @@ void test_gaitPlanner_calculateStep_time() {
     BezierState gaitPlanner;
     body_state_t body_state = {
         128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
-    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
+    CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
     const int num_steps = 1000;
 
     unsigned long start = millis();
@@ -20,7 +20,8 @@ void test_gaitPlanner_calculateStep_time() {
     unsigned long max_duration = num_steps / 2; // Minimum 0.5 ms per step
 
     char message[50];
-    snprintf(message, sizeof(message), "The step calculation took: %lu ms (%lu ms per iter)", duration, duration / num_steps);
+    snprintf(message, sizeof(message), "The step calculation took: %lu ms (%lu ms per iter)", duration,
+             duration / num_steps);
     ESP_LOGI("Test planner", message);
     TEST_ASSERT_MESSAGE(duration <= max_duration, message);
 }