增加龙头模式

v1.3/src/SerialParser.cpp

@@ -30,11 +30,11 @@ void parseSingleParam(char* cmd) {
   float param_val = atof(eq_pos + 1);  // 转为浮点值
 
   // 匹配参数名并赋值（访问全局对象，已在 SerialParser.h 中声明）
-  if (strcmp(param_name, "Kp") == 0) {
+  if (strcmp(param_name, "kp") == 0) {
     pid_super_balance.P = param_val;
-  } else if (strcmp(param_name, "Ki") == 0) {
+  } else if (strcmp(param_name, "ki") == 0) {
     pid_super_balance.I = param_val;
-  } else if (strcmp(param_name, "Kd") == 0) {
+  } else if (strcmp(param_name, "kd") == 0) {
     pid_super_balance.D = param_val;
   } else if (strcmp(param_name, "zaccel_threshold") == 0) {
     zaccel_threshold = param_val;

v1.3/src/SerialParser.h

@@ -8,6 +8,7 @@
 // -------------------------- 全局对象声明（与 main.cpp 中定义一致）--------------------------
 // 这些对象在 main.cpp 中定义，此处用 extern 声明供 SerialParser 访问
 extern PIDController pid_super_balance;
+extern PIDController pid_speed;
 extern float zaccel_threshold;
 
 

v1.3/src/main.cpp

@@ -159,7 +159,7 @@ float gyro_control = 0;
 float speed_control = 0;
 float distance_control = 0;
 float LQR_u = 0;
-float pitch_zeropoint = 2;  // 7 设定默认俯仰角度，向前到正，向后倒负
+float pitch_zeropoint = 2;  // 2 设定默认俯仰角度，向前到正，向后倒负
 float original_pitch_zeropoint = pitch_zeropoint; // 保存原始的角度零点
 float distance_zeropoint =  0.5f; // 轮部位移零点偏置
 float pitch_adjust = 0.0f; // 俯仰角度调整,负数前倾，正数后倾
@@ -182,6 +182,7 @@ float robot_speed_diff = 0;    // 速度差
 int jump_flag = 0; // 跳跃过程计数
 int jump_pre_flag = 0; // 跳跃前蓄能计数
 
+int place_jump_flag = 0; // 原地跳跃
 int forward_jump_flag = 0; // 前跳跃标志位
 int back_jump_flag = 0; // 后跳跃标志位
 int left_jump_flag = 0; // 左跳跃标志位
@@ -223,7 +224,7 @@ float high_motor_target = -9.0f;
 
 // 超级平衡模式参数
 bool super_balance_mode = false; // 超级平衡模式
-PIDController pid_super_balance(11, 0, 0, 100000, 100); // 适配joyy实际需求
+PIDController pid_super_balance(11.5, 0, 0, 100000, 8); // 适配joyy实际需求
 
 // 舵机中位校准
 int calibrationOfs_flag = 0;
@@ -248,6 +249,9 @@ unsigned long pauseStartTime = 0;
 int currentStepIndex = 0;
 RobotAction* currentAction = NULL;
 
+//  龙头模式
+bool dragon_head_mode = false; // 龙头模式标志
+
 
 typedef struct
 {
@@ -438,6 +442,12 @@ void loop()
     motor2.target = -0.8;
   } 
 
+  // 原地跳越
+  if ( place_jump_flag > 2) {
+    motor1.target = 1.5; // 脉冲
+    motor2.target = 1.5;
+  } 
+
   // 站立计数
   if( stand_up_count > 0){
     stand_up_count ++;
@@ -572,13 +582,11 @@ void lqr_balance_loop()
     speed_target_coeff = 0.1;
   }
  
-  // 超级平衡模式
-  if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag) {
-     // 误差=实际角度 - 默认零点
+  // 超级平衡模式 误差=实际角度 - 默认零点
+  if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag && !dragon_head_mode) {
     float balance_joyy = pid_super_balance(LQR_angle -pitch_zeropoint);
     balance_joyy = constrain(balance_joyy, -100.0f, 100.0f); 
     wrobot.joyy = balance_joyy;
-
   }
 
   speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy));  // 最大8v
@@ -657,8 +665,7 @@ void lqr_balance_loop()
     pid_lqr_u.error_prev = 0; // 输出积分清零
   }
 
-  // 平衡控制参数自适应
-  // 考虑 leg_height_base = 0 
+
   if (wrobot.height < 50){
     pid_speed.P = 0.7;
   }else if (wrobot.height < 64){
@@ -666,6 +673,7 @@ void lqr_balance_loop()
   }else{
     pid_speed.P = 0.5;
   }
+  
 }
 
 // 腿部动作控制（舵机）
@@ -674,7 +682,7 @@ void leg_loop()
   // 如果坐下就不控制腿部
   if(sitting_down 
     || jump_flag || jump_pre_flag 
-    || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag
+    || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag || place_jump_flag
     || obstacle_enconter_count
   ){ 
       return; // 跳过后续leg高度计算
@@ -808,6 +816,7 @@ void jump_loop()
     left_jump_flag = 0;
     right_jump_flag = 0;
     forward_jump_flag = 0;
+    place_jump_flag = 0;
     back_jump_flag = 0;
     startLEDBlink(CRGB::Yellow, 100,  2 );
     return; // 跳过后续leg高度计算
@@ -848,6 +857,14 @@ void jump_loop()
           back_jump_flag = 0;
         }
       
+      }else if( place_jump_flag ){ // 原地跳跃
+        place_jump_flag ++ ;
+        
+        if(place_jump_flag > 20){
+          place_jump_flag = 0;
+        }
+
+
       }else{ // 跳跃开始
 
         jump_flag = 1;
@@ -876,16 +893,16 @@ void jump_loop()
     ACC[1] = 0;
     Speed[0] = 0;
     Speed[1] = 0;
-    Position[0] = (SERVO0_MIN  + 100); // 100 约等于食指宽度
-    Position[1] = (SERVO1_MIN  - 100); 
+    Position[0] = (SERVO0_MIN  + 80); // 100 约等于食指宽度
+    Position[1] = (SERVO1_MIN  - 80); 
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
   }
 
   // 轮子着地
-  if (jump_flag > 200) //  2000=6s ,1s=2000/6=333.33
+  // if (jump_flag > 200) //  2000=6s ,1s=2000/6=333.33
+  if (jump_flag > 150) //  2000=6s ,1s=2000/6=333.33
   {
     jump_flag = 0; // 跳跃过程结束
-    resetZeroPoint();
   }
 
 }
@@ -900,15 +917,15 @@ void yaw_loop()
     return;
   }
 
- // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
+ // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数0.1~1
   float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; 
   coeff = constrain(coeff, 0.1f, 1.0f); 
   float yaw_target = wrobot.joyx * coeff; 
 
-  // 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限）
-  float yaw_angle_control_coeff = 1.0f;
-  if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转
-    yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); 
+  // 2. 根据abs(wrobot.joyx)，提高PID响应，临时放大系数
+  float yaw_angle_fator = 1.0f;
+  if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10 && !dragon_head_mode){ // 原地打转
+    yaw_angle_fator = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); 
     // 根据转向速度，智能计算wrobot.height 
     // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
     // 反向映射：绝对值30→60，绝对值100→0（线性过渡）
@@ -922,13 +939,19 @@ void yaw_loop()
     startLEDBlink(CRGB::Red, 200, 1);
   }
 
-  float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; 
+  float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_fator; 
+  
   // 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适）
   yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f);
 
+
   YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 
   float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
+  
   YAW_output = yaw_angle_control + yaw_gyro_control;
+
+
+
 }
 
 // 重置距离零点
@@ -936,11 +959,11 @@ void resetZeroPoint() {
   distance_zeropoint =  LQR_distance;
   pid_lqr_u.error_prev = 0;
   pid_super_balance.error_prev = 0.0f; // 重置积分项
-   pitch_adjust = 0.0f;
+  pitch_adjust = 0.0f;
 }
 
 /*========================================
- * 控制器数据处理（共用函数）
+ * xbox 手柄控制
  * ========================================*/
 void processControllerData(const XboxControllerNotificationParser& remoteData) {
 
@@ -1020,19 +1043,43 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
       wrobot.joyy = 0;
     } 
 
+      // 龙头模式
+    if (remoteData.btnStart && !last_btnStart) {
+      dragon_head_mode = !dragon_head_mode; // 切换模式
+      if (dragon_head_mode) {
+        setLEDColor(CRGB::Purple); // 紫色LED标识龙头模式
+      } else {
+        setLEDColor(CRGB::Black);  // 关闭LED
+      }
+    }
+
     // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
-    int convertedJ1PotX = constrain(
-      map(remoteData.joyLHori, 0, 65535, -100, 100),  
-      -100, 100
-    );
-    if(abs(convertedJ1PotX) > deadZone) { // 左右运动  -100 到 100
-      wrobot.joyx = convertedJ1PotX; 
-    } else {
-      wrobot.joyx = 0;
+    if(!dragon_head_mode){ // 龙头模式
+       int convertedJ1PotX = constrain(
+        map(remoteData.joyLHori, 0, 65535, -100, 100),  
+        -100, 100
+      );
+      if(abs(convertedJ1PotX) > deadZone) { // 左右运动  -100 到 100
+        wrobot.joyx = convertedJ1PotX; 
+      } else {
+        wrobot.joyx = 0;
+      }
+
+    }else{
+      int convertedJ2PotX = constrain(
+        map(remoteData.joyRHori, 0, 65535, -100, 100),  
+        -100, 100
+      );
+      if(abs(convertedJ2PotX) > deadZone) { // 左右运动  -100 到 100
+        wrobot.joyx = convertedJ2PotX * 0.3; 
+      } else {
+        wrobot.joyx = 0;
+      }
     }
    
     // 智能计算wrobot.height ，根据前后速度，
     // convertedJ2PotY 范围-100到100，默认0，输出范围30（最矮）-50（最高）, 默认 50
+    // wrobot.height 0 最矮，50最高
     wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
     wrobot.height = constrain(wrobot.height, 30, 50);
     wrobot.height = lpf_height(wrobot.height);
@@ -1113,6 +1160,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   // 跳跃 右摇杆按键
   if (remoteData.btnRS && !last_btnRS  && robot_enabled) { // 1=true，0=false
     wrobot.jump = 1; // 跳跃
+    place_jump_flag = 1; // 跳跃
   }else{
     wrobot.jump = 0; // 不跳跃
   }
@@ -1179,17 +1227,20 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   last_btnShare = remoteData.btnShare; // 更新btnY的上一次状态
 
   // 超级平衡模式切换
-if (remoteData.btnSelect && !last_btnSelect) {
-    super_balance_mode = !super_balance_mode;
-    if (super_balance_mode) {
-      setLEDColor(CRGB::Blue);
-      zaccel_threshold = super_zaccel_threshold;
-      resetZeroPoint(); // 重置位移零点，确保初始平衡
-    } else {
-      cancel_super_mode();
-    }
-}
-last_btnSelect = remoteData.btnSelect;
+  if (remoteData.btnSelect && !last_btnSelect) { 
+
+      pid_super_balance.error_prev = 0.0f;
+
+      super_balance_mode = !super_balance_mode;
+      if (super_balance_mode) {
+        setLEDColor(CRGB::Blue);
+        zaccel_threshold = super_zaccel_threshold;
+        resetZeroPoint(); // 重置位移零点，确保初始平衡
+      }else{
+        cancel_super_mode();
+      }
+  }
+  last_btnSelect = remoteData.btnSelect;
 
 
   // 舵机中位校准 中间十字左右键同时按下
@@ -1206,6 +1257,7 @@ last_btnSelect = remoteData.btnSelect;
   if(calibrationOfs_flag > 500){
     calibrationOfs_flag =0;
   }
+
   last_btnSelect = remoteData.btnSelect;
   last_btnStart = remoteData.btnStart;
 }
@@ -1296,9 +1348,6 @@ void cancel_super_mode(){
 // 障碍检测
 void obstacle_detect() {
 
-  // Serial.print("LQR_speed");
-  // Serial.println(LQR_speed);
-
   // 自动越障前进模式下,持续300次循环，防止刚启动由于惯性无法立即向前
   if(!obstacle_btn_down){
     return;