diff --git a/v1.3/src/RobotActions.h b/v1.3/src/RobotActions.h index 60940e7..bcd02bf 100644 --- a/v1.3/src/RobotActions.h +++ b/v1.3/src/RobotActions.h @@ -95,5 +95,4 @@ ActionStep step_left[] = { }; RobotAction action_step_left = CREATE_ACTION("action_step_left", step_left); - #endif \ No newline at end of file diff --git a/v1.3/src/VoltageMonitor.cpp b/v1.3/src/VoltageMonitor.cpp index f093635..12ab78e 100644 --- a/v1.3/src/VoltageMonitor.cpp +++ b/v1.3/src/VoltageMonitor.cpp @@ -41,7 +41,8 @@ void bat_check() digitalWrite(LED_BAT, HIGH); }else{ digitalWrite(LED_BAT, LOW); - startLEDBlink(CRGB::Red, 200, 5); + + startLEDBlink(CRGB::Red, 200, 5); // rgb 红灯闪烁 } bat_check_num = 0; } diff --git a/v1.3/src/main.cpp b/v1.3/src/main.cpp index d793269..4e0a487 100644 --- a/v1.3/src/main.cpp +++ b/v1.3/src/main.cpp @@ -28,8 +28,6 @@ void cancel_super_mode(); // 跨越障碍函数 void obstacle_detect(); // 越障高度检测 void obstacle_loop(); // 越障总函数 -void obstacle_loop0(); // 越障总函数 -void obstacle_loop1(); // 越障总函数 void obstacle_cancel(); // 常规运动控制 @@ -428,22 +426,16 @@ void loop() motor2.target = 5.0; } - // 向前越障 - if ( forward_jump_flag > 2) { - motor1.target = 2.3; // 脉冲 - motor2.target = 2.3; - } - // 向前跳跃 if ( forward_jump_flag > 2) { - motor1.target = 2.3; // 脉冲 - motor2.target = 2.3; + motor1.target = 2.0; // 脉冲 + motor2.target = 2.0; } // 向后跳跃 if ( back_jump_flag > 2 ) { - motor1.target = -1.0; // 脉冲 - motor2.target = -1.0; + motor1.target = -0.8; // 脉冲 + motor2.target = -0.8; } // 站立计数 @@ -457,11 +449,6 @@ void loop() // 越障跳跃控制 if(obstacle_enconter_count){ - // 大仰角倒地,失败 - // if(abs(LQR_angle) > 60.0f){ - // obstacle_status = "obstacle_failed"; - // } - // 监测前倾角度,进入motor_goforward if(obstacle_status == "pitch_adjust" && LQR_angle > obstacle_pitch){ obstacle_status = "pitch_adjust_over"; @@ -586,29 +573,13 @@ void lqr_balance_loop() } // 超级平衡模式 - if (super_balance_mode && !is_falling) { - - // if(abs(LQR_angle) < 4.0f ){ - // pid_super_balance.error_prev = 0.0f; // 重置积分项 - // } - + if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag) { // 误差=实际角度 - 默认零点 float balance_joyy = pid_super_balance(LQR_angle -pitch_zeropoint); balance_joyy = constrain(balance_joyy, -100.0f, 100.0f); wrobot.joyy = balance_joyy; - - // Serial.print("LQR_angle:"); - // Serial.print(LQR_angle); - // Serial.print(",balance_joyy:"); - // Serial.println(balance_joyy); - } - // else{ - // // Serial.print("LQR_angle:"); - // // Serial.print(LQR_angle); - // Serial.print(",wrobot.joyy:"); - // Serial.println(wrobot.joyy); - // } + } speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v @@ -830,6 +801,18 @@ void jump_loop() jump_pre_flag = 1; } + if( abs(LQR_angle) > 20.0f && jump_pre_flag == 1){ // 车身没有平稳,不能跳跃 + wrobot.jump = 0; + jump_pre_flag = 0; // 重置跳跃准备标志 + jump_flag = 0; // 跳跃过程结束 + left_jump_flag = 0; + right_jump_flag = 0; + forward_jump_flag = 0; + back_jump_flag = 0; + startLEDBlink(CRGB::Yellow, 100, 2 ); + return; // 跳过后续leg高度计算 + } + if(jump_pre_flag > 0 ){ jump_pre_flag ++; } @@ -902,6 +885,7 @@ void jump_loop() if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33 { jump_flag = 0; // 跳跃过程结束 + resetZeroPoint(); } } @@ -952,6 +936,7 @@ void resetZeroPoint() { distance_zeropoint = LQR_distance; pid_lqr_u.error_prev = 0; pid_super_balance.error_prev = 0.0f; // 重置积分项 + pitch_adjust = 0.0f; } /*======================================== @@ -1107,7 +1092,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { } // 还原 - if (remoteData.btnLS) { + if (remoteData.btnLS) { roll_adjust = original_roll_adjust; leg_height_base = original_leg_height_base; pitch_zeropoint = original_pitch_zeropoint; @@ -1176,7 +1161,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { if (trigRT > 0 && obstacle_mode && robot_enabled) { obstacle_btn_down++; wrobot.joyy = 20; - // wrobot.height = 50.0f; // 50是默认 } // 越障模式进入过程取消 if (trigRT == 0 && last_trigRT > 0) {