From ada5b9aad980a54781fcee52a1052ed0a5a61a5a Mon Sep 17 00:00:00 2001 From: myperworld Date: Sat, 29 Nov 2025 19:37:20 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E8=B6=8A=E9=9A=9C=E6=A8=A1?= =?UTF-8?q?=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- v1.3/src/SerialParser.cpp | 28 ++++++ v1.3/src/SerialParser.h | 17 +++- v1.3/src/main.cpp | 207 ++++++++++++++++++++++---------------- 3 files changed, 164 insertions(+), 88 deletions(-) diff --git a/v1.3/src/SerialParser.cpp b/v1.3/src/SerialParser.cpp index a2db745..3fbd6ac 100644 --- a/v1.3/src/SerialParser.cpp +++ b/v1.3/src/SerialParser.cpp @@ -38,6 +38,34 @@ void parseSingleParam(char* cmd) { pid_super_balance.D = param_val; } else if (strcmp(param_name, "zaccel_threshold") == 0) { zaccel_threshold = param_val; + // } else if (strcmp(param_name, "go_shrink_v") == 0) { + // go_shrink_v = param_val; + // } else if (strcmp(param_name, "servo_up_height") == 0) { + // servo_up_height = param_val; + // } else if (strcmp(param_name, "servo_down_height") == 0) { + // servo_down_height = param_val; + // } else if (strcmp(param_name, "servo_down_count_max") == 0) { + // servo_down_count_max = param_val; + + + } else if (strcmp(param_name, "mid_obstacle_pitch") == 0) { + mid_obstacle_pitch = param_val; + } else if (strcmp(param_name, "mid_servo_up_height") == 0) { + mid_servo_up_height = param_val; + } else if (strcmp(param_name, "mid_servo_down_height") == 0) { + mid_servo_down_height = param_val; + } else if (strcmp(param_name, "mid_motor_target") == 0) { + mid_motor_target = param_val; + + } else if (strcmp(param_name, "high_obstacle_pitch") == 0) { + high_obstacle_pitch = param_val; + } else if (strcmp(param_name, "high_servo_up_height") == 0) { + high_servo_up_height = param_val; + } else if (strcmp(param_name, "high_servo_down_height") == 0) { + high_servo_down_height = param_val; + } else if (strcmp(param_name, "high_motor_target") == 0) { + high_motor_target = param_val; + } else { Serial.println("参数名错误！"); return; diff --git a/v1.3/src/SerialParser.h b/v1.3/src/SerialParser.h index 191d672..235233c 100644 --- a/v1.3/src/SerialParser.h +++ b/v1.3/src/SerialParser.h @@ -10,7 +10,22 @@ extern PIDController pid_super_balance; extern float zaccel_threshold; -// 串口缓冲区全局变量（定义在 SerialParser.cpp 中） + +// extern float obstacle_pitch; +// extern int servo_up_height; +// extern int servo_down_height; +// extern float motor_target; + +extern float mid_obstacle_pitch ; +extern int mid_servo_up_height; +extern int mid_servo_down_height; +extern float mid_motor_target; + +extern float high_obstacle_pitch; +extern int high_servo_up_height; +extern int high_servo_down_height; +extern float high_motor_target; + extern char serial_buf[32]; extern uint8_t buf_idx; diff --git a/v1.3/src/main.cpp b/v1.3/src/main.cpp index f2832a6..d793269 100644 --- a/v1.3/src/main.cpp +++ b/v1.3/src/main.cpp @@ -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; // 清空补偿，快速回稳 -} \ No newline at end of file +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; // 跳出越障模式 +}