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-19 21:20:48 +08:00
parent 51c92b423a
commit 66e23202a7
4 changed files with 49 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
static float k[12][3] = {62.680622,-74.772126,-13.135672,
1.620796,-4.331826,-0.454705,
32.093563,-25.558681,-16.605856,
19.128242,-17.562747,-10.815380,
225.373594,-201.324771,53.236052,
13.575849,-13.013546,3.845604,
76.704297,-72.201666,20.359891,
4.520141,-4.051799,1.189345,
139.418565,-125.750074,34.110069,
88.295852,-79.170124,21.430975,
-163.725633,127.852860,110.004619,
-9.557678,7.476790,4.655220};
static float k[12][3] = {64.570926,-78.151551,-15.098827,
1.716153,-4.573289,-0.526836,
25.565621,-20.634320,-17.563574,
11.633020,-11.021911,-13.087704,
212.754066,-196.450645,56.227305,
12.051262,-11.941105,4.523285,
74.149148,-71.296364,23.673363,
4.449069,-4.214434,1.145555,
115.321993,-106.373546,29.626387,
82.459182,-74.915697,20.072512,
-163.793049,130.808796,114.419419,
-13.396998,11.357026,4.288949};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;