diff --git a/application/chassis/balance.c b/application/chassis/balance.c index b9b951b..4c49b83 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -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); diff --git a/application/chassis/lqr_calc.h b/application/chassis/lqr_calc.h index 40c4458..e4fb297 100644 --- a/application/chassis/lqr_calc.h +++ b/application/chassis/lqr_calc.h @@ -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;