From 21f7370f94fa322d785ceb67995a501d5bb4a6d4 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 19 Mar 2026 21:21:30 +0800 Subject: [PATCH] =?UTF-8?q?=E7=89=9B=E5=A4=A7=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/referee.c | 20 ++++++++++++++++++++ User/device/referee_proto_types.h | 1 + User/module/balance_chassis.c | 5 ++++- User/module/balance_chassis.h | 3 +++ User/module/cmd/cmd.c | 25 ++++++++++++++----------- User/module/cmd/cmd_behavior.c | 14 ++++++++++---- User/module/cmd/cmd_types.h | 12 ++++++------ User/module/config.c | 8 ++++---- User/module/shoot.c | 2 +- User/task/ctrl_chassis.c | 7 +++++-- 10 files changed, 68 insertions(+), 29 deletions(-) diff --git a/User/device/referee.c b/User/device/referee.c index 43e41a0..8048d21 100644 --- a/User/device/referee.c +++ b/User/device/referee.c @@ -649,6 +649,26 @@ uint8_t Referee_UIRefresh(Referee_UI_t *ui) { ui->screen->width * 0.4, ui->screen->height * 0.2, ui->screen->width * 0.4f + sinf(ui->chassis_ui.angle) * 46.0f, ui->screen->height * 0.2f + cosf(ui->chassis_ui.angle) * 46.0f); + UI_DrawLine(Referee_GetGrapicAdd(ui), "7", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12, + ui->screen->width * 0.4, ui->screen->height * 0.4, + ui->screen->width * 0.4f - sinf(ui->chassis_ui.l0[0]) * 46.0f, + ui->screen->height * 0.4f - cosf(ui->chassis_ui.l0[0]) * 46.0f); + UI_DrawLine(Referee_GetGrapicAdd(ui), "8", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12, + ui->screen->width * 0.4, ui->screen->height * 0.6, + ui->screen->width * 0.4f - sinf(ui->chassis_ui.theta[0]) * 46.0f, + ui->screen->height * 0.6f - cosf(ui->chassis_ui.theta[0]) * 46.0f); + UI_DrawLine(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12, + ui->screen->width * 0.4, ui->screen->height * 0.4, + ui->screen->width * 0.4f - sinf(ui->chassis_ui.l0[1]) * 46.0f, + ui->screen->height * 0.4f - cosf(ui->chassis_ui.l0[1]) * 46.0f); + UI_DrawLine(Referee_GetGrapicAdd(ui), "a", UI_GRAPIC_OPERATION_ADD, + UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12, + ui->screen->width * 0.4, ui->screen->height * 0.8, + ui->screen->width * 0.4f - sinf(ui->chassis_ui.theta[1]) * 46.0f, + ui->screen->height * 0.8f - cosf(ui->chassis_ui.theta[1]) * 46.0f); float start_pos_h = 0.0f; switch (ui->chassis_ui.mode) { case CHASSIS_MODE_RELAX: diff --git a/User/device/referee_proto_types.h b/User/device/referee_proto_types.h index b07ccfe..97d9364 100644 --- a/User/device/referee_proto_types.h +++ b/User/device/referee_proto_types.h @@ -59,6 +59,7 @@ typedef struct { Referee_Status_t ref_status; float chassis_power_limit; float chassis_pwr_buff; + } Referee_ForChassis_t; typedef struct { diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 3db59d7..2e72033 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1476,6 +1476,9 @@ int8_t Chassis_VMCControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { */ void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui) { ui->mode = c->mode; - // ui->angle = c->feedback.yaw.rotor_abs_angle - c->mech_zero; + ui->l0[0] = c->vmc_[0].leg.L0; + ui->l0[1] = c->vmc_[1].leg.L0; + ui->theta[0] = c->vmc_[0].leg.theta; + ui->theta[1] = c->vmc_[1].leg.theta; // #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪" } diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index d32428d..eed675b 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -102,6 +102,9 @@ typedef struct { typedef struct { Chassis_Mode_t mode; float angle; + /* 轮腿角度和距轮长 */ + float l0[2]; + float theta[2]; } Chassis_RefereeUI_t; /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */ diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index fcee311..82f1dd3 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/cmd/cmd_types.h" #include "module/shoot.h" #include #include @@ -174,9 +175,6 @@ static void CMD_PC_BuildShootCmd(CMD_t *ctx) { ctx->output.shoot.cmd.ready = true; ctx->output.shoot.cmd.firecmd = ctx->input.pc.mouse.l_click; - - CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT); - } #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT */ @@ -234,8 +232,9 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { /* 根据AI模式决定射击行为 */ switch (ctx->input.nuc.mode) { case 0: - case 1: - ctx->output.shoot.cmd.ready = true; + case 1: + /* mode=1 只控云台，不让射击机构进入 ready，避免摩擦轮继续预转。 */ + ctx->output.shoot.cmd.ready = false; ctx->output.shoot.cmd.firecmd = false; break; case 2: @@ -248,6 +247,10 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { ctx->output.shoot.cmd.firecmd = false; } break; + default: + ctx->output.shoot.cmd.ready = false; + ctx->output.shoot.cmd.firecmd = false; + break; } } #endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT */ @@ -378,6 +381,7 @@ static void CMD_RC_BuildBalanceChassisCmd(CMD_t *ctx) { /* 拨轮传递目标高度 */ ctx->output.balance_chassis.cmd.height = ctx->input.rc.dial; ctx->output.balance_chassis.cmd.jump_trigger = false; + } #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */ @@ -391,13 +395,12 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) { return; } - ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - - float vx = 0.0f, wz = 0.0f; - - 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.vx = 0.0f; 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.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_BALANCE_CHASSIS); } #endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */ diff --git a/User/module/cmd/cmd_behavior.c b/User/module/cmd/cmd_behavior.c index e47e96a..8fe1500 100644 --- a/User/module/cmd/cmd_behavior.c +++ b/User/module/cmd/cmd_behavior.c @@ -19,7 +19,7 @@ int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) { ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens; #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS - ctx->output.balance_chassis.cmd.move_vec.vy += ctx->config->sensitivity.move_sens; + ctx->output.balance_chassis.cmd.move_vec.vx = ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -29,7 +29,7 @@ int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) { ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens; #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS - ctx->output.balance_chassis.cmd.move_vec.vy -= ctx->config->sensitivity.move_sens; + ctx->output.balance_chassis.cmd.move_vec.vx = -ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -39,7 +39,7 @@ int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) { ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens; #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS - ctx->output.balance_chassis.cmd.move_vec.vx -= ctx->config->sensitivity.move_sens; + ctx->output.balance_chassis.cmd.move_vec.vy = -ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -49,7 +49,7 @@ int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) { ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens; #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS - ctx->output.balance_chassis.cmd.move_vec.vx += ctx->config->sensitivity.move_sens; + ctx->output.balance_chassis.cmd.move_vec.vy = ctx->config->sensitivity.move_sens; #endif return CMD_OK; } @@ -132,6 +132,9 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) { #endif #if CMD_ENABLE_MODULE_ARM ctx->output.arm.source = CMD_SRC_RC; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.source = CMD_SRC_RC; #endif } else if(ctx->active_source == CMD_SRC_RC) { ctx->active_source = CMD_SRC_PC; @@ -146,6 +149,9 @@ int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) { #endif #if CMD_ENABLE_MODULE_ARM ctx->output.arm.source = CMD_SRC_PC; +#endif +#if CMD_ENABLE_MODULE_BALANCE_CHASSIS + ctx->output.balance_chassis.source = CMD_SRC_PC; #endif } return CMD_OK; diff --git a/User/module/cmd/cmd_types.h b/User/module/cmd/cmd_types.h index 7fa40da..e8027a9 100644 --- a/User/module/cmd/cmd_types.h +++ b/User/module/cmd/cmd_types.h @@ -254,12 +254,12 @@ typedef enum { /* 行为-按键映射宏表 */ #define BEHAVIOR_CONFIG_COUNT (12) #define CMD_BEHAVIOR_TABLE(X) \ - X(FORE, CMD_KEY_W, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ - X(BACK, CMD_KEY_S, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ - X(LEFT, CMD_KEY_A, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ - X(RIGHT, CMD_KEY_D, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ - X(ACCELERATE, CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ - X(ROTOR, CMD_KEY_CTRL, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS|CMD_MODULE_BALANCE_CHASSIS) \ + X(FORE, CMD_KEY_W, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ + X(BACK, CMD_KEY_S, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ + X(LEFT, CMD_KEY_A, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ + X(RIGHT, CMD_KEY_D, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ + X(ACCELERATE, CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ + X(ROTOR, CMD_KEY_CTRL, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ X(FIRE, CMD_KEY_L_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_SHOOT) \ X(FIRE_MODE, CMD_KEY_B, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_SHOOT) \ X(CLIMB, CMD_KEY_R, CMD_ACTIVE_PRESSED, CMD_MODULE_BALANCE_CHASSIS) \ diff --git a/User/module/config.c b/User/module/config.c index 59cbda5..1e52b66 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -529,7 +529,7 @@ Config_RobotParam_t robot_config = { }, .sensitivity = { .mouse_sens = 60.0f, - .move_sens = 1.0f, + .move_sens = 0.99f, .move_fast_mult = 2.0f, .move_slow_mult = 0.4f, }, @@ -537,14 +537,14 @@ Config_RobotParam_t robot_config = { #if CMD_ENABLE_MODULE_GIMBAL .gimbal_sw_up = GIMBAL_MODE_RELAX, .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, - .gimbal_sw_down = GIMBAL_MODE_RECOVER, - // .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL, + // .gimbal_sw_down = GIMBAL_MODE_RECOVER, + .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL, #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_LEG_TEST, + .balance_sw_mid =CHASSIS_MODE_WHELL_LEG_BALANCE, .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE, #endif }, diff --git a/User/module/shoot.c b/User/module/shoot.c index c962a6c..7f7106e 100644 --- a/User/module/shoot.c +++ b/User/module/shoot.c @@ -180,7 +180,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed) switch(s->param->basic.projectileType) { case SHOOT_PROJECTILE_17MM: - s->target_variable.fric_rpm=5000.0f; + s->target_variable.fric_rpm=6500.0f; break; case SHOOT_PROJECTILE_42MM: s->target_variable.fric_rpm=4000.0f;//6500 diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 7880d55..deea00f 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -31,6 +31,7 @@ Chassis_CMD_t chassis_cmd = { }; Chassis_IMU_t chassis_imu; Referee_ForChassis_t chassis_ref; /* 裁判系统底盘数据 */ +Chassis_RefereeUI_t chassis_ui; /* 发送到裁判系统UI的数据结构 */ /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -101,13 +102,15 @@ void Task_ctrl_chassis(void *argument) { Chassis_UpdateFeedback(&chassis); osMessageQueueGet(task_runtime.msgq.cap.power_limit, &chassis.power_limit, NULL, 0); Chassis_Control(&chassis, &chassis_cmd); - + Chassis_DumpUI(&chassis, &chassis_ui); ///* 功率限制：裁判系统在线时使用下发上限，否则使用保守默认值 */ // float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING) // ? chassis_ref.chassis_power_limit // : 500.0f; //(&chassis, power_limit); - +osMessageQueueReset(task_runtime.msgq.referee.ui.tochassis); // 重置消息队列，防止阻塞 + osMessageQueuePut(task_runtime.msgq.referee.ui.tochassis, &chassis_ui, 0, 0); + /* 将底盘状态UI数据发送到裁判系统进程，由裁判系统进程负责调用 osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞 osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);