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