TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

changed ID

Browse Source
This commit is contained in:
TuxMonkey
2026-04-18 16:16:20 +08:00
parent d0b3771bf9
commit df12ca7256
2 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
application/chassis/balance.c
View File

@@ -104,17 +104,17 @@ void BalanceInit()
.speed_feedback_source = MOTOR_FEED,
},
.motor_type = DM8009P};
joint_conf.can_init_config.tx_id = 1;
joint_conf.can_init_config.rx_id = 11;
joint_conf.can_init_config.tx_id = 3;
joint_conf.can_init_config.rx_id = 14;
joint[LF] = lf = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 2;
joint_conf.can_init_config.rx_id = 12;
joint[LB] = lb = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 3;
joint_conf.can_init_config.rx_id = 13;
joint[LB] = lb = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 1;
joint_conf.can_init_config.rx_id = 12;
joint[RF] = rf = DMMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 4;
joint_conf.can_init_config.rx_id = 14;
joint_conf.can_init_config.tx_id = 0;
joint_conf.can_init_config.rx_id = 11;
joint[RB] = rb = DMMotorInit(&joint_conf);
// 驱动轮电机
@@ -132,9 +132,9 @@ void BalanceInit()
};
driven_conf.can_init_config.tx_id = 1;
driven[LD] = l_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[RD] = r_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
driven[LD] = l_driven = LKMotorInit(&driven_conf);
// 腿长控制
PID_Init_Config_s leg_length_pid_conf = {
@@ -452,10 +452,10 @@ static void LegControl() /* 腿长控制和Roll补偿 */
static void WattLimitSet() /* 设定运动模态的输出 */
{
DMMotorSetRef(lf, 0.2857f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改
DMMotorSetRef(lb, 0.2857f * -l_side.T_back);
DMMotorSetRef(rf, 0.2857f * r_side.T_front);
DMMotorSetRef(rb, 0.2857f * r_side.T_back);
DMMotorSetRef(lf, 1.0f * -l_side.T_front); // 根据扭矩常数计算得到的系数 todo 需修改
DMMotorSetRef(lb, 1.0f * -l_side.T_back);
DMMotorSetRef(rf, 1.0f * r_side.T_front);
DMMotorSetRef(rb, 1.0f * r_side.T_back);
LKMotorSetRef(l_driven, 195.3125 * l_side.T_wheel);
LKMotorSetRef(r_driven, 195.3125 * -r_side.T_wheel);

24
application/chassis/lqr_calc.h
View File

@@ -9,18 +9,18 @@
*/
static void CalcLQR(LinkNPodParam *p, ChassisParam *chassis)
{
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};
static float k[12][3] = {152.073959,-322.787113,4.602472,
-18.091924,-50.039341,3.160226,
124.677598,-113.871572,-13.279757,
86.314525,-98.184173,-8.140564,
162.907111,-185.705008,74.114015,
14.280240,-18.420891,8.593112,
-148.548074,99.259778,48.506697,
-37.972970,41.871624,3.715364,
192.255883,-224.641960,95.484838,
93.227296,-117.550338,60.336158,
-343.472953,311.036102,57.349362,
-41.609057,38.668058,-0.809331,};
float T[2] = {0}; // 0 T_wheel 1 T_hip
float l = p->leg_len;
float lsqr = l * l;