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

牛大了

Browse Source
This commit is contained in:
Robofish 2026-03-19 21:21:30 +08:00
parent da7f4d6ea9
commit 21f7370f94
10 changed files with 68 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
User/device/referee.c
View File

@ -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:

1
User/device/referee_proto_types.h
View File

@ -59,6 +59,7 @@ typedef struct {
Referee_Status_t ref_status;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForChassis_t;
typedef struct {

5
User/module/balance_chassis.c
View File

@ -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应该是跟随云台的那个角,我没找着在哪"
}

3
User/module/balance_chassis.h
View File

@ -102,6 +102,9 @@ typedef struct {
typedef struct {
Chassis_Mode_t mode;
float angle;
/* 轮腿角度和距轮长 */
float l0[2];
float theta[2];
} Chassis_RefereeUI_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */

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

@ -3,6 +3,7 @@
*/
#include "cmd.h"
#include "bsp/time.h"
#include "module/cmd/cmd_types.h"
#include "module/shoot.h"
#include <stdint.h>
#include <string.h>
@ -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 */

14
User/module/cmd/cmd_behavior.c
View File

@ -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;

12
User/module/cmd/cmd_types.h
View File

@ -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) \

8
User/module/config.c
View File

@ -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
},

2
User/module/shoot.c
View File

@ -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

7
User/task/ctrl_chassis.c
View File

@ -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);