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

合并

Browse Source
This commit is contained in:
xxxxm 2026-03-21 02:04:44 +08:00 committed by Robofish
parent 9fa8a6cb81
commit d3096aa537
4 changed files with 70 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -406,6 +406,13 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) {
ctx->output.balance_chassis.cmd.move_vec.vy = 0.0f;
ctx->output.balance_chassis.cmd.move_vec.wz = 0.0f;
ctx->output.balance_chassis.cmd.height += (float)ctx->input.pc.mouse.z * sens->mouse_sens * 0.001f * ctx->timer.dt;
if (ctx->output.balance_chassis.cmd.height > 1.0f) {
ctx->output.balance_chassis.cmd.height = 1.0f;
} else if (ctx->output.balance_chassis.cmd.height < 0.2f) {
ctx->output.balance_chassis.cmd.height = 0.2f;
}
ctx->output.balance_chassis.cmd.jump_trigger = false;
ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_BALANCE_CHASSIS);

12
User/module/config.c
View File

@ -146,16 +146,16 @@ Config_RobotParam_t robot_config = {
},
.jamDetection={
.enable=true,
.threshold=200.0f,
.threshold=90.0f,
.suspectedTime=0.5f,
},
.heatControl={
.enable=true,
.safe_shots=5, // 安全出弹余量
.safe_shots=0.10f, // 安全出弹余量比例
.nmax=15.0f, // 最大射频 Hz
.Hwarn=200.0f, // 热量预警值
.Hsatu=100.0f, // 热量饱和值
.Hthres=50.0f, // 热量阈值
.Hwarn=0.50f, // 预警阈值比例（70%）
.Hsatu=0.25f, // 饱和阈值比例（40%）
.Hthres=0.05f, // 停射阈值比例（8%）
},
.motor={
.fric = {
@ -480,7 +480,7 @@ Config_RobotParam_t robot_config = {
.climb = {
.forward_speed = 1.5f, /* 上台阶前进速度 (m/s) */
.theta_retract_threshold = 0.9f, /* 腿后摆收腿阈值 (rad)，约30° */
.theta_retract_threshold = 0.98f, /* 腿后摆收腿阈值 (rad)，约30° */
.tp_scale = 0.08f, /* 摆力矩缩放：削弱到10%，让腿自由后摆 */
.settle_time_ms = 500, /* 收腿后稳定时间 (ms) */
},

53
User/module/shoot.c
View File

@ -307,8 +307,8 @@ static int8_t Shoot_FuseHeatData(Shoot_t *s)
s->heatcontrol.Hnow_last = s->heatcontrol.Hnow; /* 记录本次值 */
}
/* 融合值就是裁判系统值（作为基准） */
s->heatcontrol.Hnow_fused = s->heatcontrol.Hnow;
/* 连射场景下，裁判数据存在刷新延迟；取更保守的热量用于限热 */
s->heatcontrol.Hnow_fused = fmaxf(s->heatcontrol.Hnow, s->heatcontrol.Hnow_estimated);
} else {
/* 裁判系统数据无效，仅使用自主估计值 */
s->heatcontrol.Hnow_fused = s->heatcontrol.Hnow_estimated;
@ -415,26 +415,8 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
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.Hnow_estimated > s->heatcontrol.Hmax) {
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
}
/* 增加发射计数 */
/* 确认发射状态：仅记录检测计数。
* 估计热量在实际下发拨弹时已预扣，避免重复累加。 */
s->heatcontrol.shots_detected++;
/* 返回准备检测状态 */
@ -493,6 +475,25 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s)
float nmax = s->param->heatControl.nmax;
float ncd = s->heatcontrol.ncd;
if (nmax <= 0.0f) {
nmax = s->param->basic.shot_freq;
}
if (ncd < 0.0f) {
ncd = 0.0f;
} else if (ncd > nmax) {
ncd = nmax;
}
if (Hwarn <= 0.0f) {
Hwarn = s->heatcontrol.Hmax * 0.7f;
}
if (Hsatu <= 0.0f || Hsatu >= Hwarn) {
Hsatu = Hwarn * 0.5f;
}
if (Hthres <= 0.0f || Hthres >= Hsatu) {
Hthres = fmaxf(Hsatu * 0.5f, s->heatcontrol.Hgen);
}
/* 剩余热量大于预警值：最大射频 */
if (Hres > Hwarn) {
return nmax;
@ -552,6 +553,14 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
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_shooted++;
/* 实际发射指令下发时，立刻预扣估计热量，抑制裁判数据延迟导致的超热。 */
if (s->param->heatControl.enable && s->heatcontrol.ref_online && s->heatcontrol.Hgen > 0.0f) {
s->heatcontrol.Hnow_estimated += s->heatcontrol.Hgen;
if (s->heatcontrol.Hnow_estimated > s->heatcontrol.Hmax) {
s->heatcontrol.Hnow_estimated = s->heatcontrol.Hmax;
}
}
}
return SHOOT_OK;

28
User/task/ctrl_shoot.c
View File

@ -38,13 +38,23 @@ static int print_shoot(const void *data, char *buf, size_t size) {
" Fric1 : %.1f rpm (target: %.1f)\r\n"
" Fric_Avg : %.1f rpm\r\n"
" Trig : %.1f rpm (angle: %.2f deg)\r\n"
" Output : Fric0=%.1f Fric1=%.1f Trig=%.1f\r\n",
" Output : Fric0=%.1f Fric1=%.1f Trig=%.1f\r\n"
" Heat : online=%d Hnow=%.1f Hest=%.1f Hfused=%.1f Hres=%.1f Hmax=%.1f Hgen=%.1f ncd=%.2f avail=%u\r\n",
shoot->running_state,
shoot->feedback.fric[0].rotor_speed, shoot->target_variable.fric_rpm,
shoot->feedback.fric[1].rotor_speed, shoot->target_variable.fric_rpm,
shoot->var_fric.normalized_fil_avgrpm,
shoot->feedback.trig.feedback.rotor_speed, shoot->feedback.trig.feedback.rotor_abs_angle,
shoot->output.out_fric[0], shoot->output.out_fric[1], shoot->output.outagl_trig);
shoot->output.out_fric[0], shoot->output.out_fric[1], shoot->output.outagl_trig,
shoot->heatcontrol.ref_online ? 1 : 0,
shoot->heatcontrol.Hnow,
shoot->heatcontrol.Hnow_estimated,
shoot->heatcontrol.Hnow_fused,
shoot->heatcontrol.Hres,
shoot->heatcontrol.Hmax,
shoot->heatcontrol.Hgen,
shoot->heatcontrol.ncd,
(unsigned int)shoot->heatcontrol.shots_available);
return 0;
}
@ -80,8 +90,20 @@ void Task_ctrl_shoot(void *argument) {
shoot.heatcontrol.ref_online = true;
shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit;
shoot.heatcontrol.Hcd = (float)shoot_ref.robot_status.shooter_barrel_cooling_value;
switch (shoot.param->basic.projectileType) {
case SHOOT_PROJECTILE_17MM:
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_17mm_barrel_heat;
shoot.heatcontrol.Hgen = 10.0f;
break;
case SHOOT_PROJECTILE_42MM:
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
shoot.heatcontrol.Hgen = 10.0f; /* 42mm弹丸每发产生热量 */
shoot.heatcontrol.Hgen = 100.0f;
break;
default:
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_17mm_barrel_heat;
shoot.heatcontrol.Hgen = 10.0f;
break;
}
} else {
shoot.heatcontrol.ref_online = false;
}