From de3912ff108adb46b2f7f47332174aad21a89a9e Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Fri, 22 Aug 2025 12:21:12 +0200
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Adds=20kinematics=20for=20spot=20pi?=
 =?UTF-8?q?co?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/features.ini         |  3 ++-
 esp32/include/features.h   |  6 ++++--
 esp32/include/kinematics.h | 10 +++++++++-
 esp32/include/motion.h     |  2 +-
 4 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/esp32/features.ini b/esp32/features.ini
index 0c3fa4c..3427e8e 100644
--- a/esp32/features.ini
+++ b/esp32/features.ini
@@ -2,7 +2,8 @@
 build_flags = 
   ; Kinematics - Choose only one
   -D SPOTMICRO_ESP32
-  ;-D SPOTMICRO_YERTLE
+;   -D SPOTMICRO_ESP32_MINI
+;   -D SPOTMICRO_YERTLE
 
   ; Firmware flags
   -D USE_SLEEP=1
diff --git a/esp32/include/features.h b/esp32/include/features.h
index 3227f44..1facc33 100644
--- a/esp32/include/features.h
+++ b/esp32/include/features.h
@@ -79,16 +79,18 @@
 
 static_assert(!(USE_JSON == 1 && USE_MSGPACK == 1), "Cannot set both USE_JSON and USE_MSGPACK to 1 simultaneously");
 
-#if defined(SPOTMICRO_ESP32) && defined(SPOTMICRO_YERTLE)
+#if defined(SPOTMICRO_ESP32) && defined(SPOTMICRO_ESP32_MINI) && defined(SPOTMICRO_YERTLE)
 #error "Only one kinematics variant must be defined"
 #endif
 
-#if !defined(SPOTMICRO_ESP32) && !defined(SPOTMICRO_YERTLE)
+#if !defined(SPOTMICRO_ESP32) && !defined(SPOTMICRO_ESP32_MINI) && !defined(SPOTMICRO_YERTLE)
 #error "You must define one kinematics variant"
 #endif
 
 #if defined(SPOTMICRO_ESP32)
 #define KINEMATICS_VARIANT_STR "SPOTMICRO_ESP32"
+#elif defined(SPOTMICRO_ESP32_MINI)
+#define KINEMATICS_VARIANT_STR "SPOTMICRO_ESP32_MINI"
 #elif defined(SPOTMICRO_YERTLE)
 #define KINEMATICS_VARIANT_STR "SPOTMICRO_YERTLE"
 #else
diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h
index 5c9eeef..cea8cb8 100644
--- a/esp32/include/kinematics.h
+++ b/esp32/include/kinematics.h
@@ -29,6 +29,14 @@ 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 l3 = 52.0f / 100.0f;
+    static constexpr float l4 = 65.0f / 100.0f;
+
+    static constexpr float L = 120.0f / 100.0f;
+    static constexpr float W = 78.5f / 100.0f;
 #elif defined(SPOTMICRO_YERTLE)
     static constexpr float l1 = 35.0f / 100.0f;
     static constexpr float l2 = 0.0f;
@@ -156,7 +164,7 @@ class Kinematics {
         float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3));
         result[0] = RAD_TO_DEG_F(theta1);
         result[1] = RAD_TO_DEG_F(theta2);
-#if defined(SPOTMICRO_ESP32)
+#if defined(SPOTMICRO_ESP32) || defined(SPOTMICRO_ESP32_MINI)
         result[2] = RAD_TO_DEG_F(theta3);
 #elif defined(SPOTMICRO_YERTLE)
         result[2] = RAD_TO_DEG_F(theta3 + theta2);
diff --git a/esp32/include/motion.h b/esp32/include/motion.h
index 7e76055..c71b636 100644
--- a/esp32/include/motion.h
+++ b/esp32/include/motion.h
@@ -154,7 +154,7 @@ 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};
-#if defined(SPOTMICRO_ESP32)
+#if defined(SPOTMICRO_ESP32) || defined(SPOTMICRO_ESP32_MINI)
     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)