实现开机后，小车自动起立功能。

Shibo-main-V4-ESPNOW/.vscode/settings.json

@@ -3,6 +3,7 @@
     "files.associations": {
         "istream": "cpp",
         "ostream": "cpp",
-        "algorithm": "cpp"
+        "algorithm": "cpp",
+        "iterator": "cpp"
     }
 }

Shibo-main-V4-ESPNOW/README.md

@@ -1,8 +1,5 @@
 此项目是世博同学轮足机器人的Arduino工程转换而成的PlatformIO工程
-世博同学轮足机器人项目地址：https://github.com/MuShibo/Micro-Wheeled_leg-Robot
 
-优化内容：
+可以使用vscode进行编译下载，解决了Arduino编译巨慢的问题。
-同时大量优化了小车平衡性能，增加左右摇摆功能。
+
-可以通过开源的esp32遥控器进行遥控，无需联网，更加随意。
+世博同学轮足机器人项目地址：https://github.com/MuShibo/Micro-Wheeled_leg-Robot
-优化了轮子，更加大，稳定性杠杠的，stl文件已经开源在.stl文件夹中。
-By Tommy

Shibo-main-V4-ESPNOW/src/main.cpp

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // Copyright (c) 2024 Mu Shibo
-// Modify by Tommy 2025.8.9
+// Modify by Tommy 2025.8.11
 // -----------------------------------------------------------------------------
 
 #include <esp_now.h>
@@ -69,8 +69,8 @@ float gyro_control = 0;
 float speed_control = 0;
 float distance_control = 0;
 float LQR_u = 0;
-float angle_zeropoint = 5;  // 3.4 在理想平衡状态下俯仰角度，向前到正，向后倒负
+float angle_zeropoint = 5;  // 俯仰角度，向前到正，向后倒负
-float distance_zeropoint = -256.0; // 轮部位移零点偏置（-256为一个不可能的位移值，将其作为未刷新的标志）
+float distance_zeropoint = 1; // 轮部位移零点偏置，默认1，可以通电后就自动起立
 
 // YAW轴控制数据
 float YAW_gyro = 0;
@@ -107,11 +107,9 @@ static const adc_unit_t unit = ADC_UNIT_1;
 // 电量显示LED
 #define LED_BAT 13
 
-
 // 定义死区参数
 const int deadZone = 15;
 
-
 typedef struct {
   byte j1PotX;  // 左摇杆X  (0-255)
   byte j1PotY;  // 左摇杆Y  (0-255)
@@ -245,7 +243,6 @@ void setup()
   Position[1] = 2048;
   // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
   sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
-  delay(500);
 
   // 电压检测
   adc_calibration_init();
@@ -331,10 +328,10 @@ void loop()
   //  Serial.println(LQR_angle)
 
   // 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了
-  // if(LQR_angle < -40.0f ||  LQR_angle > 55.0f || jump_flag > 1  ) {
+  if(LQR_angle < -40.0f ||  LQR_angle > 55.0f ) {
-  //   motor1.target = 0;
+    motor1.target = 0;
-  //   motor2.target = 0;
+    motor2.target = 0;
-  // }
+  }
 
   // 迭代计算FOC相电压
   motor1.loopFOC();
@@ -346,6 +343,7 @@ void loop()
 } // -----------------  void loop()
 
 // lqr自平衡控制
+// uint8_t k=0;
 void lqr_balance_loop()
 {
   // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
@@ -367,16 +365,20 @@ void lqr_balance_loop()
   LQR_gyro = (float)mpu6050.getGyroY(); // pitch 角速度
 
   // 如果刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
-  // LQR_distance 和 LQR_speed都是零
+  // LQR_distance 和 LQR_speed趋近于0
-  // Serial.print("LQR_distance:");
+  // if(k==100)
+  // {
+  //   k=0;
+  //   Serial.print("distance:");
   //   Serial.print(LQR_distance);
-  // Serial.print(",LQR_speed:");
+  //   Serial.print(",speed:");
   //   Serial.print(LQR_speed);
-  // Serial.print(",LQR_angle:");
+  //   Serial.print(",angle:");
   //   Serial.print(LQR_angle);
-  // Serial.print(",LQR_gyro:");
+  //   Serial.print(",gyro:");
-  // Serial.print(LQR_gyro);
+  //   Serial.println(LQR_gyro);
-
+  // }
+  // k++;
 
   // 计算pitch的角度和角速度的pid控制值
   // 实际pitch角度与零点的差值，目标值是零
@@ -467,15 +469,21 @@ void lqr_balance_loop()
     pid_speed.P = 0.5;
   }
 
-  // 平衡控制参数自适应
+  // if(k==100)
-// if (wrobot.height >= 32 && wrobot.height <= 80) { // 假设有效高度范围32-80cm
+  // {
-//     // 线性插值公式：P = P_max - (height - height_min)*(P_max - P_min)/(height_max - height_min)
+  //   k=0;
-//     pid_speed.P = 0.7f - (wrobot.height - 32.0f) * (0.7f - 0.5f) / (80.0f - 32.0f);
+  //   Serial.print("LQR_u:"); //LQR_u = angle_control + gyro_control + distance_control + speed_control
-//     // pid_speed.P = 0.5f + 0.2f * exp(-0.05f * (wrobot.height - 32.0f)); // 非线性曲线
+  //   Serial.print(LQR_u);
-//     pid_speed.P = constrain(pid_speed.P, 0.5f, 0.7f); // 限制在0.5-0.7范围
+  //   Serial.print(",angle:");
-// } else {
+  //   Serial.print(angle_control);
-//     pid_speed.P = 0.5f; // 超出范围使用默认值
+  //   Serial.print(",gyro:");
-// }
+  //   Serial.print(gyro_control);
+  //   Serial.print(",distance:");
+  //   Serial.print(distance_control);
+  //   Serial.print(",speed:");
+  //   Serial.println(speed_control);
+  // }
+  // k++;
 
 }
 
@@ -501,10 +509,8 @@ void leg_loop()
     ACC[1] = 8;
 
     // 舵机速度
-    // Speed[0] = 200;
+    Speed[0] = 300;
-    // Speed[1] = 200;
+    Speed[1] = 300;
-    Speed[0] = 350;
-    Speed[1] = 350;
     float roll_angle = (float)mpu6050.getAngleX() + 2.0;
     leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
 
@@ -574,9 +580,6 @@ void leg_loop()
     else if (Position[1] < SERVO2_MAX) // 舵机2最高
       Position[1] = SERVO2_MAX;
     
-    // Position[0] = constrain(Position[0], SERVO1_MIN, SERVO1_MAX);
-    // Position[1] = constrain(Position[1], SERVO2_MAX, SERVO2_MIN); // 注意参数顺序
-    
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
   }
 }
@@ -591,8 +594,6 @@ void jump_loop()
     ACC[1] = 0;
     Speed[0] = 0;
     Speed[1] = 0;
-    // Position[0] = 2048 + 12 + 8.4 * (60 - 32); // 值越大，腿伸的越长，原来80 改为60降低腿部高度，即车重心
-    // Position[1] = 2048 - 12 - 8.4 * (60 - 32);
     Position[0] = SERVO1_MAX; 
     Position[1] = SERVO2_MAX;
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);