♣️ Simplifices angle updating

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);

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++) {
+                update_angles(rest_angles, new_angles, false);
-                    float new_angle = lerp(angles[i], rest_angles[i], 0.5);
-                    if (new_angle != angles[i]) {
-                        angles[i] = new_angle;
-                        updated = true;
-                    }
-                }
                 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] >= 50) walk_dir = -1;
-                if (lp[0][1] <= 100) 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},