优化越障模式

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;

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;
 

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){
+    // if(abs(LQR_angle) > 60.0f){
-       obstacle_status = "obstacle_failed";
+    //    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_status == "motor_go"){
-      
+      motor1.target = motor2.target = motor_target; 
-      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; 
-    }
   }
 
 
@@ -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{
+    // wrobot.height = 50.0f; // 50是默认
-    obstacle_btn_down = 0;
+  }
-    obstacle_status = "";
+    // 越障模式进入过程取消
-    obstacle_pitch_adjust_count = 0;
+  if (trigRT == 0 && last_trigRT > 0) {
-    obstacle_motor_go_count = 0;
+    obstacle_cancel(); 
-    obstacle_flag_count = 0;
+    setLEDColor(CRGB::Black);
-    pitch_adjust = 0.0f;
   }
   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 ){ // 低障碍，速度较慢
+  // if( trigRT > 0 && trigRT < 400 ){ // 蓝灯，最低障碍
-    obstacle_level = "low";
+  //   obstacle_level = "low";
-    setLEDColor(CRGB::Green);
+  //   setLEDColor(CRGB::Blue);
-  }else if( trigRT >900 ){ // 最高障碍，速度较慢
+  //   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{ // 中度障碍
+    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);
+    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;
+      obstacle_enconter_count = 1;
-
-      startLEDBlink(CRGB::Yellow, 100, 2);
-
   }
 }
 
-
+// 磕头越障
-// 越障难度控制
 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){
     obstacle_pitch_adjust_count++;
     pitch_adjust -=5.0f;
   }
+
   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,19 +1388,16 @@ 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[0] = (SERVO0_MIN + servo_up_height); // MAX=390
-      Position[1] = (SERVO1_MAX+50); 
+      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_go:向前滚动 + 舵机收缩(shrink)
-   
-  // motor_goforward:向前滚动 + 舵机收缩
    if( obstacle_status == "pitch_adjust_over" && !sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1]) ){
-    obstacle_status = "motor_goforward";
+      obstacle_status = "motor_go";
       obstacle_pitch_adjust_count = 0;
       obstacle_motor_go_count = 1;
       pitch_adjust = 0.0f;
@@ -1372,38 +1406,37 @@ void obstacle_loop()
       if(obstacle_motor_go_count == 1){
         ACC[0] = ACC[1] = 250;
         Speed[0] = Speed[1] = 2500;
-      Position[0] = (SERVO0_MIN);
+        Position[0] = (SERVO0_MIN + servo_down_height);
-      Position[1] = (SERVO1_MIN); 
+        Position[1] = (SERVO1_MIN - servo_down_height); 
         sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
-      resetZeroPoint();
+      }
   }
 
-  }
+  // 舵机收缩完毕
-
-  // 舵机收缩完毕，并且进入平衡状态，或者越障失败，越障结束
   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_cancel();
-    obstacle_status = "";
+    resetZeroPoint();   
-    obstacle_pitch_adjust_count = 0;
-    obstacle_motor_go_count = 0;
-    obstacle_flag_count = 0;
-    pitch_adjust = 0.0f; //负数前倾，正数后倾
-    obstacle_mode = false; // 跳出越障模式
    
     startLEDBlink(CRGB::Green, 100,  2 );
   }
 }
 
 
-void cancel_super_mode(){
+void obstacle_cancel()
-  super_balance_mode = false;
+{
-  zaccel_threshold = normal_zaccel_threshold;
+    obstacle_btn_down = 0;
-  setLEDColor(CRGB::Black);
+    obstacle_status = "";
-  resetZeroPoint(); 
+    
-  wrobot.joyy = 0; // 清空补偿，快速回稳
+    obstacle_enconter_count = 0;
+
+    obstacle_motor_go_count = 0;
+
+    obstacle_pitch_adjust_count = 0;
+
+    pitch_adjust = 0.0f; //负数前倾，正数后倾
+    obstacle_mode = false; // 跳出越障模式
 }