diff --git a/User/device/referee.h b/User/device/referee.h index e74582b..a771bb5 100644 --- a/User/device/referee.h +++ b/User/device/referee.h @@ -574,11 +574,6 @@ typedef struct { osTimerId_t ui_slow_timer_id; } 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 { /* UI缓冲数据 */ UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM]; diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 9219749..c13f9e5 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -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_pit = -ctx->input.rc.joy_left.y * 2.5f; + ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_right.x * 5.0f; + ctx->output.gimbal.cmd.delta_pit = ctx->input.rc.joy_right.y * 5.0f; } #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 static void CMD_RC_BuildShootCmd(CMD_t *ctx) { if (ctx->input.online[CMD_SRC_RC]) { - ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; + ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST; } else { 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; uint32_t kb = ctx->input.pc.keyboard.bitmap; - if (kb & CMD_KEY_W) vx += sens->move_sens; - if (kb & CMD_KEY_S) vx -= sens->move_sens; - if (kb & CMD_KEY_A) wz += sens->move_sens; - if (kb & CMD_KEY_D) wz -= sens->move_sens; - 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; } + //案件的命令要放到behavior里, + // if (kb & CMD_KEY_W) vx += sens->move_sens; + // if (kb & CMD_KEY_S) vx -= sens->move_sens; + // if (kb & CMD_KEY_A) wz += sens->move_sens; + // if (kb & CMD_KEY_D) wz -= sens->move_sens; + // 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.wz = wz; diff --git a/User/module/config.c b/User/module/config.c index 0ee13be..35e95c3 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -110,19 +110,19 @@ Config_RobotParam_t robot_config = { .projectileType=SHOOT_PROJECTILE_17MM, .fric_num=2, .extra_deceleration_ratio=1.0f, - .num_trig_tooth=5, + .num_trig_tooth=8, .shot_freq=1.0f, .shot_burst_num=3, .ratio_multilevel = {1.0f}, }, .jamDetection={ .enable=true, - .threshold=310.0f, + .threshold=200.0f, .suspectedTime=0.5f, }, .heatControl={ .enable=true, - .nmax=2.0f, // 最大射频 Hz + .nmax=18.0f, // 最大射频 Hz .Hwarn=200.0f, // 热量预警值 .Hsatu=100.0f, // 热量饱和值 .Hthres=50.0f, // 热量阈值 @@ -151,10 +151,10 @@ Config_RobotParam_t robot_config = { } }, .trig = { - .can = BSP_FDCAN_2, - .id = 0x205, - .module = MOTOR_M3508, - .reverse = false, + .can = BSP_CAN_1, + .id = 0x203, + .module = MOTOR_M2006, + .reverse = true, .gear=true, }, }, @@ -180,12 +180,12 @@ Config_RobotParam_t robot_config = { .range=-1.0f, }, .trig_2006 = { - .k=2.5f, + .k=1.0f, .p=1.0f, .i=0.1f, - .d=0.04f, - .i_limit=0.4f, - .out_limit=1.0f, + .d=0.05f, + .i_limit=0.8f, + .out_limit=0.5f, .d_cutoff_freq=-1.0f, .range=M_2PI, }, @@ -195,7 +195,7 @@ Config_RobotParam_t robot_config = { .i=0.3f, .d=0.5f, .i_limit=0.2f, - .out_limit=1.0f, + .out_limit=0.9f, .d_cutoff_freq=-1.0f, .range=-1.0f, }, diff --git a/User/module/shoot.c b/User/module/shoot.c index f03bc53..1774165 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -46,7 +46,6 @@ void Task(void *argument) { /* 发射检测参数 */ #define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值,单位rpm */ #define SHOT_DETECT_SUSPECTED_TIME 0.0005f /* 发射嫌疑持续时间阈值,单位秒 */ -#define FRIC_READY_RPM_RATIO 0.95f /* 摩擦轮准备好的转速比例 */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ @@ -236,40 +235,6 @@ static float Shoot_CalcAvgFricRpm(Shoot_t *s) 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 热量数据融合:用裁判系统数据修正自主估计 * @@ -353,18 +318,29 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *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.lastAvgFricRpm = s->heatcontrol.avgFricRpm; + s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; // 记录初始基准转速 } } break; case SHOOT_HEAT_DETECT_READY: - /* 准备检测状态:监测摩擦轮掉速 */ - /* 检测掉速(当前转速低于目标转速超过阈值) */ - if (Shoot_DetectShotByRpmDrop(s)) { + /* 准备检测状态:监测摩擦轮相对当前的基准发生掉速 */ + s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(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.detectTime = s->timer.now; /* 记录进入嫌疑状态的时间 */ } @@ -377,45 +353,47 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) case SHOOT_HEAT_DETECT_SUSPECTED: /* 发射嫌疑状态:持续检测掉速 */ - /* 更新掉速量 */ 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; - /* 若掉速消失(转速恢复正常),回到准备状态 */ - if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD) { + /* 若掉速消失(转速刚掉下又马上弹回基准附近),回到准备状态 (阈值的 60%) */ + if (s->heatcontrol.rpmDrop < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.6f) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; } - /* 若嫌疑状态持续超过阈值时间,确认发射 */ + /* 若掉速状态持续超过设定的嫌疑时间,确认这是一次发射 */ else if ((s->timer.now - s->heatcontrol.detectTime) >= SHOT_DETECT_SUSPECTED_TIME) { s->heatcontrol.detectState = SHOOT_HEAT_DETECT_CONFIRMED; } break; case SHOOT_HEAT_DETECT_CONFIRMED: - /* 确认发射状态:增加热量并返回准备状态 */ - /* 根据弹丸类型增加估计热量 */ - switch (s->param->basic.projectileType) { - case SHOOT_PROJECTILE_17MM: - s->heatcontrol.Hnow_estimated += 10.0f; - break; - case SHOOT_PROJECTILE_42MM: - s->heatcontrol.Hnow_estimated += 100.0f; - break; - default: - s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen; - break; + if (s->heatcontrol.unverified_shots > 0) { + s->heatcontrol.unverified_shots--; + } + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_RECOVERING; + s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; /* 记录进入此时的转速为谷底初值 */ + break; + + case SHOOT_HEAT_DETECT_RECOVERING: + + s->heatcontrol.avgFricRpm = Shoot_CalcAvgFricRpm(s); + + if (s->heatcontrol.avgFricRpm < s->heatcontrol.valleyRpm) { + s->heatcontrol.valleyRpm = s->heatcontrol.avgFricRpm; } - /* 限制估计热量不超过最大值 */ - if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) { - s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax; + if ((s->heatcontrol.avgFricRpm - s->heatcontrol.valleyRpm > 30.0f) || + (s->target_variable.fric_rpm - s->heatcontrol.avgFricRpm < SHOT_DETECT_RPM_DROP_THRESHOLD * 0.8f)) { + + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; + + s->heatcontrol.baselineRpm = s->heatcontrol.avgFricRpm; } - /* 增加发射计数 */ - s->heatcontrol.shots_detected++; - - /* 返回准备检测状态 */ - s->heatcontrol.detectState = SHOOT_HEAT_DETECT_READY; + /* 如果摩擦轮停止 */ + if (s->running_state == SHOOT_STATE_IDLE) { + s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE; + } break; default: @@ -423,7 +401,7 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s) break; } - /* 善后工作:热量冷却(每个周期都执行) */ + /* 热量冷却*/ if (s->heatcontrol.Hnow_estimated > 0.0f && s->heatcontrol.Hcd > 0.0f) { s->heatcontrol.Hnow_estimated -= s->heatcontrol.Hcd * s->timer.dt; 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); @@ -500,18 +508,22 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s) */ 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; } /* 更新热量控制数据 */ Shoot_UpdateHeatControl(s); + if (s->var_trig.num_toShoot == 0) { + return SHOOT_OK; /* 即使没有弹丸待发,热量更新照常执行 */ + } + /* 根据热量控制计算实际射频 */ float actual_freq = Shoot_CaluFreqByHeat(s); 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); /* 使用热量控制计算出的射频,而不是配置的固定射频 */ @@ -519,7 +531,24 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd) { s->var_trig.time_lastShoot=s->timer.now; 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; } @@ -623,16 +652,17 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) return SHOOT_ERR_NULL; // 参数错误 } uint8_t fric_num = s->param->basic.fric_num; - static float pos; + if(s->mode==SHOOT_MODE_SAFE){ for(int i=0;iparam->motor.fric[i].param); } - MOTOR_RM_Relax(&s->param->motor.trig);\ - pos=s->target_variable.trig_angle=s->var_trig.trig_agl; + MOTOR_RM_Relax(&s->param->motor.trig); + s->target_variable.trig_angle=s->var_trig.trig_agl; } else{ + Shoot_CaluTargetAngle(s, cmd); switch(s->running_state) { 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]); } - 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.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_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, - pos, + s->target_variable.trig_angle, s->var_trig.trig_agl, 0, s->timer.dt); @@ -731,7 +761,6 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) break; case SHOOT_STATE_FIRE:/*射击*/ - Shoot_CaluTargetAngle(s, cmd); for(int i=0;iparam->motor.fric[i].level-1; @@ -776,7 +805,7 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd) if(!cmd->firecmd) { 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; } break; diff --git a/User/module/shoot.h b/User/module/shoot.h index 59e42e4..cd50342 100644 --- a/User/module/shoot.h +++ b/User/module/shoot.h @@ -35,7 +35,8 @@ typedef enum { SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */ SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */ SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */ - SHOOT_HEAT_DETECT_CONFIRMED /* 确认发射状态 */ + SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */ + SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */ }Shoot_HeatDetectionFSM_State_t; typedef enum { SHOOT_STATE_IDLE = 0,/* 熄火 */ @@ -114,7 +115,8 @@ typedef struct { Shoot_HeatDetectionFSM_State_t detectState; // 检测状态 float detectTime; // 检测计时器 float avgFricRpm; // 摩擦轮平均转速 - float lastAvgFricRpm; // 上次摩擦轮平均转速 + float baselineRpm; // 动态基准转速,用于连发掉速检测的参照 + float valleyRpm; // 掉速谷底转速,用于判断掉速回升 float rpmDrop; // 转速下降量 bool shotDetected; // 检测到发射标志 @@ -123,6 +125,7 @@ typedef struct { float Hnow_fused; // 融合后的热量值 uint16_t shots_detected; // 检测到的发射数 uint16_t shots_available;// 当前热量可发射弹数 + uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数 }Shoot_HeatControl_t; typedef struct {