实现向前跳跃，运动中跳跃，落地平衡功能。

Shibo-main-TOMMY/test/main.cpp

@@ -1,7 +1,13 @@
-// -----------------------------------------------------------------------------
-// Copyright (c) 2024 Mu Shibo
-// Modify by Tommy 2025.8.11
-// -----------------------------------------------------------------------------
+/* -----------------------------------------------------------------------------
+Copyright (c) 2024 Mu Shibo
+ Modify by Tommy 2025.8.11
+2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。
+2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
+2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。
+2025-09-14 增加坐下和起立功能。
+2025-09-16 修复舵机发热问题。
+世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
+----------------------------------------------------------------------------- */
 
 #include <esp_now.h>
 #include <WiFi.h>
@@ -82,13 +88,17 @@ float YAW_angle_total = 0;
 float YAW_angle_zero_point = -10;
 float YAW_output = 0;
 
-
 // 腿部舵机控制数据
 byte ID[2];
 s16 Position[2];
 u16 Speed[2];
 byte ACC[2];
 
+// 腿部舵机全局变量
+int lastPosition0 = 2048;
+int lastPosition1 = 2048;
+#define SERVO_DEAD_ZONE 5  // 设置死区阈值 最小更新间隔(ms)
+
 // 逻辑处理标志位
 float robot_speed = 0;         // 记录当前轮部转速
 float robot_speed_last = 0;    // 记录上一时刻的轮部转速
@@ -631,7 +641,7 @@ void leg_loop()
   {
       Position[0] = SERVO1_MIN;
       Position[1] = SERVO2_MIN;
-      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+      safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
       return; // 跳过后续控制
   }
 
@@ -703,7 +713,7 @@ void leg_loop()
     else if (Position[1] < SERVO2_MAX) // 舵机2最高
       Position[1] = SERVO2_MAX;
     
-    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
   }
 }
 
@@ -916,4 +926,32 @@ void resetToDefault() {
     pid_lqr_u.error_prev = 0;
 
   
-}
+}
+
+// 优化的舵机控制函数，避免舵机发热
+void safeSyncWritePosEx(byte* id, byte idCount, s16* position, u16* speed, byte* acc) {
+
+  // 检查位置变化是否超过死区
+  bool needUpdate = false;
+  for(int i = 0; i < idCount; i++) {
+    if(id[i] == 1 && abs(position[i] - lastPosition0) > SERVO_DEAD_ZONE) {
+      needUpdate = true;
+      break;
+    }
+    if(id[i] == 2 && abs(position[i] - lastPosition1) > SERVO_DEAD_ZONE) {
+      needUpdate = true;
+      break;
+    }
+  }
+  
+  // 如果需要更新，发送指令并记录新位置和时间
+  if(needUpdate) {
+    sms_sts.SyncWritePosEx(id, idCount, position, speed, acc);
+    
+    // 更新记录的位置
+    for(int i = 0; i < idCount; i++) {
+      if(id[i] == 1) lastPosition0 = position[i];
+      if(id[i] == 2) lastPosition1 = position[i];
+    }
+  }
+}