实现开机后，小车自动起立功能。
This commit is contained in:
myperworld committed
1 parent
faa6206090
commit
a48241aa8f
3 files changed
+40
-41
No files matched your search
+2
-1
@@ -3,6 +3,7 @@
|
||||
"files.associations": {
|
||||
"istream": "cpp",
|
||||
"ostream": "cpp",
|
||||
"algorithm": "cpp"
|
||||
"algorithm": "cpp",
|
||||
"iterator": "cpp"
|
||||
}
|
||||
}
|
||||
@@ -1,8 +1,5 @@
|
||||
此项目是世博同学轮足机器人的Arduino工程转换而成的PlatformIO工程
|
||||
世博同学轮足机器人项目地址：https://github.com/MuShibo/Micro-Wheeled_leg-Robot
|
||||
|
||||
优化内容：
|
||||
同时大量优化了小车平衡性能，增加左右摇摆功能。
|
||||
可以通过开源的esp32遥控器进行遥控，无需联网，更加随意。
|
||||
优化了轮子，更加大，稳定性杠杠的，stl文件已经开源在.stl文件夹中。
|
||||
By Tommy
|
||||
可以使用vscode进行编译下载，解决了Arduino编译巨慢的问题。
|
||||
|
||||
世博同学轮足机器人项目地址：https://github.com/MuShibo/Micro-Wheeled_leg-Robot
|
||||
@@ -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 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:");
|
||||
// LQR_distance 和 LQR_speed趋近于0
|
||||
// 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(LQR_gyro);
|
||||
|
||||
// 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);
|
||||
|
||||
Reference in new issue
Block a user