diff --git a/v1.3/src/main.cpp b/v1.3/src/main.cpp index 0e25db3..d08bbcd 100644 --- a/v1.3/src/main.cpp +++ b/v1.3/src/main.cpp @@ -21,7 +21,8 @@ void yaw_loop(); void leg_loop(); void jump_loop(); -// 跨越障碍 +// 跨越障碍函数 +void obstacle_detect(); // 越障高度检测 void obstacle_loop(); // 越障总函数 @@ -33,8 +34,6 @@ void jump_charge(); // 腿部弯曲(蓄能) void processControllerData(const XboxControllerNotificationParser& data); void runActionSequence(); -// 障碍检测函数 -void obstacle_detect(); const int deadZone = 10; // 摇杆死区阈值 int trigRT = 0; @@ -178,6 +177,7 @@ 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开启 @@ -413,10 +413,12 @@ void loop() // 越障跳跃控制 if(obstacle_flag_count){ - - if(abs(LQR_angle) > 60.0f){ // 大仰角倒地,失败 + + // 大仰角倒地,失败 + if(abs(LQR_angle) > 60.0f){ obstacle_status = "obstacle_failed"; } + // 监测前倾角度,进入motor_goforward if(obstacle_status == "pitch_adjust" && LQR_angle > 23.0f){ obstacle_status = "pitch_adjust_over"; @@ -426,14 +428,16 @@ void loop() if(obstacle_status == "pitch_adjust" ){ motor1.target = motor2.target = -0.2f; } + // 控制电机速度 if(obstacle_status == "motor_goforward"){ - // trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f); - if( trigRT < 400 ){ // 低障碍,速度较慢 - trig_v_target = -1.5f; - }else{ - trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f); + 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.1f; } motor1.target = motor2.target = trig_v_target; @@ -1096,9 +1100,9 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { if(!obstacle_mode && last_trigRT == 0 && xboxController.xboxNotif.trigRT > 0 && robot_enabled){ obstacle_mode = true; // 可以进入越障模式 } - + // 进入越障模式 trigRT = xboxController.xboxNotif.trigRT; - if (trigRT > 0 && obstacle_mode && robot_enabled) { + if (trigRT > 0 && obstacle_mode && robot_enabled) { obstacle_btn_down++; wrobot.joyy = 20; }else{ @@ -1223,6 +1227,19 @@ void obstacle_detect() { if(!obstacle_btn_down){ return; } + + // 越障高度检测 + if( trigRT < 400 ){ // 低障碍,速度较慢 + obstacle_level = "low"; + setLEDColor(CRGB::Green); + }else if( trigRT >900 ){ // 最高障碍,速度较慢 + obstacle_level = "high"; + setLEDColor(CRGB::Red); + }else{ // 中度障碍 + obstacle_level = "mid"; + setLEDColor(CRGB::Yellow); + } + // 使其向前倾斜,防止向后倒 pitch_adjust = -2.0f; @@ -1232,12 +1249,9 @@ void obstacle_detect() { && LQR_speed > 0.0f && LQR_speed < 0.1f && robot_enabled && !sitting_down && !jump_flag ){ - // Serial.print("obstacle_btn_down"); - // Serial.println(obstacle_btn_down); obstacle_flag_count = 1; - // 开始红色闪烁,每次持续100毫秒,闪烁1次 - startLEDBlink(CRGB::Red, 100, 1); + startLEDBlink(CRGB::Yellow, 100, 2); } } @@ -1266,14 +1280,14 @@ void obstacle_loop() obstacle_pitch_adjust_count = 1; // 如果扳机键按到底,伸长腿 - if (obstacle_pitch_adjust_count == 1 && trigRT > 900 ){ + 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); sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); - startLEDBlink(CRGB::Yellow, 100, 1 ); + // startLEDBlink(CRGB::Yellow, 100, 1 ); }