优化xbox系列、盖世小鸡手柄控制
This commit is contained in:
myperworld committed
1 parent
1a97bc32c3
commit
e2ff6beaa8
6 files changed
+2706
-563
No files matched your search
@@ -33,11 +33,19 @@ void handleLEDBlink();
|
||||
|
||||
// Xbox函数
|
||||
void process_xbox_notif();
|
||||
void process_xbox_string();
|
||||
void processControllerData(const XboxControllerNotificationParser& data);
|
||||
|
||||
const int deadZone = 10; // 摇杆死区阈值
|
||||
bool last_btnbtnRS = 0;
|
||||
bool last_btnA = 0;
|
||||
bool last_btnB = 0;
|
||||
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;
|
||||
|
||||
/************实例定义*************/
|
||||
|
||||
@@ -153,8 +161,12 @@ 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; // 轮子是否离地标记 默认离地
|
||||
|
||||
|
||||
// 电压检测
|
||||
@@ -179,7 +191,6 @@ int sitting_down = 0; // 标记是否正在执行坐下动作
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int mode = 0; // 0:左摇杆左右转向控制（智能），1:左摇杆上下摇摆控制（手动）
|
||||
int height = 30;
|
||||
int roll;
|
||||
int jump = 0; // 0 =不跳跃 1= 跳跃
|
||||
@@ -287,10 +298,8 @@ void loop()
|
||||
{
|
||||
|
||||
// 处理LED闪烁
|
||||
// handleLEDBlink();
|
||||
|
||||
// 处理xbox手柄通知
|
||||
process_xbox_notif();
|
||||
handleLEDBlink();
|
||||
process_xbox_notif(); // 处理xbox手柄通知
|
||||
|
||||
bat_check(); // 电压检测
|
||||
delayMicroseconds(1500);
|
||||
@@ -300,6 +309,7 @@ void loop()
|
||||
jump_loop(); // 跳跃动作
|
||||
leg_loop(); // 腿部动作控制
|
||||
|
||||
|
||||
// 记录上一次的遥控数据数据
|
||||
wrobot.joyx_last = wrobot.joyx;
|
||||
wrobot.joyy_last = wrobot.joyy;
|
||||
@@ -328,6 +338,29 @@ 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) {
|
||||
@@ -518,7 +551,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高度计算
|
||||
}
|
||||
|
||||
@@ -545,7 +578,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);
|
||||
@@ -563,7 +595,6 @@ void leg_loop()
|
||||
right_height = wrobot.height + LegHeightDiff;
|
||||
// Serial.print("右脚抬高");
|
||||
}
|
||||
}
|
||||
|
||||
// 根据新的高度计算舵机位置
|
||||
// id[0] 正面看右手，id[1] 正面看左手
|
||||
@@ -599,8 +630,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);
|
||||
@@ -689,8 +720,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(); // 执行闪烁逻辑
|
||||
}
|
||||
}
|
||||
|
||||
@@ -786,46 +823,37 @@ void handleLEDBlink() {
|
||||
* ========================================*/
|
||||
void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
|
||||
/*
|
||||
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) + "," + // 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);
|
||||
// Serial.println("processControllerData");
|
||||
|
||||
// rgb led 上键 闪烁6次：红红黄黄绿绿
|
||||
if(remoteData.btnDirUp && !isBlinking){
|
||||
isBlinking = true;
|
||||
blinkCount = 0;
|
||||
blinkState = 0;
|
||||
previousBlinkMillis = millis();
|
||||
}
|
||||
|
||||
// 坐下 back键
|
||||
// 坐下
|
||||
if (remoteData.btnLB && robot_enabled) {
|
||||
robot_enabled = false;
|
||||
sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
|
||||
}
|
||||
// 起立 start键
|
||||
// 起立
|
||||
if (remoteData.btnRB && !robot_enabled) {
|
||||
robot_enabled = true;
|
||||
sitting_down = 0; // 清除坐下动作标记
|
||||
@@ -858,10 +886,6 @@ String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
|
||||
wrobot.joyy = 0;
|
||||
}
|
||||
|
||||
|
||||
if(wrobot.mode == 0) // 智能模式
|
||||
{
|
||||
|
||||
// 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
|
||||
int convertedJ1PotX = constrain(
|
||||
map(remoteData.joyLHori, 0, 65535, -100, 100),
|
||||
@@ -879,61 +903,65 @@ String str = String(xboxController.xboxNotif.btnY) + "," + // Y键
|
||||
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.15;
|
||||
}
|
||||
// 左摇摆
|
||||
if(remoteData.btnDirLeft) {
|
||||
roll_adjust -= 0.15;
|
||||
}
|
||||
|
||||
// 上下高度 左摇杆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.15;
|
||||
}
|
||||
// 下降
|
||||
if(remoteData.btnDirDown) {
|
||||
leg_height_base += 0.15;
|
||||
}
|
||||
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
|
||||
// );
|
||||
|
||||
// 左扳机 rgb led 上键 闪烁6次：红红黄黄绿绿
|
||||
if(remoteData.trigLT && !isBlinking){
|
||||
isBlinking = true;
|
||||
blinkCount = 0;
|
||||
blinkState = 0;
|
||||
previousBlinkMillis = millis();
|
||||
}
|
||||
|
||||
// 跳跃 右摇杆按键
|
||||
if (remoteData.btnRS && !last_btnbtnRS) { // 1=true，0=false
|
||||
if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false
|
||||
wrobot.jump = 1; // 跳跃
|
||||
}else{
|
||||
wrobot.jump = 0; // 不跳跃
|
||||
}
|
||||
last_btnbtnRS = remoteData.btnRS;
|
||||
last_btnRS = remoteData.btnRS;
|
||||
|
||||
// 切换智能模式和手动模式
|
||||
if (remoteData.btnY) { // 1:左摇杆上下摇摆控制（手动）
|
||||
wrobot.mode = 1;
|
||||
|
||||
// 向左跳跃
|
||||
if (remoteData.btnX && !last_btnX && robot_enabled) {
|
||||
left_jump_flag = 1; // 跳跃
|
||||
}
|
||||
if (remoteData.btnA) { // 0:左摇杆左右转向控制（智能）
|
||||
wrobot.mode = 0;
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
last_btnX = remoteData.btnX;
|
||||
|
||||
// 向右跳跃
|
||||
if (remoteData.btnB && !last_btnB && robot_enabled) {
|
||||
right_jump_flag = 1; // 跳跃
|
||||
}
|
||||
last_btnB = remoteData.btnB;
|
||||
|
||||
}
|
||||
|
||||
/* Xbox数据处理函数（直接传递原生数据） */
|
||||
@@ -948,7 +976,7 @@ void process_xbox_notif(){
|
||||
}
|
||||
}
|
||||
else{
|
||||
Serial.println("Xbox controller not connected");
|
||||
// Serial.println("Xbox controller not connected");
|
||||
}
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user