优化龙头模式

v1.3/src/main.cpp

@@ -253,7 +253,11 @@ RobotAction* currentAction = NULL;
 
 //  龙头模式
 bool dragon_head_mode = false; // 龙头模式标志
-
+float dragon_joyx_factor = 1.0f;
+int convertedJ1PotX;
+int convertedJ2PotX;
+int last_convertedJ1PotX;
+int last_convertedJ2PotX;
 
 typedef struct
 {
@@ -585,7 +589,7 @@ void lqr_balance_loop()
   }
  
   // 超级平衡模式 误差=实际角度 - 默认零点
-  if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag && !dragon_head_mode) {
+  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;
@@ -667,9 +671,10 @@ void lqr_balance_loop()
     pid_lqr_u.error_prev = 0; // 输出积分清零
   }
 
-
-  if (wrobot.height < 50){
-    pid_speed.P = 0.7;
+  if(jump_flag){
+    pid_speed.P = 0.5;
+  } if (wrobot.height < 50){
+    pid_speed.P = 0.8;
   }else if (wrobot.height < 64){
     pid_speed.P = 0.6;
   }else{
@@ -1056,36 +1061,37 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     }
 
     // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100
-    if(!dragon_head_mode){ // 龙头模式
-       int convertedJ1PotX = constrain(
+    if(!dragon_head_mode){ // 双手控制模式
+      convertedJ1PotX = constrain(
         map(remoteData.joyLHori, 0, 65535, -100, 100),  
         -100, 100
       );
       if(abs(convertedJ1PotX) > deadZone
-          && ( (last_joyLHori > 0 && ( remoteData.joyLHori- last_joyLHori)> 0 ) 
-              || (last_joyLHori <0 && ( remoteData.joyLHori- last_joyLHori)< 0 )
+          && ( (last_convertedJ1PotX > 0 &&  convertedJ1PotX >= last_convertedJ1PotX ) 
+              || (last_convertedJ1PotX < 0 &&  convertedJ1PotX <= last_convertedJ1PotX)
           )) { // 左右运动  -100 到 100
         wrobot.joyx = convertedJ1PotX; 
       } else {
         wrobot.joyx = 0;
       }
 
-    }else{
-      int convertedJ2PotX = constrain(
+    }else{ // 龙头模式
+      convertedJ2PotX = constrain(
         map(remoteData.joyRHori, 0, 65535, -100, 100),  
         -100, 100
       );
       if(abs(convertedJ2PotX) > deadZone
-      && ( (last_joyRHori > 0 && ( remoteData.joyRHori- last_joyRHori)> 0 ) 
-              || (last_joyRHori <0 && ( remoteData.joyRHori- last_joyRHori)< 0 )
+      && ( (last_convertedJ2PotX > 0 &&  convertedJ2PotX >= last_convertedJ2PotX ) 
+              || (last_convertedJ2PotX <0 &&  convertedJ2PotX <= last_convertedJ2PotX )
       )) { // 左右运动  -100 到 100
-        wrobot.joyx = convertedJ2PotX * 0.3; 
+        wrobot.joyx = convertedJ2PotX * dragon_joyx_factor; 
       } else {
         wrobot.joyx = 0;
       }
     }
-    last_joyRHori = remoteData.joyRHori;
-    last_joyLHori = remoteData.joyLHori;
+
+    last_convertedJ1PotX = convertedJ1PotX;
+    last_convertedJ2PotX = convertedJ2PotX;
 
    
     // 智能计算wrobot.height ，根据前后速度，