优化倒地自平衡和手柄震动反馈功能

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

@@ -164,9 +164,8 @@ int jump_pre_flag = 0; // 跳跃前蓄能计数
 int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地
 
 
-// 倒地起来辅助计数
-int loopCounter = 0; // loop函数执行计数
-int resetToDefaultCounter = 0; // resetToDefault执行计数
+// 倒地恢复计数
+int fallRecoveryResetCounter = 0;
 
 // 小车状态控制
 // bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
@@ -314,16 +313,20 @@ void loop()
   motor1.target = (-0.5) * (LQR_u + YAW_output); // target正数小车向前，负数向后
   motor2.target = (-0.5) * (LQR_u - YAW_output);
 
-  loopCounter++;
-  // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
-  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
-  if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
-    resetZeroPoint();
-    resetToDefaultCounter++;
-    motor1.target = 0;
-    motor2.target = 0;
+  // fallRecoveryResetCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
+  // 向后倒地-31度，向前倒地（带支架）52度
+  if ( ( 
+          (LQR_angle < -30.0f  && fallRecoveryResetCounter < 130) // 向后倒地
+          || (LQR_angle > 48.0f && fallRecoveryResetCounter < 500) // 向前到底
+        )   
+     && !sitting_down && robot_enabled && wrobot.joyy == 0 ) {
+          resetZeroPoint();
+          fallRecoveryResetCounter++;
+          motor1.target = 0;
+          motor2.target = 0; 
   }
 
+
   // 大仰角到底，轮子就不要转了 修改
   if (abs(LQR_angle) > 120.0f) {
       motor1.target = 0;
@@ -332,7 +335,7 @@ void loop()
 
   // 达到平衡角度后，重置重置计数器
   if( abs(LQR_angle) < 25.0f){
-    resetToDefaultCounter = 0;
+    fallRecoveryResetCounter = 0;
   }
 
   // 向左跳跃
@@ -379,7 +382,7 @@ void loop()
     }else{
 
       // 通过简单的开环控制前后左右
-      float speed_coeff = 0.02; // 速度系数：值越大，移动越快（建议0.01~0.02）
+      float speed_coeff = 0.04; // 速度系数：值越大，移动越快
       motor1.target = (-0.5) * (wrobot.joyy +  wrobot.joyx) * speed_coeff; 
       motor2.target = (-0.5) * (wrobot.joyy -  wrobot.joyx) * speed_coeff;
 
@@ -489,7 +492,7 @@ void lqr_balance_loop()
          resetZeroPoint(); 
       }
       if( jump_flag == 0){
-         XboxKeyVibration();  // 大运动状态手柄震动
+         XboxKeyVibration();  // 大减速运动状态手柄震动
       }
     }
     last_lqr_speed = LQR_speed;  // 每隔100ms更新一次历史转速