deepseek/wrobot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
myperworld committed 2025-10-17 13:18:12 +08:00
1 parent 9cc5186a58
commit e33a6b894d
10 files changed
+606 -356
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.2/Shibo-main-TOMMY/src/main.cpp
+184 -176
View File
@@ -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,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");
}
}