// /* // * 射击模组 // */ // /* Includes ----------------------------------------------------------------- */ // #include "shoot.h" // #include "bsp/pwm.h" // #include "bsp/time.h" // #include "bsp/can.h" // #include "component/pid.h" // #include "component/limiter.h" // #include "component/user_math.h" // #include "device/motor_rm.h" // #include // /* Private typedef ---------------------------------------------------------- */ // /* Private define ----------------------------------------------------------- */ // #define BULLET_SPEED_LIMIT_17MM (25.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); // PID_Reset(&(s->pid.fric[0])); // PID_Reset(&(s->pid.fric[1])); // LowPassFilter2p_Reset(&(s->filter.in.fric[0]), 0.0f); // LowPassFilter2p_Reset(&(s->filter.in.fric[1]), 0.0f); // LowPassFilter2p_Reset(&(s->filter.out.fric[0]), 0.0f); // LowPassFilter2p_Reset(&(s->filter.out.fric[1]), 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; // } // // static float Shoot_CalcRPMFromSpeed(float speed, float radius, float *rpm) { // // if (rpm == NULL) return -1; // // if (radius <= 0.0f) return -2; // // /* 限制弹丸速度 */ // // speed = Limit_Abs(speed, BULLET_SPEED_LIMIT_17MM); // // /* 计算转速 */ // // *rpm = speed / (2.0f * M_PI * radius) * 60.0f; // // 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; /* 设置默认模式 */ // PID_Init(&(s->pid.fric[0]), KPID_MODE_NO_D, target_freq, // &(param->fric_pid_param)); // PID_Init(&(s->pid.fric[1]), KPID_MODE_NO_D, target_freq, // &(param->fric_pid_param)); // PID_Init(&(s->pid.trig), KPID_MODE_CALC_D, target_freq, // &(param->trig_pid_param)); // LowPassFilter2p_Init(&(s->filter.in.fric[0]), target_freq, // param->low_pass_cutoff_freq.in.fric); // LowPassFilter2p_Init(&(s->filter.in.fric[1]), target_freq, // param->low_pass_cutoff_freq.in.fric); // LowPassFilter2p_Init(&(s->filter.out.fric[0]), target_freq, // param->low_pass_cutoff_freq.out.fric); // LowPassFilter2p_Init(&(s->filter.out.fric[1]), target_freq, // param->low_pass_cutoff_freq.out.fric); // 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_CAN_Init(); // MOTOR_RM_Register(s->param->fric_motor_param[0]); // MOTOR_RM_Register(s->param->fric_motor_param[1]); // MOTOR_RM_Register(s->param->trig_motor_param); // return SHOOT_OK; // } // /** // * \brief 更新射击的反馈信息 // * // * \param s 包含射击数据的结构体 // * // * \return 函数运行结果 // */ // int8_t Shoot_UpdateFeedback(Shoot_t *s) { // if (s == NULL) return -1; // MOTOR_RM_Update(s->param->fric_motor_param[0]); // MOTOR_RM_Update(s->param->fric_motor_param[1]); // MOTOR_RM_Update(s->param->trig_motor_param); // MOTOR_RM_t *motor; // motor = MOTOR_RM_GetMotor(s->param->fric_motor_param[0]); // s->feedback.fric[0] = motor->feedback; // motor = MOTOR_RM_GetMotor(s->param->fric_motor_param[1]); // s->feedback.fric[1] = motor->feedback; // motor = MOTOR_RM_GetMotor(s->param->trig_motor_param); // s->feedback.trig = motor->feedback; // return SHOOT_OK; // } // /** // * \brief 运行射击控制逻辑 // * // * \param s 包含射击数据的结构体 // * \param s_cmd 射击控制指令 // * // * \return 函数运行结果 // */ // int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *s_cmd) { // if (s == NULL) return -1; // if (s_cmd == NULL) return -1; // Shoot_SetMode(s, s_cmd->mode); /* 设置射击模式 */ // /* 计算摩擦轮转速的目标值 */ // // 根据弹丸射速计算转速，这里需要实现具体的计算逻辑 // // s->setpoint.fric_rpm[0] = calculate_rpm_from_speed(s_cmd->bullet_speed); // // s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[0]; // switch (s->mode) { // case SHOOT_MODE_RELAX: // MOTOR_RM_SetOutput(s->param->fric_motor_param[0], 0.0f); // MOTOR_RM_SetOutput(s->param->fric_motor_param[1], 0.0f); // MOTOR_RM_SetOutput(s->param->trig_motor_param, 0.0f); // break; // case SHOOT_MODE_SAFE: // /*摩擦轮速度环到0，拨弹位置环到设定位置*/ // s->setpoint.fric_rpm[0] = 0.0f; // s->setpoint.fric_rpm[1] = 0.0f; // break; // case SHOOT_MODE_LOADED: // // TODO: 实现上膛模式的控制逻辑 // break; // } // return SHOOT_OK; // } // /** // * \brief 射击输出值 // * // * \param s 包含射击数据的结构体 // */ // void Shoot_Output(Shoot_t *s) { // if (s == NULL) return; // MOTOR_RM_Ctrl(s->param->fric_motor_param[0]); // }