更新打印件

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

@@ -82,10 +82,10 @@ CRGB blinkColors[6] = {CRGB::Red, CRGB::Red, CRGB::Yellow, CRGB::Yellow, CRGB::G
 
 // 舵机位置限制参数
 #define LEG_HEIGHT_BASE 20  //20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
-#define SERVO0_MIN  2048 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
-#define SERVO1_MIN  2048 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
-#define SERVO0_MAX (2048 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 
-#define SERVO1_MAX (2048 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
+#define SERVO0_MIN  2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
+#define SERVO1_MIN  2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
+#define SERVO0_MAX (2047 + 12 + 8.4 * (35 + 10)) // 2438 舵机1最高 
+#define SERVO1_MAX (2047 - 12 - 8.4 * (35 + 10)) // 1658 舵机2最高
 float leg_position_add = 0;    // roll轴平衡控制量
 int left_height;
 int right_height;
@@ -102,10 +102,10 @@ byte ACC[2];
 // float LegHeightDiff ;  # 根据前后转向运动时，智能计算两腿高度差
 // left_height，right_height  = wrobot.height + LegHeightDiff  # 根据前后左右智能计算两腿高度差
 // leg_position_add = pid_roll_angle(lpf_roll(roll_angle)); 根据roll自动计算
-// Position[0] 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
-// Position[1] 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
-// Position 2048 - 12 - 8.4 * (right_height                              - LEG_HEIGHT_BASE) - leg_position_add;
-// Position 2048 - 12 - 8.4 * (wrobot.height + LegHeightDiff  - LEG_HEIGHT_BASE) -  pid_roll_angle(lpf_roll(roll_angle));
+// Position[0] 2047 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+// Position[1] 2047 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+// Position 2047 - 12 - 8.4 * (right_height                              - LEG_HEIGHT_BASE) - leg_position_add;
+// Position 2047 - 12 - 8.4 * (wrobot.height + LegHeightDiff  - LEG_HEIGHT_BASE) -  pid_roll_angle(lpf_roll(roll_angle));
 //                              遥控器          两腿高度差             固定值               roll轴平衡控制量
 
 // LQR自平衡控制器参数
@@ -143,6 +143,7 @@ int wheel_ground_flag = 1; // 轮子是否离地标记 默认着地
 
 // 定义摇杆数值死区参数
 const int deadZone = 10;
+int wrobot_move_stop_flag = 0; // 摇杆没有前后左右运动指令，默认存在摇杆指令
 
 // 电压检测
 #define LED_BAT 13
@@ -493,7 +494,7 @@ void loop()
   }
 
   // target目标电压 0~8V, 0为不转, 8为最大转速
-  // Serial.print("LQR_angle:"); //  屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
+  // Serial.print("LQR_angle:"); //  屁股向后倒 -33~-34度,舵机2047的时候-42度，头向前到底 75度，完全垂直88度
   // Serial.print(LQR_angle);
   // Serial.print(",loopCounter:");
   // Serial.print(loopCounter);
@@ -619,7 +620,8 @@ void lqr_balance_loop()
   // 前进后退跳跃的系数都不一样
   float speed_target_coeff = 0.1; 
   if( jump_flag ){ // 跳跃
-    speed_target_coeff = 0.12;
+    // speed_target_coeff = 0.12;
+    speed_target_coeff = 0.1;
   }else if(wrobot.joyy > 0 ){ // 前进
     speed_target_coeff = 0.18;
   }else if(wrobot.joyy < 0){ // 后退 
@@ -662,15 +664,34 @@ void lqr_balance_loop()
       // Serial.print(robot_speed_diff);
       // Serial.print(",jump_flag:");
       // Serial.println(jump_flag); // 120左右
+      if( jump_flag ){ // 落地点作为新的位移零点
+         distance_zeropoint = LQR_distance;
+         pid_lqr_u.error_prev = 0; 
+      }
     }
     last_lqr_speed = LQR_speed;  // 每隔100ms更新一次历史转速
     last_speed_record_time = current_time;  // 更新记录时间
   }
    
 
+  // 判断摇杆是否没有前后左右运动指令
+  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
+  {
+    wrobot_move_stop_flag = 1; 
+  }
+  // 没有摇杆指令，小车微微移动，需要重置位移零点，让小车自己回来
+  if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5)) 
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    // pid_lqr_u.error_prev = 0;  // 输出积分清零 如果有这个会前后漂移
+    wrobot_move_stop_flag = 0;
+  }
+  
   // 实时重置位移零点和积分情形
+  // if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) 
+  //   || jump_flag > 0 || wrobot.joyy != 0 || sitting_down ) 
   if (abs(robot_speed_diff) > 10 || (abs(LQR_speed) > 15) 
-    || jump_flag > 0 || wrobot.joyy != 0 || sitting_down ) 
+    || wrobot.joyy != 0 || sitting_down ) 
   {
     distance_zeropoint = LQR_distance;
     pid_lqr_u.error_prev = 0;  // 输出积分清零
@@ -764,10 +785,10 @@ void leg_loop()
 
   // 根据新的高度计算舵机位置
   // id[0] 正面看右手，id[1] 正面看左手
-  // 2048是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
+  // 2047是舵机中间位置，8.4是系数，32是默认高度，最高80，最低32
   // const int LEG_HEIGHT_BASE   这值越小，机身越高，实测范围 0（最高），52（最低）
-  Position[0] = 2048  + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
-  Position[1] = 2048  - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+  Position[0] = 2047  + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+  Position[1] = 2047  - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
 
   // Serial.print("roll_angle:");
   // Serial.print(roll_angle);