mirror of
https://gitee.com/dlmu-cone/bf_original_balance_chassis
synced 2026-07-24 19:47:44 +08:00
简化了电机测量的命名,完成初版平衡底盘的功能编写,待测试方向和符号的正确性
This commit is contained in:
NeoZng
@@ -26,12 +26,8 @@ static attitude_t *imu_data;
|
||||
static SuperCapInstance *super_cap;
|
||||
static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
|
||||
// 电机
|
||||
static HTMotorInstance *lf;
|
||||
static HTMotorInstance *rf;
|
||||
static HTMotorInstance *lb;
|
||||
static HTMotorInstance *rb;
|
||||
static LKMotorInstance *l_driven;
|
||||
static LKMotorInstance *r_driven;
|
||||
static HTMotorInstance *lf, *rf, *lb, *rb;
|
||||
static LKMotorInstance *l_driven, *r_driven;
|
||||
// 底盘板和云台板通信
|
||||
static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
|
||||
@@ -40,27 +36,51 @@ static Chassis_Upload_Data_s chassis_feed_send;
|
||||
static LinkNPodParam left_side, right_side;
|
||||
static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
|
||||
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
|
||||
static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
|
||||
static PIDInstance leg_length_pid; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
|
||||
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
|
||||
|
||||
/* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/
|
||||
|
||||
/**
|
||||
* @brief 将电机和imu的数据组装为LinkNPodParam结构体
|
||||
* @note 由于两个连杆和轮子连接时,只有一个使用了轴承,另外一个直接和电机的定子连接,
|
||||
* 所以轮子的转速和电机的转速不一致,因此和地面的速度应为电机转速减去杆的转速,得到的才是轮子的转速
|
||||
*
|
||||
* @todo angle direction to be verified
|
||||
*/
|
||||
static void ParamAssemble()
|
||||
{
|
||||
{ // 电机的角度是逆时针为正,左侧全部取反
|
||||
left_side.phi1 = PI2 - LIMIT_LINK_RAD - lb->measure.total_angle;
|
||||
left_side.phi4 = -lf->measure.total_angle + LIMIT_LINK_RAD;
|
||||
left_side.phi1_w = -lb->measure.speed_rads;
|
||||
left_side.phi4_w = -lf->measure.speed_rads;
|
||||
left_side.wheel_dist = -l_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
|
||||
left_side.wheel_w = -l_driven->measure.speed_rads;
|
||||
|
||||
right_side.phi1 = PI2 - LIMIT_LINK_RAD + rb->measure.total_angle;
|
||||
right_side.phi4 = rf->measure.total_angle + LIMIT_LINK_RAD;
|
||||
right_side.phi1_w = rb->measure.speed_rads;
|
||||
right_side.phi4_w = rf->measure.speed_rads;
|
||||
right_side.wheel_dist = r_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
|
||||
right_side.wheel_w = r_driven->measure.speed_rads;
|
||||
|
||||
if(chassis_cmd_recv.wz != 0) // 若有转向指令,则使用IMU积分得到的位置,否则左右轮位置相反会产生阻碍转向的力矩
|
||||
{
|
||||
// left_side.wheel_dist = 0.5*imu_data->Accel[3];
|
||||
// right_side.wheel_dist = 0.5*imu_data->Accel[3];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
|
||||
* @todo 测试两种的效果,留下其中一种;
|
||||
* 若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码
|
||||
*
|
||||
* @param p 5连杆和腿的参数
|
||||
*/
|
||||
static void Link2Pod(LinkNPodParam *p)
|
||||
{ // 拟将功能封装到vmc_project.h中
|
||||
float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2;
|
||||
float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2t, phi5t;
|
||||
xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4);
|
||||
yD = THIGH_LEN * arm_sin_f32(p->phi4);
|
||||
xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1);
|
||||
@@ -68,20 +88,16 @@ static void Link2Pod(LinkNPodParam *p)
|
||||
BD = powf(xD - xB, 2) + powf(yD - yB, 2);
|
||||
A0 = 2 * CALF_LEN * (xD - xB);
|
||||
B0 = 2 * CALF_LEN * (yD - yB);
|
||||
p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
|
||||
xC = xB + CALF_LEN * arm_cos_f32(p->phi2);
|
||||
yC = yB + CALF_LEN * arm_sin_f32(p->phi2);
|
||||
#ifdef ANGLE_DIFF_VMC
|
||||
float p5t = atan2f(yC, xC - JOINT_DISTANCE / 2); // 避免重复计算
|
||||
float plt = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
|
||||
p->phi5_w = (p5t - p->phi5) / balance_dt;
|
||||
p->pod_len_w = (plt - p->pod_len) / balance_dt;
|
||||
p->phi5 = p5t;
|
||||
p->pod_len = plt;
|
||||
#endif
|
||||
phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
|
||||
xC = xB + CALF_LEN * arm_cos_f32(phi2t);
|
||||
yC = yB + CALF_LEN * arm_sin_f32(phi2t);
|
||||
|
||||
p->phi2 = phi2t;
|
||||
p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
|
||||
p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
|
||||
p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC
|
||||
p->theta = p->phi5 - 0.5 * PI - imu_data->Pitch;
|
||||
p->height = p->pod_len * arm_cos_f32(p->theta);
|
||||
|
||||
#ifdef VELOCITY_DIFF_VMC
|
||||
float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度)
|
||||
@@ -94,24 +110,46 @@ static void Link2Pod(LinkNPodParam *p)
|
||||
BD = powf(xD - xB, 2) + powf(yD - yB, 2);
|
||||
A0 = 2 * CALF_LEN * (xD - xB);
|
||||
B0 = 2 * CALF_LEN * (yD - yB);
|
||||
phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
|
||||
xC = xB + CALF_LEN * arm_cos_f32(phi2);
|
||||
yC = yB + CALF_LEN * arm_sin_f32(phi2);
|
||||
phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
|
||||
xC = xB + CALF_LEN * arm_cos_f32(phi2t);
|
||||
yC = yB + CALF_LEN * arm_sin_f32(phi2t);
|
||||
phi5t = atan2f(yC, xC - JOINT_DISTANCE / 2);
|
||||
// 差分计算腿长变化率和腿角速度
|
||||
p->pod_w = (atan2f(yC, xC - JOINT_DISTANCE / 2) - p->phi5) / balance_dt;
|
||||
p->phi2_w = (phi2t - p->phi2) / balance_dt; // 稍后用于修正轮速
|
||||
p->pod_w = (phi5t - p->phi5) / balance_dt;
|
||||
p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt;
|
||||
#endif // VELOCITY_DIFF_VMC
|
||||
p->theta_w = (phi5t - 0.5 * PI - imu_data->Pitch - p->theta) / balance_dt;
|
||||
p->height_v = p->pod_v * arm_cos_f32(p->theta) - p->pod_len * arm_sin_f32(p->theta) * p->theta_w; // 这很酷!PDE!
|
||||
#endif
|
||||
|
||||
p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]) * WHEEL_RADIUS; // 修正轮速
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
|
||||
* 由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
|
||||
*
|
||||
* @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
|
||||
* 而轮子的输出则只经过转向PID的反馈增益计算
|
||||
*
|
||||
* @todo 确定使用查表还是多项式拟合
|
||||
*
|
||||
*/
|
||||
static void CalcLQR(LinkNPodParam *p)
|
||||
static void CalcLQR(LinkNPodParam *p, float target_x)
|
||||
{
|
||||
float *gain_list = LookUpKgain(p->pod_len); // K11,K12... K21,K22... K26
|
||||
float T[2]; // 0 T_wheel, 1 T_pod;
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
T[i] = gain_list[i * 6 + 0] * -p->theta +
|
||||
gain_list[i * 6 + 1] * -p->theta_w +
|
||||
gain_list[i * 6 + 2] * (target_x - p->wheel_dist) +
|
||||
gain_list[i * 6 + 3] * -p->wheel_w +
|
||||
gain_list[i * 6 + 4] * -imu_data->Pitch +
|
||||
gain_list[i * 6 + 5] * -imu_data->Gyro[3];
|
||||
}
|
||||
p->T_wheel = T[0];
|
||||
p->T_pod = T[1];
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -124,7 +162,7 @@ static void SynthesizeMotion()
|
||||
left_side.T_pod += anti_crash_pid.Output;
|
||||
right_side.T_pod -= anti_crash_pid.Output;
|
||||
|
||||
// PIDCalculate(&swerving_pid, imu_data->Yaw, 0); // 对速度闭环还是使用角度增量闭环?
|
||||
PIDCalculate(&swerving_pid, imu_data->Yaw, chassis_cmd_recv.wz); // 对速度闭环还是使用角度增量闭环?
|
||||
left_side.T_wheel -= swerving_pid.Output;
|
||||
right_side.T_wheel += swerving_pid.Output;
|
||||
}
|
||||
@@ -135,7 +173,7 @@ static void SynthesizeMotion()
|
||||
*/
|
||||
static void LegControl(LinkNPodParam *p, float target_length)
|
||||
{
|
||||
p->F_pod += PIDCalculate(&leg_length_pid, p->pod_len, target_length);
|
||||
p->F_pod += PIDCalculate(&leg_length_pid, p->height, target_length);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -177,8 +215,14 @@ static void FlyDetect()
|
||||
* @brief 功率限制,一般不需要
|
||||
*
|
||||
*/
|
||||
static void WattLimit(LinkNPodParam *p)
|
||||
static void WattLimit()
|
||||
{
|
||||
HTMotorSetRef(&lf, left_side.T_front);
|
||||
HTMotorSetRef(&lb, left_side.T_back);
|
||||
HTMotorSetRef(&rf, right_side.T_front);
|
||||
HTMotorSetRef(&rb, right_side.T_back);
|
||||
LKMotorSetRef(&l_driven, left_side.T_wheel);
|
||||
LKMotorSetRef(&r_driven, right_side.T_wheel);
|
||||
}
|
||||
|
||||
void BalanceInit()
|
||||
@@ -320,13 +364,32 @@ void BalanceTask()
|
||||
{
|
||||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm);
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
HTMotorStop(lf);
|
||||
HTMotorStop(rf);
|
||||
HTMotorStop(lb);
|
||||
HTMotorStop(rb);
|
||||
LKMotorStop(l_driven);
|
||||
LKMotorStop(r_driven);
|
||||
}
|
||||
else
|
||||
{
|
||||
HTMotorEnable(lf);
|
||||
HTMotorEnable(rf);
|
||||
HTMotorEnable(lb);
|
||||
HTMotorEnable(rb);
|
||||
LKMotorEnable(l_driven);
|
||||
LKMotorEnable(r_driven);
|
||||
}
|
||||
|
||||
ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起
|
||||
// 将五连杆映射成单杆
|
||||
Link2Pod(&left_side);
|
||||
Link2Pod(&right_side);
|
||||
// 根据单杆计算处的角度和杆长,计算反馈增益
|
||||
CalcLQR(&left_side);
|
||||
CalcLQR(&right_side);
|
||||
CalcLQR(&left_side, chassis_cmd_recv.vx); // @todo,需要确定速度or位置闭环
|
||||
CalcLQR(&right_side, chassis_cmd_recv.vx);
|
||||
// 腿长控制
|
||||
LegControl(&left_side, 0);
|
||||
LegControl(&right_side, 0);
|
||||
@@ -339,11 +402,10 @@ void BalanceTask()
|
||||
VMCProject(&right_side);
|
||||
|
||||
FlyDetect(); // 滞空检测
|
||||
// 电机输出限幅
|
||||
WattLimit(&left_side);
|
||||
WattLimit(&right_side);
|
||||
|
||||
WattLimit(); // 电机输出限幅
|
||||
|
||||
// code to go here... 裁判系统,UI,多机通信
|
||||
|
||||
CANCommSend(chassis_comm, (uint8_t*)&chassis_feed_send);
|
||||
|
||||
CANCommSend(chassis_comm, (uint8_t *)&chassis_feed_send);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user