191 lines
6.3 KiB
C
191 lines
6.3 KiB
C
|
||
#include "shoot_control.h"
|
||
|
||
#include "can.h"
|
||
#include "component/pid.h"
|
||
#include "component/filter.h"
|
||
#include "component/at9s_pro_cmd.h"
|
||
#include "device/motor_rm.h"
|
||
#include <math.h>
|
||
#include "bsp/dwt.h"
|
||
#include "device/vofa.h"
|
||
#define MAX_RPM_3508 7000.0f
|
||
uint32_t shoot_ctrl_cnt_last;
|
||
float shoot_ctrl_dt;
|
||
bool last_firecmd=false;
|
||
//要一个函数实现任意几个电机达到同样目标转速
|
||
KPID_t pid_shoot_3508_rpm[2];
|
||
KPID_Params_t pid_shoot_3508_rpm_params ={1.0f, 1.8f, 0.0f, 0.0f, 0.0f, 0.9f ,0.0f,-1.0};
|
||
|
||
KPID_t pid_shoot_3508_errrpm[2];
|
||
KPID_Params_t pid_shoot_3508_errrpm_params ={1.0f, 4.0f, 0.40f, 0.04f, 0.25f, 0.25f ,40.0f,-1.0};
|
||
|
||
KPID_t pid_shoot_3508_stop[2];
|
||
KPID_Params_t pid_shoot_3508_stop_params ={1, 1.2f, 0.5f, 0.01f, 0.0f, 0.0f ,0.0f,-1.0};
|
||
|
||
static LowPassFilter2p_t spd_lpf[2];
|
||
static LowPassFilter2p_t out_lpf[2];
|
||
|
||
typedef enum {
|
||
SHOOT_STATE_IDLE = 0, // 熄火
|
||
SHOOT_STATE_READY, // 准备射击
|
||
SHOOT_STATE_FIRE // 射击
|
||
} ShootState_t;
|
||
|
||
ShootState_t shoot_state = SHOOT_STATE_IDLE;
|
||
|
||
//状态机 熄火-准备射击-射击
|
||
MOTOR_RM_Param_t shoot_motor_param[2];
|
||
MOTOR_RM_t* shoot_motor_feedback[2];
|
||
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
|
||
void shoot_motor_Init(float freq)
|
||
{
|
||
// VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
|
||
DWT_Init(168);
|
||
BSP_CAN_Init();
|
||
shoot_motor_param[0].can=BSP_CAN_2;
|
||
shoot_motor_param[0].id=0x201;
|
||
shoot_motor_param[0].module=MOTOR_M3508;
|
||
shoot_motor_param[0].reverse=false;
|
||
shoot_motor_param[0].gear=false;
|
||
MOTOR_RM_Register(&shoot_motor_param[0]);
|
||
shoot_motor_param[1].can=BSP_CAN_2;
|
||
shoot_motor_param[1].id=0x202;
|
||
shoot_motor_param[1].module=MOTOR_M3508;
|
||
shoot_motor_param[1].reverse=true;
|
||
shoot_motor_param[1].gear=false;
|
||
MOTOR_RM_Register(&shoot_motor_param[1]);
|
||
for(int i=0;i<2;i++)
|
||
{
|
||
PID_Init(&pid_shoot_3508_rpm[i], KPID_MODE_CALC_D, freq,&pid_shoot_3508_rpm_params);
|
||
PID_Init(&pid_shoot_3508_errrpm[i], KPID_MODE_CALC_D, freq,&pid_shoot_3508_errrpm_params);
|
||
PID_Init(&pid_shoot_3508_stop[i], KPID_MODE_CALC_D, freq,&pid_shoot_3508_stop_params);
|
||
LowPassFilter2p_Init(&spd_lpf[i], 1000.0f, 30.0f);
|
||
LowPassFilter2p_Init(&out_lpf[i], 1000.0f, 30.0f);
|
||
}
|
||
|
||
}
|
||
float target_rpm=1000.0f;
|
||
float shoot_out[2]={0,0};
|
||
float shoot_errout[2];
|
||
float shoot_errlpfout[2];
|
||
float filtered_rpm[2];
|
||
|
||
float errtosee;
|
||
void shoot_control(COMP_AT9S_CMD_t *rc)
|
||
{
|
||
MOTOR_RM_UpdateAll();
|
||
errtosee = shoot_motor_feedback[0]->feedback.rotor_speed - shoot_motor_feedback[1]->feedback.rotor_speed;
|
||
if(rc->online)
|
||
{
|
||
float rel_rpm[2];
|
||
for(int i=0;i<2;i++)
|
||
{
|
||
|
||
filtered_rpm[i] = LowPassFilter2p_Apply(&spd_lpf[i], shoot_motor_feedback[i]->feedback.rotor_speed);
|
||
rel_rpm[i]=filtered_rpm[i]/MAX_RPM_3508;
|
||
if(rel_rpm[i]>1.0f)rel_rpm[i]=1.0f;
|
||
if(rel_rpm[i]<-1.0f)rel_rpm[i]=-1.0f;
|
||
}
|
||
float err_out[2];
|
||
double rpm_avg;
|
||
switch(shoot_state)
|
||
{
|
||
case SHOOT_STATE_IDLE:
|
||
|
||
//位置闭写成环
|
||
for(int i=0;i<2;i++)
|
||
{
|
||
PID_ResetIntegral(&pid_shoot_3508_errrpm[i]);
|
||
MOTOR_RM_Relax(&shoot_motor_param[i]);
|
||
}
|
||
|
||
if(rc->shoot.ready)
|
||
{
|
||
shoot_state=SHOOT_STATE_READY;
|
||
}
|
||
break;
|
||
case SHOOT_STATE_READY:
|
||
|
||
for(int i=0;i<2;i++)
|
||
{
|
||
shoot_motor_feedback[i] = MOTOR_RM_GetMotor(&shoot_motor_param[i]);
|
||
shoot_ctrl_dt=DWT_GetDeltaT(&shoot_ctrl_cnt_last);
|
||
shoot_out[i]=PID_Calc(&pid_shoot_3508_rpm[i],target_rpm/MAX_RPM_3508,rel_rpm[i],0,shoot_ctrl_dt);
|
||
for(int i=0;i<2;i++){
|
||
rpm_avg=(rel_rpm[0]+rel_rpm[1])/2.0f;
|
||
}
|
||
shoot_errout[i]=PID_Calc(&pid_shoot_3508_errrpm[i],rpm_avg,rel_rpm[i],0,shoot_ctrl_dt);
|
||
/*若输出和大于1，按比例缩放，写成math里的函数*/
|
||
shoot_errlpfout[i] = LowPassFilter2p_Apply(&out_lpf[i], shoot_errout[i]);
|
||
ScaleSumTo1(&shoot_out[i], &shoot_errlpfout[i]);
|
||
err_out[i] = shoot_out[i] + shoot_errout[i];
|
||
// err_out[i] = LowPassFilter2p_Apply(&out_lpf[i], err_out[i]);
|
||
MOTOR_RM_SetOutput(&shoot_motor_param[i], err_out[i]);
|
||
}
|
||
if(!rc->shoot.ready)
|
||
{
|
||
shoot_state=SHOOT_STATE_IDLE;
|
||
}
|
||
else if(last_firecmd==false&&rc->shoot.firecmd==true)//可以加一个到达目标速度的判断
|
||
{
|
||
shoot_state=SHOOT_STATE_FIRE;
|
||
}
|
||
break;
|
||
case SHOOT_STATE_FIRE:
|
||
for(int i=0;i<2;i++)
|
||
{
|
||
shoot_motor_feedback[i] = MOTOR_RM_GetMotor(&shoot_motor_param[i]);
|
||
shoot_ctrl_dt=DWT_GetDeltaT(&shoot_ctrl_cnt_last);
|
||
shoot_out[i]=PID_Calc(&pid_shoot_3508_rpm[i],target_rpm/MAX_RPM_3508,rel_rpm[i],0,shoot_ctrl_dt);
|
||
for(int i=0;i<2;i++){
|
||
rpm_avg=(rel_rpm[0]+rel_rpm[1])/2.0f;
|
||
}
|
||
shoot_errout[i]=PID_Calc(&pid_shoot_3508_errrpm[i],rpm_avg,rel_rpm[i],0,shoot_ctrl_dt);
|
||
/*若输出和大于1，按比例缩放，写成math里的函数*/
|
||
ScaleSumTo1(&shoot_out[i], &shoot_errout[i]);
|
||
err_out[i] = shoot_out[i];
|
||
MOTOR_RM_SetOutput(&shoot_motor_param[i], err_out[i]);
|
||
}
|
||
|
||
shoot_state=SHOOT_STATE_READY;
|
||
|
||
break;
|
||
default:
|
||
shoot_state=SHOOT_STATE_IDLE;
|
||
break;
|
||
}
|
||
last_firecmd=rc->shoot.firecmd;
|
||
|
||
|
||
|
||
}else{
|
||
shoot_state=SHOOT_STATE_IDLE;
|
||
|
||
for(int i=0;i<2;i++)
|
||
{
|
||
PID_Reset(&pid_shoot_3508_rpm[i]);
|
||
PID_Reset(&pid_shoot_3508_stop[i]);
|
||
MOTOR_RM_Relax(&shoot_motor_param[0]);
|
||
}
|
||
}
|
||
MOTOR_RM_Ctrl(&shoot_motor_param[0]);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|