修复重力感应相关bug

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

@@ -64,7 +64,7 @@ MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
 MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
 
 // PID控制器实例
-PIDController pid_angle(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
+PIDController pid_pitch(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
 PIDController pid_gyro(0.06, 0, 0, 100000, 8); 
 PIDController pid_distance(0.5, 0, 0, 100000, 8);
 PIDController pid_speed(0.7, 0, 0, 100000, 8);
@@ -92,9 +92,11 @@ MPU6050 mpu6050(I2Ctwo);
 
 // 机身平衡角度调整 roll轴
 float roll_adjust = 0.0f;  
+float original_roll_adjust = roll_adjust;  
 
 // 机身高度
 float leg_height_base = 20.0f;  // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
+float original_leg_height_base = leg_height_base ; 
 
 #define SERVO0_MIN  2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
 #define SERVO1_MIN  2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
@@ -142,8 +144,8 @@ float gyro_control = 0;
 float speed_control = 0;
 float distance_control = 0;
 float LQR_u = 0;
-float angle_zeropoint = 8;  // 5 设定默认俯仰角度，向前到正，向后倒负
+float pitch_zeropoint = 7;  // 8 设定默认俯仰角度，向前到正，向后倒负
-float original_angle_zeropoint = 8; // 保存原始的角度零点
+float original_pitch_zeropoint = pitch_zeropoint; // 保存原始的角度零点
 float distance_zeropoint =  0.5f; // 轮部位移零点偏置
 // float pitch_adjust = 0.0f; // 俯仰角度调整
 
@@ -350,7 +352,6 @@ void loop()
   // 达到平衡角度后，重置重置计数器
   if( abs(LQR_angle) < 25.0f){
     fallRecoveryResetCounter = 0;
-    stand_up_count = 0;
   }
   // Serial.println(String(LQR_angle));
 
@@ -378,6 +379,14 @@ void loop()
     motor2.target = -1.0;
   } 
 
+  // 站立计数
+  if( stand_up_count > 0){
+    stand_up_count ++;
+  }
+  if (stand_up_count > 500){
+    stand_up_count = 0;
+  }
+  
   // 小车坐下
   if (sitting_down) {
     sitting_down ++;
@@ -432,7 +441,7 @@ void lqr_balance_loop()
   LQR_u = angle_control + gyro_control + distance_control + speed_control
           = 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
 
-    angle_control = pid_angle(  LQR_angle       -   angle_zeropoint) // 实际pitch角度-目标pitch角度
+    angle_control = pid_pitch(  LQR_angle       -   pitch_zeropoint) // 实际pitch角度-目标pitch角度
                                 LQR_angle = mpu6050.getAngleY() 
     
     gyro_control = pid_gyro(  LQR_gyro - 0   ) // 实际角速度，目标值是零
@@ -467,7 +476,7 @@ void lqr_balance_loop()
   
   LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
 
-  angle_control = pid_angle(LQR_angle - angle_zeropoint); // pitch_adjust 
+  angle_control = pid_pitch(LQR_angle - pitch_zeropoint); // pitch_adjust 
   gyro_control = pid_gyro(LQR_gyro);
 
   // 前进后退跳跃的系数都不一样
@@ -551,7 +560,7 @@ 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); // 小转矩非线性补偿
-    angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
+    pitch_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
   }
   else
   {
@@ -561,6 +570,7 @@ void lqr_balance_loop()
   // 平衡控制参数自适应
   if (wrobot.height < 50){
     pid_speed.P = 0.7;
+    // pid_speed.P = 0.65;
   }else if (wrobot.height < 64){
     pid_speed.P = 0.6;
   }else{
@@ -581,8 +591,12 @@ void leg_loop()
   // 1. 获取并处理MPU6050 Z轴加速度
   float z_accel = mpu6050.getAccZ(); // 1.1~1.2左右
 
-  // 2. 快速下坠检测与处理
+  // 2. 触发重力感应
-  if (z_accel >1.8f && !is_falling && stand_up_count == 0) {
+  if (z_accel >1.8f && !is_falling 
+    && stand_up_count == 0
+    && roll_adjust == original_roll_adjust
+    && leg_height_base == original_leg_height_base
+  ) {
     is_falling = true;
     // 腿部收缩
     ACC[0] = 250;
@@ -595,7 +609,7 @@ void leg_loop()
     return; // 跳过后续正常高度计算
   }
 
-  // 3. 下坠结束判断与恢复逻辑
+  // // 3. 下坠结束判断与恢复逻辑
   if (is_falling) {
     if (!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])) {
       is_falling = false;
@@ -938,8 +952,8 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
 
     // 还原
      if (remoteData.btnLS) {
-      roll_adjust = 0.0f; 
+      roll_adjust = original_roll_adjust; 
-      leg_height_base = 20.0f;
+      leg_height_base = original_leg_height_base;
     }
 
     //  右扳机 高度向下 ：原始0-1023