增加原地快速旋转

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

@@ -1,6 +1,6 @@
 /* ---------------------------------
 Copyright (c) 2024 Mu Shibo
- Modify by Tommy 2025.8.11
+Modify by Tommy 2025.10.08
 ------------------------------------- */
 #include <Arduino.h>
 #include <esp_now.h>
@@ -26,9 +26,9 @@ void leg_loop();
 void jump_loop();
 void resetToDefault();
 
-// 警灯相关函数
+// RGB相关函数
 void handleLEDBlink();
-
+void updateBreathLED(int joyx_abs); // 呼吸灯控制函数声明
 /************实例定义*************/
 
 // 电机实例
@@ -67,8 +67,8 @@ SMS_STS sms_sts;
 MPU6050 mpu6050(I2Ctwo);
 
 // FastLED配置
+#define RGB_PIN 21
 #define NUM_LEDS 2
-#define DATA_PIN 21
 CRGB leds[NUM_LEDS];
 unsigned long previousBlinkMillis = 0;
 int blinkState = 0;
@@ -76,6 +76,13 @@ 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
 
@@ -95,7 +102,7 @@ float LegHeightDiff = 0; // 左右腿高度差
 int lastPosition0 = SERVO0_MIN;
 int lastPosition1 = SERVO1_MIN;
 
-// 舵机方法参数
+// 舵机运动参数
 byte ID[2] = {1, 2};  
 s16 Position[2];
 u16 Speed[2];
@@ -193,8 +200,7 @@ typedef struct {
   int joyLeftY; // 左摇杆Y轴（上-下，940-0 ，中心默认：425 ）
   int joyRightX; // 右摇杆X轴（左-右，0-960：中心默认：487 ）
   int joyRightY; // 右摇杆Y轴（上-下，1080-0，中心默认：525 ）
-  // 12个按键（布尔值：1=按下，0=未按下）
+  bool btnJoyLeft; // 左摇杆按压键 1=按下，0=未按下
-  bool btnJoyLeft; // 左摇杆按压键
   bool btnJoyRight; // 右摇杆按压键
   bool btnStart; // Start键
   bool btnBack; // Back键
@@ -229,11 +235,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   }
   // 起立 start键
   if (remoteData.btnStart && !robot_enabled) {
-    // 先使能舵机
-    sms_sts.EnableTorque(ID[0], 1);
-    sms_sts.EnableTorque(ID[1], 1);
-    delay(10);  // 等待舵机响应
-
     robot_enabled = true;
     sitting_down = 0; // 清除坐下动作标记
     resetToDefault(); // 重置参数准备起立
@@ -332,15 +333,13 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   //   wrobot.height = 0;
   // }
 
-  // 智能计算wrobot.height 根据前后速度 输出高度
+  // 根据前后速度，智能计算wrobot.height 
   // convertedJ2PotY 范围-100到100，默认0，输出范围20-50,默认 30
-  // 1. 取输入的绝对值，统一处理正负方向
+  // 反向映射：绝对值30→60，绝对值100→0（线性过渡）
-  int j2Y_abs = abs(convertedJ2PotY);
+  wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30);
-  // 2. 反向映射：绝对值30→60，绝对值100→0（线性过渡）
+  // 强制约束输出在0~60范围内（防止异常值）
-  wrobot.height = map(j2Y_abs, 0, 100, 50, 30);
-  // 3. 强制约束输出在0~60范围内（防止异常值）
   wrobot.height = constrain(wrobot.height, 30, 50);
-  // 4. 使用低波过滤器，平滑数据
+  // 使用低波过滤器，平滑数据
   wrobot.height = lpf_height(wrobot.height);
 
   // if(abs(convertedJ2PotX) > deadZone) {
@@ -442,7 +441,7 @@ void setup()
   resetToDefault(); // 重置到默认参数状态
 
   // FastLED初始化
-  FastLED.addLeds<WS2812, DATA_PIN, GRB>(leds, NUM_LEDS);
+  FastLED.addLeds<WS2812, RGB_PIN, GRB>(leds, NUM_LEDS);
   FastLED.setBrightness(50); // 设置亮度
   fill_solid(leds, NUM_LEDS, CRGB::Black); // 初始化为熄灭
   FastLED.show();
@@ -477,12 +476,6 @@ void loop()
   motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
   motor2.target = (-0.5) * (LQR_u - YAW_output);
 
-  // 当不前进后退，只是原地打转时，线性提高转向速度
-  if(abs(wrobot.joyy) < 5 && abs(wrobot.joyx) > 10){
-    float speed_gain = map(abs(wrobot.joyx), 0, 100, 1.0, 2.5); 
-    motor1.target = motor1.target * speed_gain;
-    motor2.target = motor2.target * speed_gain;
-  }
 
   // 小车倒地后，重置所有参数 测试
   loopCounter++;
@@ -521,12 +514,10 @@ void loop()
   // 向前跳跃
   if(forward_jump_flag){
     forward_jump_flag++;
-    if (forward_jump_flag < 25) {
+    if (forward_jump_flag < 20) {
       // 保持向前脉冲
-      // motor1.target = 1.8;
+      motor1.target = 1.8;
-      // motor2.target = 1.8;
+      motor2.target = 1.8;
-      motor1.target = 2.0;
-      motor2.target = 2.0;
     } else {
       forward_jump_flag = 0;
       wrobot.jump = 1; // 跳跃
@@ -593,15 +584,11 @@ void loop()
 
     // 收缩双腿，舵机到最低位置
     if (sitting_down == 5) {
-      ACC[0] = ACC[1] = 200;
+      ACC[0] = ACC[1] = 150;
-      Speed[0] = Speed[1] = 300;
+      Speed[0] = Speed[1] = 200;
       Position[0] = SERVO0_MIN;
       Position[1] = SERVO1_MIN;
       sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
-    }else if (sitting_down == 6) {
-        // 失能舵机1和2（关闭扭矩）
-        sms_sts.EnableTorque(ID[0], 0);  // ID[0]为1
-        sms_sts.EnableTorque(ID[1], 0);  // ID[1]为2
     }
    
   }
@@ -791,7 +778,7 @@ void leg_loop()
     ACC[0] = ACC[1] = 100; // max 230
     Speed[0] = Speed[1] = 400;
 
-    float roll_angle = (float)mpu6050.getAngleX() + 5.0; // 实测偏差roll角度偏差
+    float roll_angle = (float)mpu6050.getAngleX() ; // + 5.0 实测偏差roll角度偏差
     leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
 
     // 左后腿默认等于 wrobot.height
@@ -921,15 +908,37 @@ void yaw_loop()
     return;
   }
 
-  // YAW_output = 0.03*(YAW_Kp*yaw_target + YAW_Kd*YAW_gyro);
+ // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数
-  // 直接用wrobot.joyx作为转向目标，避免累加式角度积分，导致松开后仍有输出
+  float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; 
-  float yaw_target = wrobot.joyx * 0.1; 
+  coeff = constrain(coeff, 0.1f, 1.0f); 
-  float yaw_angle_control = pid_yaw_angle(yaw_target); 
+  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); 
+    // 根据转向速度，智能计算wrobot.height 
+    // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
+    // 反向映射：绝对值30→60，绝对值100→0（线性过渡）
+    wrobot.height = map(abs(wrobot.joyx), 0, 100, 50, 30);
+    // 强制约束输出在0~60范围内（防止异常值）
+    wrobot.height = constrain(wrobot.height, 30, 50);
+    // 使用低波过滤器，平滑数据
+    wrobot.height = lpf_height(wrobot.height);
+
+    // 增加灯光效果
+    updateBreathLED(abs(wrobot.joyx)); // 传入joyx绝对值，控制呼吸节奏
+
+  }else{
+     yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 1.5); 
+  }
+  float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; 
+  // 限制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;
-
 }
 
 // 电压检测初始化
@@ -1028,4 +1037,37 @@ 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
+  }
 }