TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 11:37:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

修改与轮电机固连杆，使用机体速度计算LQR增益

Browse Source
This commit is contained in:
kai
2024-04-28 16:14:10 +08:00
parent 41e13aee44
commit b7e4c80cb4
8 changed files with 58 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
application/chassis/linkNleg.h
View File

@@ -68,10 +68,12 @@ void Link2Leg(LinkNPodParam *p, ChassisParam *chassis)
float phi2_pred = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
xC = xB + CALF_LEN * mcos(phi2_pred);
yC = yB + CALF_LEN * msin(phi2_pred);
float phi3_pred = atan2f(yC - yD, xC - xD);
float phi5_pred = atan2f(yC, xC - JOINT_DISTANCE / 2);
// 差分计算腿长变化率和腿角速度
p->phi2_w = (phi2_pred - p->phi2) / predict_dt; // 稍后用于修正轮速
p->phi2_w = (phi2_pred - p->phi2) / predict_dt;
p->phi3_w = (phi3_pred - p->phi3) / predict_dt; // 稍后用于修正轮速
p->phi5_w = (phi5_pred - p->phi5) / predict_dt;
p->legd = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->leg_len) / predict_dt;
p->theta_w = ((phi5_pred - 0.5 * PI - (chassis->pitch + chassis->pitch_w * predict_dt) - p->theta) / predict_dt); // 可以不考虑机体? -predict_dt*chassis.pitch_w