TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 19:47:44 +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

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;