From 632f603fdaa52c8c9c67ad095d9b7568c35e844f Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Wed, 2 Jul 2025 22:53:58 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=91=94=20Calculate=20default=20feet=20pos?=
 =?UTF-8?q?itions=20from=20kinematics?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/lib/ESP32-sveltekit/kinematics.h |  9 ++++++++-
 esp32/lib/ESP32-sveltekit/motion.h     | 11 +++++++----
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/esp32/lib/ESP32-sveltekit/kinematics.h b/esp32/lib/ESP32-sveltekit/kinematics.h
index 2db8240..5c9eeef 100644
--- a/esp32/lib/ESP32-sveltekit/kinematics.h
+++ b/esp32/lib/ESP32-sveltekit/kinematics.h
@@ -35,7 +35,7 @@ class Kinematics {
     static constexpr float l3 = 130.0f / 100.0f;
     static constexpr float l4 = 130.0f / 100.0f;
 
-    static constexpr float L = 207.5f / 100.0f;
+    static constexpr float L = 240.0f / 100.0f;
     static constexpr float W = 78.0f / 100.0f;
 #else
 #error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE"
@@ -53,6 +53,13 @@ class Kinematics {
     body_state_t currentState;
 
   public:
+    static constexpr float default_feet_positions[4][4] = {
+        {mountOffsets[0][0], -1, mountOffsets[0][2] + l1, 1},
+        {mountOffsets[1][0], -1, mountOffsets[1][2] - l1, 1},
+        {mountOffsets[2][0], -1, mountOffsets[2][2] + l1, 1},
+        {mountOffsets[3][0], -1, mountOffsets[3][2] - l1, 1},
+    };
+
     esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) {
         esp_err_t ret = ESP_OK;
 
diff --git a/esp32/lib/ESP32-sveltekit/motion.h b/esp32/lib/ESP32-sveltekit/motion.h
index 14eb00a..ed38265 100644
--- a/esp32/lib/ESP32-sveltekit/motion.h
+++ b/esp32/lib/ESP32-sveltekit/motion.h
@@ -35,7 +35,7 @@ class MotionService {
         socket.onSubscribe(ANGLES_EVENT,
                            std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
 
-        body_state.updateFeet(default_feet_positions);
+        body_state.updateFeet(kinematics.default_feet_positions);
     }
 
     void anglesEvent(JsonObject &root, int originId) {
@@ -75,7 +75,7 @@ class MotionService {
                 body_state.psi = command.ry / 8;
                 body_state.xm = command.ly / 2 / 100;
                 body_state.zm = command.lx / 2 / 100;
-                body_state.updateFeet(default_feet_positions);
+                body_state.updateFeet(kinematics.default_feet_positions);
                 break;
             }
         }
@@ -154,10 +154,13 @@ class MotionService {
     float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 
     float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
-    float default_feet_positions[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}};
-
+#if defined(SPOTMICRO_ESP32)
     float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
     float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+#elif defined(SPOTMICRO_YERTLE)
+    float rest_angles[12] = {0, 45, -45, 0, 45, -45, 0, 45, -45, 0, 45, -45};
+    float calibration_angles[12] = {0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0};
+#endif
 };
 
 #endif