From a48241aa8f55e1c3fa423510041bee00e3e25bdd Mon Sep 17 00:00:00 2001 From: myperworld Date: Mon, 11 Aug 2025 21:28:43 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=BC=80=E6=9C=BA=E5=90=8E?= =?UTF-8?q?=EF=BC=8C=E5=B0=8F=E8=BD=A6=E8=87=AA=E5=8A=A8=E8=B5=B7=E7=AB=8B?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Shibo-main-V4-ESPNOW/.vscode/settings.json | 3 +- Shibo-main-V4-ESPNOW/README.md | 9 +-- Shibo-main-V4-ESPNOW/src/main.cpp | 77 +++++++++++----------- 3 files changed, 44 insertions(+), 45 deletions(-) diff --git a/Shibo-main-V4-ESPNOW/.vscode/settings.json b/Shibo-main-V4-ESPNOW/.vscode/settings.json index 1262e2c..d9e96b6 100644 --- a/Shibo-main-V4-ESPNOW/.vscode/settings.json +++ b/Shibo-main-V4-ESPNOW/.vscode/settings.json @@ -3,6 +3,7 @@ "files.associations": { "istream": "cpp", "ostream": "cpp", - "algorithm": "cpp" + "algorithm": "cpp", + "iterator": "cpp" } } \ No newline at end of file diff --git a/Shibo-main-V4-ESPNOW/README.md b/Shibo-main-V4-ESPNOW/README.md index 7dddf74..3d58a19 100644 --- a/Shibo-main-V4-ESPNOW/README.md +++ b/Shibo-main-V4-ESPNOW/README.md @@ -1,8 +1,5 @@ 此项目是世博同学轮足机器人的Arduino工程转换而成的PlatformIO工程 -世博同学轮足机器人项目地址：https://github.com/MuShibo/Micro-Wheeled_leg-Robot -优化内容： -同时大量优化了小车平衡性能，增加左右摇摆功能。 -可以通过开源的esp32遥控器进行遥控，无需联网，更加随意。 -优化了轮子，更加大，稳定性杠杠的，stl文件已经开源在.stl文件夹中。 -By Tommy \ No newline at end of file +可以使用vscode进行编译下载，解决了Arduino编译巨慢的问题。 + +世博同学轮足机器人项目地址：https://github.com/MuShibo/Micro-Wheeled_leg-Robot \ No newline at end of file diff --git a/Shibo-main-V4-ESPNOW/src/main.cpp b/Shibo-main-V4-ESPNOW/src/main.cpp index 1767b45..0186ada 100644 --- a/Shibo-main-V4-ESPNOW/src/main.cpp +++ b/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 @@ -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 distance_zeropoint = -256.0; // 轮部位移零点偏置（-256为一个不可能的位移值，将其作为未刷新的标志） +float angle_zeropoint = 5; // 俯仰角度，向前到正，向后倒负 +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 ) { - // motor1.target = 0; - // motor2.target = 0; - // } + if(LQR_angle < -40.0f || LQR_angle > 55.0f ) { + motor1.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都是零 - // Serial.print("LQR_distance:"); - // Serial.print(LQR_distance); - // Serial.print(",LQR_speed:"); - // Serial.print(LQR_speed); - // Serial.print(",LQR_angle:"); - // Serial.print(LQR_angle); - // Serial.print(",LQR_gyro:"); - // Serial.print(LQR_gyro); - + // LQR_distance 和 LQR_speed趋近于0 + // if(k==100) + // { + // k=0; + // Serial.print("distance:"); + // Serial.print(LQR_distance); + // Serial.print(",speed:"); + // Serial.print(LQR_speed); + // Serial.print(",angle:"); + // Serial.print(LQR_angle); + // Serial.print(",gyro:"); + // Serial.println(LQR_gyro); + // } + // k++; // 计算pitch的角度和角速度的pid控制值 // 实际pitch角度与零点的差值，目标值是零 @@ -467,15 +469,21 @@ void lqr_balance_loop() pid_speed.P = 0.5; } - // 平衡控制参数自适应 -// if (wrobot.height >= 32 && wrobot.height <= 80) { // 假设有效高度范围32-80cm -// // 线性插值公式：P = P_max - (height - height_min)*(P_max - P_min)/(height_max - height_min) -// pid_speed.P = 0.7f - (wrobot.height - 32.0f) * (0.7f - 0.5f) / (80.0f - 32.0f); -// // pid_speed.P = 0.5f + 0.2f * exp(-0.05f * (wrobot.height - 32.0f)); // 非线性曲线 -// pid_speed.P = constrain(pid_speed.P, 0.5f, 0.7f); // 限制在0.5-0.7范围 -// } else { -// pid_speed.P = 0.5f; // 超出范围使用默认值 -// } + // if(k==100) + // { + // k=0; + // Serial.print("LQR_u:"); //LQR_u = angle_control + gyro_control + distance_control + speed_control + // Serial.print(LQR_u); + // Serial.print(",angle:"); + // Serial.print(angle_control); + // 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[1] = 200; - Speed[0] = 350; - Speed[1] = 350; + Speed[0] = 300; + Speed[1] = 300; 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);