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

README.md

@@ -3,18 +3,11 @@
 2025-08-16
 优化了原世博开源轮足代码，使用的是vscode platformio。
 修改的地方挺多的不罗列了，代码增加了很多备注。
-
+2025-08-11增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
-2025-08-11
+2025-08-16对几种运动中倒地的极限状态进行优化，实现自平衡起身。
-增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
+2025-09-14增加坐下和起立功能。
-
+2025-09-16修复舵机发热问题。
-2025-08-16
+2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
-对几种运动中倒地的极限状态进行优化，实现自平衡起身。
-
-2025-09-14
-增加坐下和起立功能。
-
-2025-09-16
-修复舵机发热问题。
 
 
 世博开源轮足优化版-TOMMY版本项目地址：

Shibo-main-TOMMY/src/main.cpp

@@ -1,7 +1,14 @@
-// -----------------------------------------------------------------------------
+/* -----------------------------------------------------------------------------
-// Copyright (c) 2024 Mu Shibo
+Copyright (c) 2024 Mu Shibo
-// Modify by Tommy 2025.8.11
+ Modify by Tommy 2025.8.11
-// -----------------------------------------------------------------------------
+2025-08-16 优化了原世博开源轮足平衡逻辑，代码增加了较多备注。
+2025-08-11 增加开机自平衡站立，简化控制，通过 esp32 遥控器直接控制，无需配网。
+2025-08-16 对几种运动中倒地的极限状态进行优化，实现自平衡起身。
+2025-09-14 增加坐下和起立功能。
+2025-09-16 修复舵机发热问题。
+2025-09-19 实现向前跳跃，运动中跳跃，落地平衡功能。
+世博开源轮足优化版-TOMMY版本项目地址： https://gitee.com/myperworld/wrobot
+----------------------------------------------------------------------------- */
 
 #include <esp_now.h>
 #include <WiFi.h>
@@ -53,12 +60,12 @@ MPU6050 mpu6050(I2Ctwo);
 #define pi 3.1415927
 
 // 舵机位置限制参数
-#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
+#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手 =2048-1900=148
-#define SERVO1_MAX 2400 // 舵机1最高
+#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的) 2048+148=2196
-#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
+#define SERVO1_MAX 2400 // 舵机1最高 =2048-2499=-451
-#define SERVO2_MAX 1800 // 舵机2最高
+#define SERVO2_MAX 1800 // 舵机2最高 2048-451=1597
 
-#define  LEG_HEIGHT_BASE 32  // 腿部默认高度基准值
+#define  LEG_HEIGHT_BASE 20  // 32 腿部默认高度基准值，最越小，机身越高
 
 // LQR自平衡控制器参数
 float LQR_angle = 0;
@@ -124,6 +131,9 @@ bool last_robot_enabled = true; // 记录上一次的小车状态
 bool sitting_down = false; // 标记是否正在执行坐下动作
 float original_angle_zeropoint = 5; // 保存原始的角度零点
 
+// 向前跳跃控制变量
+int forward_jump_flag = 0;     // 向前跳跃状态标志
+
 typedef struct {
   byte j1PotX;  // 左摇杆X  (0-255)
   byte j1PotY;  // 左摇杆Y  (0-255)
@@ -222,9 +232,9 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
     wrobot.dir = 4; // 不跳跃
   }
 
-  // led蓝灯爆闪
+  // 向前跳跃
-  if (lastbuttonLB && !convertedbuttonLB) {
+  if (lastbuttonLB && !convertedbuttonLB && robot_enabled) {
-    blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
+     forward_jump_flag = 1;
   }
   // 更新上次 lastbuttonRB 的值
   lastbuttonLB = convertedbuttonLB;
@@ -260,7 +270,7 @@ void setup()
   // Wifi初始化 - 修改：设置通道为6
   WiFi.mode(WIFI_STA); // 设置为STA模式
   // 设置WiFi通道为6
-  // esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
+  esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
   
   // Wifi初始化
   // WiFi_SetAP(); 
@@ -383,8 +393,8 @@ void loop()
       
       // 先给个向后退脉冲
       if (sit_counter < 30) {
-        // 设置电机目标为正，使小车向后运动
+        // 阶段1：设置电机目标为负数，使小车向后运动
-        motor1.target = -1.0;  // 2V电压，向后转（轮子向前转，小车向后运动）
+        motor1.target = -1.0;  // 负数小车向后，正数向前
         motor2.target = -1.0;
       } else {
         // 阶段2：停止电机，让小车自然向后倒
@@ -400,10 +410,10 @@ void loop()
       if (sit_counter == 0) {
         Position[0] = SERVO1_MIN;
         Position[1] = SERVO2_MIN;
-        Speed[0] = 100;
-        Speed[1] = 100;
         ACC[0] = 5;
         ACC[1] = 5;
+        Speed[0] = 100;
+        Speed[1] = 100;
         sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
       }
 
@@ -430,7 +440,6 @@ void loop()
   lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms
   yaw_loop();         // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms
   leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms
-  // jump_loop();
 
   // 记录上一次的遥控数据数据
   wrobot.dir_last = wrobot.dir;
@@ -500,6 +509,21 @@ void loop()
   //     Serial.println(robot_speed);
   // }
 
+  // 处理向前跳跃
+  if (forward_jump_flag) {
+    forward_jump_flag++;
+    if (forward_jump_flag < 25) {
+      // 保持向前脉冲
+      motor1.target = 1.8;
+      motor2.target = 1.8;
+    } else {
+      // 跳跃结束，重置状态
+      forward_jump_flag = 0;
+
+      wrobot.dir = 5; // 跳跃
+    }
+  }
+
   // 迭代计算FOC相电压
   motor1.loopFOC();
   motor2.loopFOC();
@@ -564,6 +588,12 @@ void lqr_balance_loop()
     distance_zeropoint = LQR_distance; // 位移零点重置
   }
 
+  // 轮足离地，重置位移零点
+  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 )) 
+  {
+    distance_zeropoint = LQR_distance;
+  }
+
   // 计算pitch的角度和角速度的pid控制值
   // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
   // 计算位移误差，并通过位移 PID 控制器得到位移控制量
@@ -577,9 +607,10 @@ void lqr_balance_loop()
   robot_speed = LQR_speed;
   // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
   // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
-  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) 
+  // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) 
+  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100)) 
   {
-    distance_zeropoint = LQR_distance;    // 位移零点重置
+    // distance_zeropoint = LQR_distance;    // 位移零点重置
     LQR_u = angle_control + gyro_control;  // 位移和速度控制分量被忽略
     pid_lqr_u.error_prev = 0;             // 输出积分清零
   }
@@ -644,12 +675,12 @@ void leg_loop()
   if (jump_flag == 0) // 不处于跳跃状态
   {
     // 舵机加速度
-    ACC[0] = 8;
+    ACC[0] = 150; // max 230
-    ACC[1] = 8;
+    ACC[1] = 150;
 
     // 舵机速度
-    Speed[0] = 300;
+    Speed[0] = 500;
-    Speed[1] = 300;
+    Speed[1] = 500;
     float roll_angle = (float)mpu6050.getAngleX() + 2.0;
     leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
 
@@ -716,9 +747,29 @@ void jump_loop()
 { // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
   // 跳跃，腿部以最快速度伸到最长
   // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
+
+   // 缩到最低，开启jump_flag计数
   if ((wrobot.dir == 5) && (jump_flag == 0))
   { 
+      ACC[0] = 150;
+      ACC[1] = 150;
+      Speed[0] = 300;
+      Speed[1] = 300;
+      Position[0] = SERVO1_MIN; 
+      Position[1] = SERVO2_MIN;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+      
+      jump_flag = 1;
+  }
+
+  if (jump_flag > 0) // 跳跃计数;
+  {
+    jump_flag ++;
+  }
+
   // 跳跃开始
+  if(jump_flag == 20)
+  {
     ACC[0] = 0;
     ACC[1] = 0;
     Speed[0] = 0;
@@ -726,31 +777,31 @@ void jump_loop()
     Position[0] = SERVO1_MAX; 
     Position[1] = SERVO2_MAX;
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
-
-    jump_flag = 1;
-
   }
-  if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
-  {
-    jump_flag++;
 
-    if ((jump_flag > 30) && (jump_flag < 35))
+   // 跳跃中，腿部缩起来
+  if ((jump_flag > 35) && (jump_flag < 40))
   {
     ACC[0] = 0;
     ACC[1] = 0;
     Speed[0] = 0;
     Speed[1] = 0;
-      Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
+    // Position[0] = 2048 + 12 + 8.4 * (35 - 32)=2048+12+8.4*35-8.4*32=2048+12+8.4*13=2048+12+105.2=2163; // 降低腿部高度，即车重心
-      Position[1] = 2048 - 12 - 8.4 * (35 - 32);
+    // Position[1] = 2048 - 12 - 8.4 * (35 - 32)=2048-12-8.4*35+8.4*32=2048-12-8.4*13=2048-12-105.2=1943;
+    Position[0] = SERVO1_MIN; // 降低腿部高度，即车重心
+    Position[1] = SERVO2_MIN;
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
 
-      jump_flag = 40;
+    jump_flag = 45;
   }
-    if (jump_flag > 200)
+
+  // jump_flag归零
+  if (jump_flag > 350) //  2000=6s ,1s=2000/6=333.33
   {
     jump_flag = 0; // 跳跃过程结束
+
   }
-  }
+  
 }
 
 // yaw轴转向控制 （转向控制）

Shibo-main-TOMMY/test/main copy.cpp

@@ -0,0 +1,976 @@
+/* -----------------------------------------------------------------------------
+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>
+#include "esp_wifi.h"
+#include "main.h"
+
+/************实例定义*************/
+
+// 电机实例
+BLDCMotor motor1 = BLDCMotor(7);
+BLDCMotor motor2 = BLDCMotor(7);
+BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
+BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
+
+// 编码器实例
+TwoWire I2Cone = TwoWire(0);
+TwoWire I2Ctwo = TwoWire(1);
+MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
+MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
+
+// PID控制器实例
+PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
+PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
+PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
+PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
+PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
+PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
+PIDController pid_lqr_u(1, 15, 0, 100000, 8);
+PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
+PIDController pid_roll_angle(8, 0, 0, 100000, 450);
+
+// 低通滤波器实例
+LowPassFilter lpf_joyy(0.2);
+LowPassFilter lpf_zeropoint(0.1);
+LowPassFilter lpf_roll(0.3);
+
+// WebServer实例
+WebServer webserver;                               // server服务器
+WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
+RobotProtocol rp(20);
+
+// STS舵机实例
+SMS_STS sms_sts;
+
+// MPU6050实例
+MPU6050 mpu6050(I2Ctwo);
+
+/************参数定义*************/
+#define pi 3.1415927
+
+// 舵机位置限制参数
+#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手 =2048-1900=148
+#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的) 2048+148=2196
+#define SERVO1_MAX 2400 // 舵机1最高 =2048-2499=-451
+#define SERVO2_MAX 1800 // 舵机2最高 2048-451=1597
+
+#define  LEG_HEIGHT_BASE 20  // 32 腿部默认高度基准值，最越小，机身越高
+
+// LQR自平衡控制器参数
+float LQR_angle = 0;
+float LQR_gyro = 0;
+float LQR_gyroZ = 0;
+float LQR_speed = 0;
+float LQR_distance = 0;
+float angle_control = 0;
+float gyro_control = 0;
+float speed_control = 0;
+float distance_control = 0;
+float LQR_u = 0;
+float angle_zeropoint = 5;  // 5 设定默认俯仰角度，向前到正，向后倒负
+float distance_zeropoint =  0.5f; // 轮部位移零点偏置
+
+// YAW轴控制数据
+float YAW_gyro = 0;
+float YAW_angle = 0;
+float YAW_angle_last = 0;
+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;    // 记录上一时刻的轮部转速
+int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
+int jump_flag = 0;             // 跳跃时段标识
+float leg_position_add = 0;    // roll轴平衡控制量
+
+// 电压检测
+uint16_t bat_check_num = 0;
+int BAT_PIN = 35; // select the input pin for the ADC
+static esp_adc_cal_characteristics_t adc_chars;
+static const adc1_channel_t channel = ADC1_CHANNEL_7;
+static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
+static const adc_atten_t atten = ADC_ATTEN_DB_11;
+static const adc_unit_t unit = ADC_UNIT_1;
+
+int loopCounter = 0; // loop函数执行计数
+int resetToDefaultCounter = 0; // resetToDefault执行计数
+
+// 电量显示LED
+#define LED_BAT 13
+
+// 定义摇杆数值死区参数
+const int deadZone = 15;
+
+// 小车状态控制
+bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
+bool last_robot_enabled = true; // 记录上一次的小车状态
+bool sitting_down = false; // 标记是否正在执行坐下动作
+float original_angle_zeropoint = 5; // 保存原始的角度零点
+
+typedef struct {
+  byte j1PotX;  // 左摇杆X  (0-255)
+  byte j1PotY;  // 左摇杆Y  (0-255)
+  byte j2PotX;  // 右摇杆X  (0-255)
+  byte j2PotY;  // 右摇杆Y (0-255)
+  bool buttonR1;  //底部右按键 1：true （默认），0：false
+  bool buttonR2;  //底部左按键 1：true （默认），0：false
+  bool j1Button;  //左摇杆按键 1：true （默认），0：false
+  bool j2Button;  //右摇杆按键 1：true （默认），0：false
+  bool buttonLB;  //顶部左按键 1：true （默认），0：false
+  bool buttonRB;  //顶部右按键 1：true （默认），0：false
+  bool tSwitch1;  //拨动开关   1：true （默认），0：false 
+  byte roll;      // 默认 127
+  byte pitch;     // 默认 127
+  bool buttonR3;  // 默认 1
+} JoystickData;
+
+JoystickData remoteData;
+
+// 蓝灯爆闪
+bool lastbuttonLB = false;
+
+// ESP-NOW接收回调函数
+void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+  memcpy(&remoteData, incomingData, sizeof(remoteData));
+
+  // Serial.println("收到数据");
+  // Serial.print("j1PotX="); Serial.print(remoteData.j1PotX);
+  // Serial.print(", j1PotY="); Serial.print(remoteData.j1PotY);
+  // Serial.print(", j2PotX="); Serial.print(remoteData.j2PotX);
+  // Serial.println(", j2PotY="); Serial.println(remoteData.j2PotY);
+  // Serial.print(", buttonR1="); Serial.print(remoteData.buttonR1);
+  // Serial.print(", buttonR2="); Serial.print(remoteData.buttonR2);
+  // Serial.print(", j1Button="); Serial.print(remoteData.j1Button);
+  // Serial.print(", j2Button="); Serial.print(remoteData.j2Button);
+  // Serial.print(", buttonLB="); Serial.print(remoteData.buttonLB);
+  // Serial.print(", buttonRB="); Serial.println(remoteData.buttonRB);
+  // Serial.print(", tSwitch1="); Serial.print(remoteData.tSwitch1);
+  // Serial.print(", roll="); Serial.print(remoteData.roll);
+  // Serial.print(", pitch="); Serial.print(remoteData.pitch);
+  // Serial.print(", buttonR3="); Serial.println(remoteData.buttonR3);
+
+    // 取反按钮值，使得默认显示 0 false，按下显示 1 true
+  int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
+  int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
+  int convertedbuttonR1 = !remoteData.buttonR1; // 底部右按键（取反：默认0，按下1）
+  int convertedbuttonR2 = !remoteData.buttonR2; // 底部左按键（取反：默认0，按下1）
+
+
+  // 处理坐下/起立指令
+  if (convertedbuttonR2 && robot_enabled) { // 底部左按键按下，坐下
+    robot_enabled = false;
+    sitting_down = true; // 标记开始坐下动作
+    original_angle_zeropoint = angle_zeropoint; // 保存当前的角度零点
+  }
+  if (convertedbuttonR1 && !robot_enabled) { // 底部右按键按下，起立
+    robot_enabled = true;
+    sitting_down = false; // 清除坐下动作标记
+    resetToDefault(); // 重置参数准备起立
+  }
+
+  // 如果小车处于坐下状态或正在执行坐下动作，不处理其他指令
+  if (!robot_enabled || sitting_down) {
+    wrobot.dir = 4; // 强制设为"不跳跃"
+    wrobot.joyx = 0; // 清空左右转向指令
+    wrobot.joyy = 0; // 清空前后移动指令
+    wrobot.roll = 0; // 清空左右摇摆指令
+    wrobot.height = 0; // 清空高度指令
+    return;
+  }
+
+  // 跳跃状态不接受指令
+  if( jump_flag > 0){
+    wrobot.dir = 4; // 强制设为“不跳跃”
+    wrobot.joyx = 0; // 清空左右转向指令
+    wrobot.joyy = 0; // 清空前后移动指令
+    wrobot.roll = 0; // 清空左右摇摆指令
+    return;
+  }
+
+  // 摇杆输出范围0-255，转换到 -100 到 100
+  // roll 左右摇摆
+  int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
+  // 上下高度
+  int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0  y轴取反，这样可以使机身下降
+  // 左右旋转
+  int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
+  // 前后速度
+  int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
+  
+
+  // 跳跃
+  if (convertedbuttonRB) { // 1=true，0=false
+    wrobot.dir = 5; // 跳跃
+  }else{
+    wrobot.dir = 4; // 不跳跃
+  }
+
+  // led蓝灯爆闪
+  if (lastbuttonLB && !convertedbuttonLB) {
+    blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
+  }
+  // 更新上次 lastbuttonRB 的值
+  lastbuttonLB = convertedbuttonLB;
+
+  if(abs(convertedJ1PotY) > deadZone) {    // 上下高度 15 ~ 75
+    wrobot.height = convertedJ1PotY;
+  }else{
+    wrobot.height = 0;
+  }
+
+  if(abs(convertedJ1PotX) > deadZone) {    // 左右摇摆 -30 ~ 30
+    wrobot.roll  = convertedJ1PotX;
+  }else{
+    wrobot.roll = 0;
+  }
+  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+    wrobot.joyy = convertedJ2PotY;
+  }else{
+    wrobot.joyy = 0;
+  } 
+  if(abs(convertedJ2PotX) > deadZone) { // 左右运动  -100 到 100
+    wrobot.joyx = convertedJ2PotX; 
+  } else {
+    wrobot.joyx = 0;
+  }
+}
+
+void setup()
+{
+  Serial.begin(115200);   // 通讯串口
+  Serial2.begin(1000000); // 腿部sts舵机
+
+  // Wifi初始化 - 修改：设置通道为6
+  WiFi.mode(WIFI_STA); // 设置为STA模式
+  // 设置WiFi通道为6
+  esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
+  
+  // Wifi初始化
+  // WiFi_SetAP(); 
+  webserver.begin();
+  webserver.on("/", HTTP_GET, basicWebCallback);
+  websocket.begin();
+  websocket.onEvent(webSocketEventCallback);
+
+  // 初始化ESP-NOW
+  // WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
+  if (esp_now_init() != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  esp_now_register_recv_cb(OnDataRecv);
+
+  // Serial.print("WIFI channel:");
+  // Serial.println(WiFi.channel());
+
+  // 舵机初始化
+  sms_sts.pSerial = &Serial2;
+  ID[0] = 1;
+  ID[1] = 2;
+  ACC[0] = 30;
+  ACC[1] = 30;
+  Speed[0] = 400;
+  Speed[1] = 400;
+  Position[0] = 2048;
+  Position[1] = 2048;
+  // Position[0] = SERVO1_MIN;
+  // Position[1] = SERVO2_MIN;
+  // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
+  sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+  // 电压检测
+  adc_calibration_init();
+  adc1_config_width(width);
+  adc1_config_channel_atten(channel, atten);
+  esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
+
+  // 电量显示LED
+  pinMode(LED_BAT, OUTPUT);
+
+  // 编码器设置
+  I2Cone.begin(19, 18, 400000UL);
+  I2Ctwo.begin(23, 5, 400000UL);
+  sensor1.init(&I2Cone);
+  sensor2.init(&I2Ctwo);
+
+  // mpu6050设置
+  mpu6050.begin();
+  mpu6050.calcGyroOffsets(true);
+
+  // 连接motor对象与编码器对象
+  motor1.linkSensor(&sensor1);
+  motor2.linkSensor(&sensor2);
+
+  // 速度环PID参数
+  motor1.PID_velocity.P = 0.05;
+  motor1.PID_velocity.I = 1;
+  motor1.PID_velocity.D = 0;
+
+  motor2.PID_velocity.P = 0.05;
+  motor2.PID_velocity.I = 1;
+  motor2.PID_velocity.D = 0;
+
+  // 驱动器设置
+  motor1.voltage_sensor_align = 6;
+  motor2.voltage_sensor_align = 6;
+  driver1.voltage_power_supply = 8;
+  driver2.voltage_power_supply = 8;
+  driver1.init();
+  driver2.init();
+
+  // 连接motor对象与驱动器对象
+  motor1.linkDriver(&driver1);
+  motor2.linkDriver(&driver2);
+
+  motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
+  motor2.torque_controller = TorqueControlType::voltage;
+  motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
+  motor2.controller = MotionControlType::torque;
+    
+
+  // monitor相关设置
+  motor1.useMonitoring(Serial);
+  motor2.useMonitoring(Serial);
+  // 电机初始化
+  motor1.init();
+  motor1.initFOC();
+  motor2.init();
+  motor2.initFOC();
+
+  resetToDefault(); // 重置到默认参数状态
+
+  delay(500);
+}
+
+void loop()
+{
+  
+  // 检测小车状态变化  坐 -> 站
+  if (robot_enabled && !last_robot_enabled) {
+    // 从坐下状态变为起立状态，重置参数
+    resetToDefault();
+  }
+  last_robot_enabled = robot_enabled;
+  
+  // 如果小车处于坐下状态，只执行必要的功能
+  if (!robot_enabled) {
+    bat_check();        // 电压检测
+    web_loop();         // Web数据更新
+    mpu6050.update();   // IMU数据更新
+    
+    // 如果正在执行坐下动作，逐步调整角度零点让小车向后倒
+    if (sitting_down) {
+      // 缓慢调整角度零点，使小车向后倒
+      static int sit_counter = 0;
+      sit_counter++;
+      
+      // 先给个向后退脉冲
+      if (sit_counter < 30) {
+        // 阶段1：设置电机目标为正，使小车向后运动
+        motor1.target = -1.0;  // 2V电压，向后转（轮子向前转，小车向后运动）
+        motor2.target = -1.0;
+      } else {
+        // 阶段2：停止电机，让小车自然向后倒
+        motor1.target = 0;
+        motor2.target = 0;
+      }
+      motor1.loopFOC();
+      motor2.loopFOC();
+      motor1.move();
+      motor2.move();
+
+      // 当sit_counter为0时，设置舵机到最低位置
+      if (sit_counter == 0) {
+        Position[0] = SERVO1_MIN;
+        Position[1] = SERVO2_MIN;
+        ACC[0] = 5;
+        ACC[1] = 5;
+        Speed[0] = 100;
+        Speed[1] = 100;
+        sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+      }
+
+      if (sit_counter < 100) { // 分100步完成坐下动作
+        // 逐步将平衡零点变成负数，小车抬头（向后倒）
+        angle_zeropoint -- ;
+      } else {
+        // 坐下动作完成，设置到最低位置和最终角度零点
+        sit_counter = 0;
+        sitting_down = false;
+        angle_zeropoint = -20; // 最终的角度零点，确保小车向后倒
+      }
+    } else {
+      // 已经坐下，保持最低位置和向后倒的角度零点
+      angle_zeropoint = -15; // 保持向后倒的角度零点
+    }
+    
+    return; // 跳过其他控制
+  }
+  
+  bat_check();        // 电压检测 <1ms
+  web_loop();         // Web数据更新 <2ms 
+  mpu6050.update();   // IMU数据更新  <1ms
+  lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms
+  yaw_loop();         // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms
+  leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms
+
+  // 记录上一次的遥控数据数据
+  wrobot.dir_last = wrobot.dir;
+  wrobot.joyx_last = wrobot.joyx;
+  wrobot.joyy_last = wrobot.joyy;
+
+  /* 将自平衡计算输出转矩赋给电机
+  motor.target 负数小车向前转，正数小车向后转，
+  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  LQR_u = angle_control + gyro_control + distance_control + speed_control
+
+   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
+                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
+   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
+                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
+    
+    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
+   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
+                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
+                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
+   
+    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
+                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
+                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
+                  //LQR_u与实际shaft_velocity速度正相关
+   */
+  motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取负号是因为电机是反的
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+
+  // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
+  //  Serial.println(LQR_angle);
+
+  // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
+  loopCounter++;
+  // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
+  // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
+  if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
+      resetToDefault();
+      resetToDefaultCounter++;
+      motor1.target = 0;
+      motor2.target = 0;
+      // Serial.println("resetToDefault");
+  }
+
+  if( abs(LQR_angle) < 25.0f){
+    resetToDefaultCounter = 0;
+  }
+
+  // target目标电压 0~8V, 0为不转, 8为最大转速
+  // Serial.print("LQR_angle:");
+  // Serial.println(LQR_angle);
+  // // Serial.print(",loopCounter:");
+  // // Serial.print(loopCounter);
+  // Serial.print(",resetToDefaultCounter:");
+  // Serial.print(resetToDefaultCounter);
+  // // Serial.print(",motor1.target:");
+  // // Serial.print(motor1.target);
+  // Serial.print(",motor2.target:");
+  // Serial.println(motor2.target);
+
+      // 监测速度变化
+  // int speed_delta = robot_speed - robot_speed_last;
+  // if(speed_delta > 0){
+  //     Serial.print("speed_delta:");
+  //     Serial.print(speed_delta);
+  //     Serial.print(",robot_speed:");
+  //     Serial.println(robot_speed);
+  // }
+
+  // 迭代计算FOC相电压
+  motor1.loopFOC();
+  motor2.loopFOC();
+
+  // 设置轮部电机输出
+  motor1.move();
+  motor2.move();
+
+} // loop()
+
+// lqr自平衡控制
+void lqr_balance_loop()
+{
+
+  // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
+  // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
+  // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
+  
+  // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
+  // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) 
+        // + LQR_k2*LQR_gyro 
+        // + LQR_k3*(LQR_distance - distance_zeropoint) 
+        // + LQR_k4*LQR_speed;
+
+  // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
+  // 编码器反馈shaft_angle角度，shaft_velocity速度
+  // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
+  // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
+  // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
+  LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
+  LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
+  LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
+  LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
+
+  // 计算pitch的角度和角速度的pid控制值
+  // 实际pitch角度与零点的差值，目标值是零
+  // 计算角度误差，并通过 PID 控制器得到角度控制量
+  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
+  // 实际pitch 角速度与目标值的差值，目标值是零
+  // 通过 PID 控制器对角速度进行处理，得到角速度控制量
+  gyro_control = pid_gyro(LQR_gyro);
+
+  // 运动细节优化处理
+  if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    pid_lqr_u.error_prev = 0;          // 输出积分清零
+  }
+  // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
+  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
+  {
+    wrobot_move_stop_flag = 1;
+  }
+  if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    wrobot_move_stop_flag = 0;
+  }
+
+  if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+  }
+
+  // 计算pitch的角度和角速度的pid控制值
+  // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
+  // 计算位移误差，并通过位移 PID 控制器得到位移控制量
+  distance_control = pid_distance(LQR_distance - distance_zeropoint); 
+  // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
+  // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
+  speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
+
+  // 轮部离地检测, 连续记录两次的轮部转速
+  robot_speed_last = robot_speed; // 记录连续两次的轮部转速
+  robot_speed = LQR_speed;
+  // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
+  // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
+  // if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) 
+  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag > 0 and jump_flag < 100)) 
+  {
+    distance_zeropoint = LQR_distance;    // 位移零点重置
+    LQR_u = angle_control + gyro_control;  // 位移和速度控制分量被忽略
+    pid_lqr_u.error_prev = 0;             // 输出积分清零
+  }
+  else // 当轮部未离地时，LQR_u：4个参数
+  {
+    // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
+    LQR_u = angle_control + gyro_control + distance_control + speed_control;
+    // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
+    
+  }
+
+  // 小车没有控制的时候自稳定状态
+  // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
+  if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
+  {
+
+    LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
+    // Serial.println(LQR_u);
+
+    // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
+    // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
+    // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
+    // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
+    angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
+  }
+  else
+  {
+    pid_lqr_u.error_prev = 0; // 输出积分清零
+  }
+
+  // 平衡控制参数自适应
+  if (wrobot.height < 50)
+  {
+    pid_speed.P = 0.7;
+  }
+  else if (wrobot.height < 64)
+  {
+    pid_speed.P = 0.6;
+  }
+  else
+  {
+    pid_speed.P = 0.5;
+  }
+
+}
+
+// 腿部动作控制（舵机）
+void leg_loop()
+{
+
+  // 倒地时强制腿部收缩
+  if (LQR_angle < -30.0f ||  LQR_angle > 45.0f) 
+  {
+      Position[0] = SERVO1_MIN;
+      Position[1] = SERVO2_MIN;
+      safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
+      return; // 跳过后续控制
+  }
+
+  jump_loop();
+
+  if (jump_flag == 0) // 不处于跳跃状态
+  {
+    // 舵机加速度
+    ACC[0] = 150; // max 230
+    ACC[1] = 150;
+
+    // 舵机速度
+    Speed[0] = 500;
+    Speed[1] = 500;
+    float roll_angle = (float)mpu6050.getAngleX() + 2.0;
+    leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
+
+    int roll_offset = wrobot.roll;
+    int left_height, right_height;
+
+    // Serial.print("roll_offset: ");
+    // Serial.println(roll_offset);
+
+    // 如果控制了 roll 角度，则调整左右脚的高度
+    // 计算三角函数值
+    float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
+    float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
+    float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
+    if (roll_offset > 10) {
+      // 正数，左脚高度 60，右脚高度 40
+      // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
+      left_height = wrobot.height + RollOffset + heightAdjust;
+      right_height = wrobot.height + RollOffset - heightAdjust;
+      leg_position_add = 0; // 不使用roll角度 pid计算
+    } else if (roll_offset < -10) {
+      // 负数，左脚高度 40，右脚高度 60
+      left_height = wrobot.height + RollOffset - heightAdjust;
+      right_height = wrobot.height + RollOffset + heightAdjust;
+      leg_position_add = 0;
+    } else { // 未控制 roll 角度
+      // roll 为 0，左右恢复到同一高度
+      left_height = wrobot.height;
+      right_height = wrobot.height;
+      // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
+      // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
+      if (abs(LQR_angle) > 25.0f) 
+      {
+        leg_position_add = 0;
+      }
+    }
+
+    // 根据新的高度计算舵机位置
+    // id 1 [0] 正面看右手，id 2 正面看左手 [1]
+    // 舵机id 1：1900（机身最矮）  2400（机身最高）2400-1900=500
+    // 舵机id 2：2300（机身最矮）  1800（机身最高）2300-1800=500
+    // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
+    // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高）-52（最低）
+    Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+    Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+
+    // 限制舵机位置范围
+    if (Position[0] < SERVO1_MIN) // 舵机1最低
+      Position[0] = SERVO1_MIN;
+    else if (Position[0] > SERVO1_MAX) // 舵机1最高
+      Position[0] = SERVO1_MAX;
+
+    if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
+      Position[1] = SERVO2_MIN;
+    else if (Position[1] < SERVO2_MAX) // 舵机2最高
+      Position[1] = SERVO2_MAX;
+    
+    safeSyncWritePosEx(ID, 2, Position, Speed, ACC);
+  }
+}
+
+// 跳跃控制
+void jump_loop()
+{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
+  // 跳跃，腿部以最快速度伸到最长
+  // if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
+
+   // 缩到最低，开启jump_flag计数
+  if ((wrobot.dir == 5) && (jump_flag == 0))
+  { 
+      ACC[0] = 150;
+      ACC[1] = 150;
+      Speed[0] = 300;
+      Speed[1] = 300;
+      Position[0] = SERVO1_MIN; 
+      Position[1] = SERVO2_MIN;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+      
+      jump_flag = 1;
+  }
+
+  if (jump_flag > 0) // 跳跃计数;
+  {
+    jump_flag ++;
+  }
+
+  // 跳跃开始
+  if(jump_flag == 20)
+  {
+    ACC[0] = 0;
+    ACC[1] = 0;
+    Speed[0] = 0;
+    Speed[1] = 0;
+    Position[0] = SERVO1_MAX; 
+    Position[1] = SERVO2_MAX;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+  }
+
+   // 跳跃中，腿部缩起来
+  if ((jump_flag > 35) && (jump_flag < 40))
+  {
+    ACC[0] = 0;
+    ACC[1] = 0;
+    Speed[0] = 0;
+    Speed[1] = 0;
+    // Position[0] = 2048 + 12 + 8.4 * (35 - 32)=2048+12+8.4*35-8.4*32=2048+12+8.4*13=2048+12+105.2=2163; // 降低腿部高度，即车重心
+    // Position[1] = 2048 - 12 - 8.4 * (35 - 32)=2048-12-8.4*35+8.4*32=2048-12-8.4*13=2048-12-105.2=1943;
+    Position[0] = SERVO1_MIN; // 降低腿部高度，即车重心
+    Position[1] = SERVO2_MIN;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+    jump_flag = 45;
+  }
+
+  // jump_flag归零
+  if (jump_flag > 350) //  2000=6s ,1s=2000/6=333.33
+  {
+    jump_flag = 0; // 跳跃过程结束
+  }
+  
+}
+
+// yaw轴转向控制 （转向控制）
+void yaw_loop()
+{
+  // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
+  yaw_angle_addup();
+
+  YAW_angle_total += wrobot.joyx * 0.01; // 0.002
+  float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
+  //  yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
+  // 然后，将这两个控制量相加，得到最终的 YAW_output。
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+}
+
+// Web数据更新
+void web_loop()
+{
+  webserver.handleClient();
+  websocket.loop();
+  rp.spinOnce(); // 更新web端回传的控制信息
+}
+
+// yaw轴角度累加函数
+void yaw_angle_addup()
+{
+  YAW_angle = (float)mpu6050.getAngleZ();// 左右偏航角度
+  YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度（转速），用于纠正小车前后走直线时的角度偏差
+
+  if (YAW_angle_zero_point == (-10))
+  {
+    YAW_angle_zero_point = YAW_angle;
+  }
+
+  float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
+  if (YAW_angle > YAW_angle_last)
+  {
+    yaw_angle_1 = YAW_angle - YAW_angle_last;
+    yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
+  }
+  else
+  {
+    yaw_angle_1 = YAW_angle - YAW_angle_last;
+    yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
+  }
+
+  if (abs(yaw_angle_1) > abs(yaw_angle_2))
+  {
+    yaw_addup_angle = yaw_angle_2;
+  }
+  else
+  {
+    yaw_addup_angle = yaw_angle_1;
+  }
+
+  YAW_angle_total = YAW_angle_total + yaw_addup_angle;
+  YAW_angle_last = YAW_angle;
+}
+
+void basicWebCallback(void)
+{
+  webserver.send(300, "text/html", basic_web);
+}
+
+void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
+{
+  if (type == WStype_TEXT)
+  {
+    String payload_str = String((char *)payload);
+    StaticJsonDocument<300> doc;
+    DeserializationError error = deserializeJson(doc, payload_str);
+
+    String mode_str = doc["mode"];
+    if (mode_str == "basic")
+    {
+      rp.parseBasic(doc);
+    }
+  }
+}
+
+// 电压检测初始化
+void adc_calibration_init()
+{
+  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
+  {
+    printf("eFuse Two Point: Supported\n");
+  }
+  else
+  {
+    printf("eFuse Two Point: NOT supported\n");
+  }
+  // Check Vref is burned into eFuse
+  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
+  {
+    printf("eFuse Vref: Supported\n");
+  }
+  else
+  {
+    printf("eFuse Vref: NOT supported\n");
+  }
+}
+
+// 电压检测
+void bat_check()
+{
+  if (bat_check_num > 1000)
+  {
+    // 电压读取
+    uint32_t sum = 0;
+    sum = analogRead(BAT_PIN);
+    uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
+    double battery = (voltage * 3.97) / 1000.0;
+
+    // Serial.println(battery);
+    // 电量显示
+    if (battery > 7.8)
+      digitalWrite(LED_BAT, HIGH);
+    else
+      digitalWrite(LED_BAT, LOW);
+
+    bat_check_num = 0;
+  }
+  else
+    bat_check_num++;
+}
+
+// LED闪烁函数(不要用for，会阻塞其他程序)
+void blinkLed() {
+  digitalWrite(LED_BAT, HIGH);
+  delay(200);
+  digitalWrite(LED_BAT, LOW);
+  delay(200);
+  digitalWrite(LED_BAT, HIGH);
+  delay(200);
+  digitalWrite(LED_BAT, LOW);
+  delay(200);
+}
+
+// 重置到默认参数状态，用于自平衡起立
+void resetToDefault() {
+
+    angle_zeropoint = 5;
+
+    LQR_speed = 0;
+    LQR_distance = 0;
+    speed_control = 0;
+    distance_control = 0;
+    distance_zeropoint =  0.5f;
+    LQR_u = 0;
+    
+    // YAW轴控制数据
+    YAW_gyro = 0;
+    YAW_angle = 0;
+    YAW_angle_last = 0;
+    YAW_angle_total = 0;
+    YAW_angle_zero_point = -10;
+    YAW_output = 0;
+
+    // 逻辑处理标志位
+    robot_speed = 0;
+    robot_speed_last = 0;
+    wrobot_move_stop_flag = 0;
+    jump_flag = 0;
+    leg_position_add = 0;
+
+    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];
+    }
+  }
+}

Shibo-main-TOMMY/test/main.cpp

@@ -1,7 +1,13 @@
-// -----------------------------------------------------------------------------
+/* -----------------------------------------------------------------------------
-// Copyright (c) 2024 Mu Shibo
+Copyright (c) 2024 Mu Shibo
-// Modify by Tommy 2025.8.11
+ 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);
   }
 }
 
@@ -917,3 +927,31 @@ void resetToDefault() {
 
   
 }
+
+// 优化的舵机控制函数，避免舵机发热
+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];
+    }
+  }
+}