robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bc

Browse Source
This commit is contained in:
xxxxm 2026-03-14 12:41:14 +08:00
parent 96da4ad6dc
commit abc8885795
5 changed files with 133 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
User/device/referee.h
View File

@ -574,11 +574,6 @@ typedef struct {
osTimerId_t ui_slow_timer_id; osTimerId_t ui_slow_timer_id;
} Referee_t; } Referee_t;
/* Referee_ChassisUI_t 已移至 module/chassis.h */
/* Referee_CapUI_t 已移至 device/supercap.h */
/* Referee_GimbalUI_t 已移至 module/gimbal.h */
/* Referee_ShootUI_t 已移至 module/shoot.h */
typedef struct __packed { typedef struct __packed {
/* UI缓冲数据 */ /* UI缓冲数据 */
UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM]; UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM];

19
User/module/cmd/cmd.c
View File

@ -59,8 +59,8 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
} }
/* 左摇杆控制云台 */ /* 左摇杆控制云台 */
ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f; ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_right.x * 5.0f;
ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f; ctx->output.gimbal.cmd.delta_pit = ctx->input.rc.joy_right.y * 5.0f;
} }
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */ #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */
@ -68,7 +68,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT
static void CMD_RC_BuildShootCmd(CMD_t *ctx) { static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
if (ctx->input.online[CMD_SRC_RC]) { if (ctx->input.online[CMD_SRC_RC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
} else { } else {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
} }
@ -393,12 +393,13 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) {
float vx = 0.0f, wz = 0.0f; float vx = 0.0f, wz = 0.0f;
uint32_t kb = ctx->input.pc.keyboard.bitmap; uint32_t kb = ctx->input.pc.keyboard.bitmap;
if (kb & CMD_KEY_W) vx += sens->move_sens; //案件的命令要放到behavior里，
if (kb & CMD_KEY_S) vx -= sens->move_sens; // if (kb & CMD_KEY_W) vx += sens->move_sens;
if (kb & CMD_KEY_A) wz += sens->move_sens; // if (kb & CMD_KEY_S) vx -= sens->move_sens;
if (kb & CMD_KEY_D) wz -= sens->move_sens; // if (kb & CMD_KEY_A) wz += sens->move_sens;
if (kb & CMD_KEY_SHIFT) { vx *= sens->move_fast_mult; wz *= sens->move_fast_mult; } // if (kb & CMD_KEY_D) wz -= sens->move_sens;
if (kb & CMD_KEY_CTRL) { vx *= sens->move_slow_mult; wz *= sens->move_slow_mult; } // if (kb & CMD_KEY_SHIFT) { vx *= sens->move_fast_mult; wz *= sens->move_fast_mult; }
// if (kb & CMD_KEY_CTRL) { vx *= sens->move_slow_mult; wz *= sens->move_slow_mult; }
ctx->output.balance_chassis.cmd.move_vec.vx = vx; ctx->output.balance_chassis.cmd.move_vec.vx = vx;
ctx->output.balance_chassis.cmd.move_vec.wz = wz; ctx->output.balance_chassis.cmd.move_vec.wz = wz;

24
User/module/config.c
View File

@ -110,19 +110,19 @@ Config_RobotParam_t robot_config = {
.projectileType=SHOOT_PROJECTILE_17MM, .projectileType=SHOOT_PROJECTILE_17MM,
.fric_num=2, .fric_num=2,
.extra_deceleration_ratio=1.0f, .extra_deceleration_ratio=1.0f,
.num_trig_tooth=5, .num_trig_tooth=8,
.shot_freq=1.0f, .shot_freq=1.0f,
.shot_burst_num=3, .shot_burst_num=3,
.ratio_multilevel = {1.0f}, .ratio_multilevel = {1.0f},
}, },
.jamDetection={ .jamDetection={
.enable=true, .enable=true,
.threshold=310.0f, .threshold=200.0f,
.suspectedTime=0.5f, .suspectedTime=0.5f,
}, },
.heatControl={ .heatControl={
.enable=true, .enable=true,
.nmax=2.0f, // 最大射频 Hz .nmax=18.0f, // 最大射频 Hz
.Hwarn=200.0f, // 热量预警值 .Hwarn=200.0f, // 热量预警值
.Hsatu=100.0f, // 热量饱和值 .Hsatu=100.0f, // 热量饱和值
.Hthres=50.0f, // 热量阈值 .Hthres=50.0f, // 热量阈值
@ -151,10 +151,10 @@ Config_RobotParam_t robot_config = {
} }
}, },
.trig = { .trig = {
.can = BSP_FDCAN_2, .can = BSP_CAN_1,
.id = 0x205, .id = 0x203,
.module = MOTOR_M3508, .module = MOTOR_M2006,
.reverse = false, .reverse = true,
.gear=true, .gear=true,
}, },
}, },
@ -180,12 +180,12 @@ Config_RobotParam_t robot_config = {
.range=-1.0f, .range=-1.0f,
}, },
.trig_2006 = { .trig_2006 = {
.k=2.5f, .k=1.0f,
.p=1.0f, .p=1.0f,
.i=0.1f, .i=0.1f,
.d=0.04f, .d=0.05f,
.i_limit=0.4f, .i_limit=0.8f,
.out_limit=1.0f, .out_limit=0.5f,
.d_cutoff_freq=-1.0f, .d_cutoff_freq=-1.0f,
.range=M_2PI, .range=M_2PI,
}, },
@ -195,7 +195,7 @@ Config_RobotParam_t robot_config = {
.i=0.3f, .i=0.3f,
.d=0.5f, .d=0.5f,
.i_limit=0.2f, .i_limit=0.2f,
.out_limit=1.0f, .out_limit=0.9f,
.d_cutoff_freq=-1.0f, .d_cutoff_freq=-1.0f,
.range=-1.0f, .range=-1.0f,
}, },

183
User/module/shoot.c
View File

@ -46,7 +46,6 @@ void Task(void *argument) {
/* 发射检测参数 */ /* 发射检测参数 */
#define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */ #define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */
#define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */ #define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值，单位秒 */
#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
@ -236,40 +235,6 @@ static float Shoot_CalcAvgFricRpm(Shoot_t *s)
return (fric_num > 0) ? (sum / fric_num) : 0.0f; return (fric_num > 0) ? (sum / fric_num) : 0.0f;
} }
/**
* \brief 发射检测：通过摩擦轮掉速检测发射
*
* \param s 包含发射数据的结构体
*
* \return 是否检测到掉速超过阈值
*/
static bool Shoot_DetectShotByRpmDrop(Shoot_t *s)
{
if (s == NULL) {
return false;
}
/* 只在READY和FIRE状态下检测，避免停机时误判 */
if (s->running_state != SHOOT_STATE_READY &&
s->running_state != SHOOT_STATE_FIRE) {
return false;
}
/* 计算当前摩擦轮平均转速 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
/* 计算当前转速与目标转速的差值（掉速量） */
s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm;
/* 判断是否发生明显掉速 */
if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) {
return true;
}
return false;
}
/** /**
* \brief 热量数据融合：用裁判系统数据修正自主估计 * \brief 热量数据融合：用裁判系统数据修正自主估计
* *
@ -353,18 +318,29 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
/* 计算当前平均转速 */ /* 计算当前平均转速 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
/* 检查摩擦轮是否达到目标转速的85%以上 */ /* 检查摩擦轮是否初步达到设定速度 */
if (s->heatcontrol.avgFricRpm >= s->target_variable.fric_rpm * FRIC_READY_RPM_RATIO) { if (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
s->heatcontrol.lastAvgFricRpm = s->heatcontrol.avgFricRpm; s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; // 记录初始基准转速
} }
} }
break; break;
case SHOOT_HEAT_DETECT_READY: case SHOOT_HEAT_DETECT_READY:
/* 准备检测状态：监测摩擦轮掉速 */ /* 准备检测状态：监测摩擦轮相对当前的基准发生掉速 */
/* 检测掉速（当前转速低于目标转速超过阈值） */ s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
if (Shoot_DetectShotByRpmDrop(s)) {
/* 动态更新平稳时的基准转速：若当前转速比基准高，迅速成为新基准；若更低，基准非常缓慢地衰减，以适应正常波动 */
if (s->heatcontrol.avgFricRpm > s->heatcontrol.baselineRpm) {
s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm;
} else {
s->heatcontrol.baselineRpm = s->heatcontrol.baselineRpm * 0.999f + s->heatcontrol.avgFricRpm * 0.001f;
}
s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm;
/* 检测掉速（当前转速低于动态基准超过阈值） */
if (s->heatcontrol.rpmDrop > SHOT_DETECT_RPM_DROP_THRESHOLD) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_SUSPECTED;
s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */ s->heatcontrol.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */
} }
@ -377,45 +353,47 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
case SHOOT_HEAT_DETECT_SUSPECTED: case SHOOT_HEAT_DETECT_SUSPECTED:
/* 发射嫌疑状态：持续检测掉速 */ /* 发射嫌疑状态：持续检测掉速 */
/* 更新掉速量 */
s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
s->heatcontrol.rpmDrop = s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm; s->heatcontrol.rpmDrop = s->heatcontrol.baselineRpm - s->heatcontrol.avgFricRpm;
/* 若掉速消失（转速恢复正常），回到准备状态 */ /* 若掉速消失（转速刚掉下又马上弹回基准附近），回到准备状态 (阈值的 60%) */
if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) { if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.6f) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
} }
/* 若嫌疑状态持续超过阈值时间，确认发射 */ /* 若掉速状态持续超过设定的嫌疑时间，确认这是一次发射 */
else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) { else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED; s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED;
} }
break; break;
case SHOOT_HEAT_DETECT_CONFIRMED: case SHOOT_HEAT_DETECT_CONFIRMED:
/* 确认发射状态：增加热量并返回准备状态 */ if (s->heatcontrol.unverified_shots > 0) {
/* 根据弹丸类型增加估计热量 */ s->heatcontrol.unverified_shots--;
switch (s->param->basic.projectileType) { }
case SHOOT_PROJECTILE_17MM: s->heatcontrol.detectState = SHOOT_HEAT_DETECT_RECOVERING;
s->heatcontrol.Hnow_estimated += 10.0f; s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; /* 记录进入此时的转速为谷底初值 */
break; break;
case SHOOT_PROJECTILE_42MM:
s->heatcontrol.Hnow_estimated += 100.0f; case SHOOT_HEAT_DETECT_RECOVERING:
break;
default: s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s);
s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen;
break; if (s->heatcontrol.avgFricRpm < s->heatcontrol.valleyRpm) {
s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm;
} }
/* 限制估计热量不超过最大值 */ if ((s->heatcontrol.avgFricRpm - s->heatcontrol.valleyRpm > 30.0f) ||
if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f)) {
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm;
} }
/* 增加发射计数 */ /* 如果摩擦轮停止 */
s->heatcontrol.shots_detected++; if (s->running_state == SHOOT_STATE_IDLE) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE;
/* 返回准备检测状态 */ }
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY;
break; break;
default: default:
@ -423,7 +401,7 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
break; break;
} }
/* 善后工作：热量冷却（每个周期都执行） */ /* 热量冷却*/
if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) { if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) {
s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt; s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt;
if (s->heatcontrol.Hnow_estimated < 0.0f) { if (s->heatcontrol.Hnow_estimated < 0.0f) {
@ -431,6 +409,36 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
} }
} }
/* 空弹链检测与修正：拨盘触发后超过0.4秒内仍未有对应的摩擦轮掉速确认 */
if (s->heatcontrol.unverified_shots > 0 && (s->timer.now - s->var_trig.time_lastShoot) > 0.4f) {
/* 这些弹丸确定未射出（空发） */
float refund_heat = 0.0f;
switch (s->param->basic.projectileType) {
case SHOOT_PROJECTILE_17MM: refund_heat = 10.0f; break;
case SHOOT_PROJECTILE_42MM: refund_heat = 100.0f; break;
default: refund_heat = s->heatcontrol.Hgen; break;
}
refund_heat *= s->heatcontrol.unverified_shots;
s->heatcontrol.Hnow_estimated -= refund_heat;
if (s->heatcontrol.Hnow_estimated < 0.0f) {
s->heatcontrol.Hnow_estimated = 0.0f;
}
if (s->var_trig.num_shooted >= s->heatcontrol.unverified_shots) {
s->var_trig.num_shooted -= s->heatcontrol.unverified_shots;
} else {
s->var_trig.num_shooted = 0;
}
if (s->heatcontrol.shots_detected >= s->heatcontrol.unverified_shots) {
s->heatcontrol.shots_detected -= s->heatcontrol.unverified_shots;
} else {
s->heatcontrol.shots_detected = 0;
}
s->heatcontrol.unverified_shots = 0;
}
/* 数据融合 */ /* 数据融合 */
Shoot_FuseHeatData(s); Shoot_FuseHeatData(s);
@ -500,18 +508,22 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s)
*/ */
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{ {
if (s == NULL || s->var_trig.num_toShoot == 0) { if (s == NULL) {
return SHOOT_ERR_NULL; return SHOOT_ERR_NULL;
} }
/* 更新热量控制数据 */ /* 更新热量控制数据 */
Shoot_UpdateHeatControl(s); Shoot_UpdateHeatControl(s);
if (s->var_trig.num_toShoot == 0) {
return SHOOT_OK; /* 即使没有弹丸待发，热量更新照常执行 */
}
/* 根据热量控制计算实际射频 */ /* 根据热量控制计算实际射频 */
float actual_freq = Shoot_CaluFreqByHeat(s); float actual_freq = Shoot_CaluFreqByHeat(s);
float dt = s->timer.now - s->var_trig.time_lastShoot; float dt = s->timer.now - s->var_trig.time_lastShoot;
float dpos; float dpos;
dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI); dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI);
/* 使用热量控制计算出的射频，而不是配置的固定射频 */ /* 使用热量控制计算出的射频，而不是配置的固定射频 */
@ -519,7 +531,24 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{ {
s->var_trig.time_lastShoot=s->timer.now; s->var_trig.time_lastShoot=s->timer.now;
CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI); CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI);
s->var_trig.num_toShoot--; s->var_trig.num_toShoot--;
s->var_trig.num_shooted++;
/* 预测性发射：拨盘拨动即认为弹丸会射出，立即增加热量与发射计数 */
float add_heat = 0.0f;
switch (s->param->basic.projectileType) {
case SHOOT_PROJECTILE_17MM: add_heat = 10.0f; break;
case SHOOT_PROJECTILE_42MM: add_heat = 100.0f; break;
default: add_heat = s->heatcontrol.Hgen; break;
}
s->heatcontrol.Hnow_estimated += add_heat;
if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) {
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
}
s->heatcontrol.shots_detected++;
/* 增加等待摩擦轮掉速确认的弹丸数 */
s->heatcontrol.unverified_shots++;
} }
return SHOOT_OK; return SHOOT_OK;
} }
@ -623,16 +652,17 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
return SHOOT_ERR_NULL; // 参数错误 return SHOOT_ERR_NULL; // 参数错误
} }
uint8_t fric_num = s->param->basic.fric_num; uint8_t fric_num = s->param->basic.fric_num;
static float pos;
if(s->mode==SHOOT_MODE_SAFE){ if(s->mode==SHOOT_MODE_SAFE){
for(int i=0;i<fric_num;i++) for(int i=0;i<fric_num;i++)
{ {
MOTOR_RM_Relax(&s->param->motor.fric[i].param); MOTOR_RM_Relax(&s->param->motor.fric[i].param);
} }
MOTOR_RM_Relax(&s->param->motor.trig);\ MOTOR_RM_Relax(&s->param->motor.trig);
pos=s->target_variable.trig_angle=s->var_trig.trig_agl; s->target_variable.trig_angle=s->var_trig.trig_agl;
} }
else{ else{
Shoot_CaluTargetAngle(s, cmd);
switch(s->running_state) switch(s->running_state)
{ {
case SHOOT_STATE_IDLE:/*熄火等待*/ case SHOOT_STATE_IDLE:/*熄火等待*/
@ -645,7 +675,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]); MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
} }
s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->var_trig.trig_agl,0,s->timer.dt); s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.trig_angle,s->var_trig.trig_agl,0,s->timer.dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt); s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->var_trig.trig_rpm,0,s->timer.dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig); s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig); MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
@ -689,7 +719,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
} }
/* 设置拨弹电机输出 */ /* 设置拨弹电机输出 */
s->output.outagl_trig =PID_Calc(&s->pid.trig, s->output.outagl_trig =PID_Calc(&s->pid.trig,
pos, s->target_variable.trig_angle,
s->var_trig.trig_agl, s->var_trig.trig_agl,
0, 0,
s->timer.dt); s->timer.dt);
@ -731,7 +761,6 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
break; break;
case SHOOT_STATE_FIRE:/*射击*/ case SHOOT_STATE_FIRE:/*射击*/
Shoot_CaluTargetAngle(s, cmd);
for(int i=0;i<fric_num;i++) for(int i=0;i<fric_num;i++)
{ {
uint8_t level=s->param->motor.fric[i].level-1; uint8_t level=s->param->motor.fric[i].level-1;
@ -776,7 +805,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
if(!cmd->firecmd) if(!cmd->firecmd)
{ {
s->running_state=SHOOT_STATE_READY; s->running_state=SHOOT_STATE_READY;
pos=s->var_trig.trig_agl; s->target_variable.trig_angle=s->var_trig.trig_agl;
s->var_trig.num_toShoot=0; s->var_trig.num_toShoot=0;
} }
break; break;

7
User/module/shoot.h
View File

@ -35,7 +35,8 @@ typedef enum {
SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */ SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */
SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */ SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */
SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */ SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */
SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */ SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */
SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */
}Shoot_HeatDetectionFSM_State_t; }Shoot_HeatDetectionFSM_State_t;
typedef enum { typedef enum {
SHOOT_STATE_IDLE = 0,/* 熄火 */ SHOOT_STATE_IDLE = 0,/* 熄火 */
@ -114,7 +115,8 @@ typedef struct {
Shoot_HeatDetectionFSM_State_t detectState; // 检测状态 Shoot_HeatDetectionFSM_State_t detectState; // 检测状态
float detectTime; // 检测计时器 float detectTime; // 检测计时器
float avgFricRpm; // 摩擦轮平均转速 float avgFricRpm; // 摩擦轮平均转速
float lastAvgFricRpm; // 上次摩擦轮平均转速 float baselineRpm; // 动态基准转速，用于连发掉速检测的参照
float valleyRpm; // 掉速谷底转速，用于判断掉速回升
float rpmDrop; // 转速下降量 float rpmDrop; // 转速下降量
bool shotDetected; // 检测到发射标志 bool shotDetected; // 检测到发射标志
@ -123,6 +125,7 @@ typedef struct {
float Hnow_fused; // 融合后的热量值 float Hnow_fused; // 融合后的热量值
uint16_t shots_detected; // 检测到的发射数 uint16_t shots_detected; // 检测到的发射数
uint16_t shots_available;// 当前热量可发射弹数 uint16_t shots_available;// 当前热量可发射弹数
uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数
}Shoot_HeatControl_t; }Shoot_HeatControl_t;
typedef struct { typedef struct {