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-04 20:26:57 +08:00
parent 7fee0c15c3
commit 9c141e4310
1 changed files with 20 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
application/chassis/lqr_calc.h
View File

@@ -25,29 +25,27 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
float l = p->leg_len;
float lsqr = l * l;
for (uint8_t i = 0; i < 2; ++i)
{
uint8_t j = i * 6;
uint8_t i, j;
if(i == 0) // 离地时仅对速度闭环，保证落地时轮速与机体速度一致
{
T[i] = (k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) + (p->fly_flag ? 0 :
( (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
(k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
(k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
(k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
(k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w ));
}
else if(i == 1) // 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
{
T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
(k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + (p->fly_flag ? 0 :
( (k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
(k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
(k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
(k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w ));
}
}
// 离地时轮子输出置0
i = 0; j = i * 6;
T[i] = p->fly_flag ? 0 :
((k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
(k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w +
(k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
(k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
(k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
(k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w);
// 离地时关节输出仅保留 theta 和 theta_dot，保证滞空时腿部竖直
i = 1; j = i * 6;
T[i] = (k[j + 0][0] * lsqr + k[j + 0][1] * l + k[j + 0][2]) * -p->theta +
(k[j + 1][0] * lsqr + k[j + 1][1] * l + k[j + 1][2]) * -p->theta_w + (p->fly_flag ? 0 :
((k[j + 2][0] * lsqr + k[j + 2][1] * l + k[j + 2][2]) * (chassis->target_dist - chassis->dist) +
(k[j + 3][0] * lsqr + k[j + 3][1] * l + k[j + 3][2]) * (chassis->target_v - chassis->vel) +
(k[j + 4][0] * lsqr + k[j + 4][1] * l + k[j + 4][2]) * -chassis->pitch +
(k[j + 5][0] * lsqr + k[j + 5][1] * l + k[j + 5][2]) * -chassis->pitch_w));
p->T_wheel = T[0];
p->T_hip = T[1];
}