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

Compare commits

base: robofish:a1b60330a960a5ae2889eeaa7dfd727ec42f4c46
Branches Tags
robofish:main
robofish:MC_02
robofish:ai
..
compare: robofish:480e41e68a01e0a7466edc2d3eb5df99d20cd4a9
Branches Tags
robofish:MC_02
robofish:main
robofish:ai

No commits in common. "a1b60330a960a5ae2889eeaa7dfd727ec42f4c46" and "480e41e68a01e0a7466edc2d3eb5df99d20cd4a9" have entirely different histories.
a1b60330a9 ... 480e41e68a

18 changed files with 1075 additions and 1311 deletions
Show Stats Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

6
User/component/vmc.c
View File

@ -122,11 +122,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
VMC_ANGLE_NORMALIZE(leg->phi0);
// 计算角速度和长度变化率
float dphi0 = leg->phi0 - leg->last_phi0;
/* phi0 在 ±π 跳变时做循环差值处理 */
if (dphi0 > VMC_PI) dphi0 -= VMC_2PI;
if (dphi0 < -VMC_PI) dphi0 += VMC_2PI;
leg->d_phi0 = (vmc->dt > 0) ? (dphi0 / vmc->dt) : 0.0f;
leg->d_phi0 = VMC_ComputeNumericDerivative(leg->phi0, leg->last_phi0, vmc->dt);
leg->d_alpha = -leg->d_phi0;
leg->d_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);

20
User/device/referee.c
View File

@ -649,26 +649,6 @@ 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,7 +59,6 @@ typedef struct {
Referee_Status_t ref_status;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForChassis_t;
typedef struct {

5
User/module/aimbot.c
View File

@ -93,10 +93,7 @@ int8_t Aimbot_Init(Aimbot_Param_t *param) {
/* 注册 AI 指令帧队列（下层板接收／上层板发送） */
BSP_FDCAN_RegisterId(param->can, param->cmd_id,
BSP_FDCAN_DEFAULT_QUEUE_SIZE);
for (uint8_t i = 0; i < 3; i++) {
BSP_CAN_RegisterId(param->can, param->cmd_id + i,
10);
}
/* 注册反馈数据帧队列（上层板接收／下层板发送） */
for (uint8_t i = 0; i < AIMBOT_FB_FRAME_NUM; i++) {
BSP_FDCAN_RegisterId(param->can, param->fb_base_id + i,

110
User/module/balance_chassis.c
View File

@ -34,7 +34,6 @@ static void Chassis_SelectYawZeroPoint(Chassis_t *c);
static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
static void Chassis_RecoverControl(Chassis_t *c);
static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
/* Private functions -------------------------------------------------------- */
@ -86,8 +85,6 @@ static void Chassis_ResetControllers(Chassis_t *c) {
PID_Reset(&c->pid.roll_force);
PID_Reset(&c->pid.tp[0]);
PID_Reset(&c->pid.tp[1]);
PID_Reset(&c->pid.leg_test_theta[0]);
PID_Reset(&c->pid.leg_test_theta[1]);
/* 重置LQR控制器 */
LQR_Reset(&c->lqr[0]);
@ -212,11 +209,6 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
c->climb.state_start_time = 0;
}
/* 进入腿部测试模式：重置初始化标志，进入后锁定当前摆角 */
if (mode == CHASSIS_MODE_LEG_TEST) {
c->leg_test.initialized = false;
}
c->mode = mode;
return CHASSIS_OK;
}
@ -673,88 +665,6 @@ static void Chassis_RecoverControl(Chassis_t *c) {
}
/**
* @brief 腿部测试控制
* @param c 底盘结构体指针
* @param c_cmd 控制指令
* @note 进入时锁定当前摆角，vx/vy 增减左/右腿目标摆角
* PID控制摆角，恒力支撑腿长，轮子关闭
*/
static void Chassis_LegTestControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 首次进入：锁定当前摆角 */
if (!c->leg_test.initialized) {
c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
c->leg_test.initialized = true;
}
float sens = c->param->leg_test.theta_sensitivity;
float deadzone = 0.05f; /* 摇杆死区 */
/* vx 控制左腿：有输入→累加目标角度（正/反转），松手→锁住当前实际角度 */
if (fabsf(c_cmd->move_vec.vx) > deadzone) {
c->leg_test.target_theta[0] += c_cmd->move_vec.vx * sens * c->dt;
} else {
c->leg_test.target_theta[0] = c->vmc_[0].leg.theta;
}
/* vy 控制右腿：同理 */
if (fabsf(c_cmd->move_vec.vy) > deadzone) {
c->leg_test.target_theta[1] += c_cmd->move_vec.vy * sens * c->dt;
} else {
c->leg_test.target_theta[1] = c->vmc_[1].leg.theta;
}
/* 目标角度归一化到 [-π, π]，防止无限增长 */
while (c->leg_test.target_theta[0] > (float)M_PI) c->leg_test.target_theta[0] -= (float)M_2PI;
while (c->leg_test.target_theta[0] < -(float)M_PI) c->leg_test.target_theta[0] += (float)M_2PI;
while (c->leg_test.target_theta[1] > (float)M_PI) c->leg_test.target_theta[1] -= (float)M_2PI;
while (c->leg_test.target_theta[1] < -(float)M_PI) c->leg_test.target_theta[1] += (float)M_2PI;
/* 轮子关闭：只清零输出，由 Chassis_Output 统一发送，避免重复 CAN 帧 */
/* 恒定支撑力 + PID摆角控制 → VMC逆解算 → 关节力矩 */
float support_force = c->param->leg_test.support_force;
for (int i = 0; i < 2; i++) {
/* 手动计算循环误差，避免 theta 在 ±π 跳变时 PID 炸 */
float error = c->leg_test.target_theta[i] - c->vmc_[i].leg.theta;
while (error > (float)M_PI) error -= (float)M_2PI;
while (error < -(float)M_PI) error += (float)M_2PI;
/* 用误差直接算 P 和 D 输出，绕过 PID 内部的 set/fdb 差值 */
float kp = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->p;
float kd = c->pid.leg_test_theta[i].param->k * c->pid.leg_test_theta[i].param->d;
float out_limit = c->pid.leg_test_theta[i].param->out_limit;
float Tp = kp * error - kd * c->vmc_[i].leg.d_theta;
Tp = LIMIT(Tp, -out_limit, out_limit);
if (VMC_InverseSolve(&c->vmc_[i], support_force, Tp) == 0) {
if (i == 0) {
VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[0], &c->output.joint[1]);
} else {
VMC_GetJointTorques(&c->vmc_[i], &c->output.joint[3], &c->output.joint[2]);
}
} else {
c->output.joint[i * 2] = 0.0f;
c->output.joint[i * 2 + 1] = 0.0f;
}
}
/* 轮子输出清零 */
c->output.wheel[0] = 0.0f;
c->output.wheel[1] = 0.0f;
/* 安全限制 */
for (int i = 0; i < 4; i++) {
c->output.joint[i] = LIMIT(c->output.joint[i], -30.0f, 30.0f);
}
/* 发送关节指令 */
Chassis_Output(c);
}
/**
* @brief 初始化底盘
* @param c 底盘结构体指针
@ -803,8 +713,6 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.tp[1], KPID_MODE_CALC_D, target_freq, &param->tp);
PID_Init(&c->pid.leg_test_theta[0], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
PID_Init(&c->pid.leg_test_theta[1], KPID_MODE_CALC_D, target_freq, &param->leg_test_theta);
/* 初始化LQR控制器 */
LQR_Init(&c->lqr[0], &param->lqr_gains);
@ -906,11 +814,6 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
c->recover.state[1] = RECOVER_STRETCH;
c->recover.state_start_time = 0;
/* 初始化腿部测试状态 */
c->leg_test.initialized = false;
c->leg_test.target_theta[0] = 0.0f;
c->leg_test.target_theta[1] = 0.0f;
return CHASSIS_OK;
}
@ -1077,10 +980,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
Chassis_ClimbControl(c, c_cmd);
break;
case CHASSIS_MODE_LEG_TEST:
Chassis_LegTestControl(c, c_cmd);
break;
default:
return CHASSIS_ERR_MODE;
}
@ -1119,7 +1018,7 @@ void Chassis_Output(Chassis_t *c) {
// c->output.wheel[i] = LowPassFilter2p_Apply(&c->filter.wheel_out[i], c->output.wheel[i]);
// }
//cap-add//supercap-add
// PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
PowerLimit_Output(c->power_limit, c->output.wheel, 2 );
//PowerLimit_ChassicOutput()
MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
@ -1143,7 +1042,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset);
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
} else {
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 3.0f;
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
}
@ -1476,9 +1375,6 @@ 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->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;
// ui->angle = c->feedback.yaw.rotor_abs_angle - c->mech_zero;
// #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪"
}

21
User/module/balance_chassis.h
View File

@ -43,8 +43,7 @@ typedef enum {
// CHASSIS_MODE_CALIBRATE, /* 校准模式 */
CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
CHASSIS_MODE_BALANCE_ROTOR, /*小陀螺*/
CHASSIS_MODE_CLIMB_STEP, /* 上台阶模式 */
CHASSIS_MODE_LEG_TEST /* 腿部测试模式：PID锁摆角+恒力支撑，摇杆控制摆角 */
CHASSIS_MODE_CLIMB_STEP /* 上台阶模式 */
} Chassis_Mode_t;
/* 跳跃状态枚举 */
@ -102,9 +101,6 @@ typedef struct {
typedef struct {
Chassis_Mode_t mode;
float angle;
/* 轮腿角度和距轮长 */
float l0[2];
float theta[2];
} Chassis_RefereeUI_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
@ -178,14 +174,6 @@ typedef struct {
uint32_t settle_time_ms; /* 收腿后稳定时间 (ms) */
} climb;
/* 腿部测试参数 */
struct {
float support_force; /* 恒定支撑力 (N) */
float theta_sensitivity; /* 摇杆→摆角灵敏度 (rad/s) */
} leg_test;
KPID_Params_t leg_test_theta; /* 腿部测试摆角PID（独立于tp） */
/* 倒地自起参数 */
struct {
float stretch_velocity; /* 伸腿角速度 (rad/s) */
@ -279,12 +267,6 @@ typedef struct {
uint32_t state_start_time; /* 当前状态开始时间 (ms) */
} climb;
/* 腿部测试相关 */
struct {
float target_theta[2]; /* 左右腿目标摆角 (rad)，进入时锁定当前值 */
bool initialized; /* 是否已锁定初始摆角 */
} leg_test;
/* 电机离线检测 */
struct {
bool any_offline; /* 是否有电机离线 */
@ -304,7 +286,6 @@ typedef struct {
KPID_t tp[2];
KPID_t leg_length[2]; /* 两条腿的腿长PID */
KPID_t leg_theta[2]; /* 两条腿的摆角PID */
KPID_t leg_test_theta[2]; /* 腿部测试摆角PID */
} pid;
/* 滤波器 */

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

@ -3,8 +3,6 @@
*/
#include "cmd.h"
#include "bsp/time.h"
#include "module/cmd/cmd_types.h"
#include "module/shoot.h"
#include <stdint.h>
#include <string.h>
@ -70,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_CONTINUE;
ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
} else {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
}
@ -175,6 +173,9 @@ 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 */
@ -207,16 +208,11 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
/* 使用AI提供的云台控制数据 */
// if (ctx->input.nuc.mode!=0) {
if (ctx->input.nuc.mode!=0) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_AI_CONTROL;
ctx->output.gimbal.cmd.mode_ai = (uint8_t)ctx->input.nuc.mode; /* 直接使用AI模式细分字段 */
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
ctx->output.gimbal.cmd.ff_vel_yaw = ctx->input.nuc.gimbal.vel.yaw;
ctx->output.gimbal.cmd.ff_vel_pit = ctx->input.nuc.gimbal.vel.pit;
ctx->output.gimbal.cmd.ff_accl_yaw = ctx->input.nuc.gimbal.accl.yaw;
ctx->output.gimbal.cmd.ff_accl_pit = ctx->input.nuc.gimbal.accl.pit;
// }
}
}
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */
@ -232,9 +228,8 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
/* 根据AI模式决定射击行为 */
switch (ctx->input.nuc.mode) {
case 0:
case 1:
/* mode=1 只控云台，不让射击机构进入 ready，避免摩擦轮继续预转。 */
ctx->output.shoot.cmd.ready = false;
case 1:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
break;
case 2:
@ -247,10 +242,6 @@ 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 */
@ -381,7 +372,6 @@ 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 */
@ -395,12 +385,13 @@ static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) {
return;
}
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);
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.vy = 0.0f;
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */
@ -644,19 +635,19 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE);
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source==CMD_SRC_RC) {
if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.source = CMD_SRC_NUC;
// #if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
// if (ctx->input.online[CMD_SRC_NUC]) {
// if (ctx->active_source==CMD_SRC_RC) {
// if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
// ctx->output.gimbal.source = CMD_SRC_NUC;
// ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.source = CMD_SRC_NUC;
#endif
}
}
}
#endif
// }
// }
// }
// #endif
return CMD_OK;
}

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.vx = ctx->config->sensitivity.move_sens;
ctx->output.balance_chassis.cmd.move_vec.vy += 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.vx = -ctx->config->sensitivity.move_sens;
ctx->output.balance_chassis.cmd.move_vec.vy -= 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.vy = -ctx->config->sensitivity.move_sens;
ctx->output.balance_chassis.cmd.move_vec.vx -= 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.vy = ctx->config->sensitivity.move_sens;
ctx->output.balance_chassis.cmd.move_vec.vx += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
@ -132,9 +132,6 @@ 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;
@ -149,9 +146,6 @@ 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_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(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(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) \

31
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=15.0f,
.shot_freq=1.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=15.0f, // 最大射频 Hz
.nmax=18.0f, // 最大射频 Hz
.Hwarn=200.0f, // 热量预警值
.Hsatu=100.0f, // 热量饱和值
.Hthres=50.0f, // 热量阈值
@ -491,20 +491,6 @@ Config_RobotParam_t robot_config = {
.phi_tolerance = 0.5f, /* 体姿接近站立容差 (rad)，约28° */
.kd = 3.0f, /* MIT速度模式阻尼 */
},
.leg_test = {
.support_force = 30.0f, /* 恒定支撑力 (N) */
.theta_sensitivity = 1.0f, /* 摇杆→摆角灵敏度 (rad/s) */
},
.leg_test_theta = {
.k = 20.0f,
.p = 5.0f,
.i = 0.0f,
.d = 1.0f,
.i_limit = 0.0f,
.out_limit = 30.0f, /* 摆角力矩限幅 (Nm) */
.d_cutoff_freq = 30.0f,
.range = M_2PI, /* 启用循环误差处理 */
},
.lqr_offset = {
.theta = 0.0f,
.x = 0.0f,
@ -529,22 +515,21 @@ Config_RobotParam_t robot_config = {
},
.sensitivity = {
.mouse_sens = 60.0f,
.move_sens = 0.99f,
.move_sens = 1.0f,
.move_fast_mult = 2.0f,
.move_slow_mult = 0.4f,
},
.rc_mode_map = {
#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_mid = GIMBAL_MODE_ABSOLUTE,
.gimbal_sw_mid = GIMBAL_MODE_RECOVER,
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
#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_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
.balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡，避免不熟悉操作导致摔倒
.balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
#endif
},

59
User/module/gimbal.c
View File

@ -99,13 +99,13 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.pit_omega));
PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.yaw_angle));
&(g->param->pid.aimbot.yaw_angle));
PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.yaw_omega));
&(g->param->pid.aimbot.yaw_omega));
PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.pit_angle));
&(g->param->pid.aimbot.pit_angle));
PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.pit_omega));
&(g->param->pid.aimbot.pit_omega));
/* Recover模式PID（单环，编码器反馈） */
PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
@ -193,7 +193,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
g->lask_wakeup = BSP_TIME_Get_us();
/* AI自瞄保护：mode_ai==0 丢失目标，保持AI模式但锁定当前姿态 */
Gimbal_SetMode(g, g_cmd->mode);
/* 处理yaw控制命令，软件限位 */
@ -216,8 +215,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
}
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL && g_cmd->mode_ai != 0) {
/* AI 模式且有目标(mode_ai==1/2)：跟踪，用AI下发的setpoint */
if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) {
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
g->setpoint.eulr.pit = g_cmd->setpoint_pit;
} else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
@ -253,35 +252,25 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
break;
case GIMBAL_MODE_AI_CONTROL:
if (g_cmd->mode_ai == 0) {
/* 丢失目标：setpoint保持不变，用普通PID锁住当前姿态，无前馈 */
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt);
/* --- YAW --- */
// 位置环: Kp * (pos_ref - pos_fdb)
yaw_omega_set_point = PID_Calc(&(g->pid.aimbot.yaw_angle),
g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
// 速度环: Kd * (vel_ref - vel_fdb)，用上位机下发的 yaw_vel 作为前馈参考
g->out.yaw = PID_Calc(&(g->pid.aimbot.yaw_omega),
yaw_omega_set_point + g_cmd->ff_vel_yaw,
g->feedback.imu.gyro.z, 0.0f, g->dt)
+ g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt);
} else {
/* mode_ai==1/2：跟踪目标，带前馈 */
/* --- YAW --- */
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
yaw_omega_set_point += g_cmd->ff_vel_yaw;
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt);
g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA;
/* --- PIT --- */
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
pit_omega_set_point -= g_cmd->ff_vel_pit;
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt);
g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA;
}
/* --- PITCH --- (反馈轴与 ABSOLUTE 模式一致: eulr.rol / gyro.y) */
pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle),
g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.aimbot.pit_omega),
pit_omega_set_point + g_cmd->ff_vel_pit,
g->feedback.imu.gyro.y, 0.0f, g->dt)
+ g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc
break;
case GIMBAL_MODE_RECOVER:
g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,

1
User/module/gimbal.h
View File

@ -41,7 +41,6 @@ typedef struct {
typedef struct {
Gimbal_Mode_t mode;
uint8_t mode_ai; /* AI模式细分，0表示不控制，1表示控制云台但不开火，2表示控制云台且开火 */
float delta_yaw;
float delta_pit;
/* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */

2027
User/module/shoot.c
View File

File diff suppressed because it is too large Load Diff

1
User/module/shoot.h
View File

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

7
User/task/ctrl_chassis.c
View File

@ -31,7 +31,6 @@ 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 --------------------------------------------------------- */
@ -102,15 +101,13 @@ 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);

10
User/task/ctrl_shoot.c
View File

@ -4,7 +4,6 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/mrobot.h"
@ -21,7 +20,6 @@
Shoot_t shoot;
Shoot_CMD_t shoot_cmd;
Referee_ForShoot_t shoot_ref;
Shoot_AI_t shoot_forai;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -73,17 +71,11 @@ 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.heatcontrol.Hgen = 100.0f; /* 42mm弹丸每发产生热量 */
}
Shoot_UpdateFeedback(&shoot);
Shoot_SetMode(&shoot,shoot_cmd.mode);

4
User/task/init.c
View File

@ -10,10 +10,8 @@
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "module/cmd/cmd.h"
#include "device/dr16.h"
#include "device/referee.h"
#include "module/aimbot.h"
/* USER INCLUDE END */
@ -64,7 +62,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
task_runtime.msgq.cmd.nuc= osMessageQueueNew(2u, sizeof(Aimbot_AIResult_t), NULL);
/* AI */
task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL);
task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);