修复跳跃bug

v1.3/src/RobotActions.h

@@ -95,5 +95,4 @@ ActionStep step_left[] = {
 };
 RobotAction action_step_left = CREATE_ACTION("action_step_left", step_left);
 
-
 #endif

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;
   }

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; // 脉冲
+    motor1.target = 2.0; // 脉冲
-    motor2.target = 2.3;
+    motor2.target = 2.0;
   } 
 
   // 向后跳跃
   if ( back_jump_flag > 2 ) {
-    motor1.target = -1.0; // 脉冲
+    motor1.target = -0.8; // 脉冲
-    motor2.target = -1.0;
+    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 (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag) {
-   
-    // if(abs(LQR_angle) < 4.0f ){
-    //    pid_super_balance.error_prev = 0.0f; // 重置积分项
-    // }
-
      // 误差=实际角度 - 默认零点
     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) {