diff --git a/User/device/motor_dm.h b/User/device/motor_dm.h index 20e91d4..4e678cd 100644 --- a/User/device/motor_dm.h +++ b/User/device/motor_dm.h @@ -43,7 +43,7 @@ typedef struct { /* Exported functions prototypes -------------------------------------------- */ /** - * @brief 注册一个LK电机 + * @brief 注册一个DM电机 * @param param 电机参数 * @return */ @@ -62,11 +62,31 @@ int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param); */ int8_t MOTOR_DM_UpdateAll(void); +/** + * @brief MIT模式控制电机 + * @param param 电机参数 + * @param output MIT控制输出参数 + * @return 操作结果，0为成功，负值为失败 + */ int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output); +/** + * @brief 位置速度双环控制电机 + * @param param 电机参数 + * @param target_pos 目标位置 + * @param target_vel 目标速度 + * @return 操作结果，0为成功，负值为失败 + */ int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel); -int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel); +/** + * @brief 位置速度双环控制电机 + * @param param 电机参数 + * @param target_pos 目标位置 + * @param target_vel 目标速度 + * @return 操作结果，0为成功，负值为失败 + */ +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel); /** * @brief 获取指定电机的实例指针 @@ -75,6 +95,11 @@ int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel); */ MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param); +/** + * @brief 使能电机 + * @param param 电机参数 + * @return 操作结果，0为成功，负值为失败 + */ int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param); /** diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c index e38073a..1b61374 100644 --- a/User/device/motor_rm.c +++ b/User/device/motor_rm.c @@ -139,13 +139,13 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { motor->feedback.rotor_speed = rotor_speed; motor->feedback.torque_current = torque_current; } + while (motor->feedback.rotor_abs_angle < 0) { + motor->feedback.rotor_abs_angle += M_2PI; + } + while (motor->feedback.rotor_abs_angle >= M_2PI) { + motor->feedback.rotor_abs_angle -= M_2PI; + } if (motor->motor.reverse) { - while (motor->feedback.rotor_abs_angle < 0) { - motor->feedback.rotor_abs_angle += M_2PI; - } - while (motor->feedback.rotor_abs_angle >= M_2PI) { - motor->feedback.rotor_abs_angle -= M_2PI; - } motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; motor->feedback.rotor_speed = -motor->feedback.rotor_speed; motor->feedback.torque_current = -motor->feedback.torque_current; diff --git a/User/module/config.c b/User/module/config.c index 91f7035..5d63e22 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -23,6 +23,8 @@ Config_RobotParam_t robot_config = { .shoot_param = { .trig_step_angle=M_2PI/8, + .shot_delay_time=0.05f, + .shot_burst_num=1, .fric_motor_param[0] = { .can = BSP_CAN_2, .id = 0x201, @@ -41,7 +43,7 @@ Config_RobotParam_t robot_config = { .can = BSP_CAN_1, .id = 0x201, .module = MOTOR_M2006, - .reverse = false, + .reverse = true, .gear=true, }, .fric_follow = { @@ -54,6 +56,7 @@ Config_RobotParam_t robot_config = { .d_cutoff_freq=30.0f, .range=-1.0f, }, + .fric_err = { .k=1.0f, .p=4.0f, @@ -64,15 +67,25 @@ Config_RobotParam_t robot_config = { .d_cutoff_freq=40.0f, .range=-1.0f, }, + .trig_omg = { + .k=1.0f, + .p=1.5f, + .i=0.3f, + .d=0.5f, + .i_limit=0.2f, + .out_limit=0.9f, + .d_cutoff_freq=-1.0f, + .range=-1.0f, + }, .trig = { .k=1.0f, - .p=1.2f, - .i=0.0f, + .p=1.0f, + .i=0.1f, .d=0.05f, - .i_limit=0.2f, - .out_limit=0.9f, - .d_cutoff_freq=30.0f, - .range=-1.0f, + .i_limit=0.8f, + .out_limit=0.5f, + .d_cutoff_freq=-1.0f, + .range=M_2PI, }, .filter.fric = { .in = 30.0f, @@ -82,13 +95,13 @@ Config_RobotParam_t robot_config = { .in = 30.0f, .out = 30.0f, }, - + }, .gimbal_param = { .pid = { .yaw_omega = { - .k = 0.5f, + .k = 1.0f, .p = 1.0f, .i = 0.5f, .d = 0.0f, @@ -101,7 +114,7 @@ Config_RobotParam_t robot_config = { .k = 10.0f, .p = 3.0f, .i = 0.0f, - .d = 0.1f, + .d = 0.0f, .i_limit = 0.0f, .out_limit = 10.0f, .d_cutoff_freq = -1.0f, @@ -130,7 +143,7 @@ Config_RobotParam_t robot_config = { }, .mech_zero = { .yaw = 0.0f, - .pit = 3.8f, + .pit = 1.77f, }, .travel = { .yaw = -1.0f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 69a10c3..ac30619 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -159,7 +159,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { Gimbal_SetMode(g, g_cmd->mode); /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */ - float delta_yaw = g_cmd->delta_yaw * g->dt; + float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f; if (g->param->travel.yaw > 0) { /* 计算当前电机角度与IMU角度的偏差 */ float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw; @@ -239,7 +239,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { void Gimbal_Output(Gimbal_t *g){ MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit); MOTOR_MIT_Output_t output = {0}; - output.torque = g->out.yaw * 2.5; + output.torque = g->out.yaw * 6.0f; // 乘以减速比 + output.kd = 0.5f; MOTOR_RM_Ctrl(&g->param->pit_motor); MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output); } diff --git a/User/module/shoot.c b/User/module/shoot.c index d152e4a..6fdd1b7 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -1,16 +1,13 @@ #include "shoot.h" - -#include "bsp/can.h" -#include "component/filter.h" #include -// #include +#include "can.h" +#include "component/filter.h" +#include "component/user_math.h" #include #include "bsp/time.h" -uint32_t shoot_ctrl_cnt_last; -float shoot_ctrl_dt; -bool last_firecmd=false; +static bool last_firecmd; static inline void ScaleSumTo1(float *a, float *b) { float sum = *a + *b; @@ -21,149 +18,281 @@ static inline void ScaleSumTo1(float *a, float *b) { } } -int8_t Shoot_Init(Shoot_t *c, Shoot_Params_t *param, float target_freq) + +int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode) { - if (c == NULL || param == NULL || target_freq <= 0.0f) { + if (s == NULL) { return -1; // 参数错误 } - c->param=param; - BSP_CAN_Init(); - memset(&c->feedback, 0, sizeof(c->feedback)); + s->mode=mode; + return 0; +} +int8_t Shoot_ResetIntegral(Shoot_t *s) +{ + if (s == NULL) { + return -1; // 参数错误 + } + for(int i=0;ipid.fric_follow[i]); + PID_ResetIntegral(&s->pid.fric_err[i]); + } + PID_ResetIntegral(&s->pid.trig); + PID_ResetIntegral(&s->pid.trig_omg); + return 0; +} + +int8_t Shoot_ResetCalu(Shoot_t *s) +{ + if (s == NULL) { + return -1; // 参数错误 + } + for(int i=0;ipid.fric_follow[i]); + PID_Reset(&s->pid.fric_err[i]); + LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f); + LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f); + } + PID_Reset(&s->pid.trig); + PID_Reset(&s->pid.trig_omg); + LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f); + LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f); + return 0; +} + +int8_t Shoot_ResetOutput(Shoot_t *s) +{ + if (s == NULL) { + return -1; // 参数错误 + } + for(int i=0;ioutput.out_follow[i]=0.0f; + s->output.out_err[i]=0.0f; + s->output.out_fric[i]=0.0f; + s->output.lpfout_fric[i]=0.0f; + } + s->output.outagl_trig=0.0f; + s->output.outomg_trig=0.0f; + s->output.outlpf_trig=0.0f; + return 0; +} + +int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) +{ + if (s == NULL) { + return -1; // 参数错误 + } + s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM; + return 0; +} +/** + * \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度 + * + * \param s 包含发射数据的结构体 + * \param num 需要发射的弹丸数量 + */ +int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) +{ + if (s == NULL || s->shoot_Anglecalu.num_to_shoot == 0) { + return -1; + } + if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd) + { + s->shoot_Anglecalu.time_last_shoot=s->now; + s->target_variable.target_angle += s->param->trig_step_angle; + if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI; + else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI; + s->shoot_Anglecalu.num_to_shoot--; + } + return 0; +} + +int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq) +{ + if (s == NULL || param == NULL || target_freq <= 0.0f) { + return -1; // 参数错误 + } + s->param=param; + + BSP_CAN_Init(); for(int i=0;ifric_motor_param[i]); - PID_Init(&c->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,¶m->fric_follow); - LowPassFilter2p_Init(&c->filter.fric.in[i], target_freq, c->param->filter.fric.in); - LowPassFilter2p_Init(&c->filter.fric.out[i], target_freq, c->param->filter.fric.out); + PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,¶m->fric_follow); + LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in); + LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out); } MOTOR_RM_Register(¶m->trig_motor_param); - PID_Init(&c->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig); - - LowPassFilter2p_Init(&c->filter.trig.in, target_freq, c->param->filter.trig.in); - LowPassFilter2p_Init(&c->filter.trig.out, target_freq, c->param->filter.trig.out); + PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig); + PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,¶m->trig_omg); + + LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in); + LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out); + memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu)); + memset(&s->output,0,sizeof(s->output)); + s->target_variable.target_rpm=4000.0f; + return 0; } -int8_t Chassis_UpdateFeedback(Shoot_t *c) +int8_t Chassis_UpdateFeedback(Shoot_t *s) { - if (c == NULL) { + if (s == NULL) { return -1; // 参数错误 } - float rpm_sum=0.0f; for(int i = 0; i < SHOOT_FRIC_NUM; i++) { - MOTOR_RM_Update(&c->param->fric_motor_param[i]); - MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&c->param->fric_motor_param[i]); + /* 更新摩擦电机反馈 */ + MOTOR_RM_Update(&s->param->fric_motor_param[i]); + MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]); if(motor_fed!=NULL) { - c->feedback.fric[i]=motor_fed->motor.feedback; + s->feedback.fric[i]=motor_fed->motor.feedback; } - c->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&c->filter.fric.in[i], c->feedback.fric[i].rotor_speed); - c->feedback.fric_rpm[i] = c->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM; - if(c->feedback.fric_rpm[i]>1.0f)c->feedback.fric_rpm[i]=1.0f; - if(c->feedback.fric_rpm[i]<-1.0f)c->feedback.fric_rpm[i]=-1.0f; - rpm_sum+=c->feedback.fric_rpm[i]; + /* 滤波反馈rpm */ + s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed); + /* 归一化rpm */ + s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM; + if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f; + if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f; + /* 计算平均rpm */ + rpm_sum+=s->feedback.fric_rpm[i]; } - c->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM; - MOTOR_RM_Update(&c->param->trig_motor_param); - c->feedback.trig = MOTOR_RM_GetMotor(&c->param->trig_motor_param); - // - c->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&c->filter.trig.in, c->feedback.trig->feedback.rotor_speed); - c->feedback.trig_rpm = c->feedback.trig->feedback.rotor_speed / MAX_TRIG_RPM; - if(c->feedback.trig_rpm>1.0f)c->feedback.trig_rpm=1.0f; //如果单环效果好就删 - if(c->feedback.trig_rpm<-1.0f)c->feedback.trig_rpm=-1.0f; - // - c->errtosee = c->feedback.fric[0].rotor_speed - c->feedback.fric[1].rotor_speed; + s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM; + /* 更新拨弹电机反馈 */ + MOTOR_RM_Update(&s->param->trig_motor_param); + MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param); + if(motor_fed!=NULL) + { + s->feedback.trig=motor_fed->feedback; + } + /* 将多圈角度归化到单圈 (-M_PI, M_PI) */ + s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle; + s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π] + if (s->feedback.trig_angle_cicle > M_PI) { + s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π] + }else if (s->feedback.trig_angle_cicle < -M_PI) { + s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π] + } + + s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed); + s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM; + if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f; //如果单环效果好就删 + if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f; + + s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed; return 0; } -int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd) +int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd) { - if (c == NULL || cmd == NULL) { + if (s == NULL || cmd == NULL) { return -1; // 参数错误 } - c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; - c->lask_wakeup = BSP_TIME_Get_us(); - c->online = cmd->online; - if(!c->online /*|| c->mode==SHOOT_MODE_SAFE*/){ + s->now = BSP_TIME_Get_us() / 1000000.0f; + s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f; + s->lask_wakeup = BSP_TIME_Get_us(); + s->online = cmd->online; + if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){ for(int i=0;iparam->fric_motor_param[i]); + MOTOR_RM_Relax(&s->param->fric_motor_param[i]); } - MOTOR_RM_Relax(&c->param->trig_motor_param); + MOTOR_RM_Relax(&s->param->trig_motor_param); } else{ - switch(c->running_state) + switch(s->running_state) { case SHOOT_STATE_IDLE:/*熄火等待*/ for(int i=0;ipid.fric_follow[i]); - c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],0.0f,c->feedback.fric_rpm[i],0,c->dt); - c->output.out_fric[i]=c->output.out_follow[i]; - c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]); - MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]); + PID_ResetIntegral(&s->pid.fric_follow[i]); + s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt); + s->output.out_fric[i]=s->output.out_follow[i]; + s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); + MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); } - c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt); - MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig); + s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt); + s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt); + s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); + MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig); if(cmd->ready) - { - c->running_state=SHOOT_STATE_READY; + { + Shoot_ResetCalu(s); + Shoot_ResetIntegral(s); + Shoot_ResetOutput(s); + s->running_state=SHOOT_STATE_READY; } break; case SHOOT_STATE_READY:/*准备射击*/ + for(int i=0;i计算修正输出->加和、滤波、输出 */ - // c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt); - c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt); - - c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt); - ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]); - c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i]; - c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]); - MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]); + { /* 计算跟随输出、计算修正输出 */ + s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt); + s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt); + /* 按比例缩放并加和输出 */ + ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]); + s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i]; + /* 滤波 */ + s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); + /* 输出 */ + MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); } - c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt); - MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig); + /* 拨弹电机输出 */ + s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt); + s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt); + s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); + MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig); + + /* 检查状态机 */ if(!cmd->ready) { - c->running_state=SHOOT_STATE_IDLE; + Shoot_ResetCalu(s); + Shoot_ResetOutput(s); + s->running_state=SHOOT_STATE_IDLE; } - else if(cmd->last_firecmd==false&&cmd->firecmd==true)//可以加一个到达目标速度的判断 + else if(last_firecmd==false&&cmd->firecmd==true) { - c->running_state=SHOOT_STATE_FIRE; - c->target_variable.target_angle-=c->param->trig_step_angle; + Shoot_ResetCalu(s); + Shoot_ResetOutput(s); + s->running_state=SHOOT_STATE_FIRE; + s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num; + } break; case SHOOT_STATE_FIRE: -// c->target_variable.target_angle+=c->param->trig_step_angle; + Shoot_CaluTargetAngle(s, cmd); for(int i=0;ioutput.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt); - c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt); - c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt); - ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]); - c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i]; - c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]); - MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]); + s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt); + s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt); + ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]); + s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i]; + s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]); + MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]); } - c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt); - MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig); + s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt); + s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt); + s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); + MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig); if(!cmd->firecmd) { - c->running_state=SHOOT_STATE_READY; + Shoot_ResetCalu(s); + Shoot_ResetOutput(s); + s->running_state=SHOOT_STATE_READY; } break; default: - c->running_state=SHOOT_STATE_IDLE; + s->running_state=SHOOT_STATE_IDLE; break; } } - MOTOR_RM_Ctrl(&c->param->fric_motor_param[0]); - // BSP_TIME_Delay_us(200); - MOTOR_RM_Ctrl(&c->param->trig_motor_param); - + MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]); + MOTOR_RM_Ctrl(&s->param->trig_motor_param); + last_firecmd = cmd->firecmd; return 0; } diff --git a/User/module/shoot.h b/User/module/shoot.h index 69fd47d..3c11802 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -23,7 +23,7 @@ extern "C" { #define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */ #define MAX_FRIC_RPM 7000.0f -#define MAX_TRIG_RPM 1000.0f +#define MAX_TRIG_RPM 5000.0f /* Exported macro ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */ @@ -45,23 +45,28 @@ typedef struct { bool ready; /* 准备射击 */ bool firecmd; /* 射击指令 */ - bool last_firecmd; - } Shoot_CMD_t; - typedef struct { MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */ - MOTOR_RM_t *trig; /* 拨弹电机反馈 */ + MOTOR_Feedback_t trig; /* 拨弹电机反馈 */ float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */ float fil_trig_rpm; /* 滤波后的拨弹电机转速*/ + float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */ + float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */ float fric_avgrpm; /* 归一化摩擦轮平均转速*/ float trig_rpm; /* 归一化拨弹电机转速*/ }Shoot_Feedback_t; +typedef struct{ + float time_last_shoot; + uint8_t num_to_shoot; + uint8_t num_shooted; +}Shoot_AngleCalu_t; + typedef struct { float out_follow[SHOOT_FRIC_NUM]; float out_err[SHOOT_FRIC_NUM]; @@ -69,14 +74,17 @@ typedef struct { float lpfout_fric[SHOOT_FRIC_NUM]; - float out_trig; - float lpfout_trig; + float outagl_trig; + float outomg_trig; + float outlpf_trig; }Shoot_Output_t; /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */ typedef struct { - float trig_step_angle; /* 每次拨弹电机转动的角度 */ + float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */ + float shot_delay_time; /* 射击间隔时间，单位秒 */ + uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */ MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM]; MOTOR_RM_Param_t trig_motor_param; @@ -85,7 +93,7 @@ typedef struct { KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */ KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */ KPID_Params_t trig; /* 拨弹电机PID控制参数 */ - + KPID_Params_t trig_omg; /* 拨弹电机PID控制参数 */ /* 低通滤波器截止频率 */ struct { @@ -107,8 +115,9 @@ typedef struct { typedef struct { bool online; + float now; uint64_t lask_wakeup; - float dt; + float dt; Shoot_Params_t *param; /* */ /* 模块通用 */ @@ -117,6 +126,7 @@ typedef struct { /* 反馈信息 */ Shoot_Feedback_t feedback; /* 控制信息*/ + Shoot_AngleCalu_t shoot_Anglecalu; Shoot_Output_t output; /* 目标控制量 */ struct { @@ -129,6 +139,7 @@ typedef struct { KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */ KPID_t fric_err[SHOOT_FRIC_NUM]; /* */ KPID_t trig; + KPID_t trig_omg; } pid; /* 滤波器 */ @@ -152,32 +163,32 @@ typedef struct { /** * \brief 初始化发射 * - * \param c 包含发射数据的结构体 + * \param s 包含发射数据的结构体 * \param param 包含发射参数的结构体指针 * \param target_freq 任务预期的运行频率 * * \return 函数运行结果 */ -int8_t Shoot_Init(Shoot_t *c, Shoot_Params_t *param, float target_freq); +int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq); /** * \brief 更新反馈 * - * \param c 包含发射数据的结构体 + * \param s 包含发射数据的结构体 * * \return 函数运行结果 */ -int8_t Chassis_UpdateFeedback(Shoot_t *c); +int8_t Chassis_UpdateFeedback(Shoot_t *s); /** * \brief 初始化发射 * - * \param c 包含发射数据的结构体 + * \param s 包含发射数据的结构体 * \param cmd 包含发射命令的结构体 * * \return 函数运行结果 */ -int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd); +int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd); diff --git a/debug.jdebug b/debug.jdebug index dc4775f..584cc18 100644 --- a/debug.jdebug +++ b/debug.jdebug @@ -22,12 +22,12 @@ void OnProjectLoad (void) { // // Dialog-generated settings // - Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)"); - Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)"); Project.SetDevice ("STM32F407IG"); Project.SetHostIF ("USB", "207400620"); Project.SetTargetIF ("SWD"); Project.SetTIFSpeed ("4 MHz"); + Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)"); + Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)"); Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd"); // // User settings