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

优化越障模式

Browse Source
This commit is contained in:
myperworld committed 2025-11-29 19:37:20 +08:00
1 parent 3d65b2f1ce
commit ada5b9aad9
3 files changed
+164 -88
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.3/src/main.cpp
+120 -87
View File
@@ -28,6 +28,9 @@ void cancel_super_mode();
// 跨越障碍函数
void obstacle_detect(); // 越障高度检测
void obstacle_loop(); // 越障总函数
void obstacle_loop0(); // 越障总函数
void obstacle_loop1(); // 越障总函数
void obstacle_cancel();
// 常规运动控制
void resetZeroPoint();
@@ -41,7 +44,6 @@ void runActionSequence();
const int deadZone = 10; // 摇杆死区阈值
int trigRT = 0;
int last_trigRT = 0;
float trig_v_target = 0.0f;
bool last_btnA = 0;
bool last_btnB = 0;
bool last_btnX = 0;
@@ -190,17 +192,40 @@ int right_jump_flag = 0; // 右跳跃标志位
// 越障模式
String obstacle_status = "";
String obstacle_level = ""; // 越障高度，low/mid/high
bool obstacle_mode = false; // 越障模式
unsigned long obstacle_btn_down = 0; // 0=不开启 >0开启
int obstacle_flag_count = 0; // 越障标记计数
int obstacle_enconter_count = 0; // 遇到障碍计数
int obstacle_pitch_adjust_count = 0; // 前倾控制
int obstacle_motor_go_count = 0; // 电机前进计数
float obstacle_pitch = 20.0f;
int servo_up_height = 0;
int servo_down_height = 50;
float motor_target = -3.0f;
// 蓝灯，最低障碍
float low_obstacle_pitch = 21.0f;
int low_servo_up_height = 0;
int low_servo_down_height = 30;
float low_motor_target = -4.0f;
// 黄灯，中度障碍
float mid_obstacle_pitch = 25.0f;
int mid_servo_up_height = 0;
int mid_servo_down_height = 30;
float mid_motor_target = -8.0f;
// 红灯，最高障碍
float high_obstacle_pitch = 30.0f;
int high_servo_up_height = 380; // +390=MAX
int high_servo_down_height = 0;
float high_motor_target = -9.0f;
// 超级平衡模式参数
bool super_balance_mode = false; // 超级平衡模式
PIDController pid_super_balance(10, 0, 0, 100000, 100); // 适配joyy实际需求
PIDController pid_super_balance(11, 0, 0, 100000, 100); // 适配joyy实际需求
// 舵机中位校准
int calibrationOfs_flag = 0;
@@ -430,15 +455,15 @@ void loop()
}
// 越障跳跃控制
if(obstacle_flag_count){
if(obstacle_enconter_count){
// 大仰角倒地，失败
if(abs(LQR_angle) > 60.0f){
obstacle_status = "obstacle_failed";
}
// if(abs(LQR_angle) > 60.0f){
// obstacle_status = "obstacle_failed";
// }
// 监测前倾角度，进入motor_goforward
if(obstacle_status == "pitch_adjust" && LQR_angle > 23.0f){
if(obstacle_status == "pitch_adjust" && LQR_angle > obstacle_pitch){
obstacle_status = "pitch_adjust_over";
}
@@ -448,18 +473,10 @@ void loop()
}
// 控制电机速度
if(obstacle_status == "motor_goforward"){
if(obstacle_level == "low"){ // 低障碍，速度较慢
trig_v_target = -1.2f;
}else if(obstacle_level == "high"){ // 最高障碍，速度较慢
trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f);
}else{ // 中度障碍，加个系数
trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f) * 1.2f;
}
motor1.target = motor2.target = trig_v_target;
if(obstacle_status == "motor_go"){
motor1.target = motor2.target = motor_target;
}
}
@@ -687,7 +704,7 @@ void leg_loop()
if(sitting_down
|| jump_flag || jump_pre_flag
|| left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag
|| obstacle_flag_count
|| obstacle_enconter_count
){
return; // 跳过后续leg高度计算
}
@@ -704,7 +721,7 @@ void leg_loop()
&& roll_adjust == original_roll_adjust
&& leg_height_base == original_leg_height_base
&& !obstacle_btn_down
&& !obstacle_flag_count
&& !obstacle_enconter_count
&& (millis() - last_falling_trigger_time > FALLING_COOLDOWN) // 核心：5秒冷却
) {
is_falling = true;
@@ -1159,13 +1176,12 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
if (trigRT > 0 && obstacle_mode && robot_enabled) {
obstacle_btn_down++;
wrobot.joyy = 20;
}else{
obstacle_btn_down = 0;
obstacle_status = "";
obstacle_pitch_adjust_count = 0;
obstacle_motor_go_count = 0;
obstacle_flag_count = 0;
pitch_adjust = 0.0f;
// wrobot.height = 50.0f; // 50是默认
}
// 越障模式进入过程取消
if (trigRT == 0 && last_trigRT > 0) {
obstacle_cancel();
setLEDColor(CRGB::Black);
}
last_trigRT = trigRT;
@@ -1285,7 +1301,15 @@ void runActionSequence() {
}
}
// 退出超级平衡模式
void cancel_super_mode(){
super_balance_mode = false;
zaccel_threshold = normal_zaccel_threshold;
setLEDColor(CRGB::Black);
resetZeroPoint();
}
// 障碍检测
void obstacle_detect() {
// Serial.print("LQR_speed");
@@ -1297,53 +1321,66 @@ void obstacle_detect() {
}
// 越障高度检测
if( trigRT < 400 ){ // 低障碍，速度较慢
obstacle_level = "low";
setLEDColor(CRGB::Green);
}else if( trigRT >900 ){ // 最高障碍，速度较慢
// if( trigRT > 0 && trigRT < 400 ){ // 蓝灯，最低障碍
// obstacle_level = "low";
// setLEDColor(CRGB::Blue);
// obstacle_pitch = low_obstacle_pitch;
// servo_up_height = low_servo_up_height;
// servo_down_height = low_servo_down_height;
// motor_target = low_motor_target;
// }else
if( trigRT >= 900 ){ // 红灯，最高障碍
obstacle_level = "high";
setLEDColor(CRGB::Red);
}else{ // 中度障碍
setLEDColor(CRGB::Red);
obstacle_pitch = high_obstacle_pitch;
servo_up_height = high_servo_up_height;
servo_down_height = high_servo_down_height;
motor_target = high_motor_target;
}else{ // 黄灯，中度障碍
obstacle_level = "mid";
setLEDColor(CRGB::Yellow);
setLEDColor(CRGB::Yellow);
obstacle_pitch = mid_obstacle_pitch;
servo_up_height = mid_servo_up_height;
servo_down_height = mid_servo_down_height;
motor_target = mid_motor_target;
}
// 使其向前倾斜，防止向后倒
pitch_adjust = -2.0f;
// 速度停滞
// 检测速度停滞
if ( obstacle_btn_down > 300
&& obstacle_flag_count == 0
&& obstacle_enconter_count == 0
&& LQR_speed > 0.0f && LQR_speed < 0.1f
&& robot_enabled && !sitting_down && !jump_flag
){
obstacle_flag_count = 1;
startLEDBlink(CRGB::Yellow, 100, 2);
obstacle_enconter_count = 1;
}
}
// 越障难度控制
// 磕头越障
void obstacle_loop()
{
if(obstacle_flag_count == 0){
if(obstacle_enconter_count == 0){
return;
}
obstacle_flag_count++;
obstacle_enconter_count++;
if(obstacle_pitch_adjust_count>0){
if(obstacle_pitch_adjust_count > 0){
obstacle_pitch_adjust_count++;
pitch_adjust -=5.0f;
}
if(obstacle_motor_go_count>0){
if(obstacle_motor_go_count > 0){
obstacle_motor_go_count++;
}
// 向前倾斜
if (obstacle_status == "" && obstacle_flag_count == 5){
if (obstacle_status == "" && obstacle_enconter_count == 5){
obstacle_status = "pitch_adjust";
obstacle_pitch_adjust_count = 1;
@@ -1351,59 +1388,55 @@ void obstacle_loop()
if (obstacle_pitch_adjust_count == 1 && obstacle_level == "high" ){
ACC[0] = ACC[1] = 150;
Speed[0] = Speed[1] = 700;
Position[0] = (SERVO0_MAX-50);
Position[1] = (SERVO1_MAX+50);
Position[0] = (SERVO0_MIN + servo_up_height); // MAX=390
Position[1] = (SERVO1_MIN - servo_up_height);
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
// startLEDBlink(CRGB::Yellow, 100, 1 );
startLEDBlink(CRGB::Yellow, 100, 1 );
}
}
// motor_goforward:向前滚动 + 舵机收缩
// motor_go:向前滚动 + 舵机收缩(shrink)
if( obstacle_status == "pitch_adjust_over" && !sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1]) ){
obstacle_status = "motor_goforward";
obstacle_pitch_adjust_count = 0;
obstacle_motor_go_count = 1;
pitch_adjust = 0.0f;
// 收缩腿
if(obstacle_motor_go_count == 1){
ACC[0] = ACC[1] = 250;
Speed[0] = Speed[1] = 2500;
Position[0] = (SERVO0_MIN);
Position[1] = (SERVO1_MIN);
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
resetZeroPoint();
}
obstacle_status = "motor_go";
obstacle_pitch_adjust_count = 0;
obstacle_motor_go_count = 1;
pitch_adjust = 0.0f;
// 收缩腿
if(obstacle_motor_go_count == 1){
ACC[0] = ACC[1] = 250;
Speed[0] = Speed[1] = 2500;
Position[0] = (SERVO0_MIN + servo_down_height);
Position[1] = (SERVO1_MIN - servo_down_height);
sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
}
}
// 舵机收缩完毕，并且进入平衡状态，或者越障失败，越障结束
// 舵机收缩完毕
if((
obstacle_status == "motor_goforward" && obstacle_motor_go_count > 10
obstacle_status == "motor_go" && obstacle_motor_go_count > 10
&& !sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])
&& LQR_angle < 20.0f
) || obstacle_status == "obstacle_failed"){
obstacle_btn_down = 0;
obstacle_status = "";
obstacle_pitch_adjust_count = 0;
obstacle_motor_go_count = 0;
obstacle_flag_count = 0;
pitch_adjust = 0.0f; //负数前倾，正数后倾
obstacle_mode = false; // 跳出越障模式
obstacle_cancel();
resetZeroPoint();
startLEDBlink(CRGB::Green, 100, 2 );
}
}
void cancel_super_mode(){
super_balance_mode = false;
zaccel_threshold = normal_zaccel_threshold;
setLEDColor(CRGB::Black);
resetZeroPoint();
wrobot.joyy = 0; // 清空补偿，快速回稳
}
void obstacle_cancel()
{
obstacle_btn_down = 0;
obstacle_status = "";
obstacle_enconter_count = 0;
obstacle_motor_go_count = 0;
obstacle_pitch_adjust_count = 0;
pitch_adjust = 0.0f; //负数前倾，正数后倾
obstacle_mode = false; // 跳出越障模式
}