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

Codex generated changes:

Browse Source 
1.stool-mode
2.INS direction changes
not tested,not ok
This commit is contained in:
TuxMonkey
2026-07-14 01:07:48 +08:00
parent a4ebc4b452
commit 8a13444e48
7 changed files with 286 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,20 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
static float k[12][3] = {91.443258,-113.149301,-8.780431,
2.186397,-6.908826,-0.171192,
47.943203,-39.423913,-13.297140,
30.671413,-28.308799,-9.363969,
231.778706,-224.742110,68.974495,
18.910995,-20.079072,7.490403,
50.114072,-58.670210,23.856863,
3.273746,-3.321150,1.267603,
140.969395,-137.481822,42.507159,
93.859327,-90.754295,27.811839,
-283.581279,232.017305,87.865897,
-28.193619,23.802666,3.963794,};
static float k[12][3] = {
{91.443258f, -113.149301f, -8.780431f},
{2.186397f, -6.908826f, -0.171192f},
{47.943203f, -39.423913f, -13.297140f},
{30.671413f, -28.308799f, -9.363969f},
{231.778706f, -224.742110f, 68.974495f},
{18.910995f, -20.079072f, 7.490403f},
{50.114072f, -58.670210f, 23.856863f},
{3.273746f, -3.321150f, 1.267603f},
{140.969395f, -137.481822f, 42.507159f},
{93.859327f, -90.754295f, 27.811839f},
{-283.581279f, 232.017305f, 87.865897f},
{-28.193619f, 23.802666f, 3.963794f},
};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;
@@ -48,4 +50,4 @@ static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
p->T_wheel = T[0];
p->T_hip = T[1];
}
}