304 lines
11 KiB
C
304 lines
11 KiB
C
|
|
#include "shoot_control.h"
|
|
#include <string.h>
|
|
#include "can.h"
|
|
#include "component/filter.h"
|
|
#include "component/user_math.h"
|
|
#include <math.h>
|
|
#include "bsp/time.h"
|
|
|
|
static bool last_firecmd;
|
|
|
|
static inline void ScaleSumTo1(float *a, float *b) {
|
|
float sum = *a + *b;
|
|
if (sum > 1.0f) {
|
|
float scale = 1.0f / sum;
|
|
*a *= scale;
|
|
*b *= scale;
|
|
}
|
|
}
|
|
|
|
|
|
int8_t Shoot_SetMode(shoot_t *s, Shoot_Mode_t mode)
|
|
{
|
|
if (s == NULL) {
|
|
return -1; // 参数错误
|
|
}
|
|
s->mode=mode;
|
|
return 0;
|
|
}
|
|
|
|
int8_t Shoot_ResetIntegral(shoot_t *s)
|
|
{
|
|
if (s == NULL) {
|
|
return -1; // 参数错误
|
|
}
|
|
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
{
|
|
PID_ResetIntegral(&s->pid.fric_follow[i]);
|
|
PID_ResetIntegral(&s->pid.fric_err[i]);
|
|
}
|
|
PID_ResetIntegral(&s->pid.trig);
|
|
return 0;
|
|
}
|
|
|
|
int8_t Shoot_ResetCalu(shoot_t *s)
|
|
{
|
|
if (s == NULL) {
|
|
return -1; // 参数错误
|
|
}
|
|
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
{
|
|
PID_Reset(&s->pid.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);
|
|
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;i<SHOOT_FRIC_NUM;i++)
|
|
{
|
|
s->output.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.out_trig=0.0f;
|
|
s->output.lpfout_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=target_speed;
|
|
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;i<SHOOT_FRIC_NUM;i++){
|
|
MOTOR_RM_Register(¶m->fric_motor_param[i]);
|
|
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(&s->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig);
|
|
|
|
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));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int8_t Chassis_UpdateFeedback(shoot_t *s)
|
|
{
|
|
if (s == NULL) {
|
|
return -1; // 参数错误
|
|
}
|
|
float rpm_sum=0.0f;
|
|
for(int i = 0; i < SHOOT_FRIC_NUM; 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)
|
|
{
|
|
s->feedback.fric[i]=motor_fed->motor.feedback;
|
|
}
|
|
/* 滤波反馈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];
|
|
}
|
|
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->motor.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.trig_angle_cicle = fmodf(s->feedback.trig.rotor_abs_angle, M_2PI);
|
|
// if(s->feedback.trig_angle_cicle<0.0f)
|
|
// {
|
|
// s->feedback.trig_angle_cicle+=M_2PI;
|
|
// }
|
|
// s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
|
|
// CircleAdd(&s->feedback.trig_angle_cicle, 0.0f, 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;
|
|
}
|
|
float a;
|
|
int8_t Shoot_Control(shoot_t *s, Shoot_CMD_t *cmd)
|
|
{
|
|
if (s == NULL || cmd == NULL) {
|
|
return -1; // 参数错误
|
|
}
|
|
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->running_state = cmd->state;
|
|
s->online = cmd->online;
|
|
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
|
|
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
{
|
|
MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
|
|
}
|
|
MOTOR_RM_Relax(&s->param->trig_motor_param);
|
|
}
|
|
else{
|
|
a=CircleError(s->target_variable.target_angle, s->feedback.trig_angle_cicle, M_2PI);
|
|
switch(s->running_state)
|
|
{
|
|
case SHOOT_STATE_IDLE:/*熄火等待*/
|
|
for(int i=0;i<SHOOT_FRIC_NUM;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]);
|
|
}
|
|
s->output.out_trig=PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
|
|
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.out_trig);
|
|
if(cmd->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<SHOOT_FRIC_NUM;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]);
|
|
}
|
|
/* 拨弹电机输出 */
|
|
s->output.out_trig=PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
|
|
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.out_trig);
|
|
|
|
/* 检查状态机 */
|
|
if(!cmd->ready)
|
|
{
|
|
// Shoot_ResetCalu(s);
|
|
// Shoot_ResetOutput(s);
|
|
s->running_state=SHOOT_STATE_IDLE;
|
|
}
|
|
else if(last_firecmd==false&&cmd->firecmd==true)
|
|
{
|
|
Shoot_ResetCalu(s);
|
|
Shoot_ResetOutput(s);
|
|
s->running_state=SHOOT_STATE_FIRE;
|
|
s->shoot_Anglecalu.num_to_shoot+=1;
|
|
|
|
}
|
|
break;
|
|
case SHOOT_STATE_FIRE:
|
|
Shoot_CaluTargetAngle(s, cmd);
|
|
for(int i=0;i<SHOOT_FRIC_NUM;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]);
|
|
}
|
|
s->output.out_trig=PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
|
|
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.out_trig);
|
|
if(!cmd->firecmd)
|
|
{
|
|
Shoot_ResetCalu(s);
|
|
Shoot_ResetOutput(s);
|
|
s->running_state=SHOOT_STATE_READY;
|
|
}
|
|
break;
|
|
default:
|
|
s->running_state=SHOOT_STATE_IDLE;
|
|
break;
|
|
}
|
|
}
|
|
MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
|
|
last_firecmd = cmd->firecmd;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|