199 lines
7.6 KiB
C
199 lines
7.6 KiB
C
/*
|
||
* 配置相关
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "module/config.h"
|
||
#include "bsp/can.h"
|
||
#include "device/motor_dm.h"
|
||
#include "device/rc_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,
|
||
},
|
||
|
||
.chassis_param = {
|
||
.yaw={
|
||
.k=1.0f,
|
||
.p=1.2f,
|
||
.i=0.0f,
|
||
.d=0.05f,
|
||
.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, /* 不使用循环误差处理 */
|
||
},
|
||
|
||
.leg_length={
|
||
.k = 20.0f,
|
||
.p = 10.0f,
|
||
.i = 0.0f,
|
||
.d = 0.0f,
|
||
.i_limit = 0.0f,
|
||
.out_limit = 100.0f,
|
||
.d_cutoff_freq = -1.0f,
|
||
.range = -1.0f,
|
||
},
|
||
.leg_theta={
|
||
.k=1.0f,
|
||
.p=5.0f, /* 摆角比例系数 */
|
||
.i=0.0f, /* 摆角积分系数 */
|
||
.d=0.0f, /* 摆角微分系数 */
|
||
.i_limit=0.0f, /* 积分限幅 */
|
||
.out_limit=0.05f, /* 输出限幅，腿长差值限制 */
|
||
.d_cutoff_freq=30.0f, /* 微分截止频率 */
|
||
.range=-1.0f, /* 不使用循环误差处理 */
|
||
},
|
||
|
||
.tp={
|
||
.k=1.0f,
|
||
.p=2.0f, /* 俯仰角比例系数 */
|
||
.i=0.0f, /* 俯仰角积分系数 */
|
||
.d=0.0f, /* 俯仰角微分系数 */
|
||
.i_limit=0.0f, /* 积分限幅 */
|
||
.out_limit=2.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 = 0xFF,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
{ // 左膝关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 125,
|
||
.host_id = 0xFF,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = false,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
{ // 右膝关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 126,
|
||
.host_id = 0xFF,
|
||
.module = MOTOR_LZ_RSO3,
|
||
.reverse = true,
|
||
.mode = MOTOR_LZ_MODE_MOTION,
|
||
},
|
||
{ // 右髋关节
|
||
.can = BSP_CAN_1,
|
||
.motor_id = 127,
|
||
.host_id = 0xFF,
|
||
.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,
|
||
},
|
||
},
|
||
.yaw_motor = { // 云台Yaw轴电机
|
||
.can = BSP_CAN_2,
|
||
.can_id = 0x1,
|
||
.master_id = 0x11,
|
||
.module = MOTOR_DM_J4310,
|
||
.reverse = false,
|
||
},
|
||
|
||
.mech_zero_yaw = 1.21085676f, /* 250.5度，机械零点 */
|
||
|
||
.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 = { -1.518517857889991e+02f, 1.947427875897828e+02f, -1.148120822481098e+02f, 2.924883559935997e+00f }, // theta
|
||
.k12_coeff = { 1.210535496345407e+01f, -9.650112118530574e+00f, -7.795614823592060e+00f, 4.657327020761534e-01f }, // d_theta
|
||
.k13_coeff = { -1.165837444184349e+02f, 1.210192009641869e+02f, -4.426716583574083e+01f, 2.303172586380976e+00f }, // x
|
||
.k14_coeff = { -1.286052871916038e+02f, 1.353354867883034e+02f, -5.203199795488686e+01f, 2.622579832615006e+00f }, // d_x
|
||
.k15_coeff = { -1.105374347374162e+01f, 3.229730763109554e+01f, -2.423106279412239e+01f, 6.195204290845447e+00f }, // phi
|
||
.k16_coeff = { 8.260175276799853e-01f, 2.318023492852569e+00f, -2.927935219327240e+00f, 8.556202879801391e-01f }, // d_phi
|
||
.k21_coeff = { 5.970052798954075e+02f, -5.263867571125131e+02f, 1.275655583254851e+02f, 1.140189988320814e+01f }, // theta
|
||
.k22_coeff = { 7.451585479025395e+01f, -7.785445709151199e+01f, 2.827386050355351e+01f, -1.578620610065337e-01f }, // d_theta
|
||
.k23_coeff = { 4.622306333044981e+01f, 4.325117706159210e+00f, -3.407203207833582e+01f, 1.455475256803718e+01f }, // x
|
||
.k24_coeff = { 6.870578295004115e+01f, -1.301773374139566e+01f, -3.211146583952794e+01f, 1.644195610244500e+01f }, // d_x
|
||
.k25_coeff = { 4.493994591642744e+02f, -4.563158073471503e+02f, 1.605396531175411e+02f, -3.691208683727941e+00f }, // phi
|
||
.k26_coeff = { 5.771012099214764e+01f, -5.915171391239870e+01f, 2.096920183531924e+01f, -1.205283154896878e+00f }, // d_phi
|
||
},
|
||
.lqr_param.max_joint_torque = 20.0f, // 关节电机最大力矩 20Nm
|
||
.lqr_param.max_wheel_torque = 4.5f, // 轮毂电机最大力矩 2.5Nm
|
||
},
|
||
|
||
.rc_can_param = {
|
||
.can = BSP_CAN_2,
|
||
.mode = RC_CAN_MODE_SLAVE,
|
||
.joy_id = 0x250,
|
||
.sw_id = 0x251,
|
||
.mouse_id = 0x252,
|
||
.keyboard_id = 0x253,
|
||
},
|
||
};
|
||
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 获取机器人配置参数
|
||
* @return 机器人配置参数指针
|
||
*/
|
||
Config_RobotParam_t* Config_GetRobotParam(void) {
|
||
return &robot_config;
|
||
} |