手柄增加智能模式和手动模式，操纵机身升高降低、摇摆更加灵活。

v1.1/Shibo-main-TOMMY/.vscode/settings.json

@@ -26,6 +26,40 @@
         "fstream": "cpp",
         "utility": "cpp",
         "type_traits": "cpp",
-        "thread": "cpp"
+        "thread": "cpp",
+        "chrono": "cpp",
+        "cctype": "cpp",
+        "cerrno": "cpp",
+        "cfloat": "cpp",
+        "climits": "cpp",
+        "clocale": "cpp",
+        "condition_variable": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "exception": "cpp",
+        "functional": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "ratio": "cpp",
+        "system_error": "cpp",
+        "iomanip": "cpp",
+        "ios": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "locale": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "queue": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "cinttypes": "cpp",
+        "cstdbool": "cpp",
+        "typeinfo": "cpp"
     }
 }

v1.1/Shibo-main-TOMMY/lib/MPU6050_tockn/src/MPU6050_tockn.h

@@ -48,7 +48,7 @@ class MPU6050{
   float getGyroZ(){ return gyroZ; };
 
 	// void calcGyroOffsets(bool console = false, uint16_t delayBefore = 1000, uint16_t delayAfter = 3000);
-	void calcGyroOffsets(bool console = false, uint16_t delayBefore = 1000, uint16_t delayAfter = 500);
+	void calcGyroOffsets(bool console = false, uint16_t delayBefore = 1000, uint16_t delayAfter = 500); // 测试
 
   float getGyroXoffset(){ return gyroXoffset; };
   float getGyroYoffset(){ return gyroYoffset; };

v1.1/Shibo-main-TOMMY/src/main.cpp

@@ -24,7 +24,7 @@ void lqr_balance_loop();
 void yaw_loop();
 void leg_loop();
 void jump_loop();
-void resetToDefault();
+void resetZeroPoint();
 void jump_charge(); // 腿部弯曲（蓄能）
 
 // RGB相关函数
@@ -80,12 +80,18 @@ CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::G
 /************参数定义*************/
 #define pi 3.1415927
 
-// 舵机位置限制参数
+// 机身平衡角度调整 roll轴
-#define LEG_HEIGHT_BASE 20  //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
+float roll_adjust = 0.0f;  
+
+// 机身高度
+float leg_height_base = 20.0f;  // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
+
+// #define LEG_HEIGHT_BASE 20  //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
 #define SERVO0_MIN  2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
 #define SERVO1_MIN  2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
 #define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 
 #define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
+
 float leg_position_add = 0;    // roll轴平衡控制量
 int left_height;
 int right_height;
@@ -141,9 +147,6 @@ int jump_flag = 0; // 跳跃过程计数
 int jump_pre_flag = 0; // 跳跃前蓄能计数
 int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
 
-// 定义摇杆数值死区参数
-const int deadZone = 10;
-int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令
 
 // 电压检测
 #define LED_BAT 13
@@ -164,14 +167,10 @@ bool robot_enabled = true; // 小车使能状态，true为起立状态，false
 bool last_robot_enabled = true; // 记录上一次的小车状态
 int sitting_down = 0; // 标记是否正在执行坐下动作
 
-// 原地跳跃控制变量
-int forward_jump_flag = 0;     // 向前跳跃状态标志
-int backward_jump_flag = 0;    // 向后跳跃状态标志
-int left_jump_flag = 0;    // 向左跳跃状态标志
-int right_jump_flag = 0;    // 向右跳跃状态标志
 
 typedef struct
 {
+  int mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
   int height = 30;
   int roll;
   int jump = 0; // 0 =不跳跃 1= 跳跃
@@ -204,8 +203,7 @@ typedef struct {
 
 JoystickData remoteData;
 
-// 记录上一次按钮状态,防止连续触发
+const int deadZone = 10;
-bool last_btnY = 0; 
 bool last_btnJoyRight = 0;
 
 // ESP-NOW接收回调函数
@@ -229,7 +227,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   if (remoteData.btnStart && !robot_enabled) {
     robot_enabled = true;
     sitting_down = 0; // 清除坐下动作标记
-    resetToDefault(); // 重置参数准备起立
   }
 
   // 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
@@ -237,7 +234,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
     wrobot.jump = 0; // 禁止跳跃
     wrobot.roll = 0; // 禁止左右摇摆（起升相关）
     wrobot.height = 0; // 禁止高度调整（起升相关）
-    // 保留wrobot.joyx和wrobot.joyy，不设为0；删除return，让后续摇杆转换代码执行
   }
 
   // 跳跃状态不接受指令
@@ -255,31 +251,76 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0
   // int joyRightY;  // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0
   
-  // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
+    // 前后移动 右摇杆Y轴：原始1080-0 → 目标100到-100
-  int convertedJ1PotX = constrain(
+    int convertedJ2PotY = constrain(
-    map(remoteData.joyLeftX, 0, 900, -100, 100),  
+      map(remoteData.joyRightY, 1080, 0, 100, -100),  // 原始1080-0，目标100~-100
-    -100, 100
+      -100, 100
-  );
+    );
+    if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+      wrobot.joyy = convertedJ2PotY;
+    }else{
+      wrobot.joyy = 0;
+    } 
 
-  // 上下 左摇杆Y轴：原始940-0 → 目标60到0
-  int convertedJ1PotY = constrain(
-    map(remoteData.joyLeftY, 940, 0, 60, 0),  // 原始范围是940-0，目标0-60
-    0, 60
-  );
 
-  // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100
+  if(wrobot.mode == 0) // 智能模式
-  int convertedJ2PotX = constrain(
+  {
-    map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30),  // 先映射到-30~30，再按比例放大到-100~100
-    -100, 100
-  );
 
-  // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100
+    // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
-  int convertedJ2PotY = constrain(
+    int convertedJ1PotX = constrain(
-    map(remoteData.joyRightY, 1080, 0, 100, -100),  // 原始1080-0，目标100~-100
+      map(remoteData.joyLeftX, 0, 900, -100, 100),  
-    -100, 100
+      -100, 100
-  );
+    );
+    if(abs(convertedJ1PotX) > deadZone) { // 左右运动  -100 到 100
+      wrobot.joyx = convertedJ1PotX; 
+    } else {
+      wrobot.joyx = 0;
+    }
 
-  // 跳跃  右摇杆按键
+    // 智能计算wrobot.height ，根据前后速度，
+    // convertedJ2PotY 范围-100到100，默认0，输出范围30（最矮）-50（最高）, 默认 50
+    wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
+    wrobot.height = constrain(wrobot.height, 30, 50);
+    wrobot.height = lpf_height(wrobot.height);
+
+  }
+  else if(wrobot.mode == 1) // 手动模式
+  {
+    // 摇摆角度 左摇杆X轴，先映射到-60~60，再按比例放大到-100~100
+    int convertedJ1PotX = constrain(
+      map(remoteData.joyLeftX, 0, 900, -60, 60) * (100.0/60),   // 先映射到-30~30，再按比例放大到-100~100
+      -100, 100
+    );
+    if(abs(convertedJ1PotX) > deadZone) {
+      roll_adjust = convertedJ1PotX;
+    }else{
+      roll_adjust = 0;
+    }
+
+    // 上下高度 左摇杆Y轴：原始940-0 → 目标MAX到MIN
+    int convertedJ1PotY = constrain(
+        map(remoteData.joyLeftY, 940, 0, 100, -100), // 原始范围940-0，目标范围100到-100，默认中间0
+        -100, 100
+      );
+    wrobot.height = map( convertedJ1PotY, -100, 100, 30, 70);
+    wrobot.height = constrain(wrobot.height, 30, 70);
+    wrobot.height = lpf_height(wrobot.height);
+
+
+    // 左右转向 右摇杆X轴：原始0-960 → 目标-100到100 
+    int convertedJ2PotX = constrain(
+      map(remoteData.joyRightX, 0, 960, -100, 100),  // 先映射到-30~30，再按比例放大到-100~100
+      -100, 100
+    );
+    if(abs(convertedJ2PotX) > deadZone) {
+      wrobot.joyx  = convertedJ2PotX;
+    }else{
+      wrobot.joyx = 0;
+    }
+  }
+
+
+  // 跳跃 右摇杆按键
   if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false
     wrobot.jump = 1; // 跳跃
   }else{
@@ -287,61 +328,15 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   }
   last_btnJoyRight = remoteData.btnJoyRight; 
 
-  // 向前跳跃 Y键
+  // 切换智能模式和手动模式
-  if (remoteData.btnY && robot_enabled && !last_btnY) {
+  if (remoteData.btnY) {  // 1:左摇杆上下摇摆控制（手动）
-    forward_jump_flag = 1;
+      wrobot.mode = 1; 
   }
-  last_btnY = remoteData.btnY; // 防止连续输出向前跳跃
+  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
-
+      wrobot.mode = 0; 
-  // 向后跳跃 A键
+      roll_adjust = 0.0f; 
-  // if (remoteData.btnA && robot_enabled) {
+      leg_height_base = 20.0f;
-  //   backward_jump_flag = 1;
-  // }
-  // // 向左跳跃 X键
-  // if (remoteData.btnX && robot_enabled) {
-  //   left_jump_flag = 1;
-  // }
-  // // 向右跳跃 B键
-  // if (remoteData.btnB && robot_enabled) {
-  //   right_jump_flag = 1;
-  // }
-
-  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
-    wrobot.joyy = convertedJ2PotY;
-  }else{
-    wrobot.joyy = 0;
   }
-  // Serial.print("convertedJ2PotY:");
-  // Serial.print(convertedJ2PotY);
-  // Serial.print(",wrobot.joyy");
-  // Serial.println(wrobot.joyy);
-
-  if(abs(convertedJ1PotX) > deadZone) { // 左右运动  -100 到 100
-    wrobot.joyx = convertedJ1PotX; 
-  } else {
-    wrobot.joyx = 0;
-  }
-
-  // if(abs(convertedJ1PotY) > deadZone) {
-  //   wrobot.height = convertedJ1PotY;
-  // }else{
-  //   wrobot.height = 0;
-  // }
-
-  // 根据前后速度，智能计算wrobot.height 
-  // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
-  // 反向映射：绝对值30→60，绝对值100→0（线性过渡）
-  wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
-  // 强制约束输出在0~60范围内（防止异常值）
-  wrobot.height = constrain(wrobot.height, 30, 50);
-  // 使用低波过滤器，平滑数据
-  wrobot.height = lpf_height(wrobot.height);
-
-  // if(abs(convertedJ2PotX) > deadZone) {
-  //   wrobot.roll  = convertedJ2PotX;
-  // }else{
-  //   wrobot.roll = 0;
-  // }
   
 }
 
@@ -427,14 +422,13 @@ void setup()
   // monitor相关设置
   motor1.useMonitoring(Serial);
   motor2.useMonitoring(Serial);
+  
   // 电机初始化
   motor1.init();
   motor1.initFOC();
   motor2.init();
   motor2.initFOC();
 
-  // resetToDefault(); // 重置到默认参数状态
-
   // FastLED初始化
   FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
   FastLED.setBrightness(50); // 设置亮度
@@ -450,11 +444,6 @@ void loop()
   // 处理LED闪烁
   // handleLEDBlink();
   
-  // 检测小车状态变化  坐 -> 站
-  if (robot_enabled && !last_robot_enabled) {
-    resetToDefault();
-  }
-  last_robot_enabled = robot_enabled;
   
   bat_check();        // 电压检测
   delayMicroseconds(1500);
@@ -471,51 +460,27 @@ void loop()
   motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
   motor2.target = (-0.5) * (LQR_u - YAW_output);
 
-  // 小车倒地后，重置所有参数 测试
   loopCounter++;
   // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
   // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
   if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
-      resetToDefault();
+    resetZeroPoint();
-      resetToDefaultCounter++;
+    resetToDefaultCounter++;
+    motor1.target = 0;
+    motor2.target = 0;
+  }
+
+  // 大仰角到底，轮子就不要转了 修改
+  if (abs(LQR_angle) > 120.0f) {
       motor1.target = 0;
       motor2.target = 0;
   }
 
-  // 大仰角到底，轮子就不要转了
-  // if (abs(LQR_angle) > 70.0f) {
-  //     motor1.target = 0;
-  //     motor2.target = 0;
-  // }
-
   // 达到平衡角度后，重置重置计数器
   if( abs(LQR_angle) < 25.0f){
     resetToDefaultCounter = 0;
   }
 
-  // 向前跳跃
-  if(forward_jump_flag){
-    if(forward_jump_flag == 1){
-      jump_charge();
-      forward_jump_flag = 2;
-    }
-    if(forward_jump_flag == 2){
-      if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
-        forward_jump_flag = 3;
-      }
-    }
-    if(forward_jump_flag >= 3){
-      forward_jump_flag ++ ;
-      if ( forward_jump_flag < 25) {
-        motor1.target = 1.8; // 保持向前脉冲
-        motor2.target = 1.8;
-      } else {
-        forward_jump_flag = 0;
-        jump_pre_flag = 1;
-      }
-    }
-  }
-  
 
   // 小车坐下
   if (sitting_down) {
@@ -526,9 +491,14 @@ void loop()
       if(wrobot.joyy < 0){
         motor1.target = -1.0;  // 负数小车向后，正数向前
         motor2.target = -1.0;
+        Position[0] = SERVO0_MIN; // 修改
+        Position[1] = SERVO1_MIN;
       } else { // 否则默认向前倒地 给个向前脉冲
         motor1.target = 1.0;  // 负数小车向后，正数向前
         motor2.target = 1.0;
+        Position[0] = SERVO0_MIN + 60; // 修改
+        Position[1] = SERVO1_MIN - 60;
+        
       }
     }else{
 
@@ -541,17 +511,13 @@ void loop()
 
     // 收缩双腿，舵机到最低位置
     if (sitting_down == 5) {
-      ACC[0] = ACC[1] = 150;
+      ACC[0] = ACC[1] = 100;
       Speed[0] = Speed[1] = 200;
-      Position[0] = SERVO0_MIN;
-      Position[1] = SERVO1_MIN;
       sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
     }
    
   }
   
-  //  Serial.println( "");
-
   // 迭代计算FOC相电压
   motor1.loopFOC();
   motor2.loopFOC();
@@ -608,7 +574,6 @@ void lqr_balance_loop()
   // 前进后退跳跃的系数都不一样
   float speed_target_coeff = 0.1; 
   if( jump_flag ){ // 跳跃
-    // speed_target_coeff = 0.12;
     speed_target_coeff = 0.1;
   }else if(wrobot.joyy > 0 ){ // 前进
     speed_target_coeff = 0.18;
@@ -624,17 +589,11 @@ void lqr_balance_loop()
   unsigned long current_time = millis();
   if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
     robot_speed_diff = LQR_speed - last_lqr_speed;
-    // Serial.print("，robot_speed_last:");
-    // Serial.print(last_lqr_speed);
-    // Serial.print("，robot_speed_diff:");
-    // Serial.println(robot_speed_diff);
-    // Serial.print(",wheel_ground_flag:");
-    // Serial.println(wheel_ground_flag);
 
     if(robot_speed_diff > 18.0) // 轮子离地
     {
       wheel_ground_flag = 0; // 轮子离地标记
-      // Serial.print("wheel_ground_flag:");
+       // Serial.print("TAKE OFF");
       // Serial.print(wheel_ground_flag);
       // Serial.print(",robot_speed_diff:");
       // Serial.print(robot_speed_diff);
@@ -644,44 +603,39 @@ void lqr_balance_loop()
     if( robot_speed_diff < -9.0) // 轮子着地
     {
       wheel_ground_flag = 1; // 轮子着地标记
-      // Serial.print("wheel_ground_flag:");
+      // Serial.print("LANDING:");
-      // Serial.print(wheel_ground_flag);
       // Serial.print(",robot_speed_diff:");
       // Serial.print(robot_speed_diff);
       // Serial.print(",jump_flag:");
       // Serial.println(jump_flag); // 120左右
 
       if( jump_flag ){ // 落地点作为新的位移零点
-         distance_zeropoint = LQR_distance;
+         resetZeroPoint(); 
-         pid_lqr_u.error_prev = 0; 
       }
     }
     last_lqr_speed = LQR_speed;  // 每隔100ms更新一次历史转速
     last_speed_record_time = current_time;  // 更新记录时间
   }
    
-
+  // 重置位移零点和积分情形
-  // 判断摇杆是否没有前后左右运动指令
+  // 1.判断摇杆是否没有前后左右运动指令
   if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
   {
-    // wrobot_move_stop_flag = 1; 
+    resetZeroPoint(); 
-    distance_zeropoint = LQR_distance; // 位移零点重置
-    pid_lqr_u.error_prev = 0;  // 输出积分清零 测试
   }
   
-  // 实时重置位移零点和积分情形
+  // 2. 运动中实时重置位移零点和积分情形
-  if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) 
+  if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
      || wrobot.joyy != 0 || sitting_down ) 
   {
-    distance_zeropoint = LQR_distance;
+       resetZeroPoint(); 
-    pid_lqr_u.error_prev = 0;  // 输出积分清零
   }
 
   distance_control = pid_distance(LQR_distance - distance_zeropoint);
 
   // 计算 LQR_u
-  // 必须是在跳跃时，LQR_u= 角度控制量 + 角速度控制量
+  // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
-  if ( (jump_flag > 0 && abs(robot_speed_diff) > 10) ||  (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
+  if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
   {        
     LQR_u = angle_control + gyro_control;  // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
   }
@@ -717,7 +671,7 @@ void lqr_balance_loop()
 void leg_loop()
 {
   // 如果坐下就不控制腿部
-  if(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){
+  if(sitting_down || jump_flag || jump_pre_flag ){
     return; // 跳过后续leg高度计算
   }
 
@@ -735,7 +689,7 @@ void leg_loop()
   ACC[0] = ACC[1] = 100; // max 230
   Speed[0] = Speed[1] = 400;
 
-  float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差
+  float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // 加上roll角度偏差
   leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
 
   // 左后腿默认等于 wrobot.height
@@ -744,30 +698,32 @@ void leg_loop()
   LegHeightDiff = 0; // 初始化 roll 角度补偿值
 
   // 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
-  if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) 
+  if(wrobot.mode == 0){
-  {
+    if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) 
-    LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); 
+    {
-    leg_position_add = 0; // 不需要两个脚一样高
+      LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); 
-  }
+      leg_position_add = 0; // 不需要两个脚一样高
-  if (wrobot.joyx > 10  && abs(wrobot.joyy) > 10) 
+    }
-  {
+    if (wrobot.joyx > 10  && abs(wrobot.joyy) > 10) 
-    // 左脚抬高
+    {
-    left_height = wrobot.height + LegHeightDiff ;
+      // 左脚抬高
-    // Serial.print("左脚抬高");
+      left_height = wrobot.height + LegHeightDiff ;
-  } 
+      // Serial.print("左脚抬高");
-  else if (wrobot.joyx < -10  && abs(wrobot.joyy) > 10) 
+    } 
-  {
+    else if (wrobot.joyx < -10  && abs(wrobot.joyy) > 10) 
-    // 右脚抬高
+    {
-    right_height = wrobot.height + LegHeightDiff;
+      // 右脚抬高
-    // Serial.print("右脚抬高");
+      right_height = wrobot.height + LegHeightDiff;
+      // Serial.print("右脚抬高");
+    } 
   }
   
   // 根据新的高度计算舵机位置
   // id[0] 正面看右手，id[1] 正面看左手
   // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
   // const int LEG_HEIGHT_BASE   这值越小，机身越高，实测范围 0（最高），52（最低）
-  Position[0] = 2047  + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+  Position[0] = 2047  + 8.4 * (left_height - leg_height_base ) - leg_position_add;
-  Position[1] = 2047  - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+  Position[1] = 2047  - 8.4 * (right_height - leg_height_base ) - leg_position_add;
 
   // Serial.print("roll_angle:");
   // Serial.print(roll_angle);
@@ -939,31 +895,10 @@ void bat_check()
     bat_check_num++;
 }
 
-// 重置到默认参数状态，用于自平衡起立
+// 重置距离零点
-void resetToDefault() {
+void resetZeroPoint() {
-
+  distance_zeropoint =  LQR_distance;
-    angle_zeropoint = original_angle_zeropoint;
+  pid_lqr_u.error_prev = 0;
-    LQR_speed = 0;
-    LQR_distance = 0;
-    last_lqr_speed = 0;
-    speed_control = 0;
-    distance_control = 0;
-    // 需要将位移零点设置为当前位置
-    distance_zeropoint =  (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // LQR_distance
-    pid_lqr_u.error_prev = 0;
-
-    LQR_u = 0; // 电压清零
-    
-    // YAW轴控制数据
-    YAW_gyro = 0;
-    YAW_angle = 0;
-    YAW_output = 0; 
-
-    jump_flag = 0;
-    leg_position_add = 0;
-
-    wrobot_move_stop_flag = 0;
-
 }
 
 // 添加LED闪烁处理函数

v1.1/Shibo-main-TOMMY/test/main height_base和roll_adjust.cpp

@@ -0,0 +1,943 @@
+/* ---------------------------------
+Copyright (c) 2024 Mu Shibo
+Modify by Tommy 2025.10.08
+------------------------------------- */
+#include <Arduino.h>
+#include <esp_now.h>
+#include <WiFi.h>
+#include <FastLED.h>
+#include <MPU6050_tockn.h>
+#include <SMS_STS.h>
+#include <SimpleFOC.h>
+#include <FS.h>
+#include "esp_wifi.h"
+#include "esp_adc_cal.h"
+
+/************函数申明*************/
+
+// 电压检测函数
+void adc_calibration_init();
+void bat_check();
+
+// 平衡算法函数
+void lqr_balance_loop();
+void yaw_loop();
+void leg_loop();
+void jump_loop();
+void resetZeroPoint();
+void jump_charge(); // 腿部弯曲（蓄能）
+
+// RGB相关函数
+void handleLEDBlink();
+
+/************实例定义*************/
+
+// 电机实例
+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); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
+PIDController pid_gyro(0.06, 0, 0, 100000, 8); 
+PIDController pid_distance(0.5, 0, 0, 100000, 8);
+PIDController pid_speed(0.7, 0, 0, 100000, 8);
+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);  // 新输入值占输出值的10%，历史值占90%
+LowPassFilter lpf_zeropoint(0.1);
+LowPassFilter lpf_roll(0.3);
+LowPassFilter lpf_height(0.1);  
+
+// STS舵机实例
+SMS_STS sms_sts;
+
+// MPU6050实例
+MPU6050 mpu6050(I2Ctwo);
+
+// FastLED配置
+#define RGB_PIN 21
+#define NUM_LEDS 2
+CRGB leds[NUM_LEDS];
+unsigned long previousBlinkMillis = 0;
+int blinkState = 0;
+int blinkCount = 0;
+bool isBlinking = false;
+CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
+
+/************参数定义*************/
+#define pi 3.1415927
+
+// 机身平衡角度调整 roll轴
+float roll_adjust = 0.0f;  
+const int ROLL_ADJUST_MIN = -60;  
+const int ROLL_ADJUST_MAX = 60;  
+
+// 舵机位置限制参数
+float leg_height_base = 20.0f;  // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
+const int LEG_HEIGHT_MIN = -50;  // 最小高度限制（根据机械结构调整）
+const int LEG_HEIGHT_MAX = 50;  // 最大高度限制（防止腿部过度伸展）
+
+#define SERVO0_MIN  2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
+#define SERVO1_MIN  2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
+#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 
+#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
+
+float leg_position_add = 0;    // roll轴平衡控制量
+int left_height;
+int right_height;
+float LegHeightDiff = 0; // 左右腿高度差
+
+// 舵机运动参数
+byte ID[2] = {1, 2};  
+s16 Position[2];
+u16 Speed[2];
+byte ACC[2];
+
+// 腿部高度逻辑
+// #define LEG_HEIGHT_BASE 20 # 固定值
+// float LegHeightDiff ;  # 根据前后转向运动时，智能计算两腿高度差
+// left_height，right_height  = wrobot.height + LegHeightDiff  # 根据前后左右智能计算两腿高度差
+// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
+// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+// Position 2047 - 12 - 8.4 * (right_height                              - LEG_HEIGHT_BASE) - leg_position_add;
+// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff  - LEG_HEIGHT_BASE) -  pid_roll_angle(lpf_roll(roll_angle));
+//                              遥控器          两腿高度差             固定值               roll轴平衡控制量
+
+// 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 = 8;  // 5 设定默认俯仰角度，向前到正，向后倒负
+float original_angle_zeropoint = 8; // 保存原始的角度零点
+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;
+
+// 记录轮部转速，用于判断跳跃状态
+unsigned long last_speed_record_time = 0;  // 上次记录转速的时间
+const unsigned long SPEED_RECORD_INTERVAL = 100;  // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的
+float last_lqr_speed = 0;    // 记录上一时刻的轮部转速
+float robot_speed_diff = 0;    // 速度差
+
+int jump_flag = 0; // 跳跃过程计数
+int jump_pre_flag = 0; // 跳跃前蓄能计数
+int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
+
+
+// 电压检测
+#define LED_BAT 13
+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_12;
+static const adc_unit_t unit = ADC_UNIT_1;
+
+// 倒地起来辅助计数
+int loopCounter = 0; // loop函数执行计数
+int resetToDefaultCounter = 0; // resetToDefault执行计数
+
+// 小车状态控制
+bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
+bool last_robot_enabled = true; // 记录上一次的小车状态
+int sitting_down = 0; // 标记是否正在执行坐下动作
+
+
+typedef struct
+{
+  int height = 30;
+  int roll;
+  int jump = 0; // 0 =不跳跃 1= 跳跃
+  int joyy;
+  int joyy_last;
+  int joyx;
+  int joyx_last;
+} Wrobot;
+Wrobot wrobot;
+
+typedef struct {
+  // 4个摇杆轴（模拟值：-100 到 100 ）
+  int joyLeftX; // 左摇杆X轴（左-右，0-940 ，中心默认：423 ）
+  int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ）
+  int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ）
+  int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ）
+  bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下
+  bool btnJoyRight; // 右摇杆按压键
+  bool btnStart; // Start键
+  bool btnBack; // Back键
+  bool btnUp; // 方向上键
+  bool btnDown; // 方向下键
+  bool btnLeft; // 方向左键
+  bool btnRight; // 方向右键
+  bool btnA; // A键
+  bool btnB; // B键
+  bool btnX; // X键
+  bool btnY; // Y键
+} JoystickData;
+
+JoystickData remoteData;
+
+const int deadZone = 10;
+bool last_btnJoyRight = 0;
+
+// ESP-NOW接收回调函数
+void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+  memcpy(&remoteData, incomingData, sizeof(remoteData));
+
+  // rgb led 上键 闪烁6次：红红黄黄绿绿
+  if(remoteData.btnUp && !isBlinking){
+    isBlinking = true;
+    blinkCount = 0;
+    blinkState = 0;
+    previousBlinkMillis = millis();
+  }
+
+  // 坐下 back键
+  if (remoteData.btnBack && robot_enabled) { 
+    robot_enabled = false;
+    sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
+  }
+  // 起立 start键
+  if (remoteData.btnStart && !robot_enabled) {
+    robot_enabled = true;
+    sitting_down = 0; // 清除坐下动作标记
+  }
+
+  // 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
+  if (!robot_enabled || sitting_down) {
+    wrobot.jump = 0; // 禁止跳跃
+    wrobot.roll = 0; // 禁止左右摇摆（起升相关）
+    wrobot.height = 0; // 禁止高度调整（起升相关）
+  }
+
+  // 跳跃状态不接受指令
+  if( jump_flag > 0){
+    wrobot.jump = 0; // 强制设为“不跳跃”
+    wrobot.joyx = 0; // 清空左右转向指令
+    wrobot.joyy = 0; // 清空前后移动指令
+    wrobot.roll = 0; // 清空左右摇摆指令
+    return;
+  }
+
+  // 4个摇杆轴 输出范围
+  // int joyLeftX; // 左-右，0-930（有偏移，实际范围0-900） ，映射到 -100 到 100，中心默认：0
+  // int joyLeftY; // 上-下，940-0 ，中心默认：425，映射到 60 到 0，中心默认：30
+  // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0
+  // int joyRightY;  // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0
+  
+  // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
+  int convertedJ1PotX = constrain(
+    map(remoteData.joyLeftX, 0, 900, -100, 100),  
+    -100, 100
+  );
+
+  // 上下 左摇杆Y轴：原始940-0 → 目标60到0
+  int convertedJ1PotY = constrain(
+    map(remoteData.joyLeftY, 940, 0, 60, 0),  // 原始范围是940-0，目标0-60
+    0, 60
+  );
+
+  // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100
+  int convertedJ2PotX = constrain(
+    map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30),  // 先映射到-30~30，再按比例放大到-100~100
+    -100, 100
+  );
+
+  // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100
+  int convertedJ2PotY = constrain(
+    map(remoteData.joyRightY, 1080, 0, 100, -100),  // 原始1080-0，目标100~-100
+    -100, 100
+  );
+
+  // 跳跃 右摇杆按键
+  if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false
+    wrobot.jump = 1; // 跳跃
+  }else{
+    wrobot.jump = 0; // 不跳跃
+  }
+  last_btnJoyRight = remoteData.btnJoyRight; 
+
+  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+    wrobot.joyy = convertedJ2PotY;
+  }else{
+    wrobot.joyy = 0;
+  } 
+
+  if(abs(convertedJ1PotX) > deadZone) { // 左右运动  -100 到 100
+    wrobot.joyx = convertedJ1PotX; 
+  } else {
+    wrobot.joyx = 0;
+  }
+
+  // if(abs(convertedJ1PotY) > deadZone) {
+  //   wrobot.height = convertedJ1PotY;
+  // }else{
+  //   wrobot.height = 0;
+  // }
+
+  // 根据前后速度，智能计算wrobot.height 
+  // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
+  // 反向映射：绝对值30→50，绝对值100→0（线性过渡）
+  wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
+  // 强制约束输出在0~60范围内（防止异常值）
+  wrobot.height = constrain(wrobot.height, 30, 50);
+  wrobot.height = lpf_height(wrobot.height);
+
+  // if(abs(convertedJ2PotX) > deadZone) {
+  //   wrobot.roll  = convertedJ2PotX;
+  // }else{
+  //   wrobot.roll = 0;
+  // }
+
+  // 上键（增加leg_height_base）
+  if (remoteData.btnUp) {  // 按键从松开→按下（上升沿）
+    leg_height_base -= 10.0f;  // 每次增加1（步长可调整）
+    leg_height_base = constrain(leg_height_base, LEG_HEIGHT_MIN, LEG_HEIGHT_MAX);  // 限制范围
+  }
+
+
+  // 下键（减少leg_height_base）
+  if (remoteData.btnDown) {  // 按键从松开→按下（上升沿）
+    leg_height_base += 10.0f;  // 每次减少1（步长可调整）
+    leg_height_base = constrain(leg_height_base, LEG_HEIGHT_MIN, LEG_HEIGHT_MAX);  // 限制范围
+  }
+
+  // 左键（增加roll_adjust）
+  if (remoteData.btnLeft) { 
+    roll_adjust -= 10.0f;  
+    roll_adjust = constrain(roll_adjust, ROLL_ADJUST_MIN, ROLL_ADJUST_MAX);   // 限制范围
+  }
+
+
+  // 右键（减少roll_adjust）
+  if (remoteData.btnRight) { 
+    roll_adjust += 10.0f;  
+    roll_adjust = constrain(roll_adjust, ROLL_ADJUST_MIN, ROLL_ADJUST_MAX);  // 限制范围
+  }
+
+  // 重置 leg_height_base 和 roll_adjust
+  if (remoteData.btnJoyLeft) { // 1=true，0=false
+    roll_adjust = 0.0f;  // 重置roll_adjust
+    leg_height_base = 20.0f;  // 重置leg_height_base
+  }
+}
+
+void setup()
+{
+  Serial.begin(115200);   // 通讯串口
+  Serial2.begin(1000000); // 腿部sts舵机
+
+  // 设置WiFi通道为6，为了适配遥控器模式
+  WiFi.mode(WIFI_STA); // 设置为STA模式
+  esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
+  
+  // 初始化ESP-NOW
+  if (esp_now_init() != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  esp_now_register_recv_cb(OnDataRecv);
+
+  // 舵机初始化
+  sms_sts.pSerial = &Serial2;
+
+  ID[0] = 1;
+  ID[1] = 2;
+  ACC[0] = 30;
+  ACC[1] = 30;
+  Speed[0] = 400;
+  Speed[1] = 400;
+  Position[0] = SERVO0_MIN;
+  Position[1] = SERVO1_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();
+
+  // FastLED初始化
+  FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
+  FastLED.setBrightness(50); // 设置亮度
+  fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
+  FastLED.show();
+
+  delay(500);
+}
+
+void loop()
+{
+  
+  // 处理LED闪烁
+  // handleLEDBlink();
+  
+  
+  bat_check();        // 电压检测
+  delayMicroseconds(1500);
+  mpu6050.update();   // IMU数据更新
+  lqr_balance_loop(); // lqr自平衡控制
+  yaw_loop();         // yaw轴转向控制
+  jump_loop();        // 跳跃动作
+  leg_loop();         // 腿部动作控制
+
+  // 记录上一次的遥控数据数据
+  wrobot.joyx_last = wrobot.joyx;
+  wrobot.joyy_last = wrobot.joyy;
+
+  motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+
+  loopCounter++;
+  // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
+  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
+  if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
+    resetZeroPoint();
+    resetToDefaultCounter++;
+    motor1.target = 0;
+    motor2.target = 0;
+  }
+
+  // 大仰角到底，轮子就不要转了 修改
+  if (abs(LQR_angle) > 120.0f) {
+      motor1.target = 0;
+      motor2.target = 0;
+  }
+
+  // 达到平衡角度后，重置重置计数器
+  if( abs(LQR_angle) < 25.0f){
+    resetToDefaultCounter = 0;
+  }
+
+
+  // 小车坐下
+  if (sitting_down) {
+    sitting_down ++;
+
+    if (sitting_down < 15) {
+      // 如果遥感向后运动，给个向后退脉冲，让小车向前倒地
+      if(wrobot.joyy < 0){
+        motor1.target = -1.0;  // 负数小车向后，正数向前
+        motor2.target = -1.0;
+        Position[0] = SERVO0_MIN; // 修改
+        Position[1] = SERVO1_MIN;
+      } else { // 否则默认向前倒地 给个向前脉冲
+        motor1.target = 1.0;  // 负数小车向后，正数向前
+        motor2.target = 1.0;
+        Position[0] = SERVO0_MIN + 60; // 修改
+        Position[1] = SERVO1_MIN - 60;
+      }
+    }else{
+
+      // 通过简单的开环控制前后左右
+      float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02）
+      motor1.target = (-0.5) * (wrobot.joyy +  wrobot.joyx) * speed_coeff; 
+      motor2.target = (-0.5) * (wrobot.joyy -  wrobot.joyx) * speed_coeff;
+
+    }
+
+    // 收缩双腿，舵机到最低位置
+    if (sitting_down == 5) {
+      ACC[0] = ACC[1] = 100;
+      Speed[0] = Speed[1] = 200;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    }
+   
+  }
+  
+  // 迭代计算FOC相电压
+  motor1.loopFOC();
+  motor2.loopFOC();
+
+  // 设置轮部电机输出
+  motor1.move();
+  motor2.move();
+
+} // loop()
+
+// lqr自平衡控制
+void lqr_balance_loop()
+{
+  /*
+  motor.target 负数小车向后，正数小车向前
+  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  LQR_u = angle_control + gyro_control + distance_control + speed_control
+          = 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
+
+    angle_control = pid_angle(  LQR_angle       -   angle_zeropoint) // 实际pitch角度-目标pitch角度
+                                LQR_angle = mpu6050.getAngleY() 
+    
+    gyro_control = pid_gyro(  LQR_gyro - 0   ) // 实际角速度，目标值是零
+                              LQR_gyro =  mpu6050.getGyroY()
+    
+    distance_control = pid_distance(  LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照）
+                                      LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle)  
+
+    speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差
+                              LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) 
+                              speed_target_coeff = 前进后退系数
+  yaw 偏航角控制
+  float yaw_angle_control = pid_yaw_angle(yaw_target); 
+                                          yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值
+
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制
+                                        YAW_gyro =  (float)mpu6050.getGyroZ();
+  
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+
+  motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+
+   */
+
+  LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量
+  LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s）
+  LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°）
+  LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
+
+  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
+  gyro_control = pid_gyro(LQR_gyro);
+
+  // 前进后退跳跃的系数都不一样
+  float speed_target_coeff = 0.1; 
+  if( jump_flag ){ // 跳跃
+    speed_target_coeff = 0.1;
+  }else if(wrobot.joyy > 0 ){ // 前进
+    speed_target_coeff = 0.18;
+  }else if(wrobot.joyy < 0){ // 后退 
+    speed_target_coeff = 0.11;
+  }else{
+    speed_target_coeff = 0.1;
+  }
+ 
+  speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy));  // 最大8v
+
+  // 检测轮子差速，判断轮子是否离地
+  unsigned long current_time = millis();
+  if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
+    robot_speed_diff = LQR_speed - last_lqr_speed;
+
+    if(robot_speed_diff > 18.0) // 轮子离地
+    {
+      wheel_ground_flag = 0; // 轮子离地标记
+       // Serial.print("TAKE OFF");
+      // Serial.print(wheel_ground_flag);
+      // Serial.print(",robot_speed_diff:");
+      // Serial.print(robot_speed_diff);
+      // Serial.print(",jump_flag:");
+      // Serial.println(jump_flag); // 100 左右
+    }
+    if( robot_speed_diff < -9.0) // 轮子着地
+    {
+      wheel_ground_flag = 1; // 轮子着地标记
+      // Serial.print("LANDING:");
+      // Serial.print(",robot_speed_diff:");
+      // Serial.print(robot_speed_diff);
+      // Serial.print(",jump_flag:");
+      // Serial.println(jump_flag); // 120左右
+
+      if( jump_flag ){ // 落地点作为新的位移零点
+         resetZeroPoint(); 
+      }
+    }
+    last_lqr_speed = LQR_speed;  // 每隔100ms更新一次历史转速
+    last_speed_record_time = current_time;  // 更新记录时间
+  }
+   
+  // 重置位移零点和积分情形
+  // 1.判断摇杆是否没有前后左右运动指令
+  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
+  {
+    resetZeroPoint(); 
+  }
+  
+  // 2. 运动中实时重置位移零点和积分情形
+  if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
+     || wrobot.joyy != 0 || sitting_down ) 
+  {
+       resetZeroPoint(); 
+  }
+
+  distance_control = pid_distance(LQR_distance - distance_zeropoint);
+
+  // 计算 LQR_u
+  // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
+  if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
+  {        
+    LQR_u = angle_control + gyro_control;  // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
+  }
+  else // 当轮部未离地时，LQR_u：4个参数
+  {
+    // 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量
+    LQR_u = angle_control + gyro_control + distance_control + speed_control;
+  }
+
+  // 小车没有控制的时候自稳定状态
+  // 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态
+  if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) //  && !sitting_down
+  {
+    LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
+    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(sitting_down || jump_flag || jump_pre_flag ){
+    return; // 跳过后续leg高度计算
+  }
+
+  // 机身大角度，譬如倒地时，强制腿部收缩
+  if (abs(LQR_angle) > 25.0f) 
+  {
+    ACC[0] = ACC[1] = 100;
+    Speed[0] = Speed[1] = 400;
+    Position[0] = SERVO0_MIN;
+    Position[1] = SERVO1_MIN;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    return; // 跳过后续leg高度计算
+  }
+
+  ACC[0] = ACC[1] = 100; // max 230
+  Speed[0] = Speed[1] = 400;
+
+  float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // + 5.0 实测偏差roll角度偏差
+  leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
+
+  // 左后腿默认等于 wrobot.height
+  left_height = wrobot.height;
+  right_height = wrobot.height;
+  LegHeightDiff = 0; // 初始化 roll 角度补偿值
+
+  // 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
+  if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) 
+  {
+    LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); 
+    leg_position_add = 0; // 不需要两个脚一样高
+  }
+  if (wrobot.joyx > 10  && abs(wrobot.joyy) > 10) 
+  {
+    // 左脚抬高
+    left_height = wrobot.height + LegHeightDiff ;
+    // Serial.print("左脚抬高");
+  } 
+  else if (wrobot.joyx < -10  && abs(wrobot.joyy) > 10) 
+  {
+    // 右脚抬高
+    right_height = wrobot.height + LegHeightDiff;
+    // Serial.print("右脚抬高");
+  } 
+
+  // 根据新的高度计算舵机位置
+  // id[0] 正面看右手，id[1] 正面看左手
+  // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
+  // const int LEG_HEIGHT_BASE   这值越小，机身越高，实测范围 0（最高），52（最低）
+  Position[0] = 2047  + 8.4 * (left_height - leg_height_base) - leg_position_add;
+  Position[1] = 2047  - 8.4 * (right_height - leg_height_base) - leg_position_add;
+
+  // Serial.print("roll_angle:");
+  // Serial.print(roll_angle);
+  // Serial.print(",Position[0]:");
+  // Serial.print(Position[0]);
+  // Serial.print(",Position[1]:");
+  // Serial.println(Position[1]);
+
+  // 限制舵机位置范围
+  if (Position[0] < SERVO0_MIN) // 舵机1最低
+    Position[0] = SERVO0_MIN;
+  else if (Position[0] > SERVO0_MAX) // 舵机1最高
+    Position[0] = SERVO0_MAX;
+
+  if (Position[1] > SERVO1_MIN) // 舵机2最低（和舵机1方向是反的）
+    Position[1] = SERVO1_MIN;
+  else if (Position[1] < SERVO1_MAX) // 舵机2最高
+    Position[1] = SERVO1_MAX;
+  
+  sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+}
+
+// 收缩小腿蓄能
+void jump_charge() // 腿部弯曲（蓄能）
+{
+    ACC[0] = 250;
+    ACC[1] = 250;
+    Speed[0] = 400;
+    Speed[1] = 400;
+    Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
+    Position[1] = (SERVO1_MIN  - 80); 
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+}
+
+// 跳跃控制
+void jump_loop()
+{ 
+  if ((wrobot.jump == 1) && (jump_pre_flag == 0))
+  {
+    jump_charge();
+    jump_pre_flag =  1;
+  }
+
+  if(jump_pre_flag > 0 ){
+    jump_pre_flag ++;
+  }
+  
+  if(jump_pre_flag){ 
+    if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
+      jump_flag = 1;
+      jump_pre_flag = 0;
+
+      // 跳跃开始
+      ACC[0] = 0;
+      ACC[1] = 0;
+      Speed[0] = 0;
+      Speed[1] = 0;
+      Position[0] = SERVO0_MAX + 20; // 极限距离
+      Position[1] = SERVO1_MAX - 20;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    }
+  }
+
+  if (jump_flag > 0) // 跳跃计数;
+  {
+    jump_flag ++;
+  }
+
+   // 跳跃中，腿部缩起来
+  if( (jump_flag > 30) && (jump_flag < 35) )
+  {
+    ACC[0] = 0;
+    ACC[1] = 0;
+    Speed[0] = 0;
+    Speed[1] = 0;
+    Position[0] = (SERVO0_MIN  + 100); // 100 约等于食指宽度
+    Position[1] = (SERVO1_MIN  - 100); 
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+  }
+
+  // 轮子着地
+  if (jump_flag > 200) //  2000=6s ,1s=2000/6=333.33
+  {
+    jump_flag = 0; // 跳跃过程结束
+  }
+
+}
+
+// yaw轴转向控制 （转向控制）
+void yaw_loop()
+{
+  // 跳跃中，YAW_output 设为0，避免干扰左右旋转
+  if(jump_flag) 
+  {
+    YAW_output = 0;
+    return;
+  }
+
+ // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
+  float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; 
+  coeff = constrain(coeff, 0.1f, 1.0f); 
+  float yaw_target = wrobot.joyx * coeff; 
+
+  // 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限）
+  float yaw_angle_control_coeff = 1.0f;
+  if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转
+    yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); 
+    // 根据转向速度，智能计算wrobot.height 
+    // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
+    // 反向映射：绝对值30→60，绝对值100→0（线性过渡）
+    wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30);
+    // 强制约束输出在0~60范围内（防止异常值）
+    wrobot.height = constrain(wrobot.height, 30, 50);
+    // 使用低波过滤器，平滑数据
+    wrobot.height = lpf_height(wrobot.height);
+
+    // 增加rgb效果
+    if(abs(wrobot.joyx) > 60){
+      handleLEDBlink();
+    }
+  }
+
+  float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; 
+  // 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适）
+  yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f);
+
+  YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+}
+
+// 电压检测初始化
+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++;
+}
+
+// 重置距离零点
+void resetZeroPoint() {
+  distance_zeropoint =  LQR_distance;
+  pid_lqr_u.error_prev = 0;
+}
+
+// 添加LED闪烁处理函数
+void handleLEDBlink() {
+  if (!isBlinking) return;
+  
+  unsigned long currentMillis = millis();
+  
+  if (currentMillis - previousBlinkMillis >= 100) { // 每100ms切换一次状态，实现快速闪烁
+    previousBlinkMillis = currentMillis;
+    
+    if (blinkState == 0) {
+      // LED亮
+      fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]);
+      FastLED.show();
+      blinkState = 1;
+    } else {
+      // LED灭
+      fill_solid(leds, NUM_LEDS, CRGB::Black);
+      FastLED.show();
+      blinkState = 0;
+      blinkCount++;
+      
+      // 检查是否完成6次闪烁
+      if (blinkCount >= 6) {
+        isBlinking = false;
+        blinkCount = 0;
+        // 闪烁完成后保持熄灭状态
+        fill_solid(leds, NUM_LEDS, CRGB::Black);
+        FastLED.show();
+      }
+    }
+  }
+}
+

v1.1/Shibo-main-TOMMY/test/main向前跳跃.cpp

@@ -0,0 +1,956 @@
+/* ---------------------------------
+Copyright (c) 2024 Mu Shibo
+Modify by Tommy 2025.10.08
+------------------------------------- */
+#include <Arduino.h>
+#include <esp_now.h>
+#include <WiFi.h>
+#include <FastLED.h>
+#include <MPU6050_tockn.h>
+#include <SMS_STS.h>
+#include <SimpleFOC.h>
+#include <FS.h>
+#include "esp_wifi.h"
+#include "esp_adc_cal.h"
+
+/************函数申明*************/
+
+// 电压检测函数
+void adc_calibration_init();
+void bat_check();
+
+// 平衡算法函数
+void lqr_balance_loop();
+void yaw_loop();
+void leg_loop();
+void jump_loop();
+void resetZeroPoint();
+void jump_charge(); // 腿部弯曲（蓄能）
+
+// RGB相关函数
+void handleLEDBlink();
+
+/************实例定义*************/
+
+// 电机实例
+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); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
+PIDController pid_gyro(0.06, 0, 0, 100000, 8); 
+PIDController pid_distance(0.5, 0, 0, 100000, 8);
+PIDController pid_speed(0.7, 0, 0, 100000, 8);
+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);  // 新输入值占输出值的10%，历史值占90%
+LowPassFilter lpf_zeropoint(0.1);
+LowPassFilter lpf_roll(0.3);
+LowPassFilter lpf_height(0.1);  
+
+// STS舵机实例
+SMS_STS sms_sts;
+
+// MPU6050实例
+MPU6050 mpu6050(I2Ctwo);
+
+// FastLED配置
+#define RGB_PIN 21
+#define NUM_LEDS 2
+CRGB leds[NUM_LEDS];
+unsigned long previousBlinkMillis = 0;
+int blinkState = 0;
+int blinkCount = 0;
+bool isBlinking = false;
+CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
+
+/************参数定义*************/
+#define pi 3.1415927
+
+// 舵机位置限制参数
+#define LEG_HEIGHT_BASE 20  //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
+#define SERVO0_MIN  2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
+#define SERVO1_MIN  2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
+#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 
+#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
+float leg_position_add = 0;    // roll轴平衡控制量
+int left_height;
+int right_height;
+float LegHeightDiff = 0; // 左右腿高度差
+
+// 舵机运动参数
+byte ID[2] = {1, 2};  
+s16 Position[2];
+u16 Speed[2];
+byte ACC[2];
+
+// 腿部高度逻辑
+// #define LEG_HEIGHT_BASE 20 # 固定值
+// float LegHeightDiff ;  # 根据前后转向运动时，智能计算两腿高度差
+// left_height，right_height  = wrobot.height + LegHeightDiff  # 根据前后左右智能计算两腿高度差
+// leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
+// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+// Position 2047 - 12 - 8.4 * (right_height                              - LEG_HEIGHT_BASE) - leg_position_add;
+// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff  - LEG_HEIGHT_BASE) -  pid_roll_angle(lpf_roll(roll_angle));
+//                              遥控器          两腿高度差             固定值               roll轴平衡控制量
+
+// 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 = 8;  // 5 设定默认俯仰角度，向前到正，向后倒负
+float original_angle_zeropoint = 8; // 保存原始的角度零点
+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;
+
+// 记录轮部转速，用于判断跳跃状态
+unsigned long last_speed_record_time = 0;  // 上次记录转速的时间
+const unsigned long SPEED_RECORD_INTERVAL = 100;  // 转速记录间隔(毫秒) ，因为时间相隔太近速度差不明显的
+float last_lqr_speed = 0;    // 记录上一时刻的轮部转速
+float robot_speed_diff = 0;    // 速度差
+
+int jump_flag = 0; // 跳跃过程计数
+int jump_pre_flag = 0; // 跳跃前蓄能计数
+int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
+
+
+// 电压检测
+#define LED_BAT 13
+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_12;
+static const adc_unit_t unit = ADC_UNIT_1;
+
+// 倒地起来辅助计数
+int loopCounter = 0; // loop函数执行计数
+int resetToDefaultCounter = 0; // resetToDefault执行计数
+
+// 小车状态控制
+bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
+bool last_robot_enabled = true; // 记录上一次的小车状态
+int sitting_down = 0; // 标记是否正在执行坐下动作
+
+// 原地跳跃控制变量
+int forward_jump_flag = 0;     // 向前跳跃状态标志
+int backward_jump_flag = 0;    // 向后跳跃状态标志
+int left_jump_flag = 0;    // 向左跳跃状态标志
+int right_jump_flag = 0;    // 向右跳跃状态标志
+
+typedef struct
+{
+  int height = 30;
+  int roll;
+  int jump = 0; // 0 =不跳跃 1= 跳跃
+  int joyy;
+  int joyy_last;
+  int joyx;
+  int joyx_last;
+} Wrobot;
+Wrobot wrobot;
+
+typedef struct {
+  // 4个摇杆轴（模拟值：-100 到 100 ）
+  int joyLeftX; // 左摇杆X轴（左-右，0-940 ，中心默认：423 ）
+  int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ）
+  int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ）
+  int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ）
+  bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下
+  bool btnJoyRight; // 右摇杆按压键
+  bool btnStart; // Start键
+  bool btnBack; // Back键
+  bool btnUp; // 方向上键
+  bool btnDown; // 方向下键
+  bool btnLeft; // 方向左键
+  bool btnRight; // 方向右键
+  bool btnA; // A键
+  bool btnB; // B键
+  bool btnX; // X键
+  bool btnY; // Y键
+} JoystickData;
+
+JoystickData remoteData;
+
+const int deadZone = 10;
+bool last_btnY = 0; 
+bool last_btnJoyRight = 0;
+
+// ESP-NOW接收回调函数
+void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+  memcpy(&remoteData, incomingData, sizeof(remoteData));
+
+  // rgb led 上键 闪烁6次：红红黄黄绿绿
+  if(remoteData.btnUp && !isBlinking){
+    isBlinking = true;
+    blinkCount = 0;
+    blinkState = 0;
+    previousBlinkMillis = millis();
+  }
+
+  // 坐下 back键
+  if (remoteData.btnBack && robot_enabled) { 
+    robot_enabled = false;
+    sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
+  }
+  // 起立 start键
+  if (remoteData.btnStart && !robot_enabled) {
+    robot_enabled = true;
+    sitting_down = 0; // 清除坐下动作标记
+  }
+
+  // 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
+  if (!robot_enabled || sitting_down) {
+    wrobot.jump = 0; // 禁止跳跃
+    wrobot.roll = 0; // 禁止左右摇摆（起升相关）
+    wrobot.height = 0; // 禁止高度调整（起升相关）
+  }
+
+  // 跳跃状态不接受指令
+  if( jump_flag > 0){
+    wrobot.jump = 0; // 强制设为“不跳跃”
+    wrobot.joyx = 0; // 清空左右转向指令
+    wrobot.joyy = 0; // 清空前后移动指令
+    wrobot.roll = 0; // 清空左右摇摆指令
+    return;
+  }
+
+  // 4个摇杆轴 输出范围
+  // int joyLeftX; // 左-右，0-930（有偏移，实际范围0-900） ，映射到 -100 到 100，中心默认：0
+  // int joyLeftY; // 上-下，940-0 ，中心默认：425，映射到 60 到 0，中心默认：30
+  // int joyRightX; // 左-右，0-960：中心默认：487，中心默认：423 ，映射到 -30 到 30，中心默认：0
+  // int joyRightY;  // 上-下，1080-0，中心默认：525，映射到 100 到 -100，中心默认：0
+  
+  // 左右 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
+  int convertedJ1PotX = constrain(
+    map(remoteData.joyLeftX, 0, 900, -100, 100),  
+    -100, 100
+  );
+
+  // 上下 左摇杆Y轴：原始940-0 → 目标60到0
+  int convertedJ1PotY = constrain(
+    map(remoteData.joyLeftY, 940, 0, 60, 0),  // 原始范围是940-0，目标0-60
+    0, 60
+  );
+
+  // 摇摆 右摇杆X轴：原始0-960 → 目标-100到100
+  int convertedJ2PotX = constrain(
+    map(remoteData.joyRightX, 0, 960, -30, 30) * (100.0/30),  // 先映射到-30~30，再按比例放大到-100~100
+    -100, 100
+  );
+
+  // 前后 右摇杆Y轴：原始1080-0 → 目标100到-100
+  int convertedJ2PotY = constrain(
+    map(remoteData.joyRightY, 1080, 0, 100, -100),  // 原始1080-0，目标100~-100
+    -100, 100
+  );
+
+  // 跳跃  右摇杆按键
+  if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false
+    wrobot.jump = 1; // 跳跃
+  }else{
+    wrobot.jump = 0; // 不跳跃
+  }
+  last_btnJoyRight = remoteData.btnJoyRight; 
+
+  // 向前跳跃 Y键
+  if (remoteData.btnY && robot_enabled && !last_btnY) {
+    forward_jump_flag = 1;
+  }
+  last_btnY = remoteData.btnY; // 防止连续输出向前跳跃
+
+  // 向后跳跃 A键
+  // if (remoteData.btnA && robot_enabled) {
+  //   backward_jump_flag = 1;
+  // }
+  // // 向左跳跃 X键
+  // if (remoteData.btnX && robot_enabled) {
+  //   left_jump_flag = 1;
+  // }
+  // // 向右跳跃 B键
+  // if (remoteData.btnB && robot_enabled) {
+  //   right_jump_flag = 1;
+  // }
+
+  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+    wrobot.joyy = convertedJ2PotY;
+  }else{
+    wrobot.joyy = 0;
+  } 
+
+  if(abs(convertedJ1PotX) > deadZone) { // 左右运动  -100 到 100
+    wrobot.joyx = convertedJ1PotX; 
+  } else {
+    wrobot.joyx = 0;
+  }
+
+  // if(abs(convertedJ1PotY) > deadZone) {
+  //   wrobot.height = convertedJ1PotY;
+  // }else{
+  //   wrobot.height = 0;
+  // }
+
+  // 根据前后速度，智能计算wrobot.height 
+  // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
+  // 反向映射：绝对值30→50，绝对值100→0（线性过渡）
+  wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
+  // 强制约束输出在0~60范围内（防止异常值）
+  wrobot.height = constrain(wrobot.height, 30, 50);
+  wrobot.height = lpf_height(wrobot.height);
+
+  // if(abs(convertedJ2PotX) > deadZone) {
+  //   wrobot.roll  = convertedJ2PotX;
+  // }else{
+  //   wrobot.roll = 0;
+  // }
+
+}
+
+void setup()
+{
+  Serial.begin(115200);   // 通讯串口
+  Serial2.begin(1000000); // 腿部sts舵机
+
+  // 设置WiFi通道为6，为了适配遥控器模式
+  WiFi.mode(WIFI_STA); // 设置为STA模式
+  esp_wifi_set_channel(6, WIFI_SECOND_CHAN_NONE);
+  
+  // 初始化ESP-NOW
+  if (esp_now_init() != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  esp_now_register_recv_cb(OnDataRecv);
+
+  // 舵机初始化
+  sms_sts.pSerial = &Serial2;
+
+  ID[0] = 1;
+  ID[1] = 2;
+  ACC[0] = 30;
+  ACC[1] = 30;
+  Speed[0] = 400;
+  Speed[1] = 400;
+  Position[0] = SERVO0_MIN;
+  Position[1] = SERVO1_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();
+
+  // FastLED初始化
+  FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
+  FastLED.setBrightness(50); // 设置亮度
+  fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
+  FastLED.show();
+
+  delay(500);
+}
+
+void loop()
+{
+  
+  // 处理LED闪烁
+  // handleLEDBlink();
+  
+  
+  bat_check();        // 电压检测
+  delayMicroseconds(1500);
+  mpu6050.update();   // IMU数据更新
+  lqr_balance_loop(); // lqr自平衡控制
+  yaw_loop();         // yaw轴转向控制
+  jump_loop();        // 跳跃动作
+  leg_loop();         // 腿部动作控制
+
+  // 记录上一次的遥控数据数据
+  wrobot.joyx_last = wrobot.joyx;
+  wrobot.joyy_last = wrobot.joyy;
+
+  motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+
+  loopCounter++;
+  // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
+  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
+  if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
+    resetZeroPoint();
+    resetToDefaultCounter++;
+    motor1.target = 0;
+    motor2.target = 0;
+  }
+
+  // 大仰角到底，轮子就不要转了 修改
+  if (abs(LQR_angle) > 120.0f) {
+      motor1.target = 0;
+      motor2.target = 0;
+  }
+
+  // 达到平衡角度后，重置重置计数器
+  if( abs(LQR_angle) < 25.0f){
+    resetToDefaultCounter = 0;
+  }
+
+  // 向前跳跃
+  if(forward_jump_flag){
+    if(forward_jump_flag == 1){
+      jump_charge();
+      forward_jump_flag = 2;
+    }
+    if(forward_jump_flag == 2){
+      if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
+        forward_jump_flag = 3;
+      }
+    }
+    if(forward_jump_flag >= 3){
+      forward_jump_flag ++ ;
+      // if ( forward_jump_flag < 25) {
+      //   motor1.target = 1.8; // 保持向前脉冲
+      //   motor2.target = 1.8;
+      if ( forward_jump_flag < 20) { // 测试
+        motor1.target = 1.5; // 向前脉冲
+        motor2.target = 1.5;
+      } else {
+        forward_jump_flag = 0;
+        jump_pre_flag = 1;
+      }
+    }
+  }
+  
+
+  // 小车坐下
+  if (sitting_down) {
+    sitting_down ++;
+
+    if (sitting_down < 15) {
+      // 如果遥感向后运动，给个向后退脉冲，让小车向前倒地
+      if(wrobot.joyy < 0){
+        motor1.target = -1.0;  // 负数小车向后，正数向前
+        motor2.target = -1.0;
+        Position[0] = SERVO0_MIN; // 修改
+        Position[1] = SERVO1_MIN;
+      } else { // 否则默认向前倒地 给个向前脉冲
+        motor1.target = 1.0;  // 负数小车向后，正数向前
+        motor2.target = 1.0;
+        Position[0] = SERVO0_MIN + 60; // 修改
+        Position[1] = SERVO1_MIN - 60;
+      }
+    }else{
+
+      // 通过简单的开环控制前后左右
+      float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02）
+      motor1.target = (-0.5) * (wrobot.joyy +  wrobot.joyx) * speed_coeff; 
+      motor2.target = (-0.5) * (wrobot.joyy -  wrobot.joyx) * speed_coeff;
+
+    }
+
+    // 收缩双腿，舵机到最低位置
+    if (sitting_down == 5) {
+      ACC[0] = ACC[1] = 100;
+      Speed[0] = Speed[1] = 200;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    }
+   
+  }
+  
+  // 迭代计算FOC相电压
+  motor1.loopFOC();
+  motor2.loopFOC();
+
+  // 设置轮部电机输出
+  motor1.move();
+  motor2.move();
+
+} // loop()
+
+// lqr自平衡控制
+void lqr_balance_loop()
+{
+  /*
+  motor.target 负数小车向后，正数小车向前
+  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  LQR_u = angle_control + gyro_control + distance_control + speed_control
+          = 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
+
+    angle_control = pid_angle(  LQR_angle       -   angle_zeropoint) // 实际pitch角度-目标pitch角度
+                                LQR_angle = mpu6050.getAngleY() 
+    
+    gyro_control = pid_gyro(  LQR_gyro - 0   ) // 实际角速度，目标值是零
+                              LQR_gyro =  mpu6050.getGyroY()
+    
+    distance_control = pid_distance(  LQR_distance - distance_zeropoint) // 两个电机平均旋转弧度（实际位移量） - 位移基准零点（某一时刻LQR_distance快照）
+                                      LQR_distance = (-0.5)*(motor1.shaft_angle + motor2.shaft_angle)  
+
+    speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)) // 两个电机平均速度（实际速度） - 摇杆前后输入值* 系数 ，就速度差
+                              LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) 
+                              speed_target_coeff = 前进后退系数
+  yaw 偏航角控制
+  float yaw_angle_control = pid_yaw_angle(yaw_target); 
+                                          yaw_target = wrobot.joyx * 0.1; // 使用摇杆左右转向值
+
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); // yaw 轴的角速度控制
+                                        YAW_gyro =  (float)mpu6050.getGyroZ();
+  
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+
+  motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+
+   */
+
+  LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量
+  LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s）
+  LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°）
+  LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
+
+  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
+  gyro_control = pid_gyro(LQR_gyro);
+
+  // 前进后退跳跃的系数都不一样
+  float speed_target_coeff = 0.1; 
+  if( jump_flag ){ // 跳跃
+    speed_target_coeff = 0.1;
+  }else if(wrobot.joyy > 0 ){ // 前进
+    speed_target_coeff = 0.18;
+  }else if(wrobot.joyy < 0){ // 后退 
+    speed_target_coeff = 0.11;
+  }else{
+    speed_target_coeff = 0.1;
+  }
+ 
+  speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy));  // 最大8v
+
+  // 检测轮子差速，判断轮子是否离地
+  unsigned long current_time = millis();
+  if (current_time - last_speed_record_time >= SPEED_RECORD_INTERVAL) {
+    robot_speed_diff = LQR_speed - last_lqr_speed;
+
+    if(robot_speed_diff > 18.0) // 轮子离地
+    {
+      wheel_ground_flag = 0; // 轮子离地标记
+       // Serial.print("TAKE OFF");
+      // Serial.print(wheel_ground_flag);
+      // Serial.print(",robot_speed_diff:");
+      // Serial.print(robot_speed_diff);
+      // Serial.print(",jump_flag:");
+      // Serial.println(jump_flag); // 100 左右
+    }
+    if( robot_speed_diff < -9.0) // 轮子着地
+    {
+      wheel_ground_flag = 1; // 轮子着地标记
+      // Serial.print("LANDING:");
+      // Serial.print(",robot_speed_diff:");
+      // Serial.print(robot_speed_diff);
+      // Serial.print(",jump_flag:");
+      // Serial.println(jump_flag); // 120左右
+
+      if( jump_flag ){ // 落地点作为新的位移零点
+         resetZeroPoint(); 
+      }
+    }
+    last_lqr_speed = LQR_speed;  // 每隔100ms更新一次历史转速
+    last_speed_record_time = current_time;  // 更新记录时间
+  }
+   
+  // 重置位移零点和积分情形
+  // 1.判断摇杆是否没有前后左右运动指令
+  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
+  {
+    resetZeroPoint(); 
+  }
+  
+  // 2. 运动中实时重置位移零点和积分情形
+  if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) // 这两种是启动后自平衡，速度较快
+     || wrobot.joyy != 0 || sitting_down ) 
+  {
+    if(forward_jump_flag == 0){ // 测试 如果是前跳中，会有个向前脉冲，速度也快，不需要重置位移零点
+       resetZeroPoint(); 
+    }
+  }
+
+  distance_control = pid_distance(LQR_distance - distance_zeropoint);
+
+  // 计算 LQR_u
+  // 必须是在跳跃时jump_flag > 0，LQR_u= 角度控制量 + 角速度控制量
+  if ((jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
+  {        
+    LQR_u = angle_control + gyro_control;  // 角度控制量 + 角速度控制，位移和速度控制分量被忽略
+  }
+  else // 当轮部未离地时，LQR_u：4个参数
+  {
+    // 当轮部未离地时，LQR_u =角度控制量+角速度控制量+位移控制量+速度控制量
+    LQR_u = angle_control + gyro_control + distance_control + speed_control;
+  }
+
+  // 小车没有控制的时候自稳定状态
+  // 控制量lqr_u<5V，前进后退控制量很小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0,以及不是坐下状态
+  if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) //  && !sitting_down
+  {
+    LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
+    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(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){
+    return; // 跳过后续leg高度计算
+  }
+
+  // 机身大角度，譬如倒地时，强制腿部收缩
+  if (abs(LQR_angle) > 25.0f) 
+  {
+    ACC[0] = ACC[1] = 100;
+    Speed[0] = Speed[1] = 400;
+    Position[0] = SERVO0_MIN;
+    Position[1] = SERVO1_MIN;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    return; // 跳过后续leg高度计算
+  }
+
+  ACC[0] = ACC[1] = 100; // max 230
+  Speed[0] = Speed[1] = 400;
+
+  float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差
+  leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
+
+  // 左后腿默认等于 wrobot.height
+  left_height = wrobot.height;
+  right_height = wrobot.height;
+  LegHeightDiff = 0; // 初始化 roll 角度补偿值
+
+  // 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
+  if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) 
+  {
+    LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); 
+    leg_position_add = 0; // 不需要两个脚一样高
+  }
+  if (wrobot.joyx > 10  && abs(wrobot.joyy) > 10) 
+  {
+    // 左脚抬高
+    left_height = wrobot.height + LegHeightDiff ;
+    // Serial.print("左脚抬高");
+  } 
+  else if (wrobot.joyx < -10  && abs(wrobot.joyy) > 10) 
+  {
+    // 右脚抬高
+    right_height = wrobot.height + LegHeightDiff;
+    // Serial.print("右脚抬高");
+  } 
+
+  // 根据新的高度计算舵机位置
+  // id[0] 正面看右手，id[1] 正面看左手
+  // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
+  // const int LEG_HEIGHT_BASE   这值越小，机身越高，实测范围 0（最高），52（最低）
+  Position[0] = 2047  + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+  Position[1] = 2047  - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+
+  // Serial.print("roll_angle:");
+  // Serial.print(roll_angle);
+  // Serial.print(",Position[0]:");
+  // Serial.print(Position[0]);
+  // Serial.print(",Position[1]:");
+  // Serial.println(Position[1]);
+
+  // 限制舵机位置范围
+  if (Position[0] < SERVO0_MIN) // 舵机1最低
+    Position[0] = SERVO0_MIN;
+  else if (Position[0] > SERVO0_MAX) // 舵机1最高
+    Position[0] = SERVO0_MAX;
+
+  if (Position[1] > SERVO1_MIN) // 舵机2最低（和舵机1方向是反的）
+    Position[1] = SERVO1_MIN;
+  else if (Position[1] < SERVO1_MAX) // 舵机2最高
+    Position[1] = SERVO1_MAX;
+  
+  sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+}
+
+// 收缩小腿蓄能
+void jump_charge() // 腿部弯曲（蓄能）
+{
+    ACC[0] = 250;
+    ACC[1] = 250;
+    Speed[0] = 400;
+    Speed[1] = 400;
+    Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
+    Position[1] = (SERVO1_MIN  - 80); 
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+}
+
+// 跳跃控制
+void jump_loop()
+{ 
+  if ((wrobot.jump == 1) && (jump_pre_flag == 0))
+  {
+    jump_charge();
+    jump_pre_flag =  1;
+  }
+
+  if(jump_pre_flag > 0 ){
+    jump_pre_flag ++;
+  }
+  
+  if(jump_pre_flag){ 
+    if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
+      jump_flag = 1;
+      jump_pre_flag = 0;
+
+      // 跳跃开始
+      ACC[0] = 0;
+      ACC[1] = 0;
+      Speed[0] = 0;
+      Speed[1] = 0;
+      Position[0] = SERVO0_MAX + 20; // 极限距离
+      Position[1] = SERVO1_MAX - 20;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    }
+  }
+
+  if (jump_flag > 0) // 跳跃计数;
+  {
+    jump_flag ++;
+  }
+
+   // 跳跃中，腿部缩起来
+  if( (jump_flag > 30) && (jump_flag < 35) )
+  {
+    ACC[0] = 0;
+    ACC[1] = 0;
+    Speed[0] = 0;
+    Speed[1] = 0;
+    Position[0] = (SERVO0_MIN  + 100); // 100 约等于食指宽度
+    Position[1] = (SERVO1_MIN  - 100); 
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+  }
+
+  // 轮子着地
+  if (jump_flag > 200) //  2000=6s ,1s=2000/6=333.33
+  {
+    jump_flag = 0; // 跳跃过程结束
+  }
+
+}
+
+// yaw轴转向控制 （转向控制）
+void yaw_loop()
+{
+  // 跳跃中，YAW_output 设为0，避免干扰左右旋转
+  if(jump_flag) 
+  {
+    YAW_output = 0;
+    return;
+  }
+
+ // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
+  float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; 
+  coeff = constrain(coeff, 0.1f, 1.0f); 
+  float yaw_target = wrobot.joyx * coeff; 
+
+  // 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限）
+  float yaw_angle_control_coeff = 1.0f;
+  if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转
+    yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); 
+    // 根据转向速度，智能计算wrobot.height 
+    // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
+    // 反向映射：绝对值30→60，绝对值100→0（线性过渡）
+    wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30);
+    // 强制约束输出在0~60范围内（防止异常值）
+    wrobot.height = constrain(wrobot.height, 30, 50);
+    // 使用低波过滤器，平滑数据
+    wrobot.height = lpf_height(wrobot.height);
+
+    // 增加rgb效果
+    if(abs(wrobot.joyx) > 60){
+      handleLEDBlink();
+    }
+  }
+
+  float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; 
+  // 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适）
+  yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f);
+
+  YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+}
+
+// 电压检测初始化
+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++;
+}
+
+// 重置距离零点
+void resetZeroPoint() {
+  distance_zeropoint =  LQR_distance;
+  pid_lqr_u.error_prev = 0;
+}
+
+// 添加LED闪烁处理函数
+void handleLEDBlink() {
+  if (!isBlinking) return;
+  
+  unsigned long currentMillis = millis();
+  
+  if (currentMillis - previousBlinkMillis >= 100) { // 每100ms切换一次状态，实现快速闪烁
+    previousBlinkMillis = currentMillis;
+    
+    if (blinkState == 0) {
+      // LED亮
+      fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]);
+      FastLED.show();
+      blinkState = 1;
+    } else {
+      // LED灭
+      fill_solid(leds, NUM_LEDS, CRGB::Black);
+      FastLED.show();
+      blinkState = 0;
+      blinkCount++;
+      
+      // 检查是否完成6次闪烁
+      if (blinkCount >= 6) {
+        isBlinking = false;
+        blinkCount = 0;
+        // 闪烁完成后保持熄灭状态
+        fill_solid(leds, NUM_LEDS, CRGB::Black);
+        FastLED.show();
+      }
+    }
+  }
+}
+