From e33a6b894dfd228c0e6ccb8d408e64741ea10b8f Mon Sep 17 00:00:00 2001 From: myperworld Date: Fri, 17 Oct 2025 13:18:12 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96xbox=EF=BC=8C=E7=9B=96?= =?UTF-8?q?=E4=B8=96=E5=B0=8F=E9=B8=A1=E8=93=9D=E7=89=99=E6=89=8B=E6=9F=84?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=EF=BC=8C=E5=A2=9E=E5=8A=A0=E5=8A=A8=E4=BD=9C?= =?UTF-8?q?=E9=9B=86=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 4 +- v1.2/Shibo-main-TOMMY/.vscode/settings.json | 3 +- v1.2/Shibo-main-TOMMY/src/RGBController.cpp | 50 +++ v1.2/Shibo-main-TOMMY/src/RGBController.h | 28 ++ v1.2/Shibo-main-TOMMY/src/RobotActions.h | 66 ++++ v1.2/Shibo-main-TOMMY/src/VoltageMonitor.cpp | 47 +++ v1.2/Shibo-main-TOMMY/src/VoltageMonitor.h | 23 ++ v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h | 46 +++ v1.2/Shibo-main-TOMMY/src/main.cpp | 360 ++++++++++--------- v1.2/Shibo-main-TOMMY/test/main.cpp | 335 ++++++++--------- 10 files changed, 606 insertions(+), 356 deletions(-) create mode 100644 v1.2/Shibo-main-TOMMY/src/RGBController.cpp create mode 100644 v1.2/Shibo-main-TOMMY/src/RGBController.h create mode 100644 v1.2/Shibo-main-TOMMY/src/RobotActions.h create mode 100644 v1.2/Shibo-main-TOMMY/src/VoltageMonitor.cpp create mode 100644 v1.2/Shibo-main-TOMMY/src/VoltageMonitor.h create mode 100644 v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h diff --git a/README.md b/README.md index 1ad51f9..b52e81d 100644 --- a/README.md +++ b/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 diff --git a/v1.2/Shibo-main-TOMMY/.vscode/settings.json b/v1.2/Shibo-main-TOMMY/.vscode/settings.json index 52f2391..b5dc055 100644 --- a/v1.2/Shibo-main-TOMMY/.vscode/settings.json +++ b/v1.2/Shibo-main-TOMMY/.vscode/settings.json @@ -60,6 +60,7 @@ "stdexcept": "cpp", "cinttypes": "cpp", "cstdbool": "cpp", - "typeinfo": "cpp" + "typeinfo": "cpp", + "list": "cpp" } } \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/RGBController.cpp b/v1.2/Shibo-main-TOMMY/src/RGBController.cpp new file mode 100644 index 0000000..8144739 --- /dev/null +++ b/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(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(); + } + } + } +} + \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/RGBController.h b/v1.2/Shibo-main-TOMMY/src/RGBController.h new file mode 100644 index 0000000..fa117a4 --- /dev/null +++ b/v1.2/Shibo-main-TOMMY/src/RGBController.h @@ -0,0 +1,28 @@ +/* --------------------------------- + * RGB 控制模块 + * 包含：RGB初始化、闪烁逻辑、状态管理 +------------------------------------- */ +#ifndef RGB_CONTROLLER_H +#define RGB_CONTROLLER_H + +#include +#include + +// 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 + \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/RobotActions.h b/v1.2/Shibo-main-TOMMY/src/RobotActions.h new file mode 100644 index 0000000..1d69d1d --- /dev/null +++ b/v1.2/Shibo-main-TOMMY/src/RobotActions.h @@ -0,0 +1,66 @@ +#ifndef RobotActions_h +#define RobotActions_h + +#include + +// 单个动作步骤结构体 +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 \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/VoltageMonitor.cpp b/v1.2/Shibo-main-TOMMY/src/VoltageMonitor.cpp new file mode 100644 index 0000000..a776457 --- /dev/null +++ b/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++; +} \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/VoltageMonitor.h b/v1.2/Shibo-main-TOMMY/src/VoltageMonitor.h new file mode 100644 index 0000000..31fbbf2 --- /dev/null +++ b/v1.2/Shibo-main-TOMMY/src/VoltageMonitor.h @@ -0,0 +1,23 @@ +#ifndef VOLTAGE_MONITOR_H +#define VOLTAGE_MONITOR_H + +#include +#include + +// 电压检测相关宏定义（原代码中电压模块的宏） +#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 \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h b/v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h new file mode 100644 index 0000000..0b01eb1 --- /dev/null +++ b/v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h @@ -0,0 +1,46 @@ +/* --------------------------------- + * 蓝牙手柄（Xbox）模块 + * 包含：蓝牙实体、状态变量、连接处理函数 + * 不包含：手柄数据解析逻辑（processControllerData） +------------------------------------- */ +#ifndef XBOX_BLUETOOTH_H +#define XBOX_BLUETOOTH_H + +#include +#include // 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 + \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/main.cpp b/v1.2/Shibo-main-TOMMY/src/main.cpp index fb1dfc8..551894d 100644 --- a/v1.2/Shibo-main-TOMMY/src/main.cpp +++ b/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 #include @@ -8,18 +8,14 @@ Modify by Tommy 2025.10.30 #include #include #include -#include -#include // 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(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,19 +293,20 @@ void setup() void loop() { - // 处理LED闪烁 - handleLEDBlink(); - process_xbox_notif(); // 处理xbox手柄通知 + + handleLEDBlink(); // 处理LED闪烁 + process_xbox_notif(); // 处理xbox手柄通知 - bat_check(); // 电压检测 + 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; wrobot.joyy_last = wrobot.joyy; @@ -368,17 +365,16 @@ void loop() if (sitting_down < 15) { // 如果遥感向后运动，给个向后退脉冲，让小车向前倒地 - if(wrobot.joyy < 0){ + 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 { // 否则默认向前倒地 给个向前脉冲 - motor1.target = 1.0; // 负数小车向后，正数向前 - motor2.target = 1.0; - Position[0] = SERVO0_MIN + 60; // 修改 - Position[1] = SERVO1_MIN - 60; - } }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"); - } } diff --git a/v1.2/Shibo-main-TOMMY/test/main.cpp b/v1.2/Shibo-main-TOMMY/test/main.cpp index 7437722..f6fad6f 100644 --- a/v1.2/Shibo-main-TOMMY/test/main.cpp +++ b/v1.2/Shibo-main-TOMMY/test/main.cpp @@ -8,18 +8,13 @@ Modify by Tommy 2025.10.30 #include #include #include -#include -#include // 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"); - } }