/* * 配置相关 */ /* Includes ----------------------------------------------------------------- */ #include "user_math.h" #include "module/config.h" #include "bsp/can.h" #include "shoot.h" #include "chassic.h" /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* Exported variables ------------------------------------------------------- */ // 机器人参数配置 Config_RobotParam_t robot_config = { .chassis={ .motor_3508_param[0]={BSP_CAN_1,0x201,MOTOR_M3508,false,false}, .motor_3508_param[1]={BSP_CAN_1,0x202,MOTOR_M3508,false,false}, .motor_3508_param[2]={BSP_CAN_1,0x203,MOTOR_M3508,false,false}, .motor_3508_param[3]={BSP_CAN_1,0x204,MOTOR_M3508,false,false}, .motor_6020_param[0]={BSP_CAN_1,0x205,MOTOR_GM6020,false,false}, .motor_6020_param[1]={BSP_CAN_1,0x206,MOTOR_GM6020,false,false}, .motor_6020_param[2]={BSP_CAN_2,0x207,MOTOR_GM6020,false,false}, .motor_6020_param[3]={BSP_CAN_2,0x208,MOTOR_GM6020,false,false}, .C6020Omega_param={ .k=1.0f, .p=0.5f, .i=0.1f, .d=0.0f, .i_limit=1.0f, .out_limit=1.0f, .d_cutoff_freq= -1.0f, .range=-1.0f }, .C6020Angle_param={ .k=0.2f, .p=0.21f, .i=0.1f, .d=0.0f, .i_limit=1.0f, .out_limit=30.0f, .d_cutoff_freq= -1.0f, .range=360 }, .M3508v_param={ .k=0.2f, .p=0.25f, .i=0.1f, .d=0.0f, .i_limit=1.0f, .out_limit=1.0f, .d_cutoff_freq= -1.0f, .range=-1.0f }, }, .shoot_param = { .proj=SHOOT_PROJECTILE_17MM, .fric_num=4, .extra_deceleration_ratio=1.0f, .num_trig_tooth=2, .shot_freq=20.0f, .shot_burst_num=10, .num_multilevel=1, .jam_enable=false, .jam_threshold=120.0f, .jam_suspected_time=0.5f, .trig_motor_param = { .can = BSP_CAN_2, .id = 0x207, .module = MOTOR_M2006, .reverse = true, .gear=true, }, // .fric_motor_param = (Shoot_MOTOR_RM_Param_t[]){ // { // .param = { // .can = BSP_CAN_2, // .id = 0x201, // .module = MOTOR_M3508, // .reverse = false, // .gear=true,}, // .level = 1}, // { // .param = { // .can = BSP_CAN_2, // .id = 0x203, // .module = MOTOR_M3508, // .reverse = true, // .gear=true,}, // .level = 1}, //// {.param = {.can = BSP_CAN_1, .id = 0x203, .module = MOTOR_M3508}, .level = 2}, // {.param = {.can = BSP_CAN_1, .id = 0x204, .module = MOTOR_M3508}, .level = 2}, // }, .fric_follow = { .k=1.0f, .p=1.8f, .i=0.0f, .d=0.0f, .i_limit=0.0f, .out_limit=0.9f, .d_cutoff_freq=30.0f, .range=-1.0f, }, .fric_err = { .k=1.0f, .p=4.0f, .i=0.4f, .d=0.04f, .i_limit=0.25f, .out_limit=0.25f, .d_cutoff_freq=40.0f, .range=-1.0f, }, .trig_2006 = { .k=2.5f, .p=1.0f, .i=0.1f, .d=0.04f, .i_limit=0.4f, .out_limit=1.0f, .d_cutoff_freq=-1.0f, .range=M_2PI, }, .trig_omg_2006 = { .k=1.0f, .p=1.5f, .i=0.3f, .d=0.5f, .i_limit=0.2f, .out_limit=1.0f, .d_cutoff_freq=-1.0f, .range=-1.0f, }, .trig_3508 = { .k=0.5f, .p=1.8f, .i=0.3f, .d=0.1f, .i_limit=0.15f, .out_limit=1.0f, .d_cutoff_freq=-1.0f, .range=M_2PI, }, .trig_omg_3508 = { .k=1.0f, .p=1.0f, .i=0.0f, .d=0.0f, .i_limit=0.0f, .out_limit=1.0f, .d_cutoff_freq=-1.0f, .range=-1.0f, }, .filter.fric = { .in = 30.0f, .out = 30.0f, }, .filter.trig = { .in = 30.0f, .out = 30.0f, }, }, .gimbal_param = { /*欧拉角限位和电机角度限位*/ .Limit_t= { .pit_max=0.69, .pit_min=-0.47, .yaw_max= 1.0,//yaw的 .yaw_min=-1.0, }, .feedforward={ .imu = { .yaw=false, .pit=false, .coefficient_yaw=0, .coefficient_pit=0, }, }, /*零点参数*/ .zero={ .pit_encoder=0, .yaw_encoder=0, }, .motor={ /*按自己需求选择电机*/ .pit=DM, .yaw=RM, /*是否开启限位*/ .limit_yaw=false, .limit_pit=true, .pit_rm_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false}, .yaw_rm_motor={BSP_CAN_2,0x209,MOTOR_GM6020,false,false}, /*达妙电机参数自己配*/ .pit_dm_motor={}, .yaw_dm_motor={}, }, .dm_Params_t={ .yaw_dm={.kd=0.3,}, .yaw_dm_Reduction_ratio=8.0f,//减速比 .pit_dm={.kd=1.0,}, .pit_dm_Reduction_ratio=5.0f, }, .low_pass_cutoff_freq = { .out = -1.0f, .gyro = 1000.0f, }, .pid = { /*欧拉角控制参数*/ .yaw_omega = { .k = 0.45f, .p = 1.0f, .i = 6.0f, .d = 0.0008f,//0 .i_limit = 1.0f, .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .range = -1.0f, }, .yaw_angle = { .k = 10.0f, .p = 2.0f , .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, .range = M_2PI, }, .pit_omega = { .k = 0.25f, .p = 1.0f, .i = 0.0f, .d = 0.001901f, .i_limit = 1.0f, .out_limit = 1.0f, .d_cutoff_freq = -1.0f, .range = -1.0f, }, .pit_angle = { .k = 12.0f, .p = 2.1, .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, .range = M_2PI, }, } }, .telescoping={ .telescoping_motor={BSP_CAN_1,0x208,MOTOR_M2006,false,false}, .circle=-1000.0,//负数是顺时针 .pid={ .Telescoping_velocity={ .k=1.0f, .p=1.0f, .i=0.0f, .d=0.0f, .i_limit=0.2f, .out_limit=1.0f, .d_cutoff_freq=-1.0f, .range=-1.0f, }, .Telescoping_angle={ .k=1.0f,//空载时pid参数超调 .p=25.0f, .i=0.3f, .d=2.0f,//1.5 .i_limit=0.4f, .out_limit=20000.0f, .d_cutoff_freq=-1.0f, .range=0, }, }, .low_pass_cutoff_freq={ .out=30.0f, }, }, }; /** * @brief 获取机器人配置参数 * @return 机器人配置参数指针 */ Config_RobotParam_t* Config_GetRobotParam(void) { return &robot_config; } int8_t Config_ShootInit(void) { int fric_num = robot_config.shoot_param.fric_num; int num_multilevel = robot_config.shoot_param.num_multilevel; robot_config.shoot_param.fric_motor_param = (Shoot_MOTOR_RM_Param_t *)BSP_Malloc(fric_num * sizeof(Shoot_MOTOR_RM_Param_t)); if (robot_config.shoot_param.fric_motor_param == NULL) { BSP_Free(robot_config.shoot_param.fric_motor_param); return -1; // 内存分配失败 } robot_config.shoot_param.ratio_multilevel = (float *)BSP_Malloc(num_multilevel * sizeof(float)); if (robot_config.shoot_param.ratio_multilevel == NULL) { BSP_Free(robot_config.shoot_param.ratio_multilevel); return -1; // 内存分配失败 } /* 初始化摩擦轮电机参数 */ for (uint8_t i = 0; i < fric_num; i++) { robot_config.shoot_param.fric_motor_param[i].param = (MOTOR_RM_Param_t){ .can = BSP_CAN_2, .id = 0x201 + i, .module = MOTOR_M3508, /*设置电机反装；example*********************** .reverse = (i == 0||1||3||5) ? true : false,*/ .reverse = (i == 1) ? true : false, .gear = false, }; } /*规定电机属于哪级发射；example************************ robot_config.shoot_param.fric_motor_param[0].level=1;*/ robot_config.shoot_param.fric_motor_param[0].level=1; robot_config.shoot_param.fric_motor_param[1].level=1; /*规定各级摩擦轮转速比；example********************* robot_config.shoot_param.ratio_multilevel[0]=1.0f;*/ robot_config.shoot_param.ratio_multilevel[0]=1.0f; return SHOOT_OK; }