优化xbox系列、盖世小鸡手柄控制

v1.2/Shibo-main-TOMMY/.gitignore

@@ -1,4 +1,5 @@
 .pio
+.test
 .vscode/.browse.c_cpp.db*
 .vscode/c_cpp_properties.json
 .vscode/launch.json

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

@@ -33,11 +33,19 @@ void handleLEDBlink();
 
 // Xbox函数
 void process_xbox_notif();
-void process_xbox_string();
 void processControllerData(const XboxControllerNotificationParser& data);
 
 const int deadZone = 10; // 摇杆死区阈值
-bool last_btnbtnRS = 0;
+bool last_btnA = 0;
+bool last_btnB = 0;
+bool last_btnX = 0;
+bool last_btnY = 0;
+bool last_btnRS = 0;
+bool last_btnLS = 0;
+bool last_btnDirRight = 0;
+bool last_btnDirLeft = 0;
+bool last_btnDirUp = 0;
+bool last_btnDirDown = 0;
 
 /************实例定义*************/
 
@@ -153,8 +161,12 @@ float last_lqr_speed = 0;    // 记录上一时刻的轮部转速
 float robot_speed_diff = 0;    // 速度差
 
 int jump_flag = 0; // 跳跃过程计数
+int forward_jump_flag = 0; // 前跳跃标志位
+int back_jump_flag = 0; // 后跳跃标志位
+int left_jump_flag = 0; // 左跳跃标志位
+int right_jump_flag = 0; // 右跳跃标志位
 int jump_pre_flag = 0; // 跳跃前蓄能计数
-int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
+int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地
 
 
 // 电压检测
@@ -179,7 +191,6 @@ int sitting_down = 0; // 标记是否正在执行坐下动作
 
 typedef struct
 {
-  int mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
   int height = 30;
   int roll;
   int jump = 0; // 0 =不跳跃 1= 跳跃
@@ -287,10 +298,8 @@ void loop()
 {
   
   // 处理LED闪烁
-  // handleLEDBlink();
-  
-  // 处理xbox手柄通知
-  process_xbox_notif();
+  handleLEDBlink();
+  process_xbox_notif();  // 处理xbox手柄通知
   
   bat_check();        // 电压检测
   delayMicroseconds(1500);
@@ -300,6 +309,7 @@ void loop()
   jump_loop();        // 跳跃动作
   leg_loop();         // 腿部动作控制
 
+   
   // 记录上一次的遥控数据数据
   wrobot.joyx_last = wrobot.joyx;
   wrobot.joyy_last = wrobot.joyy;
@@ -328,6 +338,29 @@ void loop()
     resetToDefaultCounter = 0;
   }
 
+  // 向左跳跃
+  if(left_jump_flag){
+    left_jump_flag ++ ;
+    if ( left_jump_flag < 30) {
+      motor1.target = 5.5; // 脉冲
+      motor2.target = -5.5;
+    } else {
+      left_jump_flag = 0;
+      jump_pre_flag = 1;
+    }
+  }
+  
+  // 向右跳跃
+  if(right_jump_flag){
+    right_jump_flag ++ ;
+    if ( right_jump_flag < 30) {
+      motor1.target = -5.5; // 脉冲
+      motor2.target = 5.5;
+    } else {
+      right_jump_flag = 0;
+      jump_pre_flag = 1;
+    }
+  }
 
   // 小车坐下
   if (sitting_down) {
@@ -518,7 +551,7 @@ void lqr_balance_loop()
 void leg_loop()
 {
   // 如果坐下就不控制腿部
-  if(sitting_down || jump_flag || jump_pre_flag ){
+  if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag){ 
     return; // 跳过后续leg高度计算
   }
 
@@ -545,7 +578,6 @@ void leg_loop()
   LegHeightDiff = 0; // 初始化 roll 角度补偿值
 
   // 智能计算左右脚高度差， 如果同时有转向和前后指令 ，自动计算腿部高度补偿
-  if(wrobot.mode == 0){
     if (abs(wrobot.joyx) > 10 && abs(wrobot.joyy) > 10) 
     {
       LegHeightDiff = map(abs(wrobot.joyy), 0, 100, 0, 10); 
@@ -563,7 +595,6 @@ void leg_loop()
       right_height = wrobot.height + LegHeightDiff;
       // Serial.print("右脚抬高");
     } 
-  }
   
   // 根据新的高度计算舵机位置
   // id[0] 正面看右手，id[1] 正面看左手
@@ -599,8 +630,8 @@ void jump_charge() // 腿部弯曲（蓄能）
 {
     ACC[0] = 250;
     ACC[1] = 250;
-    Speed[0] = 400;
-    Speed[1] = 400;
+    Speed[0] = 450;
+    Speed[1] = 450;
     Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
     Position[1] = (SERVO1_MIN  - 80); 
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
@@ -689,8 +720,14 @@ void yaw_loop()
     wrobot.height = lpf_height(wrobot.height);
 
     // 增加rgb效果
-    if(abs(wrobot.joyx) > 60){
-      handleLEDBlink();
+    if(abs(wrobot.joyx) > 30){
+       if(!isBlinking){  // 防止重复启动闪烁
+        isBlinking = true;
+        blinkCount = 0;  // 重置闪烁计数
+        blinkState = 0;  // 重置闪烁状态
+        previousBlinkMillis = millis();  // 重置计时
+      }
+      handleLEDBlink();  // 执行闪烁逻辑
     }
   }
 
@@ -786,46 +823,37 @@ void handleLEDBlink() {
  * ========================================*/
 void processControllerData(const XboxControllerNotificationParser& remoteData) {
 
-/*
-String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
-            String(xboxController.xboxNotif.btnX) + "," + // X键
-            String(xboxController.xboxNotif.btnB) + "," + // B键
-            String(xboxController.xboxNotif.btnA) + "," + // A键
-            String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
-            String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
-            String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
-            String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
-            String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
-            String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
-            String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
-            String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
-            String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
-            String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
-            String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
-            String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
-            String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
-            String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
-            String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
-            String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
-            String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
-            String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
-  Serial.println(str);
-  */
+// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
+//             String(xboxController.xboxNotif.btnX) + "," + // X键
+//             String(xboxController.xboxNotif.btnB) + "," + // B键
+//             String(xboxController.xboxNotif.btnA) + "," + // A键
+//             String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
+//             String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
+//             String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
+//             String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
+//             String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
+//             String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
+//             String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
+//             String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
+//             String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
+//             String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
+//             String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
+//             String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
+//             String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
+//             String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
+//             String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
+//             String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
+//             String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
+//             String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
+  // Serial.println(str);
+  // Serial.println("processControllerData");
 
-  // rgb led 上键 闪烁6次：红红黄黄绿绿
-  if(remoteData.btnDirUp && !isBlinking){
-    isBlinking = true;
-    blinkCount = 0;
-    blinkState = 0;
-    previousBlinkMillis = millis();
-  }
-
-  // 坐下 back键
+  // 坐下
   if (remoteData.btnLB && robot_enabled) { 
     robot_enabled = false;
     sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
   }
-  // 起立 start键
+  // 起立
   if (remoteData.btnRB && !robot_enabled) {
     robot_enabled = true;
     sitting_down = 0; // 清除坐下动作标记
@@ -858,10 +886,6 @@ String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
       wrobot.joyy = 0;
     } 
 
-
-  if(wrobot.mode == 0) // 智能模式
-  {
-
     // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
     int convertedJ1PotX = constrain(
       map(remoteData.joyLHori, 0, 65535, -100, 100),  
@@ -879,61 +903,65 @@ String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
     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.joyLHori, 0, 65535, -30, 30) * (100.0/30),   // 先映射到-30~30，再按比例放大到-100~100
-      -100, 100
-    );
-    if(abs(convertedJ1PotX) >  (deadZone * 2)  ) { // 防止同一个摇杆上下左右移动相互干扰
-      roll_adjust = convertedJ1PotX;
-    }else{
-      roll_adjust = 0;
+    // 右摇摆
+    if(remoteData.btnDirRight) { 
+      roll_adjust += 0.15;
+    }
+    // 左摇摆
+    if(remoteData.btnDirLeft) { 
+      roll_adjust -= 0.15;
     }
 
-    // 上下高度 左摇杆Y轴：原始0-65535 → 目标MAX到MIN
-    int convertedJ1PotY = constrain(
-        map(remoteData.joyLVert, 0, 65535, 100, -100), // 原始范围0-65535，目标范围100到-100，默认中间0
-        -100, 100
-      );
-    if(abs(convertedJ1PotY) < (deadZone * 2) ) { 
-      convertedJ1PotY = 0;
+    // 抬升
+    if(remoteData.btnDirUp) { 
+      leg_height_base -= 0.15;
+    }
+    // 下降
+    if(remoteData.btnDirDown) { 
+      leg_height_base += 0.15;
     }
-    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.joyRHori, 0, 65535, -100, 100),  // 原始范围0-65535，目标范围-100到100，默认中间0
-      -100, 100
-    );
-    if(abs(convertedJ2PotX) >  ( deadZone * 2)) {
-      wrobot.joyx  = convertedJ2PotX;
-    }else{
-      wrobot.joyx = 0;
+    // 还原
+     if (remoteData.btnLS) {
+      roll_adjust = 0.0f; 
+      leg_height_base = 20.0f;
     }
+
+    //  右扳机 高度向下 ：原始0-1023
+    // leg_height_base = constrain(
+    //     map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20
+    //     20, 60
+    //   );
+
+  // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿
+  if(remoteData.trigLT && !isBlinking){
+    isBlinking = true;
+    blinkCount = 0;
+    blinkState = 0;
+    previousBlinkMillis = millis();
   }
 
   // 跳跃 右摇杆按键
-  if (remoteData.btnRS && !last_btnbtnRS) { // 1=true，0=false
+  if (remoteData.btnRS && !last_btnRS  && robot_enabled) { // 1=true，0=false
     wrobot.jump = 1; // 跳跃
   }else{
     wrobot.jump = 0; // 不跳跃
   }
-  last_btnbtnRS = remoteData.btnRS; 
+  last_btnRS = remoteData.btnRS; 
 
-  // 切换智能模式和手动模式
-  if (remoteData.btnY) {  // 1:左摇杆上下摇摆控制（手动）
-      wrobot.mode = 1; 
+
+  // 向左跳跃
+  if (remoteData.btnX && !last_btnX  && robot_enabled) { 
+    left_jump_flag = 1; // 跳跃
   }
-  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
-      wrobot.mode = 0; 
-      roll_adjust = 0.0f; 
-      leg_height_base = 20.0f;
+  last_btnX = remoteData.btnX; 
+
+  // 向右跳跃
+  if (remoteData.btnB && !last_btnB  && robot_enabled) { 
+    right_jump_flag = 1; // 跳跃
   }
+  last_btnB = remoteData.btnB; 
+
 }
 
 /* Xbox数据处理函数（直接传递原生数据） */
@@ -948,7 +976,7 @@ void process_xbox_notif(){
     }
   }
   else{
-    Serial.println("Xbox controller not connected");
+    // Serial.println("Xbox controller not connected");
   }
 }
 

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

@@ -1,18 +1,14 @@
 /* ---------------------------------
 Copyright (c) 2024 Mu Shibo
-Modify by Tommy 2025.10.08
+Modify by Tommy 2025.10.30
 ------------------------------------- */
 #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"
-
+#include <esp_adc_cal.h>
 #include <XboxSeriesXControllerESP32_asukiaaa.hpp>  // xbox手柄
 
 /************ xbox手柄实体 *************/
@@ -35,10 +31,17 @@ void jump_charge(); // 腿部弯曲（蓄能）
 // RGB相关函数
 void handleLEDBlink();
 
-// xbox
-void process_xbox_string();
+// Xbox函数
 void process_xbox_notif();
+void processControllerData(const XboxControllerNotificationParser& data);
 
+const int deadZone = 10; // 摇杆死区阈值
+bool last_btnA = 0;
+bool last_btnB = 0;
+bool last_btnX = 0;
+bool last_btnY = 0;
+bool last_btnRS = 0;
+bool last_btnLS = 0;
 
 /************实例定义*************/
 
@@ -154,6 +157,8 @@ float last_lqr_speed = 0;    // 记录上一时刻的轮部转速
 float robot_speed_diff = 0;    // 速度差
 
 int jump_flag = 0; // 跳跃过程计数
+int left_jump_flag = 0; // 左跳跃标志位
+int right_jump_flag = 0; // 右跳跃标志位
 int jump_pre_flag = 0; // 跳跃前蓄能计数
 int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
 
@@ -191,181 +196,12 @@ typedef struct
 } 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
-  
-    // 前后移动 右摇杆Y轴：原始1080-0 → 目标100到-100
-    int convertedJ2PotY = constrain(
-      map(remoteData.joyRightY, 1080, 0, 100, -100),  // 原始1080-0，目标100~-100
-      -100, 100
-    );
-    if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
-      wrobot.joyy = convertedJ2PotY;
-    }else{
-      wrobot.joyy = 0;
-    } 
-
-
-  if(wrobot.mode == 0) // 智能模式
-  {
-
-    // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
-    int convertedJ1PotX = constrain(
-      map(remoteData.joyLeftX, 0, 900, -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{
-    wrobot.jump = 0; // 不跳跃
-  }
-  last_btnJoyRight = remoteData.btnJoyRight; 
-
-  // 切换智能模式和手动模式
-  if (remoteData.btnY) {  // 1:左摇杆上下摇摆控制（手动）
-      wrobot.mode = 1; 
-  }
-  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
-      wrobot.mode = 0; 
-      roll_adjust = 0.0f; 
-      leg_height_base = 20.0f;
-  }
-  
-}
 
 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;
 
@@ -445,8 +281,8 @@ void setup()
   fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
   FastLED.show();
 
-
-  Serial.println("Starting NimBLE Client");
+  // Xbox手柄初始化
+  Serial.println("Starting Xbox Bluetooth Client");
   xboxController.begin();
 
 
@@ -457,10 +293,8 @@ void loop()
 {
   
   // 处理LED闪烁
-  // handleLEDBlink();
-  
-  // 处理xbox手柄通知
-  process_xbox_notif();
+  handleLEDBlink();
+  process_xbox_notif();  // 处理xbox手柄通知
   
   bat_check();        // 电压检测
   delayMicroseconds(1500);
@@ -470,6 +304,7 @@ void loop()
   jump_loop();        // 跳跃动作
   leg_loop();         // 腿部动作控制
 
+   
   // 记录上一次的遥控数据数据
   wrobot.joyx_last = wrobot.joyx;
   wrobot.joyy_last = wrobot.joyy;
@@ -498,6 +333,28 @@ void loop()
     resetToDefaultCounter = 0;
   }
 
+  // 向左跳跃
+  if(left_jump_flag){
+    left_jump_flag ++ ;
+    if ( left_jump_flag < 30) {
+      motor1.target = 5.5; // 脉冲
+      motor2.target = -5.5;
+    } else {
+      left_jump_flag = 0;
+      jump_pre_flag = 1;
+    }
+  }
+  // 向右跳跃
+  if(right_jump_flag){
+    right_jump_flag ++ ;
+    if ( right_jump_flag < 30) {
+      motor1.target = -5.5; // 脉冲
+      motor2.target = 5.5;
+    } else {
+      right_jump_flag = 0;
+      jump_pre_flag = 1;
+    }
+  }
 
   // 小车坐下
   if (sitting_down) {
@@ -688,7 +545,7 @@ void lqr_balance_loop()
 void leg_loop()
 {
   // 如果坐下就不控制腿部
-  if(sitting_down || jump_flag || jump_pre_flag ){
+  if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag){ 
     return; // 跳过后续leg高度计算
   }
 
@@ -769,8 +626,8 @@ void jump_charge() // 腿部弯曲（蓄能）
 {
     ACC[0] = 250;
     ACC[1] = 250;
-    Speed[0] = 400;
-    Speed[1] = 400;
+    Speed[0] = 450;
+    Speed[1] = 450;
     Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
     Position[1] = (SERVO1_MIN  - 80); 
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
@@ -951,48 +808,193 @@ void handleLEDBlink() {
   }
 }
 
-void process_xbox_string(){
+/*========================================
+ * 控制器数据处理（共用函数）
+ * ========================================*/
+void processControllerData(const XboxControllerNotificationParser& remoteData) {
 
-    String str = String(xboxController.xboxNotif.btnY) + "," + // 默认0 ，按下1
-            String(xboxController.xboxNotif.btnX) + "," +
-            String(xboxController.xboxNotif.btnB) + "," +
-            String(xboxController.xboxNotif.btnA) + "," +
-            String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
-            String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
-            String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
-            String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
-            String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
-            String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
-            String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
-            String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
-            String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
-            String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
-            String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
-            String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
-            String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
-            String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
-            String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
-            String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
-            String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
-            String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
+// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
+//             String(xboxController.xboxNotif.btnX) + "," + // X键
+//             String(xboxController.xboxNotif.btnB) + "," + // B键
+//             String(xboxController.xboxNotif.btnA) + "," + // A键
+//             String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
+//             String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
+//             String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
+//             String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
+//             String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
+//             String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
+//             String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
+//             String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
+//             String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
+//             String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
+//             String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
+//             String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
+//             String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
+//             String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
+//             String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
+//             String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
+//             String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
+//             String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
+  // Serial.println(str);
 
-            Serial.println(str);
 
-};
 
+  // 坐下
+  if (remoteData.btnLB && robot_enabled) { 
+    robot_enabled = false;
+    sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
+  }
+  // 起立
+  if (remoteData.btnRB && !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;
+  }
+
+    // 前后移动 右摇杆Y轴
+    int convertedJ2PotY = constrain(
+      map(remoteData.joyRVert, 0, 65535, 100, -100),  // 原始0~65535，目标100~-100
+      -100, 100
+    );
+    if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+      wrobot.joyy = convertedJ2PotY;
+    }else{
+      wrobot.joyy = 0;
+    } 
+
+
+  if(wrobot.mode == 0) // 智能模式
+  {
+
+    // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
+    int convertedJ1PotX = constrain(
+      map(remoteData.joyLHori, 0, 65535, -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.joyLHori, 0, 65535, -30, 30) * (100.0/30),   // 先映射到-30~30，再按比例放大到-100~100
+      -100, 100
+    );
+    if(abs(convertedJ1PotX) >  (deadZone * 2)  ) { // 防止同一个摇杆上下左右移动相互干扰
+      roll_adjust = convertedJ1PotX;
+    }else{
+      roll_adjust = 0;
+    }
+
+    // 上下高度 左摇杆Y轴：原始0-65535 → 目标MAX到MIN
+    int convertedJ1PotY = constrain(
+        map(remoteData.joyLVert, 0, 65535, 100, -100), // 原始范围0-65535，目标范围100到-100，默认中间0
+        -100, 100
+      );
+    if(abs(convertedJ1PotY) < (deadZone * 2) ) { 
+      convertedJ1PotY = 0;
+    }
+    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.joyRHori, 0, 65535, -100, 100),  // 原始范围0-65535，目标范围-100到100，默认中间0
+      -100, 100
+    );
+    if(abs(convertedJ2PotX) >  ( deadZone * 2)) {
+      wrobot.joyx  = convertedJ2PotX;
+    }else{
+      wrobot.joyx = 0;
+    }
+  }
+
+    //  右扳机 高度向下 ：原始0-1023
+    leg_height_base = constrain(
+        map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20
+        20, 60
+      );
+    // leg_height_base = lpf_height(leg_height_base);
+
+  // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿
+  if(remoteData.trigLT && !isBlinking){
+    isBlinking = true;
+    blinkCount = 0;
+    blinkState = 0;
+    previousBlinkMillis = millis();
+  }
+
+  // 跳跃 右摇杆按键
+  if (remoteData.btnRS && !last_btnRS  && robot_enabled) { // 1=true，0=false
+    wrobot.jump = 1; // 跳跃
+  }else{
+    wrobot.jump = 0; // 不跳跃
+  }
+  last_btnRS = remoteData.btnRS; 
+
+  // 让左跳跃
+  if (remoteData.btnX && !last_btnX  && robot_enabled) { 
+    left_jump_flag = 1; // 跳跃
+  }
+  last_btnX = remoteData.btnX; 
+
+  // 让右跳跃
+  if (remoteData.btnB && !last_btnB  && robot_enabled) { 
+    right_jump_flag = 1; // 跳跃
+  }
+  last_btnB = remoteData.btnB; 
+
+  // 切换智能模式和手动模式
+  if (remoteData.btnY) {  // 1:左摇杆上下摇摆控制（手动）
+      wrobot.mode = 1; 
+  }
+  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
+      wrobot.mode = 0; 
+      roll_adjust = 0.0f; 
+      leg_height_base = 20.0f;
+  }
+}
+
+/* Xbox数据处理函数（直接传递原生数据） */
 void process_xbox_notif(){
-   xboxController.onLoop();
+  xboxController.onLoop();
   if (xboxController.isConnected()){
     if (xboxController.isWaitingForFirstNotification()){
-      Serial.println("waiting for first xbox notification");
+      Serial.println("Waiting for first Xbox data...");
     }else{
-      process_xbox_string();
+      // Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮
+      processControllerData(xboxController.xboxNotif);
     }
   }
   else{
-    Serial.println("xbox not connected");
-    // if (xboxController.getCountFailedConnection() > 2){
-    //   ESP.restart();
-    // }
+    // Serial.println("Xbox controller not connected");
   }
 }
+

v1.2/Shibo-main-TOMMY/test/main.cpp

@@ -1,18 +1,14 @@
 /* ---------------------------------
 Copyright (c) 2024 Mu Shibo
-Modify by Tommy 2025.10.08
+Modify by Tommy 2025.10.30
 ------------------------------------- */
 #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"
-
+#include <esp_adc_cal.h>
 #include <XboxSeriesXControllerESP32_asukiaaa.hpp>  // xbox手柄
 
 /************ xbox手柄实体 *************/
@@ -33,12 +29,20 @@ void resetZeroPoint();
 void jump_charge(); // 腿部弯曲（蓄能）
 
 // RGB相关函数
+void updateBluetoothLED();
 void handleLEDBlink();
 
-// xbox
-void process_xbox_string();
+// Xbox函数
 void process_xbox_notif();
+void processControllerData(const XboxControllerNotificationParser& data);
 
+const int deadZone = 10; // 摇杆死区阈值
+bool last_btnA = 0;
+bool last_btnB = 0;
+bool last_btnX = 0;
+bool last_btnY = 0;
+bool last_btnRS = 0;
+bool last_btnLS = 0;
 
 /************实例定义*************/
 
@@ -77,7 +81,7 @@ SMS_STS sms_sts;
 // MPU6050实例
 MPU6050 mpu6050(I2Ctwo);
 
-// FastLED配置
+// rgb 配置
 #define RGB_PIN 21
 #define NUM_LEDS 2
 CRGB leds[NUM_LEDS];
@@ -87,6 +91,12 @@ int blinkCount = 0;
 bool isBlinking = false;
 CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
 
+// 蓝牙连接状态标志
+bool isBluetoothConnected = false;
+unsigned long previousBlueBlinkMillis = 0; // 蓝牙蓝灯闪烁计时（新增）
+bool blueBlinkState = 0; // 蓝牙蓝灯闪烁状态（新增，0=灭，1=亮）
+
+
 /************参数定义*************/
 #define pi 3.1415927
 
@@ -153,7 +163,10 @@ const unsigned long SPEED_RECORD_INTERVAL = 100;  // 转速记录间隔(毫秒)
 float last_lqr_speed = 0;    // 记录上一时刻的轮部转速
 float robot_speed_diff = 0;    // 速度差
 
+// 跳跃状态标志位
 int jump_flag = 0; // 跳跃过程计数
+int left_jump_flag = 0; // 左跳跃标志位
+int right_jump_flag = 0; // 右跳跃标志位
 int jump_pre_flag = 0; // 跳跃前蓄能计数
 int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
 
@@ -191,181 +204,12 @@ typedef struct
 } 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
-  
-    // 前后移动 右摇杆Y轴：原始1080-0 → 目标100到-100
-    int convertedJ2PotY = constrain(
-      map(remoteData.joyRightY, 1080, 0, 100, -100),  // 原始1080-0，目标100~-100
-      -100, 100
-    );
-    if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
-      wrobot.joyy = convertedJ2PotY;
-    }else{
-      wrobot.joyy = 0;
-    } 
-
-
-  if(wrobot.mode == 0) // 智能模式
-  {
-
-    // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
-    int convertedJ1PotX = constrain(
-      map(remoteData.joyLeftX, 0, 900, -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{
-    wrobot.jump = 0; // 不跳跃
-  }
-  last_btnJoyRight = remoteData.btnJoyRight; 
-
-  // 切换智能模式和手动模式
-  if (remoteData.btnY) {  // 1:左摇杆上下摇摆控制（手动）
-      wrobot.mode = 1; 
-  }
-  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
-      wrobot.mode = 0; 
-      roll_adjust = 0.0f; 
-      leg_height_base = 20.0f;
-  }
-  
-}
 
 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;
 
@@ -445,8 +289,8 @@ void setup()
   fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
   FastLED.show();
 
-
-  Serial.println("Starting NimBLE Client");
+  // Xbox手柄初始化
+  Serial.println("Starting Xbox Bluetooth Client");
   xboxController.begin();
 
 
@@ -456,14 +300,16 @@ void setup()
 void loop()
 {
   
-  // 处理LED闪烁
-  // handleLEDBlink();
+ // 处理LED闪烁
+  handleLEDBlink();
+  // 处理蓝牙状态蓝灯，优先级低于自定义闪烁
+  updateBluetoothLED();
   
   // 处理xbox手柄通知
   process_xbox_notif();
   
   bat_check();        // 电压检测
-  delayMicroseconds(1500);
+  // delayMicroseconds(1500);
   mpu6050.update();   // IMU数据更新
   lqr_balance_loop(); // lqr自平衡控制
   yaw_loop();         // yaw轴转向控制
@@ -498,6 +344,28 @@ void loop()
     resetToDefaultCounter = 0;
   }
 
+  // 向左跳跃
+  if(left_jump_flag){
+    left_jump_flag ++ ;
+    if ( left_jump_flag < 30) {
+      motor1.target = 5.5; // 脉冲
+      motor2.target = -5.5;
+    } else {
+      left_jump_flag = 0;
+      jump_pre_flag = 1;
+    }
+  }
+  // 向右跳跃
+  if(right_jump_flag){
+    right_jump_flag ++ ;
+    if ( right_jump_flag < 30) {
+      motor1.target = -5.5; // 脉冲
+      motor2.target = 5.5;
+    } else {
+      right_jump_flag = 0;
+      jump_pre_flag = 1;
+    }
+  }
 
   // 小车坐下
   if (sitting_down) {
@@ -688,7 +556,7 @@ void lqr_balance_loop()
 void leg_loop()
 {
   // 如果坐下就不控制腿部
-  if(sitting_down || jump_flag || jump_pre_flag ){
+  if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag){ 
     return; // 跳过后续leg高度计算
   }
 
@@ -769,8 +637,8 @@ void jump_charge() // 腿部弯曲（蓄能）
 {
     ACC[0] = 250;
     ACC[1] = 250;
-    Speed[0] = 400;
-    Speed[1] = 400;
+    Speed[0] = 450;
+    Speed[1] = 450;
     Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
     Position[1] = (SERVO1_MIN  - 80); 
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
@@ -918,6 +786,33 @@ void resetZeroPoint() {
   pid_lqr_u.error_prev = 0;
 }
 
+// 蓝牙状态对应的蓝灯控制
+void updateBluetoothLED() {
+  // 仅在未执行自定义闪烁时，才显示蓝牙状态蓝灯
+  if (!isBlinking) {
+    if (isBluetoothConnected) {
+      // 蓝牙已连：蓝灯常亮
+      fill_solid(leds, NUM_LEDS, CRGB::Blue);
+      FastLED.show();
+    } else {
+      // 蓝牙未连：蓝灯500ms闪烁（周期1秒）
+      unsigned long currentMillis = millis();
+      if (currentMillis - previousBlueBlinkMillis >= 500) {
+        previousBlueBlinkMillis = currentMillis;
+        // 切换蓝灯状态（亮/灭）
+        if (blueBlinkState == 0) {
+          fill_solid(leds, NUM_LEDS, CRGB::Blue);
+          blueBlinkState = 1;
+        } else {
+          fill_solid(leds, NUM_LEDS, CRGB::Black);
+          blueBlinkState = 0;
+        }
+        FastLED.show();
+      }
+    }
+  }
+}
+
 // 添加LED闪烁处理函数
 void handleLEDBlink() {
   if (!isBlinking) return;
@@ -950,75 +845,196 @@ void handleLEDBlink() {
     }
   }
 }
-typedef struct {
-  bool btnY; // Y键
-  bool btnX; // X键
-  bool btnB; // B键
-  bool btnA; // A键
-  bool btnLB; // 左肩键
-  bool btnRB; // 右肩键
-  bool btnSelect; // 中间十字 左键 试图键
-  bool btnStart; // 中间十字 右键 菜单键
-  bool btnXbox; // 中间十字 上键 home键
-  bool btnShare; // 中间十字 下键 截图键
-  bool btnLS; // 左摇杆键
-  bool btnRS; // 右摇杆键
-  bool btnDirUp; // 上键
-  bool btnDirRight; // 右键
-  bool btnDirDown; // 下键
-  bool btnDirLeft; // 左键
-  int joyLHori; // 左摇杆 水平轴 （左-右 0 -65535）
-  int joyLVert; // 左摇杆 垂直轴 （上-下 0 -65535）
-  int joyRHori; // 右摇杆 水平轴 （左-右 0 -65535）
-  int joyRVert; // 右摇杆 垂直轴 （上-下 0 -65535）
-  int trigLT; // 左扳机键 （松开- 按下 0 -1023）
-  int trigRT; // 右扳机键 （松开- 按下 0 -1023）
-} ControllerData;
 
-ControllerData controllerData;
+/*========================================
+ * 控制器数据处理（共用函数）
+ * ========================================*/
+void processControllerData(const XboxControllerNotificationParser& remoteData) {
 
-void process_xbox_string(){
+// String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
+//             String(xboxController.xboxNotif.btnX) + "," + // X键
+//             String(xboxController.xboxNotif.btnB) + "," + // B键
+//             String(xboxController.xboxNotif.btnA) + "," + // A键
+//             String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
+//             String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
+//             String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
+//             String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
+//             String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
+//             String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
+//             String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
+//             String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
+//             String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
+//             String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
+//             String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
+//             String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
+//             String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
+//             String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
+//             String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
+//             String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
+//             String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
+//             String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
+  // Serial.println(str);
 
-    String str = String(xboxController.xboxNotif.btnY) + "," + // 默认0 ，按下1
-            String(xboxController.xboxNotif.btnX) + "," +
-            String(xboxController.xboxNotif.btnB) + "," +
-            String(xboxController.xboxNotif.btnA) + "," +
-            String(xboxController.xboxNotif.btnLB) + "," + // 左肩键
-            String(xboxController.xboxNotif.btnRB) + "," + // 右肩键
-            String(xboxController.xboxNotif.btnSelect) + "," + // 中间十字 左键 试图键
-            String(xboxController.xboxNotif.btnStart) + "," + // 中间十字 右键 菜单键
-            String(xboxController.xboxNotif.btnXbox) + "," + // 中间十字 上键 home键
-            String(xboxController.xboxNotif.btnShare) + "," + // 中间十字 下键 截图键
-            String(xboxController.xboxNotif.btnLS) + "," + // 左摇杆键
-            String(xboxController.xboxNotif.btnRS) + "," + // 右摇杆键
-            String(xboxController.xboxNotif.btnDirUp) + "," + // 上键
-            String(xboxController.xboxNotif.btnDirRight) + "," + // 右键
-            String(xboxController.xboxNotif.btnDirDown) + "," + // 下键
-            String(xboxController.xboxNotif.btnDirLeft) + "," + // 左键
-            String(xboxController.xboxNotif.joyLHori) + "," + // 左摇杆 水平轴 （左-右 0 -65535）
-            String(xboxController.xboxNotif.joyLVert) + "," + // 左摇杆 垂直轴 （上-下 0 -65535）
-            String(xboxController.xboxNotif.joyRHori) + "," + // 右摇杆 水平轴 （左-右 0 -65535）
-            String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
-            String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
-            String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
 
-            Serial.println(str);
 
-};
+  // 坐下
+  if (remoteData.btnLB && robot_enabled) { 
+    robot_enabled = false;
+    sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
+  }
+  // 起立
+  if (remoteData.btnRB && !robot_enabled) {
+    robot_enabled = true;
+    sitting_down = 0; // 清除坐下动作标记
+  }
 
-void process_xbox_notif(){
-   xboxController.onLoop();
-  if (xboxController.isConnected()){
-    if (xboxController.isWaitingForFirstNotification()){
-      Serial.println("waiting for first xbox notification");
+  // 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
+  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;
+  }
+
+    // 前后移动 右摇杆Y轴
+    int convertedJ2PotY = constrain(
+      map(remoteData.joyRVert, 0, 65535, 100, -100),  // 原始0~65535，目标100~-100
+      -100, 100
+    );
+    if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+      wrobot.joyy = convertedJ2PotY;
     }else{
-      process_xbox_string();
+      wrobot.joyy = 0;
+    } 
+
+
+  if(wrobot.mode == 0) // 智能模式
+  {
+
+    // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
+    int convertedJ1PotX = constrain(
+      map(remoteData.joyLHori, 0, 65535, -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.joyLHori, 0, 65535, -30, 30) * (100.0/30),   // 先映射到-30~30，再按比例放大到-100~100
+      -100, 100
+    );
+    if(abs(convertedJ1PotX) >  (deadZone * 2)  ) { // 防止同一个摇杆上下左右移动相互干扰
+      roll_adjust = convertedJ1PotX;
+    }else{
+      roll_adjust = 0;
+    }
+
+    // 上下高度 左摇杆Y轴：原始0-65535 → 目标MAX到MIN
+    int convertedJ1PotY = constrain(
+        map(remoteData.joyLVert, 0, 65535, 100, -100), // 原始范围0-65535，目标范围100到-100，默认中间0
+        -100, 100
+      );
+    if(abs(convertedJ1PotY) < (deadZone * 2) ) { 
+      convertedJ1PotY = 0;
+    }
+    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.joyRHori, 0, 65535, -100, 100),  // 原始范围0-65535，目标范围-100到100，默认中间0
+      -100, 100
+    );
+    if(abs(convertedJ2PotX) >  ( deadZone * 2)) {
+      wrobot.joyx  = convertedJ2PotX;
+    }else{
+      wrobot.joyx = 0;
+    }
+  }
+
+    //  右扳机 高度向下 ：原始0-1023
+    leg_height_base = constrain(
+        map(remoteData.trigRT, 0, 1023, 20, 60), // 原始范围0-1023，默认0，所以leg_height_base=20
+        20, 60
+      );
+    // leg_height_base = lpf_height(leg_height_base);
+
+  // 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿
+  if(remoteData.trigLT && !isBlinking){
+    isBlinking = true;
+    blinkCount = 0;
+    blinkState = 0;
+    previousBlinkMillis = millis();
+  }
+
+  // 跳跃 右摇杆按键
+  if (remoteData.btnRS && !last_btnRS  && robot_enabled) { // 1=true，0=false
+    wrobot.jump = 1; // 跳跃
+  }else{
+    wrobot.jump = 0; // 不跳跃
+  }
+  last_btnRS = remoteData.btnRS; 
+
+  // 让左跳跃
+  if (remoteData.btnX && !last_btnX  && robot_enabled) { 
+    left_jump_flag = 1; // 跳跃
+  }
+  last_btnX = remoteData.btnX; 
+
+  // 让右跳跃
+  if (remoteData.btnB && !last_btnB  && robot_enabled) { 
+    right_jump_flag = 1; // 跳跃
+  }
+  last_btnB = remoteData.btnB; 
+
+  // 切换智能模式和手动模式
+  if (remoteData.btnY) {  // 1:左摇杆上下摇摆控制（手动）
+      wrobot.mode = 1; 
+  }
+  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
+      wrobot.mode = 0; 
+      roll_adjust = 0.0f; 
+      leg_height_base = 20.0f;
+  }
+}
+
+/* Xbox数据处理函数（直接传递原生数据） */
+void process_xbox_notif(){
+  xboxController.onLoop();
+  if (xboxController.isConnected()){
+    isBluetoothConnected = true; // 手柄已连接
+    if (xboxController.isWaitingForFirstNotification()){
+      Serial.println("Waiting for first Xbox data...");
+    }else{
+      // Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮
+      processControllerData(xboxController.xboxNotif);
     }
   }
   else{
-    Serial.println("xbox not connected");
-    // if (xboxController.getCountFailedConnection() > 2){
-    //   ESP.restart();
-    // }
+    isBluetoothConnected = false; // 手柄未连接
+    // Serial.println("Xbox controller not connected");
   }
 }
+