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

v1.2/Shibo-main-TOMMY/src/XboxBluetooth.h

@@ -23,8 +23,7 @@ enum VibrateState {
 };
 VibrateState vibrateState = VIBRATE_OFF;
 unsigned long vibrateStartMs = 0;  // 震动开始的时间戳
-const unsigned long KEY_VIBRATE_DURATION = 200;  // 按键震动持续时间
-const unsigned long TRIGGER_VIBRATE_DURATION = 300;  // 扳机震动持续时间
+unsigned long long xbox_vibrate_duration = 200;  // 按键震动持续时间 （可被函数动态更新）
 
 /************ 外部函数声明 *************/
 extern void processControllerData(const XboxControllerNotificationParser& data);
@@ -41,9 +40,9 @@ void updateXboxVibration() {
   unsigned long currentMs = millis();
   
   // 处理按键震动的"开启→等待→关闭"流程
-  if (vibrateState == VIBRATE_STARTED) {
+  if (vibrateState == VIBRATE_STARTED || vibrateState == VIBRATE_TRIGGER) {
     // 检查是否达到震动持续时间
-    if (currentMs - vibrateStartMs >= KEY_VIBRATE_DURATION) {
+    if (currentMs - vibrateStartMs >= xbox_vibrate_duration) {
       // 执行原函数中delay后的关闭逻辑
       repo.v.power.center = 0;
       repo.v.power.shake = 0;
@@ -51,19 +50,17 @@ void updateXboxVibration() {
       vibrateState = VIBRATE_OFF;  // 回到未震动状态
     }
   }
-  // 处理扳机键震动的关闭逻辑
-  else if (vibrateState == VIBRATE_TRIGGER) {
-    if (currentMs - vibrateStartMs >= TRIGGER_VIBRATE_DURATION) {
-      repo.v.power.center = 0;
-      repo.v.power.shake = 0;
-      xboxController.writeHIDReport(repo);
-      vibrateState = VIBRATE_OFF;
-    }
-  }
 }
 
 // 按键震动（完整复现原逻辑的非阻塞版本）
-void XboxKeyVibration() {
+void XboxKeyVibration(uint8_t power = 20, unsigned long duration = 200) {
+
+  power = constrain(power, 0, 100); // 约束功率范围（防止无效值，确保在0-100之间）
+  duration = constrain(duration, 50, 2000);  // 时长限制50-2000ms（合理范围）
+
+  // 核心：将传入的时长（默认200或自定义）赋值给全局变量
+  xbox_vibrate_duration = duration;
+
   // 若正在震动，先重置状态（避免重复触发导致混乱）
   if (vibrateState != VIBRATE_OFF) {
     repo.v.power.center = 0;
@@ -77,8 +74,8 @@ void XboxKeyVibration() {
   repo.v.select.left = true;
   repo.v.select.right = true;
   repo.v.select.shake = true;
-  repo.v.power.center = 20; // 30% power
-  repo.v.power.shake = 20;
+  repo.v.power.center = power; // 20% power
+  repo.v.power.shake = power;
   repo.v.timeActive = 10;  // x/100 second
   xboxController.writeHIDReport(repo);
   
@@ -88,10 +85,13 @@ void XboxKeyVibration() {
 }
 
 // 扳机键震动（同样保留完整时序）
-void XboxtrigRTVibration() {
+void XboxtrigRTVibration( unsigned long duration = 300) {
   static uint16_t TrigMax = XboxControllerNotificationParser::maxTrig;
   uint8_t power = (uint8_t)((float)xboxController.xboxNotif.trigRT / TrigMax * 100) * 0.5;
   
+  // 核心：将传入的时长（默认300或自定义）赋值给全局变量
+  xbox_vibrate_duration = duration;
+
   // 若扳机键松开，直接关闭震动
   if (power == 0) {
     repo.v.power.center = 0;
@@ -121,13 +121,13 @@ void process_xbox_notif() {
   xboxController.onLoop();
   if (xboxController.isConnected() && !xboxController.isWaitingForFirstNotification()) {
     if(!wasConnected){ // 原来未链接，现在链接成功
-        XboxKeyVibration();
+        XboxKeyVibration(100,1000);
         wasConnected = 1;
     }
     processControllerData(xboxController.xboxNotif);
   }else{
      if(wasConnected){ // 原来是链接成功的，说明断了
-        XboxKeyVibration();
+        XboxKeyVibration(80,1000);
         wasConnected = 0;
     }
   }

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更新一次历史转速