TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

添加离地检测

Browse Source
This commit is contained in:
kai
2024-05-01 23:16:36 +08:00
parent 85badbc297
commit d3f1d42f6c
2 changed files with 36 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
application/chassis/balance.c
View File

@@ -112,10 +112,10 @@ void BalanceInit()
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
.Kp = 600,
.Kd = 100,
.Kp = 1200,
.Kd = 150,
.Ki = 0,
.MaxOut = 20,
.MaxOut = 60,
.DeadBand = 0.0001f,
.Improve = PID_ChangingIntegrationRate | PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_Derivative_On_Measurement,
.Derivative_LPF_RC = 0.05,
@@ -128,7 +128,7 @@ void BalanceInit()
.Kp = 0.0008f,
.Kd = 0.0001f,
.Ki = 0.0f,
.MaxOut = 0.04,
.MaxOut = 0.05,
.DeadBand = 0.001f,
.Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
.Derivative_LPF_RC = 0.05,
@@ -152,7 +152,7 @@ void BalanceInit()
.Kp = 3,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 10,
.MaxOut = 20,
.DeadBand = 0.0f,
.Improve = PID_DerivativeFilter | PID_Derivative_On_Measurement,
.Derivative_LPF_RC = 0.05,
@@ -164,7 +164,7 @@ void BalanceInit()
.Kp = 15,
.Kd = 1,
.Ki = 0.0,
.MaxOut = 10,
.MaxOut = 30,
.DeadBand = 0.001f,
.Improve = PID_DerivativeFilter | PID_ChangingIntegrationRate | PID_Integral_Limit,
.Derivative_LPF_RC = 0.01,
@@ -212,7 +212,7 @@ static void ControlSwitch()
{
chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后
chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s
chassis_cmd_recv.delta_leglen = -0.000001f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial;
chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_;
}
}
@@ -231,6 +231,8 @@ static void ResetChassis()
l_side.target_len = r_side.target_len = 0.12;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 驱动轮支持力为定值
l_side.normal_force = r_side.normal_force = 100.0f;
// 撞墙时前后移动保证能重新站立,执行速度输入
LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w);
@@ -283,6 +285,8 @@ static void WokingStateSet()
chassis.dist = chassis.target_dist = 0;
// 角度输入为当前角度
chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw;
// 驱动轮支持力为定值
l_side.normal_force = r_side.normal_force = 100.0f;
for (uint8_t i = 0; i < JOINT_CNT; i++)
HTMotorStop(joint[i]);
@@ -379,8 +383,8 @@ static void LegControl() /* 腿长控制和Roll补偿 */
l_side.target_len += roll_compensate_pid.Output;
r_side.target_len -= roll_compensate_pid.Output;
static float gravity_comp = 80;
static float roll_extra_comp_p = 300;
static float gravity_comp = 60;
static float roll_extra_comp_p = 400;
float roll_comp = roll_extra_comp_p * chassis.roll;
l_side.F_leg = PIDCalculate(&leglen_pid_l, l_side.height, l_side.target_len) + gravity_comp - roll_comp;
r_side.F_leg = PIDCalculate(&leglen_pid_r, r_side.height, r_side.target_len) + gravity_comp + roll_comp;
@@ -421,15 +425,18 @@ void BalanceTask()
// VMC映射成关节输出
VMCProject(&l_side);
VMCProject(&r_side);
// 驱动轮支持力解算
NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
// stop表示复位尚未完成,reset表明还未切换到其他模式,故都不执行运动模态的代码
if (chassis_status == ROBOT_STOP ||
chassis_cmd_recv.chassis_mode == CHASSIS_RESET ||
chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
return; // 复位模态或急停,直接退出
else
{
// 正常模式下再进行驱动轮支持力解算
NormalForceSolve(&l_side, Chassis_IMU_data, del_t);
NormalForceSolve(&r_side, Chassis_IMU_data, del_t);
}
// 运动模态,电机输出映射和限幅
WattLimitSet();

17
application/chassis/lqr_calc.h
View File

@@ -25,6 +25,23 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
float l = p->leg_len;
float lsqr = l * l;
// 离地检测
if (p->normal_force < 20.0f)
{
for (size_t i = 0; i < 12; i++)
{
// 除 theta 和 theta_dot 的关节输出外，其余增益全部置0
if(i != 6 && i != 7)
{
for (size_t j = 0; j < 3; j++)
{
k[i][j] = 0;
}
}
}
}
// 计算增益
for (uint8_t i = 0; i < 2; ++i)
{
uint8_t j = i * 6;