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

24
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
float k[12][3] = {155.616146,-200.327241,-0.278791,
-12.564474,-24.018026,0.674594,
145.503085,-87.742553,-5.916048,
97.588867,-70.361944,-4.007305,
303.894243,-240.356086,68.074439,
18.743007,-15.738160,4.718314,
-170.799127,75.225969,18.998160,
-29.312006,20.892957,1.529644,
122.553208,-114.023608,38.147691,
63.561616,-64.916409,25.861226,
-657.233777,402.487049,66.281303,
-40.107302,25.117848,1.705173};
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};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;