diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 3d13cf6..472bb43 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -3,6 +3,7 @@ */ #include "cmd.h" #include "bsp/time.h" +#include "module/shoot.h" #include #include @@ -68,7 +69,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT static void CMD_RC_BuildShootCmd(CMD_t *ctx) { if (ctx->input.online[CMD_SRC_RC]) { - ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST; + ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; } else { ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; } diff --git a/User/module/config.c b/User/module/config.c index b16f7ed..f6f151c 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -140,7 +140,7 @@ Config_RobotParam_t robot_config = { .fric_num=2, .extra_deceleration_ratio=1.0f, .num_trig_tooth=8, - .shot_freq=1.0f, + .shot_freq=18.0f, .shot_burst_num=3, .ratio_multilevel = {1.0f}, }, diff --git a/User/module/shoot.c b/User/module/shoot.c index 6470efd..2896b7c 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -519,10 +519,11 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) float dt = s->timer.now - s->var_trig.time_lastShoot; float dpos; + float dpos_abs; dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); - + dpos_abs = fabs(dpos); /* 使用热量控制计算出的射频,而不是配置的固定射频 */ - if(actual_freq > 0.0f && dt >= 1.0f/actual_freq && cmd->firecmd && dpos<=1.0f) + if(actual_freq > 0.0f && dt >= 1.0f/actual_freq && cmd->firecmd && dpos_abs<=1.0f) { s->var_trig.time_lastShoot=s->timer.now; CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); @@ -720,6 +721,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) else if(last_firecmd==false&&cmd->firecmd==true) { s->running_state=SHOOT_STATE_FIRE; + s->target_variable.trig_angle=s->var_trig.trig_agl; /* 根据模式设置待发射弹数 */ switch(s->mode) { @@ -786,6 +788,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) { s->running_state=SHOOT_STATE_READY; pos=s->var_trig.trig_agl; + s->target_variable.trig_angle=pos; s->var_trig.num_toShoot=0; } break; @@ -796,12 +799,12 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) } } /* 输出 */ - // MOTOR_RM_Ctrl(&s->param->motor.fric[0].param); - // if(s->param->basic.fric_num>4) - // { - // MOTOR_RM_Ctrl(&s->param->motor.fric[4].param); - // } - // MOTOR_RM_Ctrl(&s->param->motor.trig); + MOTOR_RM_Ctrl(&s->param->motor.fric[0].param); + if(s->param->basic.fric_num>4) + { + MOTOR_RM_Ctrl(&s->param->motor.fric[4].param); + } + MOTOR_RM_Ctrl(&s->param->motor.trig); last_firecmd = cmd->firecmd; return SHOOT_OK; } @@ -849,7 +852,8 @@ int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd) /* 清空待发射弹 */ s->var_trig.num_toShoot=0; /* 修改拨弹盘目标角度 */ - s->target_variable.trig_angle = s->var_trig.trig_agl-(M_2PI/s->param->basic.num_trig_tooth); + s->target_variable.trig_angle = s->var_trig.trig_agl; + CircleAdd((&s->target_variable.trig_angle), -(M_2PI/s->param->basic.num_trig_tooth), M_2PI) /* 切换状态 */ s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_DEAL; /* 记录处理开始时间 */