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

sili

Browse Source
This commit is contained in:
Robofish 2026-03-19 12:08:40 +08:00
parent eea0a5a3d3
commit da7f4d6ea9
5 changed files with 1044 additions and 1010 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,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_SINGLE;
ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE;
} else {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
}

12
User/module/config.c
View File

@ -140,7 +140,7 @@ Config_RobotParam_t robot_config = {
.fric_num=2,
.extra_deceleration_ratio=1.0f,
.num_trig_tooth=8,
.shot_freq=18.0f,
.shot_freq=15.0f,
.shot_burst_num=3,
.ratio_multilevel = {1.0f},
},
@ -152,7 +152,7 @@ Config_RobotParam_t robot_config = {
.heatControl={
.enable=true,
.safe_shots=5, // 安全出弹余量
.nmax=18.0f, // 最大射频 Hz
.nmax=15.0f, // 最大射频 Hz
.Hwarn=200.0f, // 热量预警值
.Hsatu=100.0f, // 热量饱和值
.Hthres=50.0f, // 热量阈值
@ -542,10 +542,10 @@ Config_RobotParam_t robot_config = {
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX,
.balance_sw_mid = CHASSIS_MODE_RELAX,
.balance_sw_down = CHASSIS_MODE_RELAX,
// .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
// .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
// .balance_sw_mid = CHASSIS_MODE_RELAX,
// .balance_sw_down = CHASSIS_MODE_RELAX,
.balance_sw_mid = CHASSIS_MODE_LEG_TEST,
.balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
#endif
},
},

51
User/module/shoot.c
View File

@ -41,7 +41,7 @@ void Task(void *argument) {
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 1500.0f//这里可能也会影响最高发射频率，待测试
#define MAX_TRIG_RPM 2500.0f//这里可能也会影响最高发射频率，待测试
/* 发射检测参数 */
#define SHOT_DETECT_RPM_DROP_THRESHOLD 100.0f /* 摩擦轮转速下降阈值，单位rpm */
@ -52,7 +52,7 @@ void Task(void *argument) {
/* Private variables -------------------------------------------------------- */
static bool last_firecmd;
float maxTrigrpm=4000.0f;
/* Private function -------------------------------------------------------- */
/**
@ -183,7 +183,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
s->target_variable.fric_rpm=5000.0f;
break;
case SHOOT_PROJECTILE_42MM:
s->target_variable.fric_rpm=5000.0f;//6500
s->target_variable.fric_rpm=4000.0f;//6500
break;
}
return SHOOT_OK;
@ -202,6 +202,12 @@ static int8_t Shoot_UpdateHeatControl(Shoot_t *s)
return SHOOT_ERR_NULL;
}
if (!s->param->heatControl.enable || !s->heatcontrol.ref_online) {
s->heatcontrol.Hres = 0.0f;
s->heatcontrol.ncd = s->param->basic.shot_freq;
return SHOOT_OK;
}
/* 使用融合后的热量值计算剩余热量 */
s->heatcontrol.Hres = s->heatcontrol.Hmax - s->heatcontrol.Hnow_fused;
@ -288,6 +294,10 @@ static int8_t Shoot_FuseHeatData(Shoot_t *s)
return SHOOT_ERR_NULL;
}
if (!s->param->heatControl.enable || !s->heatcontrol.ref_online) {
return SHOOT_OK;
}
/* 如果裁判系统数据有效（Hmax > 0） */
if (s->heatcontrol.Hmax > 0.0f && s->heatcontrol.Hnow >= 0.0f) {
/* 检测裁判系统数据是否有更新 */
@ -320,6 +330,11 @@ static int8_t Shoot_CalcAvailableShots(Shoot_t *s)
return SHOOT_ERR_NULL;
}
if (!s->param->heatControl.enable || !s->heatcontrol.ref_online) {
s->heatcontrol.shots_available = UINT16_MAX;
return SHOOT_OK;
}
/* 计算剩余热量 */
float heat_available = s->heatcontrol.Hmax - s->heatcontrol.Hnow_fused;
@ -347,6 +362,12 @@ static int8_t Shoot_HeatDetectionFSM(Shoot_t *s)
return SHOOT_ERR_NULL;
}
if (!s->param->heatControl.enable || !s->heatcontrol.ref_online) {
s->heatcontrol.detectState = SHOOT_HEAT_DETECT_IDLE;
s->heatcontrol.shotDetected = false;
return SHOOT_OK;
}
switch (s->heatcontrol.detectState) {
case SHOOT_HEAT_DETECT_IDLE:
/* 停机状态：等待摩擦轮启动并加速到目标速度附近 */
@ -456,7 +477,7 @@ static float Shoot_CaluFreqByHeat(Shoot_t *s)
}
/* 如果热量控制未启用，返回配置的射频 */
if (!s->param->heatControl.enable) {
if (!s->param->heatControl.enable || !s->heatcontrol.ref_online) {
return s->param->basic.shot_freq;
}
@ -502,7 +523,7 @@ 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 || cmd == NULL || s->var_trig.num_toShoot == 0 || s->param->basic.num_trig_tooth == 0) {
return SHOOT_ERR_NULL;
}
@ -513,7 +534,8 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
float actual_freq = Shoot_CaluFreqByHeat(s);
/* 检查可发射弹丸数是否满足安全余量 */
if (s->param->heatControl.enable && s->heatcontrol.shots_available <= s->param->heatControl.safe_shots) {
if (s->param->heatControl.enable &&
s->heatcontrol.shots_available <= s->param->heatControl.safe_shots) {
actual_freq = 0.0f; /* 禁止发弹 */
}
@ -521,9 +543,10 @@ int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
float dpos;
float dpos_abs;
dpos = CircleError(s->target_variable.trig_angle, s->var_trig.trig_agl, M_2PI);
dpos_abs = fabs(dpos);
dpos_abs = fabsf(dpos);
/* 使用热量控制计算出的射频，而不是配置的固定射频 */
if(actual_freq > 0.0f && dt >= 1.0f/actual_freq && cmd->firecmd && dpos_abs<=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);
@ -611,7 +634,7 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s)
s->var_trig.trig_agl = M_2PI - s->var_trig.trig_agl;
}
s->var_trig.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed);
s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / maxTrigrpm;
s->var_trig.trig_rpm = s->feedback.trig.feedback.rotor_speed / MAX_TRIG_RPM;
if(s->var_trig.trig_rpm>1.0f)s->var_trig.trig_rpm=1.0f;
if(s->var_trig.trig_rpm<-1.0f)s->var_trig.trig_rpm=-1.0f;
@ -721,7 +744,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;
s->target_variable.trig_angle = s->var_trig.trig_agl;
/* 根据模式设置待发射弹数 */
switch(s->mode)
{
@ -788,7 +811,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->target_variable.trig_angle = pos;
s->var_trig.num_toShoot=0;
}
break;
@ -853,7 +876,9 @@ 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;
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->jamdetection.fsmState = SHOOT_JAMFSM_STATE_DEAL;
/* 记录处理开始时间 */
@ -999,5 +1024,5 @@ void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui) {
void Shoot_DumpAI(Shoot_t *s, Shoot_AI_t *ai) {
ai->num_shooted=s->var_trig.num_shooted;
ai->bullet_speed=22.0f;
ai->bullet_speed=12.0f;
}

1
User/module/shoot.h
View File

@ -102,6 +102,7 @@ typedef struct {
}Shoot_JamDetection_t;
typedef struct {
bool ref_online; /* 裁判系统数据是否在线 */
/* 从裁判系统读取的常量 */
float Hmax; // 热量上限
float Hcd; // 热量冷却速度

8
User/task/ctrl_shoot.c
View File

@ -4,6 +4,7 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/mrobot.h"
@ -20,6 +21,7 @@
Shoot_t shoot;
Shoot_CMD_t shoot_cmd;
Referee_ForShoot_t shoot_ref;
Shoot_AI_t shoot_forai;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -71,11 +73,17 @@ void Task_ctrl_shoot(void *argument) {
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
osMessageQueueGet(task_runtime.msgq.shoot.ref, &shoot_ref, NULL, 0);
Shoot_DumpAI(&shoot, &shoot_forai);
osMessageQueuePut(task_runtime.msgq.ai.rawdata_FromShoot, &shoot_forai, 0, 0);
if (shoot_ref.ref_status == REF_STATUS_RUNNING) {
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;
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
shoot.heatcontrol.Hgen = 10.0f; /* 42mm弹丸每发产生热量 */
} else {
shoot.heatcontrol.ref_online = false;
}
Shoot_UpdateFeedback(&shoot);
Shoot_SetMode(&shoot,shoot_cmd.mode);