优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制

README.md

@@ -27,6 +27,7 @@
           删除了其他无用页面和代码，只保留main.cpp核心页面。
 
 2025-10-10 v1.2版本改为通过xbox系列手柄进行遥控,并增加智能模式和手动模式
+2025-10-17 优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制
 
 世博轮组开源地址github:
 https://github.com/MuShibo/Micro-Wheeled_leg-Robot
@@ -34,11 +35,10 @@ https://github.com/MuShibo/Micro-Wheeled_leg-Robot
 TOMMY优化版本项目地址：
 https://gitee.com/myperworld/wrobot
 
-
 世博轮足配件以及遥控器链接地址：
 https://www.goofish.com/item?id=855415907884&categoryId=50023914
 
-QQ交流群：991727288
+所有文件在QQ群，欢迎交流：991727288（群一），156132500（群二）
 
 后续还会更新，敬请关注小红书或者B站。
 B站演示地址：https://space.bilibili.com/25336365

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

@@ -60,6 +60,7 @@
         "stdexcept": "cpp",
         "cinttypes": "cpp",
         "cstdbool": "cpp",
-        "typeinfo": "cpp"
+        "typeinfo": "cpp",
+        "list": "cpp"
     }
 }

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

@@ -0,0 +1,50 @@
+/* ---------------------------------
+ * LED 控制模块实现
+------------------------------------- */
+#include "RGBController.h"
+
+// LED 全局变量定义（与主文件解耦）
+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};
+
+// 初始化LED（替代原setup中的FastLED初始化代码）
+void initLEDs() {
+  FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
+  FastLED.setBrightness(50);
+  fill_solid(leds, NUM_LEDS, CRGB::Black);  // 初始熄灭
+  FastLED.show();
+}
+
+// 处理LED闪烁逻辑（原handleLEDBlink函数，逻辑不变）
+void handleLEDBlink() {
+  if (!isBlinking) return;
+  
+  unsigned long currentMillis = millis();
+  
+  if (currentMillis - previousBlinkMillis >= 100) {
+    previousBlinkMillis = currentMillis;
+    
+    if (blinkState == 0) {
+      fill_solid(leds, NUM_LEDS, blinkColors[blinkCount]);
+      FastLED.show();
+      blinkState = 1;
+    } else {
+      fill_solid(leds, NUM_LEDS, CRGB::Black);
+      FastLED.show();
+      blinkState = 0;
+      blinkCount++;
+      
+      if (blinkCount >= 6) {
+        isBlinking = false;
+        blinkCount = 0;
+        fill_solid(leds, NUM_LEDS, CRGB::Black);
+        FastLED.show();
+      }
+    }
+  }
+}
+    

v1.2/Shibo-main-TOMMY/src/RGBController.h

@@ -0,0 +1,28 @@
+/* ---------------------------------
+ * RGB 控制模块
+ * 包含：RGB初始化、闪烁逻辑、状态管理
+------------------------------------- */
+#ifndef RGB_CONTROLLER_H
+#define RGB_CONTROLLER_H
+
+#include <Arduino.h>
+#include <FastLED.h>
+
+// RGB 硬件配置（原代码中的宏定义）
+#define RGB_PIN 21
+#define NUM_LEDS 2
+
+// LED 全局变量（原代码中的状态变量）
+extern CRGB leds[NUM_LEDS];  // 外部声明，在主文件中定义
+extern unsigned long previousBlinkMillis;
+extern int blinkState;
+extern int blinkCount;
+extern bool isBlinking;
+extern CRGB blinkColors[6];
+
+// LED 函数声明
+void initLEDs();               // 初始化LED
+void handleLEDBlink();         // 处理LED闪烁逻辑
+
+#endif  // RGB_CONTROLLER_H
+    

v1.2/Shibo-main-TOMMY/src/RobotActions.h

@@ -0,0 +1,66 @@
+#ifndef RobotActions_h
+#define RobotActions_h
+
+#include <Arduino.h>
+
+// 单个动作步骤结构体
+typedef struct {
+  int joyx;          // 左右转向（-100~100），正面看，正数：向左转，负数，向右转
+  int joyy;          // 前后移动（-100~100）
+  int jump;          // 跳跃标记（0=不跳，1=跳）
+  float roll_adjust; // 左右平衡调整（-5~5），基数0，正面看，正数：向右摆动；负数：向左摆动
+  float leg_height_base; // 腿部基准高度（20~60，核心高度控制参数）,基数20，变小升高，变大降低
+  unsigned long stepDuration; // 步骤持续时间（毫秒）
+  unsigned long pauseDuration; // 步骤执行后的停顿时间（毫秒）
+  // 舵机参数：无需默认值，动作集定义时按需填写，不填则用结构体默认值（或省略）
+  int acc0;    // 舵机1加速度 0-250
+  int acc1;    // 舵机2加速度 0-250
+  int speed0;  // 舵机1速度 0-500
+  int speed1;  // 舵机2速度 0-500
+} ActionStep;
+
+// 结束标记  后4个舵机参数，使用默认值-1
+const ActionStep END_MARKER = {0, 0, 0, 0, 0, 0, 0, -1, -1, -1, -1};
+
+// 连续动作指令集结构体
+typedef struct {
+  String actionName;   // 动作名称
+  ActionStep* steps;   // 步骤数组（末尾必须包含END_MARKER）
+} RobotAction;
+
+// 宏：初始化RobotAction
+#define CREATE_ACTION(actionName, stepsArray) \
+  { actionName, stepsArray }
+
+// -------------------------- 自定义连续动作指令集 --------------------------
+
+ActionStep step_lift_sway[] = {
+  // {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
+// 舵机1加速度，舵机2加速度，舵机1速度，舵机2速度}
+  {0, 0, 0, 25, 10, 500, 0, -1, -1, -1, -1}, // 右摆
+  {0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
+  {0, 0, 0, -25, 10, 500, 0, -1, -1, -1, -1}, // 左摆
+  {0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
+  
+  {0, 0, 0, 25, 10, 500, 0, -1, -1, -1, -1}, // 右摆
+  {0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
+  {0, 0, 0, -25, 10, 500, 0, -1, -1, -1, -1}, // 左摆
+  {0, 0, 0, 0, -10, 200, 0, -1, -1, -1, -1}, // 回到中间最高位
+
+  {0, 0, 0, 0, 20, 200, 0, -1, -1, -1, -1}, // 回到中间中位
+  END_MARKER                       // 结束标记
+};
+RobotAction action_lift_sway = CREATE_ACTION("action_lift_sway", step_lift_sway);
+
+ActionStep step_node[] = {
+  // {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
+  // 舵机1加速度，舵机2加速度，舵机1速度，舵机2速度}
+  {0, 0, 0, 0, 40, 150, 0, 250, 250, 800, 800},  // 下降
+  {0, 0, 0, 0, 20, 150, 300, 150, 150, 400, 400}, // 回到高度20
+  {0, 0, 0, 0, 40, 150, 0, 250, 250, 800, 800},  // 下降
+  {0, 0, 0, 0, 20, 150, 0, 150, 150, 400, 400}, // 回到高度20
+  END_MARKER                       // 结束标记
+};
+RobotAction action_node = CREATE_ACTION("action_node", step_node);
+
+#endif

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

@@ -0,0 +1,47 @@
+#include "VoltageMonitor.h"
+
+// 初始化电压检测相关全局变量（与头文件声明对应）
+uint16_t bat_check_num = 0;
+esp_adc_cal_characteristics_t adc_chars;
+
+/**
+ * @brief 电压检测初始化（原代码逻辑完全保留）
+ */
+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");
+  }
+}
+
+/**
+ * @brief 电压检测逻辑（原代码逻辑完全保留）
+ */
+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;
+
+    // 电量显示（LED_BAT引脚控制）
+    if (battery >= 7.8)
+      digitalWrite(LED_BAT, HIGH);
+    else
+      digitalWrite(LED_BAT, LOW);
+    bat_check_num = 0;
+  }
+  else
+    bat_check_num++;
+}

v1.2/Shibo-main-TOMMY/src/VoltageMonitor.h

@@ -0,0 +1,23 @@
+#ifndef VOLTAGE_MONITOR_H
+#define VOLTAGE_MONITOR_H
+
+#include <Arduino.h>
+#include <esp_adc_cal.h>
+
+// 电压检测相关宏定义（原代码中电压模块的宏）
+#define LED_BAT 13                  // 电量显示LED引脚
+#define BAT_PIN 35                  // 电池电压检测ADC引脚
+static const adc1_channel_t channel = ADC1_CHANNEL_7;  // ADC通道
+static const adc_bits_width_t width = ADC_WIDTH_BIT_12;// ADC采样宽度
+static const adc_atten_t atten = ADC_ATTEN_DB_12;      // ADC衰减系数
+static const adc_unit_t unit = ADC_UNIT_1;             // ADC单元
+
+// 电压检测相关全局变量（原代码中电压模块的全局变量）
+extern uint16_t bat_check_num;                          // 电压检测计数
+extern esp_adc_cal_characteristics_t adc_chars;        // ADC校准参数
+
+// 电压检测相关函数声明（原代码中电压模块的函数）
+void adc_calibration_init();  // 电压检测初始化
+void bat_check();             // 电压检测逻辑
+
+#endif // VOLTAGE_MONITOR_H

v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h

@@ -0,0 +1,46 @@
+/* ---------------------------------
+ * 蓝牙手柄（Xbox）模块
+ * 包含：蓝牙实体、状态变量、连接处理函数
+ * 不包含：手柄数据解析逻辑（processControllerData）
+------------------------------------- */
+#ifndef XBOX_BLUETOOTH_H
+#define XBOX_BLUETOOTH_H
+
+#include <Arduino.h>
+#include <XboxSeriesXControllerESP32_asukiaaa.hpp>  // Xbox手柄库
+
+/************ Xbox手柄全局实体 *************/
+// 连接目标手柄的MAC地址（保持原地址不变）
+XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79");
+
+
+/************ 外部函数声明 *************/
+// 声明主文件中的数据处理函数（在主文件中实现）
+extern void processControllerData(const XboxControllerNotificationParser& data);
+
+/************ 蓝牙手柄核心函数 *************/
+
+// 初始化蓝牙手柄（供主文件setup()调用）
+void xboxControllerInit() {
+  // 启动蓝牙连接
+  xboxController.begin();
+}
+
+// 处理蓝牙手柄通知（循环调用，保持连接并接收数据）
+void process_xbox_notif() {
+  xboxController.onLoop();  // 维持蓝牙连接状态
+  
+  if (xboxController.isConnected()) {
+    if (xboxController.isWaitingForFirstNotification()) {
+      // 可添加调试信息（如"等待手柄数据..."）
+    } else {
+      // 接收数据后，调用主文件的解析函数
+      processControllerData(xboxController.xboxNotif);
+    }
+  } else {
+    // 可添加调试信息（如"手柄未连接"）
+  }
+}
+
+#endif  // XBOX_BLUETOOTH_H
+    

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

@@ -1,6 +1,6 @@
 /* ---------------------------------
 Copyright (c) 2024 Mu Shibo
-Modify by Tommy 2025.10.30
+Modify by Tommy 2025
 ------------------------------------- */
 #include <Arduino.h>
 #include <FastLED.h>
@@ -8,18 +8,14 @@ Modify by Tommy 2025.10.30
 #include <SMS_STS.h>
 #include <SimpleFOC.h>
 #include <FS.h>
-#include <esp_adc_cal.h>
-#include <XboxSeriesXControllerESP32_asukiaaa.hpp>  // xbox手柄
+#include "RGBController.h" // RGB
+#include "VoltageMonitor.h" // 电源
+#include "XboxBluetooth.h"  // 蓝牙手柄
+#include "RobotActions.h" 
 
-/************ xbox手柄实体 *************/
-XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79");
 
 /************函数申明*************/
 
-// 电压检测函数
-void adc_calibration_init();
-void bat_check();
-
 // 平衡算法函数
 void lqr_balance_loop();
 void yaw_loop();
@@ -29,11 +25,11 @@ void resetZeroPoint();
 void jump_charge(); // 腿部弯曲（蓄能）
 
 // RGB相关函数
-void handleLEDBlink();
+// void handleLEDBlink();
 
-// Xbox函数
-void process_xbox_notif();
+// 动作处理函数
 void processControllerData(const XboxControllerNotificationParser& data);
+void runActionSequence();
 
 const int deadZone = 10; // 摇杆死区阈值
 bool last_btnA = 0;
@@ -42,10 +38,16 @@ bool last_btnX = 0;
 bool last_btnY = 0;
 bool last_btnRS = 0;
 bool last_btnLS = 0;
+bool last_btnLB = 0;
+bool last_btnRB = 0;
 bool last_btnDirRight = 0;
 bool last_btnDirLeft = 0;
 bool last_btnDirUp = 0;
 bool last_btnDirDown = 0;
+bool last_btnSelect = 0;
+bool last_btnStart = 0;
+bool last_btnShare = 0;
+
 
 /************实例定义*************/
 
@@ -84,16 +86,6 @@ 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
 
@@ -103,15 +95,18 @@ 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最高
+#define SERVO0_ACC 100 // 舵机1加速度
+#define SERVO1_ACC 100 // 舵机2加速度
+#define SERVO0_SPEED 400 // 舵机1速度
+#define SERVO1_SPEED 400 // 舵机2速度
 
-float leg_position_add = 0;    // roll轴平衡控制量
 int left_height;
 int right_height;
+float leg_position_add = 0;    // roll轴平衡控制量
 float LegHeightDiff = 0; // 左右腿高度差
 
 // 舵机运动参数
@@ -169,39 +164,45 @@ 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; // 标记是否正在执行坐下动作
+// bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
+// bool last_robot_enabled = true; // 记录上一次的小车状态
+// int sitting_down = 0; // 标记是否正在执行坐下动作
 
+// 开机默认坐下 修改
+bool robot_enabled = false; // 开机禁用平衡，默认坐下
+bool last_robot_enabled = false; // 同步初始状态
+int sitting_down = 1; // 开机直接进入“坐下状态”，避免无控制
+
+// 动作执行状态管理
+bool isExecutingAction = false;
+bool isPausing = false;                
+unsigned long currentPauseDuration = 0; 
+unsigned long stepStartTime = 0;
+unsigned long pauseStartTime = 0;    
+int currentStepIndex = 0;
+RobotAction* currentAction = NULL;
 
 typedef struct
 {
   int height = 30;
   int roll;
-  int jump = 0; // 0 =不跳跃 1= 跳跃
+  int jump = 0; // 0=不跳跃 1=跳跃
   int joyy;
   int joyy_last;
   int joyx;
   int joyx_last;
+  int acc0 =  SERVO0_ACC;    // 舵机1加速度
+  int acc1 =  SERVO1_ACC;    // 舵机2加速度
+  int speed0 = SERVO0_SPEED;  // 舵机1速度
+  int speed1 = SERVO1_SPEED;  // 舵机2速度
 } Wrobot;
 Wrobot wrobot;
 
-
 void setup()
 {
   Serial.begin(115200);   // 通讯串口
@@ -280,16 +281,11 @@ void setup()
   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();
+  // RGB初始化
+  initLEDs();
 
   // Xbox手柄初始化
-  Serial.println("Starting Xbox Bluetooth Client");
-  xboxController.begin();
-
+  xboxControllerInit();
 
   delay(500);
 }
@@ -297,18 +293,19 @@ void setup()
 void loop()
 {
   
-  // 处理LED闪烁
-  handleLEDBlink();
-  process_xbox_notif();  // 处理xbox手柄通知
  
-  bat_check();        // 电压检测
+  handleLEDBlink();       // 处理LED闪烁
+  process_xbox_notif();   // 处理xbox手柄通知
+  
+  bat_check();            // 电压检测
   delayMicroseconds(1500);
-  mpu6050.update();   // IMU数据更新
-  lqr_balance_loop(); // lqr自平衡控制
-  yaw_loop();         // yaw轴转向控制
-  jump_loop();        // 跳跃动作
-  leg_loop();         // 腿部动作控制
+  mpu6050.update();       // IMU数据更新
+  lqr_balance_loop();     // lqr自平衡控制
+  yaw_loop();             // yaw轴转向控制
+  jump_loop();            // 跳跃动作
+  leg_loop();             // 腿部动作控制
 
+  runActionSequence();    // 执行动作序列 
 
   // 记录上一次的遥控数据数据
   wrobot.joyx_last = wrobot.joyx;
@@ -368,17 +365,16 @@ void loop()
 
     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 { // 否则默认向前倒地 给个向前脉冲
+      if(wrobot.joyy > 0){
         motor1.target = 1.0;  // 负数小车向后，正数向前
         motor2.target = 1.0;
         Position[0] = SERVO0_MIN + 60; // 修改
         Position[1] = SERVO1_MIN - 60; 
-        
+      } else { // 否则默认向后倒地 给个向后脉冲
+        motor1.target = -1.0;  // 负数小车向后，正数向前
+        motor2.target = -1.0;
+        Position[0] = SERVO0_MIN; // 修改
+        Position[1] = SERVO1_MIN;
       }
     }else{
 
@@ -555,19 +551,25 @@ void leg_loop()
     return; // 跳过后续leg高度计算
   }
 
+
   // 机身大角度，譬如倒地时，强制腿部收缩
   if (abs(LQR_angle) > 25.0f) 
   {
-    ACC[0] = ACC[1] = 100;
-    Speed[0] = Speed[1] = 400;
+    // 默认舵机速度和加速度
+    ACC[0] = SERVO0_ACC;
+    ACC[1] = SERVO1_ACC;
+    Speed[0] = SERVO0_SPEED;
+    Speed[1] = SERVO1_SPEED;
     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;
+  ACC[0] = wrobot.acc0;    // 强转为byte（舵机库要求）
+  ACC[1] = wrobot.acc1;
+  Speed[0] = wrobot.speed0; // 强转为u16（舵机库要求）
+  Speed[1] = wrobot.speed1;
 
   float roll_angle = (float)mpu6050.getAngleX() + roll_adjust; // 加上roll角度偏差
   leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
@@ -603,23 +605,10 @@ void leg_loop()
   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;
+  // 舵机1：限制在 SERVO0_MIN（下限）~ SERVO0_MAX（上限），和原逻辑一致
+  Position[0] = constrain(Position[0], SERVO0_MIN, SERVO0_MAX);
+  // 舵机2：限制在 SERVO1_MAX（下限）~ SERVO1_MIN（上限），交换参数顺序适配反向范围
+  Position[1] = constrain(Position[1], SERVO1_MAX, SERVO1_MIN);
   
   sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
 
@@ -740,84 +729,12 @@ void yaw_loop()
   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();
-      }
-    }
-  }
-}
-
 /*========================================
  * 控制器数据处理（共用函数）
  * ========================================*/
@@ -845,19 +762,32 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
 //             String(xboxController.xboxNotif.joyRVert) + "," + // 右摇杆 垂直轴 （上-下 0 -65535）
 //             String(xboxController.xboxNotif.trigLT) + "," + // 左扳机键 （松开- 按下 0 -1023）
 //             String(xboxController.xboxNotif.trigRT) ; // 右扳机键 （松开- 按下 0 -1023）
-  // Serial.println(str);
-  // Serial.println("processControllerData");
+//   Serial.println(str);
+
+ // 正在执行动作组，忽略其他指令
+  if (isExecutingAction) {
+    return; 
+  }
+
 
   // 坐下
-  if (remoteData.btnLB && robot_enabled) { 
+  if (remoteData.btnLB && !last_btnLB && robot_enabled) { 
     robot_enabled = false;
     sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
   }
+  // 第二次按坐下，让舵机失能
+  if (remoteData.btnLB && !last_btnLB && !robot_enabled) { 
+    sms_sts.EnableTorque(ID[0], 0);  // 第一个舵机失能 0 失能，1 使能
+    sms_sts.EnableTorque(ID[1], 0);  // 第二个舵机失能
+  }
+   last_btnLB = remoteData.btnLB;
+
   // 起立
   if (remoteData.btnRB && !robot_enabled) {
     robot_enabled = true;
     sitting_down = 0; // 清除坐下动作标记
   }
+   last_btnRB = remoteData.btnRB;
 
   // 修改后代码（保留摇杆指令，仅禁止起升/跳跃）
   if (!robot_enabled || sitting_down) {
@@ -905,20 +835,20 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
 
     // 右摇摆
     if(remoteData.btnDirRight) { 
-      roll_adjust += 0.15;
+      roll_adjust += 0.20;
     }
     // 左摇摆
     if(remoteData.btnDirLeft) { 
-      roll_adjust -= 0.15;
+      roll_adjust -= 0.20;
     }
 
     // 抬升
     if(remoteData.btnDirUp) { 
-      leg_height_base -= 0.15;
+      leg_height_base -= 0.20;
     }
     // 下降
     if(remoteData.btnDirDown) { 
-      leg_height_base += 0.15;
+      leg_height_base += 0.20;
     }
 
     // 还原
@@ -962,21 +892,99 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   }
   last_btnB = remoteData.btnB; 
 
+  // btnY 晃头
+  if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) {
+    isExecutingAction = true;          // 标记开始执行动作
+    currentAction = &action_lift_sway; // 指定执行的动作
+    stepStartTime = millis();          // 记录第一步开始时间
+    currentStepIndex = 0;              // 从第一步开始
+  }
+  last_btnY = remoteData.btnY; // 更新btnY的上一次状态
+
+  // btnA 点头
+  if (remoteData.btnA && !last_btnA && robot_enabled && !isExecutingAction) {
+    isExecutingAction = true;
+    currentAction = &action_node;
+    currentAction = &action_node;
+    stepStartTime = millis();
+    currentStepIndex = 0;
+  }
+  last_btnA = remoteData.btnA;
+
 }
 
-/* Xbox数据处理函数（直接传递原生数据） */
-void process_xbox_notif(){
-  xboxController.onLoop();
-  if (xboxController.isConnected()){
-    if (xboxController.isWaitingForFirstNotification()){
-      Serial.println("Waiting for first Xbox data...");
-    }else{
-      // Serial.println("Xbox controller connected !!!"); // 手柄蓝灯常亮
-      processControllerData(xboxController.xboxNotif);
+// 执行连续动作的函数
+void runActionSequence() {
+  // 若未执行动作或动作指针为空，直接返回
+  if (!isExecutingAction || currentAction == NULL) {
+    return;
+  }
+
+  // 获取当前时间
+  unsigned long currentTime = millis();
+  
+  // --------------- 停顿逻辑处理 ---------------
+  if (isPausing) {
+    // 停顿期间不更新任何控制参数（保持上一步状态）
+    unsigned long pauseElapsed = currentTime - pauseStartTime;
+    
+    // 停顿时间结束，切换到下一步
+    if (pauseElapsed >= currentPauseDuration) {
+      isPausing = false;
+      currentStepIndex++;
+      stepStartTime = currentTime; // 记录新步骤开始时间
+      // Serial.print("停顿结束，切换到步骤：");
+      // Serial.println(currentStepIndex);
+    }
+    return; // 停顿期间直接返回，不执行后续逻辑
+  }
+
+  // --------------- 正常步骤执行逻辑 ---------------
+  unsigned long elapsedTime = currentTime - stepStartTime;
+  ActionStep currentStep = currentAction->steps[currentStepIndex];
+
+  // 检查当前步骤是否为结束标记
+  if (currentStep.stepDuration == 0) {
+    isExecutingAction = false;
+    currentStepIndex = 0;
+    currentAction = NULL;
+    // 重置参数
+    wrobot.joyx = 0;
+    wrobot.joyy = 0;
+    wrobot.jump = 0;
+    roll_adjust = 0.0f;
+    leg_height_base = 20.0f;
+    wrobot.acc0 = SERVO0_ACC;   // 恢复默认加速度
+    wrobot.acc1 = SERVO1_ACC;
+    wrobot.speed0 = SERVO0_SPEED; // 恢复默认速度
+    wrobot.speed1 = SERVO1_SPEED;
+    return;
+  }
+
+  // 更新当前步骤的控制参数
+  wrobot.joyx = currentStep.joyx;
+  wrobot.joyy = currentStep.joyy;
+  wrobot.jump = currentStep.jump;
+  roll_adjust = currentStep.roll_adjust;
+  leg_height_base = currentStep.leg_height_base;
+
+   // 舵机参数同步：动作集定义了则覆盖，未定义则保持wrobot的默认值（全局常量）
+  wrobot.acc0 = (currentStep.acc0 != -1) ? currentStep.acc0 : SERVO0_ACC;
+  wrobot.acc1 = (currentStep.acc1 != -1) ? currentStep.acc1 : SERVO1_ACC;
+  wrobot.speed0 = (currentStep.speed0 != -1) ? currentStep.speed0 : SERVO0_SPEED;
+  wrobot.speed1 = (currentStep.speed1 != -1) ? currentStep.speed1 : SERVO1_SPEED;
+
+  if (elapsedTime >= currentStep.stepDuration) {
+    // 若设置了停顿时间，进入停顿状态
+    if (currentStep.pauseDuration > 0) {
+      isPausing = true;
+      currentPauseDuration = currentStep.pauseDuration; // 记录当前需要停顿的时间
+      pauseStartTime = currentTime;
+    } else {
+      // 无停顿，直接切换到下一步
+      currentStepIndex++;
+      stepStartTime = currentTime;
     }
   }
-  else{
-    // Serial.println("Xbox controller not connected");
-  }
 }
 

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

@@ -8,18 +8,13 @@ Modify by Tommy 2025.10.30
 #include <SMS_STS.h>
 #include <SimpleFOC.h>
 #include <FS.h>
-#include <esp_adc_cal.h>
-#include <XboxSeriesXControllerESP32_asukiaaa.hpp>  // xbox手柄
+#include "VoltageMonitor.h" // 电源
+#include "XboxBluetooth.h"  // 蓝牙手柄
+#include "RobotActions.h" 
 
-/************ xbox手柄实体 *************/
-XboxSeriesXControllerESP32_asukiaaa::Core xboxController("0a:27:12:5c:71:79");
 
 /************函数申明*************/
 
-// 电压检测函数
-void adc_calibration_init();
-void bat_check();
-
 // 平衡算法函数
 void lqr_balance_loop();
 void yaw_loop();
@@ -29,12 +24,11 @@ void resetZeroPoint();
 void jump_charge(); // 腿部弯曲（蓄能）
 
 // RGB相关函数
-void updateBluetoothLED();
 void handleLEDBlink();
 
-// Xbox函数
-void process_xbox_notif();
+// 动作处理函数
 void processControllerData(const XboxControllerNotificationParser& data);
+void runActionSequence();
 
 const int deadZone = 10; // 摇杆死区阈值
 bool last_btnA = 0;
@@ -43,6 +37,14 @@ 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;
+bool last_btnSelect = 0;
+bool last_btnStart = 0;
+bool last_btnShare = 0;
+
 
 /************实例定义*************/
 
@@ -81,7 +83,7 @@ SMS_STS sms_sts;
 // MPU6050实例
 MPU6050 mpu6050(I2Ctwo);
 
-// rgb 配置
+// FastLED配置
 #define RGB_PIN 21
 #define NUM_LEDS 2
 CRGB leds[NUM_LEDS];
@@ -91,12 +93,6 @@ 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
 
@@ -106,7 +102,6 @@ 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最高 
@@ -163,24 +158,15 @@ const unsigned long SPEED_RECORD_INTERVAL = 100;  // 转速记录间隔(毫秒)
 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; // 轮子是否离地标记 默认离地
 
 
-// 电压检测
-#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执行计数
@@ -190,10 +176,17 @@ bool robot_enabled = true; // 小车使能状态，true为起立状态，false
 bool last_robot_enabled = true; // 记录上一次的小车状态
 int sitting_down = 0; // 标记是否正在执行坐下动作
 
+// 动作执行状态管理
+bool isExecutingAction = false;
+bool isPausing = false;                
+unsigned long currentPauseDuration = 0; 
+unsigned long stepStartTime = 0;
+unsigned long pauseStartTime = 0;    
+int currentStepIndex = 0;
+RobotAction* currentAction = NULL;
 
 typedef struct
 {
-  int mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
   int height = 30;
   int roll;
   int jump = 0; // 0 =不跳跃 1= 跳跃
@@ -290,8 +283,7 @@ void setup()
   FastLED.show();
 
   // Xbox手柄初始化
-  Serial.println("Starting Xbox Bluetooth Client");
-  xboxController.begin();
+  xboxControllerInit();
 
 
   delay(500);
@@ -300,22 +292,21 @@ void setup()
 void loop()
 {
   
- // 处理LED闪烁
+  // 处理LED闪烁
   handleLEDBlink();
-  // 处理蓝牙状态蓝灯，优先级低于自定义闪烁
-  updateBluetoothLED();
-  
-  // 处理xbox手柄通知
-  process_xbox_notif();
+  process_xbox_notif();  // 处理xbox手柄通知
   
   bat_check();        // 电压检测
-  // delayMicroseconds(1500);
+  delayMicroseconds(1500);
   mpu6050.update();   // IMU数据更新
   lqr_balance_loop(); // lqr自平衡控制
   yaw_loop();         // yaw轴转向控制
   jump_loop();        // 跳跃动作
   leg_loop();         // 腿部动作控制
 
+  runActionSequence();
+
+   
   // 记录上一次的遥控数据数据
   wrobot.joyx_last = wrobot.joyx;
   wrobot.joyy_last = wrobot.joyy;
@@ -355,6 +346,7 @@ void loop()
       jump_pre_flag = 1;
     }
   }
+  
   // 向右跳跃
   if(right_jump_flag){
     right_jump_flag ++ ;
@@ -583,7 +575,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); 
@@ -601,7 +592,6 @@ void leg_loop()
       right_height = wrobot.height + LegHeightDiff;
       // Serial.print("右脚抬高");
     } 
-  }
   
   // 根据新的高度计算舵机位置
   // id[0] 正面看右手，id[1] 正面看左手
@@ -727,8 +717,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();  // 执行闪烁逻辑
     }
   }
 
@@ -741,78 +737,12 @@ void yaw_loop()
   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;
 }
 
-// 蓝牙状态对应的蓝灯控制
-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;
@@ -873,9 +803,12 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
 //             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 (isExecutingAction) {
+    return; 
+  }
 
   // 坐下
   if (remoteData.btnLB && robot_enabled) { 
@@ -915,10 +848,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
       wrobot.joyy = 0;
     } 
 
-
-  if(wrobot.mode == 0) // 智能模式
-  {
-
     // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
     int convertedJ1PotX = constrain(
       map(remoteData.joyLHori, 0, 65535, -100, 100),  
@@ -936,50 +865,35 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     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.20;
+    }
+    // 左摇摆
+    if(remoteData.btnDirLeft) { 
+      roll_adjust -= 0.20;
     }
 
-    // 上下高度 左摇杆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.20;
+    }
+    // 下降
+    if(remoteData.btnDirDown) { 
+      leg_height_base += 0.20;
     }
-    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
-      );
-    // leg_height_base = lpf_height(leg_height_base);
+    // 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){
@@ -997,44 +911,111 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   }
   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; 
+
+
+  // btnY
+  if (remoteData.btnY && !last_btnY && robot_enabled && !isExecutingAction) {
+    isExecutingAction = true;          // 标记开始执行动作
+    currentAction = &action_lift_sway; // 指定执行的动作
+    stepStartTime = millis();          // 记录第一步开始时间
+    currentStepIndex = 0;              // 从第一步开始
   }
-  if (remoteData.btnA) {  // 0:左摇杆左右转向控制（智能）
-      wrobot.mode = 0; 
-      roll_adjust = 0.0f; 
-      leg_height_base = 20.0f;
+  last_btnY = remoteData.btnY; // 更新btnY的上一次状态
+
+  // btnA
+  if (remoteData.btnA && !last_btnA && robot_enabled && !isExecutingAction) {
+    isExecutingAction = true;
+    currentAction = &action_wheel_sway;
+    stepStartTime = millis();
+    currentStepIndex = 0;
   }
+  last_btnA = remoteData.btnA;
+
 }
 
-/* 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);
+// 执行连续动作的函数
+void runActionSequence() {
+  // 若未执行动作或动作指针为空，直接返回
+  if (!isExecutingAction || currentAction == NULL) {
+    return;
+  }
+
+  // 获取当前时间
+  unsigned long currentTime = millis();
+  
+  // --------------- 停顿逻辑处理 ---------------
+  if (isPausing) {
+    // 停顿期间不更新任何控制参数（保持上一步状态）
+    unsigned long pauseElapsed = currentTime - pauseStartTime;
+    
+    // 停顿时间结束，切换到下一步
+    if (pauseElapsed >= currentPauseDuration) {
+      isPausing = false;
+      currentStepIndex++;
+      stepStartTime = currentTime; // 记录新步骤开始时间
+      // Serial.print("停顿结束，切换到步骤：");
+      // Serial.println(currentStepIndex);
+    }
+    return; // 停顿期间直接返回，不执行后续逻辑
+  }
+
+  // --------------- 正常步骤执行逻辑 ---------------
+  unsigned long elapsedTime = currentTime - stepStartTime;
+  ActionStep currentStep = currentAction->steps[currentStepIndex];
+
+  // 检查当前步骤是否为结束标记
+  if (currentStep.stepDuration == 0) {
+    isExecutingAction = false;
+    currentStepIndex = 0;
+    currentAction = NULL;
+    // 重置参数
+    wrobot.joyx = 0;
+    wrobot.joyy = 0;
+    wrobot.jump = 0;
+    roll_adjust = 0.0f;
+    leg_height_base = 20.0f;
+    // Serial.println("动作集执行完毕，已重置参数");
+    return;
+  }
+
+  // 更新当前步骤的控制参数
+  wrobot.joyx = currentStep.joyx;
+  wrobot.joyy = currentStep.joyy;
+  wrobot.jump = currentStep.jump;
+  roll_adjust = currentStep.roll_adjust;
+  leg_height_base = currentStep.leg_height_base;
+
+  // 检查当前步骤是否执行完毕
+  if (elapsedTime >= currentStep.stepDuration) {
+    // 若设置了停顿时间，进入停顿状态
+    if (currentStep.pauseDuration > 0) {
+      isPausing = true;
+      currentPauseDuration = currentStep.pauseDuration; // 记录当前需要停顿的时间
+      pauseStartTime = currentTime;
+      // Serial.print("步骤执行完毕，开始停顿：");
+      // Serial.print(currentStep.pauseDuration);
+      // Serial.println("ms");
+    } else {
+      // 无停顿，直接切换到下一步
+      currentStepIndex++;
+      stepStartTime = currentTime;
+      // Serial.print("步骤切换：第");
+      // Serial.print(currentStepIndex);
+      // Serial.println("步");
     }
   }
-  else{
-    isBluetoothConnected = false; // 手柄未连接
-    // Serial.println("Xbox controller not connected");
-  }
 }