/* * 射击模组 */ /* Includes ----------------------------------------------------------------- */ #include "shoot.h" #include "bsp/pwm.h" #include "component\limiter.h" #include "component\user_math.h" /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ #define HEAT_INCREASE_42MM (100.f) /* 每发射一颗42mm弹丸增加100热量 */ #define HEAT_INCREASE_17MM (10.f) /* 每发射一颗17mm弹丸增加10热量 */ #define BULLET_SPEED_LIMIT_42MM (16.0) #define BULLET_SPEED_LIMIT_17MM (30.0) /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */ /** * \brief 设置射击模式 * * \param c 包含射击数据的结构体 * \param mode 要设置的模式 * * \return 函数运行结果 */ static int8_t Shoot_SetMode(Shoot_t *s, CMD_ShootMode_t mode) { if (s == NULL) return -1; if (mode == s->mode) return SHOOT_OK; /* 切换模式后重置PID和滤波器 */ for (uint8_t i = 0; i < 2; i++) { PID_Reset(s->pid.fric + i); LowPassFilter2p_Reset(s->filter.in.fric + i, 0.0f); LowPassFilter2p_Reset(s->filter.out.fric + i, 0.0f); } PID_Reset(&(s->pid.trig)); LowPassFilter2p_Reset(&(s->filter.in.trig), 0.0f); LowPassFilter2p_Reset(&(s->filter.out.trig), 0.0f); while (fabsf(CircleError(s->setpoint.trig_angle, s->feedback.trig_angle, M_2PI)) >= M_2PI / s->param->num_trig_tooth / 2.0f) { CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth, M_2PI); } if (mode == SHOOT_MODE_LOADED) s->fire_ctrl.to_shoot = 0; s->mode = mode; return 0; } /** * @brief * * @param s * @param s_ref * @return int8_t */ static int8_t Shoot_HeatLimit(Shoot_t *s, Referee_ForShoot_t *s_ref) { Shoot_HeatCtrl_t *hc = &(s->heat_ctrl); /* 当裁判系统在线时启用热量控制与射速控制 */ if (s_ref->ref_status == REF_STATUS_RUNNING) { /* 根据机器人型号获得对应数据 */ if (s->param->model == SHOOT_MODEL_42MM) { hc->heat = s_ref->power_heat.shoot_42_heat; hc->heat_limit = s_ref->robot_status.shooter_heat_limit; hc->speed_limit = BULLET_SPEED_LIMIT_42MM; hc->cooling_rate = s_ref->robot_status.shooter_cooling_value; hc->heat_increase = HEAT_INCREASE_42MM; } else if (s->param->model == SHOOT_MODEL_17MM) { hc->heat = s_ref->power_heat.shoot_id1_17_heat; hc->heat_limit = s_ref->robot_status.shooter_heat_limit; hc->speed_limit = BULLET_SPEED_LIMIT_17MM; hc->cooling_rate = s_ref->robot_status.shooter_cooling_value; hc->heat_increase = HEAT_INCREASE_17MM; } /* 检测热量更新后,计算可发射弹丸 */ if ((hc->heat != hc->last_heat) || (hc->heat == 0)) { hc->available_shot = (uint32_t)floorf((hc->heat_limit - hc->heat) / hc->heat_increase); hc->last_heat = hc->heat; } /* 计算已发射弹丸 */ if (s_ref->shoot_data.bullet_speed != hc->last_bullet_speed) { hc->last_bullet_speed = s_ref->shoot_data.bullet_speed; } s->fire_ctrl.bullet_speed = hc->speed_limit; } else { /* 裁判系统离线,不启用热量控制 */ hc->available_shot = 10; s->fire_ctrl.bullet_speed = s->param->bullet_speed; } return 0; } /* Exported functions ------------------------------------------------------- */ /** * \brief 初始化射击 * * \param s 包含射击数据的结构体 * \param param 包含射击参数的结构体指针 * \param target_freq 任务预期的运行频率 * * \return 函数运行结果 */ int8_t Shoot_Init(Shoot_t *s, const Shoot_Params_t *param, float target_freq) { if (s == NULL) return -1; s->param = param; /* 初始化参数 */ s->mode = SHOOT_MODE_RELAX; /* 设置默认模式 */ for (uint8_t i = 0; i < 2; i++) { /* PI控制器初始化PID */ PID_Init(s->pid.fric + i, KPID_MODE_NO_D, target_freq, &(param->fric_pid_param)); LowPassFilter2p_Init(s->filter.in.fric + i, target_freq, param->low_pass_cutoff_freq.in.fric); LowPassFilter2p_Init(s->filter.out.fric + i, target_freq, param->low_pass_cutoff_freq.out.fric); } PID_Init(&(s->pid.trig), KPID_MODE_CALC_D, target_freq, &(param->trig_pid_param)); LowPassFilter2p_Init(&(s->filter.in.trig), target_freq, param->low_pass_cutoff_freq.in.trig); LowPassFilter2p_Init(&(s->filter.out.trig), target_freq, param->low_pass_cutoff_freq.out.trig); BSP_PWM_Start(BSP_PWM_SHOOT_SERVO); BSP_PWM_Set(BSP_PWM_SHOOT_SERVO, param->cover_close_duty); return 0; } /** * \brief 更新射击的反馈信息 * * \param s 包含射击数据的结构体 * \param can CAN设备结构体 * * \return 函数运行结果 */ int8_t Shoot_UpdateFeedback(Shoot_t *s, const CAN_t *can) { if (s == NULL) return -1; if (can == NULL) return -1; for (uint8_t i = 0; i < 2; i++) { s->feedback.fric_rpm[i] = can->motor.shoot.as_array[i].rotor_speed; } /* 更新拨弹电机 */ float last_trig_motor_angle = s->feedback.trig_motor_angle; s->feedback.trig_motor_angle = can->motor.shoot.named.trig.rotor_angle; float motor_angle_delta = CircleError(s->feedback.trig_motor_angle, last_trig_motor_angle, M_2PI); CircleAdd(&(s->feedback.trig_angle), motor_angle_delta / s->param->trig_gear_ratio, M_2PI); return 0; } /** * @brief 运行射击控制逻辑 * * @param s 包含射击数据的结构体 * @param s_cmd 射击控制指令 * @param s_ref 射击使用的裁判系统数据 * @param now 现在时刻 * @return int8_t */ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd, Referee_ForShoot_t *s_ref, uint32_t now) { if (s == NULL) return -1; s->dt = (float)(now - s->lask_wakeup) / 1000.0f; s->lask_wakeup = now; Shoot_SetMode(s, s_cmd->mode); /* 设置射击模式 */ Shoot_HeatLimit(s, s_ref); /* 热量控制 */ /* 根据开火模式计算发射行为 */ s->fire_ctrl.fire_mode = s_cmd->fire_mode; int32_t max_burst; switch (s_cmd->fire_mode) { case FIRE_MODE_SINGLE: /* 点射开火模式 */ max_burst = 1; break; case FIRE_MODE_BURST: /* 连发开火模式 */ max_burst = 5; break; default: break; } switch (s_cmd->fire_mode) { case FIRE_MODE_SINGLE: /* 点射开火模式 */ case FIRE_MODE_BURST: { /* 连发开火模式 */ s->fire_ctrl.first_fire = s_cmd->fire && !s->fire_ctrl.last_fire; s->fire_ctrl.last_fire = s_cmd->fire; s_cmd->fire = s->fire_ctrl.first_fire; int32_t max_shot = s->heat_ctrl.available_shot - s->fire_ctrl.shooted; if (s_cmd->fire && !s->fire_ctrl.to_shoot) { s->fire_ctrl.to_shoot = min(max_burst, max_shot); } if (s->fire_ctrl.shooted >= s->fire_ctrl.to_shoot) { s_cmd->fire = false; s->fire_ctrl.period_ms = UINT32_MAX; s->fire_ctrl.shooted = 0; s->fire_ctrl.to_shoot = 0; } else { s_cmd->fire = true; s->fire_ctrl.period_ms = s->param->min_shoot_delay; } break; } case FIRE_MODE_CONT: { /* 持续开火模式 */ float shoot_freq = HeatLimit_ShootFreq( s->heat_ctrl.heat, s->heat_ctrl.heat_limit, s->heat_ctrl.cooling_rate, s->heat_ctrl.heat_increase, s->param->model == SHOOT_MODEL_17MM); s->fire_ctrl.period_ms = (shoot_freq == 0.0f) ? UINT32_MAX : (uint32_t)(1000.f / shoot_freq); break; } default: break; } /* 根据模式选择是否使用计算出来的值 */ switch (s->mode) { case SHOOT_MODE_RELAX: case SHOOT_MODE_SAFE: s->fire_ctrl.bullet_speed = 0.0f; s->fire_ctrl.period_ms = UINT32_MAX; case SHOOT_MODE_LOADED: break; } /* 计算摩擦轮转速的目标值 */ s->setpoint.fric_rpm[1] = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius, (s->param->model == SHOOT_MODEL_17MM)); s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1]; /* 计算拨弹电机位置的目标值 */ if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) && (s_cmd->fire)) { /* 将拨弹电机角度进行循环加法,每次加(减)射出一颗弹丸的弧度变化 */ if (s_cmd->reverse_trig) { /* 反转拨弹 */ CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth, M_2PI); } else { CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth, M_2PI); s->fire_ctrl.shooted++; s->fire_ctrl.last_shoot = now; } } switch (s->mode) { case SHOOT_MODE_RELAX: for (uint8_t i = 0; i < SHOOT_ACTR_NUM; i++) { s->out[i] = 0.0f; } BSP_PWM_Stop(BSP_PWM_SHOOT_SERVO); break; case SHOOT_MODE_SAFE: case SHOOT_MODE_LOADED: /* 控制拨弹电机 */ s->feedback.trig_angle = LowPassFilter2p_Apply(&(s->filter.in.trig), s->feedback.trig_angle); s->out[SHOOT_ACTR_TRIG_IDX] = PID_Calc(&(s->pid.trig), s->setpoint.trig_angle, s->feedback.trig_angle, 0.0f, s->dt); s->out[SHOOT_ACTR_TRIG_IDX] = LowPassFilter2p_Apply( &(s->filter.out.trig), s->out[SHOOT_ACTR_TRIG_IDX]); for (uint8_t i = 0; i < 2; i++) { /* 控制摩擦轮 */ s->feedback.fric_rpm[i] = LowPassFilter2p_Apply( &(s->filter.in.fric[i]), s->feedback.fric_rpm[i]); s->out[SHOOT_ACTR_FRIC1_IDX + i] = PID_Calc(&(s->pid.fric[i]), s->setpoint.fric_rpm[i], s->feedback.fric_rpm[i], 0.0f, s->dt); s->out[SHOOT_ACTR_FRIC1_IDX + i] = LowPassFilter2p_Apply( &(s->filter.out.fric[i]), s->out[SHOOT_ACTR_FRIC1_IDX + i]); } /* 根据弹仓盖开关状态更新弹舱盖打开时舵机PWM占空比 */ if (s_cmd->cover_open) { BSP_PWM_Start(BSP_PWM_SHOOT_SERVO); BSP_PWM_Set(BSP_PWM_SHOOT_SERVO, s->param->cover_open_duty); } else { BSP_PWM_Start(BSP_PWM_SHOOT_SERVO); BSP_PWM_Set(BSP_PWM_SHOOT_SERVO, s->param->cover_close_duty); } break; } return 0; } /** * \brief 复制射击输出值 * * \param s 包含射击数据的结构体 * \param out CAN设备射击输出结构体 */ void Shoot_DumpOutput(Shoot_t *s, CAN_ShootOutput_t *out) { for (uint8_t i = 0; i < SHOOT_ACTR_NUM; i++) { out->as_array[i] = s->out[i]; } } /** * \brief 清空输出值 * * \param output 要清空的结构体 */ void Shoot_ResetOutput(CAN_ShootOutput_t *output) { int i = 0; for (i = 0; i < 3; i++) { output->as_array[i] = 0.0f; } } /** * @brief 导出射击UI数据 * * @param s 射击结构体 * @param ui UI结构体 */ void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui) { ui->mode = s->mode; ui->fire = s->fire_ctrl.fire_mode; }