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-16 08:39:02 +08:00
1 parent 1a97bc32c3
commit e2ff6beaa8
6 files changed
+2706 -563
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.2/Shibo-main-TOMMY/test/main copy.cpp
+222 -220
View File
@@ -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）
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);
};
// 坐下
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");
}
}
}