From ccf6f01e4d230aac316bd064acb3f3147854abb7 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sun, 26 May 2024 15:14:35 +0200
Subject: [PATCH] =?UTF-8?q?=E2=99=A3=EF=B8=8F=20Simplifices=20angle=20upda?=
 =?UTF-8?q?ting?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/lib/ESP32-sveltekit/Kinematics.h    |  6 +--
 esp32/lib/ESP32-sveltekit/MotionService.h | 49 ++++++++---------------
 2 files changed, 20 insertions(+), 35 deletions(-)

diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h
index 8125390..b32fbd3 100644
--- a/esp32/lib/ESP32-sveltekit/Kinematics.h
+++ b/esp32/lib/ESP32-sveltekit/Kinematics.h
@@ -53,7 +53,7 @@ static esp_err_t inverse(float a[4][4], float b[4][4])
 
 typedef  struct {
     float omega, phi, psi, xm, ym, zm;
-} position_t;
+} body_state_t;
 
 
 #define RAD2DEGREES 57.295779513082321 // 180 / PI
@@ -93,7 +93,7 @@ public:
     }
     ~Kinematics(){}
 
-    esp_err_t calculate_inverse_kinematics(float lp[4][4], position_t p, float result[12]) {
+    esp_err_t calculate_inverse_kinematics(float lp[4][4], body_state_t p, float result[12]) {
         esp_err_t ret = ESP_OK;
 
         ret = bodyIK(p);
@@ -119,7 +119,7 @@ public:
         return ret;
     }
 
-    esp_err_t bodyIK(position_t p) {
+    esp_err_t bodyIK(body_state_t p) {
         float cos_omega = cos(p.omega*DEGREES2RAD);
         float sin_omega = sin(p.omega*DEGREES2RAD);
         float cos_phi = cos(p.phi*DEGREES2RAD);
diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index 096704b..f1e3ad3 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -114,53 +114,38 @@ class MotionService
     }
 
     bool updateMotion() {
-        bool updated = false;
+        float new_angles[12] = {0,};
         switch (motionState) {
             case MOTION_STATE::IDLE:
                 break;
             case MOTION_STATE::REST:
-                for (int i = 0; i < 12; i++) {
-                    float new_angle = lerp(angles[i], rest_angles[i], 0.5);
-                    if (new_angle != angles[i]) {
-                        angles[i] = new_angle;
-                        updated = true;
-                    }
-                }
+                update_angles(rest_angles, new_angles, false);
                 break;
 
             case MOTION_STATE::STAND: {
-                float new_angles[12] = {0,};
                 kinematics.calculate_inverse_kinematics(lp, position, new_angles);
-
-                for (int i = 0; i < 12; i++) {
-                    float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
-                    if (new_angle != angles[i]) {
-                        angles[i] = new_angle;
-                        updated = true;
-                    }
-                }
-                if (updated) {
-                    ESP_LOGI("MotionService", "New angles: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]);
-                }
                 break;
             }
             case MOTION_STATE::WALK:
-                float new_angles[12] = {0,};
                 lp[0][1] += walk_dir;
-                if (lp[0][1] >= 200) walk_dir = -1;
-                if (lp[0][1] <= 100) walk_dir = 1;
+                if (lp[0][1] >= 50) walk_dir = -1;
+                if (lp[0][1] <= -100) walk_dir = 1;
 
                 kinematics.calculate_inverse_kinematics(lp, position, new_angles);
-
-                for (int i = 0; i < 12; i++) {
-                    float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
-                    if (new_angle != angles[i]) {
-                        angles[i] = new_angle;
-                        updated = true;
-                    }
-                }
                 break;
         }
+        return update_angles(new_angles, angles);
+    }
+
+    bool update_angles(float new_angles[12], float angles[12], bool useLerp = true) {
+        bool updated = false;
+        for (int i = 0; i < 12; i++) {
+            float new_angle = useLerp ? lerp(angles[i], new_angles[i] * dir[i], 0.3) : new_angles[i] * dir[i];
+            if (new_angle != angles[i]) {
+                angles[i] = new_angle;
+                updated = true;
+            }
+        }
         return updated;
     }
 
@@ -180,7 +165,7 @@ class MotionService
 
     constexpr static int MotionInterval = 100;
 
-    position_t position = {0, 0, 0, 0, 0, 0};
+    body_state_t position = {0, 0, 0, 0, 0, 0};
     float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
     float lp[4][4] = {
         { 100, -100,  100, 1},