252 lines
12 KiB
C
252 lines
12 KiB
C
/*
|
||
* 配置相关
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "module/config.h"
|
||
#include "bsp/can.h"
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
|
||
/* Exported variables ------------------------------------------------------- */
|
||
|
||
// 机器人参数配置
|
||
Config_RobotParam_t robot_config = {
|
||
|
||
.imu_param = {
|
||
.can = BSP_CAN_2,
|
||
.can_id = 0x6FF,
|
||
.device_id = 0x42,
|
||
.master_id = 0x43,
|
||
},
|
||
.joint_motors[0] = { // 左膝关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 124,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
.joint_motors[1] = { // 左髋关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 125,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
.joint_motors[2] = { // 右髋关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 126,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
.joint_motors[3] = { // 右膝关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 127,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
.wheel_motors[0] = { // 左轮电机
|
||
.can = BSP_CAN_1,
|
||
.id = 1, // 电机ID为1，对应命令ID=0x141，反馈ID=0x181
|
||
.module = MOTOR_LK_MF9025,
|
||
.reverse = false,
|
||
},
|
||
.wheel_motors[1] = { // 右轮电机
|
||
.can = BSP_CAN_1,
|
||
.id = 2, // 电机ID为2，对应命令ID=0x142，反馈ID=0x182
|
||
.module = MOTOR_LK_MF9025,
|
||
.reverse = true,
|
||
},
|
||
|
||
.chassis_param = {
|
||
.leglength={
|
||
.k = 20.0f,
|
||
.p = 1.0f,
|
||
.i = 0.0f,
|
||
.d = 0.0f,
|
||
.i_limit = 0.0f,
|
||
.out_limit = 100.0f,
|
||
.d_cutoff_freq = -1.0f,
|
||
.range = -1.0f,
|
||
},
|
||
.yaw={
|
||
.k=1.0f,
|
||
.p=1.0f,
|
||
.i=0.0f,
|
||
.d=0.0f,
|
||
.i_limit=0.0f,
|
||
.out_limit=1.0f,
|
||
.d_cutoff_freq=30.0f,
|
||
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
|
||
},
|
||
.roll={
|
||
.k=1.0f,
|
||
.p=5.0f, /* 横滚角比例系数 */
|
||
.i=0.0f, /* 横滚角积分系数 */
|
||
.d=0.2f, /* 横滚角微分系数 */
|
||
.i_limit=0.0f, /* 积分限幅 */
|
||
.out_limit=0.05f, /* 输出限幅，腿长差值限制 */
|
||
.d_cutoff_freq=30.0f, /* 微分截止频率 */
|
||
.range=-1.0f, /* 不使用循环误差处理 */
|
||
},
|
||
|
||
.tp_pid={
|
||
.k=1.0f,
|
||
.p=1.0f, /* 俯仰角比例系数 */
|
||
.i=0.0f, /* 俯仰角积分系数 */
|
||
.d=0.0f, /* 俯仰角微分系数 */
|
||
.i_limit=0.0f, /* 积分限幅 */
|
||
.out_limit=1.0f, /* 输出限幅，腿长差值限制 */
|
||
.d_cutoff_freq=30.0f, /* 微分截止频率 */
|
||
.range=-1.0f, /* 不使用循环误差处理 */
|
||
},
|
||
|
||
|
||
.low_pass_cutoff_freq = {
|
||
.in = 30.0f,
|
||
.out = 30.0f,
|
||
},
|
||
.joint_motors = {
|
||
{ // 左髋关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 124,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
{ // 左膝关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 125,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
{ // 右膝关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 126,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = true,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
{ // 右髋关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 127,
|
||
.host_id = 130,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = true,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
},
|
||
.wheel_motors = {
|
||
{ // 左轮电机
|
||
.can = BSP_CAN_1,
|
||
.id = 0x141,
|
||
.module = MOTOR_LK_MF9025,
|
||
.reverse = false,
|
||
},
|
||
{ // 右轮电机
|
||
.can = BSP_CAN_1,
|
||
.id = 0x142,
|
||
.module = MOTOR_LK_MF9025,
|
||
.reverse = true,
|
||
},
|
||
},
|
||
.mech_zero_yaw = 0.0f,
|
||
|
||
.vmc_param = {
|
||
{ // 左腿
|
||
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
|
||
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
|
||
.leg_3 = 0.258f, // 前膝连杆长度 (L3) (m)
|
||
.leg_4 = 0.215f, // 前髋连杆长度 (L4) (m)
|
||
.hip_length = 0.0f // 髋宽 (L5) (m)
|
||
},
|
||
{ // 右腿
|
||
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
|
||
.leg_2 = 0.258f, // 后膝连杆长度 (L2) (m)
|
||
.leg_3 = 0.258f, // 前膝连杆长度 (L3) (m)
|
||
.leg_4 = 0.215f, // 前髋连杆长度 (L4) (m)
|
||
.hip_length = 0.0f // 髋宽 (L5) (m)
|
||
}
|
||
},
|
||
.lqr_gains ={
|
||
// .k11_coeff = { -61.932040681074966f, 70.963671642086396f, -37.730841182566571f, -0.296475458388679f }, // theta
|
||
// .k12_coeff = { -0.586710094600108f, 0.886736272521581f, -3.626144273475104f, 0.057861910518974f }, // d_theta
|
||
// .k13_coeff = { -17.297031110481019f, 16.286794934166178f, -5.176451154477850f, -0.867260018374823f }, // x
|
||
// .k14_coeff = { -14.893387150006664f, 14.357020815277332f, -5.244645181873441f, -0.869862096507486f}, // d_x
|
||
// .k15_coeff = { -75.327876471378886f, 79.786699741548944f, -31.183500053811208f, 5.450635661115236f}, // phi
|
||
// .k16_coeff = { -3.572234723237025f, 4.164905011076312f, -1.874828497771750f, 0.477913222933688f}, // d_phi
|
||
// .k21_coeff = { 9.327090608559319f, 1.362675928111105f, -8.092777598567881f, 4.351387652359104f}, // theta
|
||
// .k22_coeff = { 3.872517778351810f, -3.344077414726880f, 0.589693555828904f, 0.310036629174646f}, // d_theta
|
||
// .k23_coeff = { -71.615766251649134f, 74.748309711530837f, -28.370490124006626f, 4.483586941100197f }, // x
|
||
// .k24_coeff = { -68.751866288568962f, 71.204785013044102f, -26.812636604518396f, 4.238479323700952f }, // d_x
|
||
// .k25_coeff = { 205.6887659080132f, -195.1219729060621f, 62.890188701113303f, 7.452313695653162f }, // phi
|
||
// .k26_coeff = { 16.162999842656344f, -15.926932704437410f, 5.474613395300429f, -0.002856083635449f }, // d_phi
|
||
|
||
|
||
// .k11_coeff = {-143.1550,156.1754,-98.5282,-11.3781}, // theta
|
||
// .k12_coeff = {8.3196,-12.4161,-8.3805,-1.6368}, // d_theta
|
||
// .k13_coeff = {-69.6189,68.5619,-23.3079,-4.1736}, // x
|
||
// .k14_coeff = {-58.4944,58.2204,-22.8021,-5.2361}, // d_x
|
||
// .k15_coeff = {-29.6753,40.9947,-20.1188,2.5142}, // phi
|
||
// .k16_coeff = {-13.1787,15.8361,-7.4061,1.1193}, // d_phi
|
||
// .k21_coeff = {-76.5141,124.3258,-73.4908,22.0942}, // theta
|
||
// .k22_coeff = {-9.1845,12.7284,-5.8169,2.8659}, // d_theta
|
||
// .k23_coeff = {-157.6244,179.9415,-77.2161,14.9075}, // x
|
||
// .k24_coeff = {-180.5666,202.7656,-85.2869,16.4847}, // d_x
|
||
// .k25_coeff = {14.3596,-14.2125,4.1210,24.1729}, // phi
|
||
// .k26_coeff = {20.5751,-19.8014,6.3128,2.8044}, // d_phi
|
||
.k11_coeff = { -2.046396270532116e+02f, 2.283572275397276e+02f, -99.051642997946473f, 2.549835715107331f }, // theta
|
||
.k12_coeff = { 0.689999868157742f, 2.004516582917084f, -5.904779142191341f, 0.331840642644740f }, // d_theta
|
||
.k13_coeff = { -59.058694050901643f, 59.363082825119605f, -20.603451414220757f, 1.137708603718630f }, // x
|
||
.k14_coeff = { -64.283929532305166f, 65.138737687498519f, -23.323482861600581f, 1.257945614978053f }, // d_x
|
||
.k15_coeff = { -15.125353856795936f, 34.329224759247211f, -22.500683150450474f, 5.036897782323629f }, // phi
|
||
.k16_coeff = { 2.707768649521343f, 0.390393176362524f, -2.018231845635338f, 0.697604163191230f }, // d_phi
|
||
.k21_coeff = { 4.135329288854244e+02f, -3.173913574866715e+02f, 50.321199991176265f, 10.217217753280829f }, // theta
|
||
.k22_coeff = { 48.042446261620519f, -45.292268634116219f, 13.273044296221686f, 0.006982339078710f }, // d_theta
|
||
.k23_coeff = { 14.015246608117772f, 10.813301135732024f, -18.216050987625373f, 6.078912501009629f }, // x
|
||
.k24_coeff = { 21.698411755946285f, 5.621435092936538f, -18.016608013978576f, 6.611542756343175f }, // d_x
|
||
.k25_coeff = { 4.798120071828828e+02f, -4.728222224549831e+02f, 1.591181202824602e+02f, -6.421997865768036f }, // phi
|
||
.k26_coeff = { 59.794709871063546f, -60.418062715734166f, 20.991263455753383f, -1.388418937916963f }, // d_phi
|
||
|
||
// .k11_coeff = { -1.996305368054721e+02f, 2.260001266392926e+02f, -1.009632659573521e+02f, 2.651403223110949e+00f }, // theta
|
||
// .k12_coeff = { 1.961704292162323e+00f, 8.251913348469651e-01f, -6.073749575127879e+00f, 3.535645826465822e-01f }, // d_theta
|
||
// .k13_coeff = { -8.161081261310467e+01f, 8.274264960693915e+01f, -2.904800049859091e+01f, 1.653742354439720e+00f }, // x
|
||
// .k14_coeff = { -7.455421501651551e+01f, 7.622235638964226e+01f, -2.773307975245958e+01f, 1.535810571728176e+00f }, // d_x
|
||
// .k15_coeff = { -9.027722377713712e+00f, 2.891936105315331e+01f, -2.106785573613789e+01f, 4.948383450314285e+00f }, // phi
|
||
// .k16_coeff = { 3.049839709048039e+00f, 4.627952499276505e-02f, -1.922806522134511e+00f, 6.902507101472589e-01f }, // d_phi
|
||
// .k21_coeff = { 4.373445515700652e+02f, -3.391497363529634e+02f, 5.569210421652366e+01f, 1.081229141610274e+01f }, // theta
|
||
// .k22_coeff = { 5.284103137531328e+01f, -5.028796043573002e+01f, 1.502532265509508e+01f, -3.077550945526411e-02f }, // d_theta
|
||
// .k23_coeff = { 3.111787495894651e+01f, 4.872956811853720e+00f, -2.278008499711769e+01f, 8.426190283284837e+00f }, // x
|
||
// .k24_coeff = { 3.329235562185018e+01f, -5.071568882184936e-01f, -1.928141689016582e+01f, 7.776043600153916e+00f }, // d_x
|
||
// .k25_coeff = { 4.604866240014122e+02f, -4.577537299814802e+02f, 1.557742271762380e+02f, -6.272551369660446e+00f }, // phi
|
||
// .k26_coeff = { 5.608646259671771e+01f, -5.728239809509379e+01f, 2.016452460533993e+01f, -1.320817827839268e+00f }, // d_phi
|
||
|
||
},
|
||
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
|
||
.lqr_param.max_wheel_torque = 2.5f, // 轮毂电机最大力矩 2.5Nm
|
||
}
|
||
};
|
||
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 获取机器人配置参数
|
||
* @return 机器人配置参数指针
|
||
*/
|
||
Config_RobotParam_t* Config_GetRobotParam(void) {
|
||
return &robot_config;
|
||
} |