优化向前跳跃，Y键

v1.1/Shibo-main-TOMMY/src/main.cpp

@@ -25,10 +25,11 @@ void yaw_loop();
 void leg_loop();
 void jump_loop();
 void resetToDefault();
+void jump_charge(); // 腿部弯曲（蓄能）
 
 // RGB相关函数
 void handleLEDBlink();
-void updateBreathLED(int joyx_abs); // 呼吸灯控制函数声明
+
 /************实例定义*************/
 
 // 电机实例
@@ -76,13 +77,6 @@ int blinkCount = 0;
 bool isBlinking = false;
 CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::Green, CRGB::Green};
 
-// 呼吸灯全局状态变量
-unsigned long breathLastMillis = 0;    // 呼吸灯上次更新时间
-uint8_t breathBrightness = 0;          // 当前呼吸灯亮度（0-255）
-bool breathIncreasing = true;          // 亮度变化方向（true=递增，false=递减）
-bool isBreathing = false;              // 呼吸灯激活标记（避免与原有闪烁冲突）
-const CRGB breathColor = CRGB::Blue;   // 呼吸灯固定颜色：蓝色
-
 /************参数定义*************/
 #define pi 3.1415927
 
@@ -149,7 +143,6 @@ int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
 
 // 定义摇杆数值死区参数
 const int deadZone = 10;
-int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令
 
 // 电压检测
 #define LED_BAT 13
@@ -210,6 +203,10 @@ typedef struct {
 
 JoystickData remoteData;
 
+// 记录上一次按钮状态,防止连续触发
+bool last_btnY = 0; 
+bool last_btnJoyRight = 0;
+
 // ESP-NOW接收回调函数
 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   memcpy(&remoteData, incomingData, sizeof(remoteData));
@@ -282,28 +279,31 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   );
 
   // 跳跃  右摇杆按键
-  if (remoteData.btnJoyRight) { // 1=true，0=false
+  if (remoteData.btnJoyRight && !last_btnJoyRight) { // 1=true，0=false
     wrobot.jump = 1; // 跳跃
   }else{
     wrobot.jump = 0; // 不跳跃
   }
+  last_btnJoyRight = remoteData.btnJoyRight; 
 
   // 向前跳跃 Y键
-  if (remoteData.btnY && robot_enabled) {
+  if (remoteData.btnY && robot_enabled && !last_btnY) {
     forward_jump_flag = 1;
   }
+  last_btnY = remoteData.btnY; // 防止连续输出向前跳跃
+
   // 向后跳跃 A键
-  if (remoteData.btnA && robot_enabled) {
+  // if (remoteData.btnA && robot_enabled) {
-    backward_jump_flag = 1;
+  //   backward_jump_flag = 1;
-  }
+  // }
-  // 向左跳跃 X键
+  // // 向左跳跃 X键
-  if (remoteData.btnX && robot_enabled) {
+  // if (remoteData.btnX && robot_enabled) {
-    left_jump_flag = 1;
+  //   left_jump_flag = 1;
-  }
+  // }
-  // 向右跳跃 B键
+  // // 向右跳跃 B键
-  if (remoteData.btnB && robot_enabled) {
+  // if (remoteData.btnB && robot_enabled) {
-    right_jump_flag = 1;
+  //   right_jump_flag = 1;
-  }
+  // }
 
   if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
     wrobot.joyy = convertedJ2PotY;
@@ -470,7 +470,6 @@ void loop()
   motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
   motor2.target = (-0.5) * (LQR_u - YAW_output);
 
-
   // 小车倒地后，重置所有参数 测试
   loopCounter++;
   // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
@@ -507,53 +506,28 @@ void loop()
 
   // 向前跳跃
   if(forward_jump_flag){
+    if(forward_jump_flag == 1){
+      jump_charge();
+      forward_jump_flag = 2;
+    }
+    if(forward_jump_flag == 2){
+      if(!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])){
+        forward_jump_flag = 3;
+      }
+    }
+    if(forward_jump_flag >= 3){
       forward_jump_flag ++ ;
-    if (forward_jump_flag < 20) {
+      if ( forward_jump_flag < 25) {
-      // 保持向前脉冲
+        motor1.target = 1.8; // 保持向前脉冲
-      motor1.target = 1.8;
         motor2.target = 1.8;
       } else {
         forward_jump_flag = 0;
-      wrobot.jump = 1; // 跳跃
+        jump_pre_flag = 1;
       }
     }
-  // 向后跳跃
-  if(backward_jump_flag){
-    backward_jump_flag++;
-    if (backward_jump_flag < 20) {
-      // 保持向后脉冲
-      // motor1.target = -1.8;
-      // motor2.target = -1.8;
-      motor1.target = -2.0;
-      motor2.target = -2.0;
-    } else {
-      backward_jump_flag = 0;
-      wrobot.jump = 1; // 跳跃
-    }
-  }
-  // 向左跳跃
-  if(left_jump_flag){
-    left_jump_flag++;
-    if (left_jump_flag < 20) { 
-      motor1.target = -2.5;
-      motor2.target = 2.5;
-    } else {
-      left_jump_flag = 0;
-      wrobot.jump = 1; // 跳跃
-    }
-  }
-  // 向右跳跃
-  if(right_jump_flag){
-    right_jump_flag++;
-    if (right_jump_flag < 20) { 
-      motor1.target = 2.5;
-      motor2.target = -2.5;
-    } else {
-      right_jump_flag = 0;
-      wrobot.jump = 1; // 跳跃
-    }
   }
   
+
   // 小车坐下
   if (sitting_down) {
     sitting_down ++;
@@ -642,27 +616,15 @@ void lqr_balance_loop()
   angle_control = pid_angle(LQR_angle - angle_zeropoint); 
   gyro_control = pid_gyro(LQR_gyro);
 
-  // 运动细节优化处理
+  // 前进后退跳跃的系数都不一样
-  // 判断摇杆是否没有前后左右运动指令
+  float speed_target_coeff = 0.1; 
-  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
-  {
-    wrobot_move_stop_flag = 1; 
-  }
-  // 没有摇杆指令，小车微微移动，需要重置位移零点，让小车自己回来
-  if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) 
-  {
-    distance_zeropoint = LQR_distance; // 位移零点重置
-    wrobot_move_stop_flag = 0;
-  }
-
-  // 解决前进后退速度不一致，前进(joyy>0）时用x系数大于，后退（joyy<0）保持系数
-  float speed_target_coeff = (wrobot.joyy > 0) ? 0.18 : 0.08; //向前系数越大，使头会向下倾，不容易稳定，向后系数大，向后冲的越快，也不容易稳定
-  
   if( jump_flag ){ // 跳跃
-    speed_target_coeff = 0.1;
+    speed_target_coeff = 0.12;
   }else if(wrobot.joyy > 0 ){ // 前进
     speed_target_coeff = 0.18;
-  }else{ // 后退 wrobot.joyy < 0
+  }else if(wrobot.joyy < 0){ // 后退 
+    speed_target_coeff = 0.11;
+  }else{
     speed_target_coeff = 0.1;
   }
   
@@ -686,16 +648,20 @@ void lqr_balance_loop()
       wheel_ground_flag = 0; // 轮子离地标记
       // Serial.print("wheel_ground_flag:");
       // Serial.print(wheel_ground_flag);
+      // Serial.print(",robot_speed_diff:");
+      // Serial.print(robot_speed_diff);
       // Serial.print(",jump_flag:");
-      // Serial.println(jump_flag);
+      // Serial.println(jump_flag); // 100 左右
     }
     if( robot_speed_diff < -9.0) // 轮子着地
     {
       wheel_ground_flag = 1; // 轮子着地标记
       // Serial.print("wheel_ground_flag:");
       // Serial.print(wheel_ground_flag);
+      // Serial.print(",robot_speed_diff:");
+      // Serial.print(robot_speed_diff);
       // Serial.print(",jump_flag:");
-      // Serial.println(jump_flag); 
+      // Serial.println(jump_flag); // 120左右
     }
     last_lqr_speed = LQR_speed;  // 每隔100ms更新一次历史转速
     last_speed_record_time = current_time;  // 更新记录时间
@@ -714,8 +680,8 @@ void lqr_balance_loop()
 
   // 计算 LQR_u
   // 轮部离地情况下 LQR_u=角度控制量+角速度控制量
-  if (abs(robot_speed_diff) > 10 ||  (jump_flag > 0 and jump_flag < 100)) 
+  // if (abs(robot_speed_diff) > 10 ||  (jump_flag > 0 and jump_flag < 100)) 
-  // if (abs(robot_speed_diff) > 10 ||  (jump_flag > 0 and jump_flag < 7)) 
+  if (abs(robot_speed_diff) > 10 ||  (jump_flag > 0 and jump_flag < 120)) // jump_flag：100离地 120着地
   {        
     LQR_u = angle_control + gyro_control;  // 位移和速度控制分量被忽略
   }
@@ -730,7 +696,6 @@ void lqr_balance_loop()
   if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) //  && !sitting_down
   {
     LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
-    // Serial.println(LQR_u);
     angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
   }
   else
@@ -752,7 +717,7 @@ void lqr_balance_loop()
 void leg_loop()
 {
   // 如果坐下就不控制腿部
-  if(sitting_down){
+  if(sitting_down || jump_flag || jump_pre_flag || forward_jump_flag ){
     return; // 跳过后续leg高度计算
   }
 
@@ -767,8 +732,6 @@ void leg_loop()
     return; // 跳过后续leg高度计算
   }
 
-  if (jump_flag == 0) // 不处于跳跃状态
-  {
   ACC[0] = ACC[1] = 100; // max 230
   Speed[0] = Speed[1] = 400;
 
@@ -825,24 +788,27 @@ void leg_loop()
     Position[1] = SERVO1_MAX;
   
   sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
 }
+
+// 收缩小腿蓄能
+void jump_charge() // 腿部弯曲（蓄能）
+{
+    ACC[0] = 250;
+    ACC[1] = 250;
+    Speed[0] = 400;
+    Speed[1] = 400;
+    Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
+    Position[1] = (SERVO1_MIN  - 80); 
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
 }
 
 // 跳跃控制
 void jump_loop()
 { 
-  
   if ((wrobot.jump == 1) && (jump_pre_flag == 0))
   {
-   // 开始收缩腿部（储能）
+    jump_charge();
-    ACC[0] = 100;
-    ACC[1] = 100;
-    Speed[0] = 200;
-    Speed[1] = 200;
-    Position[0] = (SERVO0_MIN  + 80); // 不能是SERVO1_MIN值，防止腿部碰撞
-    Position[1] = (SERVO1_MIN  - 80); 
-    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
-    delay(5);
     jump_pre_flag =  1;
   }
 
@@ -863,7 +829,6 @@ void jump_loop()
       Position[0] = SERVO0_MAX + 20; // 极限距离
       Position[1] = SERVO1_MAX - 20;
       sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
-    
     }
   }
 
@@ -920,9 +885,10 @@ void yaw_loop()
     // 使用低波过滤器，平滑数据
     wrobot.height = lpf_height(wrobot.height);
 
-    // 增加灯光效果
+    // 增加rgb效果
-    updateBreathLED(abs(wrobot.joyx)); // 传入joyx绝对值，控制呼吸节奏
+    if(abs(wrobot.joyx) > 60){
-
+      handleLEDBlink();
+    }
   }
 
   float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; 
@@ -993,7 +959,6 @@ void resetToDefault() {
     YAW_output = 0; 
 
     last_lqr_speed = 0;
-    wrobot_move_stop_flag = 0;
     jump_flag = 0;
     leg_position_add = 0;
 
@@ -1032,35 +997,3 @@ void handleLEDBlink() {
   }
 }
 
-// 新增：呼吸灯核心逻辑封装函数（仅依赖全局状态变量，其他逻辑内部完成）
-void updateBreathLED(int joyx_abs) {
-   // 若正在执行原有闪烁（上键触发），不执行当前闪烁
-  if (isBlinking) return;
-
-  // 激活闪烁状态
-  isBreathing = true;
-
-  // 映射闪烁周期：joyx绝对值越大，周期越短（闪烁越快）
-  // 范围：joyx=10 → 500ms/次；joyx=100 → 100ms/次
-  unsigned long blinkPeriod = map(joyx_abs, 10, 100, 2000, 1000);
-  blinkPeriod = constrain(blinkPeriod, 2000, 1000); // 限制最大/最小周期
-
-  // 定时切换亮暗状态
-  unsigned long currentMillis = millis();
-  if (currentMillis - breathLastMillis >= blinkPeriod) {
-    breathLastMillis = currentMillis;
-
-    // 切换状态：亮→暗 或 暗→亮
-    static bool isOn = false; // 静态变量记录当前亮暗状态
-    isOn = !isOn;
-
-    // 设置LED状态：亮时蓝色，暗时熄灭
-    if (isOn) {
-      fill_solid(leds, NUM_LEDS, CRGB::Blue);
-      FastLED.setBrightness(200); // 固定亮度，避免渐变
-    } else {
-      fill_solid(leds, NUM_LEDS, CRGB::Black);
-    }
-    FastLED.show(); // 立即刷新LED
-  }
-}