增加重力感应，主打身段要柔软

README.md

@@ -55,3 +55,5 @@ B站演示地址：https://space.bilibili.com/25336365
            增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动。
 
 2025-10-19 v1.2版本优化前后左右跳跃功能，前后长时间倒地后自恢复功能。
+
+2025-10-30 v1.2增加重力感应，主打身段要柔软。

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

@@ -15,12 +15,12 @@ Modify by Tommy 2025
 
 
 /************函数申明*************/
-
 // 平衡算法函数
 void lqr_balance_loop();
 void yaw_loop();
 void leg_loop();
 void jump_loop();
+void obstacle_jump_loop();
 void resetZeroPoint();
 void jump_charge(); // 腿部弯曲（蓄能）
 
@@ -28,6 +28,9 @@ void jump_charge(); // 腿部弯曲（蓄能）
 void processControllerData(const XboxControllerNotificationParser& data);
 void runActionSequence();
 
+// 障碍检测函数
+void detectObstacle();
+
 const int deadZone = 10; // 摇杆死区阈值
 bool last_btnA = 0;
 bool last_btnB = 0;
@@ -83,6 +86,7 @@ SMS_STS sms_sts;
 // MPU6050实例
 MPU6050 mpu6050(I2Ctwo);
 
+
 /************参数定义*************/
 #define pi 3.1415927
 
@@ -112,6 +116,10 @@ s16 Position[2];
 u16 Speed[2];
 byte ACC[2];
 
+int last_servo_load = 0; // 上一时刻的舵机2负载
+unsigned long last_servo_sample_time = 0; // 上一次负载采样时间
+const unsigned long SERVO_SAMPLE_INTERVAL = 300; // 负载采样间隔（500毫秒）
+
 // 腿部高度逻辑
 // float leg_height_base 20 # 基准值
 // float LegHeightDiff ;  # 根据前后转向运动时，智能计算两腿高度差
@@ -137,6 +145,7 @@ float LQR_u = 0;
 float angle_zeropoint = 8;  // 5 设定默认俯仰角度，向前到正，向后倒负
 float original_angle_zeropoint = 8; // 保存原始的角度零点
 float distance_zeropoint =  0.5f; // 轮部位移零点偏置
+// float pitch_adjust = 0.0f; // 俯仰角度调整
 
 // YAW轴控制数据
 float YAW_gyro = 0;
@@ -152,18 +161,23 @@ const unsigned long SPEED_RECORD_INTERVAL = 100;  // 转速记录间隔(毫秒)
 float last_lqr_speed = 0;    // 记录上一时刻的轮部转速
 float robot_speed_diff = 0;    // 速度差
 
+
 int jump_flag = 0; // 跳跃过程计数
+int jump_pre_flag = 0; // 跳跃前蓄能计数
+
 int forward_jump_flag = 0; // 前跳跃标志位
 int back_jump_flag = 0; // 后跳跃标志位
 int left_jump_flag = 0; // 左跳跃标志位
 int right_jump_flag = 0; // 右跳跃标志位
-int jump_pre_flag = 0; // 跳跃前蓄能计数
-int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地
 
+int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地
 
 // 倒地恢复计数
 int fallRecoveryResetCounter = 0;
 
+// 快速下坠检测相关参数
+bool is_falling = false;         // 快速下坠状态标志
+
 // 小车状态控制
 // bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态
 // bool last_robot_enabled = true; // 记录上一次的小车状态
@@ -173,8 +187,9 @@ int fallRecoveryResetCounter = 0;
 bool robot_enabled = false; // 开机禁用平衡，默认坐下
 bool last_robot_enabled = false; // 同步初始状态
 int sitting_down = 1; // 开机直接进入“坐下状态”，避免无控制
+int stand_up_count = 0; // 站立标志位
 
-// 动作执行状态管理
+// 动作组执行状态管理
 bool isExecutingAction = false;
 bool isPausing = false;                
 unsigned long currentPauseDuration = 0; 
@@ -296,9 +311,12 @@ void loop()
   bat_check();            // 电压检测
   delayMicroseconds(1500);
   mpu6050.update();       // IMU数据更新
+
   lqr_balance_loop();     // lqr自平衡控制
+
   yaw_loop();             // yaw轴转向控制
   jump_loop();            // 跳跃动作
+
   leg_loop();             // 腿部动作控制
     
   runActionSequence();    // 执行动作序列 
@@ -332,29 +350,30 @@ void loop()
   // 达到平衡角度后，重置重置计数器
   if( abs(LQR_angle) < 25.0f){
     fallRecoveryResetCounter = 0;
+    stand_up_count = 0;
   }
-
+  // Serial.println(String(LQR_angle));
 
   // 向左跳跃
-  if ( left_jump_flag > 2 && left_jump_flag < 20) {
+  if ( left_jump_flag > 2) {
     motor1.target = 5.0; // 脉冲
     motor2.target = -5.0;
   } 
 
   // 向右跳跃
-  if ( right_jump_flag > 2 && right_jump_flag < 20) {
+  if ( right_jump_flag > 2) {
     motor1.target = -5.0; // 脉冲
     motor2.target = 5.0;
   } 
 
   // 向前跳跃
-  if ( forward_jump_flag > 2 && forward_jump_flag < 20) {
+  if ( forward_jump_flag > 2) {
     motor1.target = 2.3; // 脉冲
     motor2.target = 2.3;
   } 
 
   // 向后跳跃
-  if ( back_jump_flag > 2 && back_jump_flag < 15) {
+  if ( back_jump_flag > 2 ) {
     motor1.target = -1.0; // 脉冲
     motor2.target = -1.0;
   } 
@@ -442,9 +461,13 @@ void lqr_balance_loop()
   LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量
   LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s）
   LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°）
+  
+  // Serial.print("LQR_angle:");
+  // Serial.println(LQR_angle);
+  
   LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
 
-  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
+  angle_control = pid_angle(LQR_angle - angle_zeropoint); // pitch_adjust 
   gyro_control = pid_gyro(LQR_gyro);
 
   // 前进后退跳跃的系数都不一样
@@ -468,23 +491,22 @@ void lqr_balance_loop()
 
     if(robot_speed_diff > 18.0) // 轮子离地
     {
-      wheel_ground_flag = 0; // 轮子离地标记
+      // wheel_ground_flag = 0; // 轮子离地标记
       //  Serial.print("TAKE OFF");
       // Serial.print(wheel_ground_flag);
       // Serial.print(",robot_speed_diff:");
-      // Serial.print(robot_speed_diff);
+      // Serial.println(robot_speed_diff);
       // Serial.print(",jump_flag:");
       // Serial.println(jump_flag); // 100 左右
     }
     if( robot_speed_diff < -9.0) // 轮子着地
     {
-      wheel_ground_flag = 1; // 轮子着地标记
+      // wheel_ground_flag = 1; // 轮子着地标记
       // Serial.print("LANDING:");
       // Serial.print(",robot_speed_diff:");
-      // Serial.print(robot_speed_diff);
+      // Serial.println(robot_speed_diff);
       // Serial.print(",jump_flag:");
       // Serial.println(jump_flag); // 120左右
-      
       if( jump_flag ){ // 落地点作为新的位移零点
          resetZeroPoint(); 
       }
@@ -550,12 +572,39 @@ void lqr_balance_loop()
 void leg_loop()
 {
   // 如果坐下就不控制腿部
-  if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag){ 
+  if(sitting_down 
+    || jump_flag || jump_pre_flag 
+    || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag){ 
     return; // 跳过后续leg高度计算
   }
 
+  // 1. 获取并处理MPU6050 Z轴加速度
+  float z_accel = mpu6050.getAccZ(); // 1.1~1.2左右
 
-  // 机身大角度，譬如倒地时，强制腿部收缩
+  // 2. 快速下坠检测与处理
+  if (z_accel >1.8f && !is_falling && stand_up_count == 0) {
+    is_falling = true;
+    // 腿部收缩
+    ACC[0] = 250;
+    ACC[1] = 250;
+    Speed[0] = 1000;
+    Speed[1] = 1000;
+    Position[0] = SERVO0_MIN + 150;
+    Position[1] = SERVO1_MIN - 150;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    return; // 跳过后续正常高度计算
+  }
+
+  // 3. 下坠结束判断与恢复逻辑
+  if (is_falling) {
+    if (!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])) {
+      is_falling = false;
+    }else{
+       return;
+    }
+  }
+
+  // 机身大角度倒地时，强制腿部收缩
   if (abs(LQR_angle) > 25.0f) 
   {
     // 默认舵机速度和加速度
@@ -569,6 +618,7 @@ void leg_loop()
     return; // 跳过后续leg高度计算
   }
 
+   // 正常腿部高度计算
   ACC[0] = wrobot.acc0;    // 强转为byte（舵机库要求）
   ACC[1] = wrobot.acc1;
   Speed[0] = wrobot.speed0; // 强转为u16（舵机库要求）
@@ -648,28 +698,28 @@ void jump_loop()
       if( left_jump_flag ){ 
         left_jump_flag ++ ;
         
-        if( left_jump_flag >= 20 ){ 
+        if( left_jump_flag > 20 ){ 
           left_jump_flag = 0;
         }
 
       }else if( right_jump_flag ){
         right_jump_flag ++ ;
         
-        if( right_jump_flag >= 20 ){
+        if( right_jump_flag > 20 ){
           right_jump_flag = 0;
         }
 
       }else if( forward_jump_flag ){
         forward_jump_flag ++ ;
         
-        if(forward_jump_flag >= 15){ // 向前跳跃只有20个脉冲
+        if(forward_jump_flag > 20){ // 向前跳跃只有20个脉冲
           forward_jump_flag = 0;
         }
 
       }else if( back_jump_flag ){
         back_jump_flag ++ ;
         
-        if(back_jump_flag >= 20){
+        if(back_jump_flag > 15){
           back_jump_flag = 0;
         }
 
@@ -803,7 +853,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     return; 
   }
 
-
   // 坐下
   if (remoteData.btnLB && !last_btnLB && robot_enabled) { 
     robot_enabled = false;
@@ -820,6 +869,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   if (remoteData.btnRB && !robot_enabled) {
     robot_enabled = true;
     sitting_down = 0; // 清除坐下动作标记
+    stand_up_count ++; // 站立标志位
   }
    last_btnRB = remoteData.btnRB;
 
@@ -880,6 +930,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
     if(remoteData.btnDirUp) { 
       leg_height_base -= 0.20;
     }
+
     // 下降
     if(remoteData.btnDirDown) { 
       leg_height_base += 0.20;
@@ -957,6 +1008,15 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
       XboxtrigRTVibration();
   }
 
+  // 舵机中位校准 中间十字左右键同时按下
+  if (remoteData.btnSelect && !last_btnSelect && remoteData.btnStart && !last_btnStart) { 
+      sms_sts.CalibrationOfs(ID[0]);
+      sms_sts.CalibrationOfs(ID[1]);
+      XboxKeyVibration(100,1000); // 震动
+      Serial.println("CalibrationOfs");
+  }
+  last_btnSelect = remoteData.btnSelect;
+  last_btnStart = remoteData.btnStart;
 }
 
 // 执行连续动作的函数