越障模式增加灯光指引，绿色低障碍1cm，黄色中等障碍2cm，红色高障碍3cm+

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开启
 
@@ -414,9 +414,11 @@ 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{
+      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,7 +1100,7 @@ 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) {
     obstacle_btn_down++;
@@ -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 );
 
     }