修复重力感应相关bug
This commit is contained in:
myperworld committed
1 parent
42fd44c28e
commit
9d56ba85f1
1 file changed
+26
-12
@@ -64,7 +64,7 @@ MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||
MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
// PID控制器实例
|
||||
PIDController pid_angle(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
|
||||
PIDController pid_pitch(1, 0, 0, 100000, 8); // 比例系数（P）, 积分系数（I）, 微分系数（D）, 积分限幅(当I=0,无效), 输出限幅（8V防止电机过载）
|
||||
PIDController pid_gyro(0.06, 0, 0, 100000, 8);
|
||||
PIDController pid_distance(0.5, 0, 0, 100000, 8);
|
||||
PIDController pid_speed(0.7, 0, 0, 100000, 8);
|
||||
@@ -92,9 +92,11 @@ MPU6050 mpu6050(I2Ctwo);
|
||||
|
||||
// 机身平衡角度调整 roll轴
|
||||
float roll_adjust = 0.0f;
|
||||
float original_roll_adjust = roll_adjust;
|
||||
|
||||
// 机身高度
|
||||
float leg_height_base = 20.0f; // 改为全局变量，初始值20 腿部默认高度基准值，值越小，机身越高， 实测范围 -10（最高），52（最低）
|
||||
float original_leg_height_base = leg_height_base ;
|
||||
|
||||
#define SERVO0_MIN 2047 // 舵机1最低位置，大腿与小腿碰到一起， 从小车正面看右手边舵机
|
||||
#define SERVO1_MIN 2047 // 舵机2最低位置，大腿与小腿碰到一起， 从小车正面看左手边舵机
|
||||
@@ -142,8 +144,8 @@ float gyro_control = 0;
|
||||
float speed_control = 0;
|
||||
float distance_control = 0;
|
||||
float LQR_u = 0;
|
||||
float angle_zeropoint = 8; // 5 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_angle_zeropoint = 8; // 保存原始的角度零点
|
||||
float pitch_zeropoint = 7; // 8 设定默认俯仰角度，向前到正，向后倒负
|
||||
float original_pitch_zeropoint = pitch_zeropoint; // 保存原始的角度零点
|
||||
float distance_zeropoint = 0.5f; // 轮部位移零点偏置
|
||||
// float pitch_adjust = 0.0f; // 俯仰角度调整
|
||||
|
||||
@@ -350,7 +352,6 @@ void loop()
|
||||
// 达到平衡角度后，重置重置计数器
|
||||
if( abs(LQR_angle) < 25.0f){
|
||||
fallRecoveryResetCounter = 0;
|
||||
stand_up_count = 0;
|
||||
}
|
||||
// Serial.println(String(LQR_angle));
|
||||
|
||||
@@ -378,6 +379,14 @@ void loop()
|
||||
motor2.target = -1.0;
|
||||
}
|
||||
|
||||
// 站立计数
|
||||
if( stand_up_count > 0){
|
||||
stand_up_count ++;
|
||||
}
|
||||
if (stand_up_count > 500){
|
||||
stand_up_count = 0;
|
||||
}
|
||||
|
||||
// 小车坐下
|
||||
if (sitting_down) {
|
||||
sitting_down ++;
|
||||
@@ -432,7 +441,7 @@ void lqr_balance_loop()
|
||||
LQR_u = angle_control + gyro_control + distance_control + speed_control
|
||||
= 俯仰角度控制 + 俯仰角速度控制 + 距离控制 + 速度控制
|
||||
|
||||
angle_control = pid_angle( LQR_angle - angle_zeropoint) // 实际pitch角度-目标pitch角度
|
||||
angle_control = pid_pitch( LQR_angle - pitch_zeropoint) // 实际pitch角度-目标pitch角度
|
||||
LQR_angle = mpu6050.getAngleY()
|
||||
|
||||
gyro_control = pid_gyro( LQR_gyro - 0 ) // 实际角速度，目标值是零
|
||||
@@ -467,7 +476,7 @@ void lqr_balance_loop()
|
||||
|
||||
LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s）
|
||||
|
||||
angle_control = pid_angle(LQR_angle - angle_zeropoint); // pitch_adjust
|
||||
angle_control = pid_pitch(LQR_angle - pitch_zeropoint); // pitch_adjust
|
||||
gyro_control = pid_gyro(LQR_gyro);
|
||||
|
||||
// 前进后退跳跃的系数都不一样
|
||||
@@ -551,7 +560,7 @@ void lqr_balance_loop()
|
||||
if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0)) // && !sitting_down
|
||||
{
|
||||
LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
|
||||
angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||
pitch_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -561,6 +570,7 @@ void lqr_balance_loop()
|
||||
// 平衡控制参数自适应
|
||||
if (wrobot.height < 50){
|
||||
pid_speed.P = 0.7;
|
||||
// pid_speed.P = 0.65;
|
||||
}else if (wrobot.height < 64){
|
||||
pid_speed.P = 0.6;
|
||||
}else{
|
||||
@@ -581,8 +591,12 @@ void leg_loop()
|
||||
// 1. 获取并处理MPU6050 Z轴加速度
|
||||
float z_accel = mpu6050.getAccZ(); // 1.1~1.2左右
|
||||
|
||||
// 2. 快速下坠检测与处理
|
||||
if (z_accel >1.8f && !is_falling && stand_up_count == 0) {
|
||||
// 2. 触发重力感应
|
||||
if (z_accel >1.8f && !is_falling
|
||||
&& stand_up_count == 0
|
||||
&& roll_adjust == original_roll_adjust
|
||||
&& leg_height_base == original_leg_height_base
|
||||
) {
|
||||
is_falling = true;
|
||||
// 腿部收缩
|
||||
ACC[0] = 250;
|
||||
@@ -595,7 +609,7 @@ void leg_loop()
|
||||
return; // 跳过后续正常高度计算
|
||||
}
|
||||
|
||||
// 3. 下坠结束判断与恢复逻辑
|
||||
// // 3. 下坠结束判断与恢复逻辑
|
||||
if (is_falling) {
|
||||
if (!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])) {
|
||||
is_falling = false;
|
||||
@@ -938,8 +952,8 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
|
||||
|
||||
// 还原
|
||||
if (remoteData.btnLS) {
|
||||
roll_adjust = 0.0f;
|
||||
leg_height_base = 20.0f;
|
||||
roll_adjust = original_roll_adjust;
|
||||
leg_height_base = original_leg_height_base;
|
||||
}
|
||||
|
||||
// 右扳机 高度向下 ：原始0-1023
|
||||
|
||||
Reference in new issue
Block a user