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-18 23:58:49 +08:00
parent a3a05aa076
commit 5133123dc3
9 changed files with 75 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
User/module/aimbot.c
View File

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

37
User/module/balance_chassis.c
View File

@ -797,7 +797,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
/* 初始化PID控制器 */ /* 初始化PID控制器 */
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length); PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length); PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, &param->leg_length);
PID_Init(&c->pid.yaw, KPID_MODE_SET_D, target_freq, &param->yaw); PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, &param->yaw);
PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, &param->rotor); PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, &param->rotor);
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll); PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, &param->roll);
PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force); PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, &param->roll_force);
@ -1143,7 +1143,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.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; c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
} else { } else {
c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 3.0f; c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x; c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
} }
@ -1224,9 +1224,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 对齐完成，切换到平衡模式 */ /* 对齐完成，切换到平衡模式 */
c->rotor_state.exiting = false; c->rotor_state.exiting = false;
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE; c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
/* 只重置 yaw/rotor PID，保留 chassis_state 避免 LQR 突变 */ Chassis_ResetControllers(c);
PID_Reset(&c->pid.yaw);
PID_Reset(&c->pid.rotor);
} }
} }
} else { } else {
@ -1244,8 +1242,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 退出过渡减速完成后，用yaw PID对齐零点 */ /* 退出过渡减速完成后，用yaw PID对齐零点 */
if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) { if (c->rotor_state.exiting && c->rotor_state.current_spin_speed == 0.0f) {
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
c->feedback.imu.gyro.z, c->dt);
} else { } else {
float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */ float current_yaw_omega = -c->feedback.imu.gyro.z; /* 取反匹配yaw_force→轮子旋转方向 */
c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed, c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
@ -1262,30 +1259,16 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI); float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI); float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
/* 迟滞切换：只有当另一个零点明显更近（差值>0.3rad≈17°）时才切换， if (fabsf(error_to_target1) <= fabsf(error_to_target2)) {
* 避免在两个零点正中间反复跳变 */ c->yaw_control.target_yaw = base_target_1;
float hysteresis = 0.3f; c->yaw_control.is_reversed = false;
if (c->yaw_control.is_reversed) {
/* 当前跟踪 target_2，只有 target_1 明显更近才切 */
if (fabsf(error_to_target1) < fabsf(error_to_target2) - hysteresis) {
c->yaw_control.target_yaw = base_target_1;
c->yaw_control.is_reversed = false;
} else {
c->yaw_control.target_yaw = base_target_2;
}
} else { } else {
/* 当前跟踪 target_1，只有 target_2 明显更近才切 */ c->yaw_control.target_yaw = base_target_2;
if (fabsf(error_to_target2) < fabsf(error_to_target1) - hysteresis) { c->yaw_control.is_reversed = true;
c->yaw_control.target_yaw = base_target_2;
c->yaw_control.is_reversed = true;
} else {
c->yaw_control.target_yaw = base_target_1;
}
} }
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
c->feedback.yaw.rotor_abs_angle, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
c->feedback.imu.gyro.z, c->dt);
} }
/* ==================== 左腿LQR控制 ==================== */ /* ==================== 左腿LQR控制 ==================== */

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

@ -210,8 +210,13 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
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 = GIMBAL_MODE_AI_CONTROL;
ctx->output.gimbal.cmd.ai_mode = ctx->input.nuc.mode;
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw; 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.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;
} }
} }
@ -635,19 +640,19 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE); 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 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->input.online[CMD_SRC_NUC]) {
// if (ctx->active_source==CMD_SRC_RC) { if (ctx->active_source==CMD_SRC_RC) {
// if (ctx->input.rc.sw[0] == CMD_SW_DOWN) { if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
// ctx->output.gimbal.source = CMD_SRC_NUC; ctx->output.gimbal.source = CMD_SRC_NUC;
// ctx->output.shoot.source = CMD_SRC_NUC; ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_MODULE_REFUI #if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.source = CMD_SRC_NUC; ctx->output.refui.source = CMD_SRC_NUC;
#endif #endif
// } }
// } }
// } }
// #endif #endif
return CMD_OK; return CMD_OK;
} }

10
User/module/config.c
View File

@ -537,13 +537,15 @@ Config_RobotParam_t robot_config = {
#if CMD_ENABLE_MODULE_GIMBAL #if CMD_ENABLE_MODULE_GIMBAL
.gimbal_sw_up = GIMBAL_MODE_RELAX, .gimbal_sw_up = GIMBAL_MODE_RELAX,
.gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
// .gimbal_sw_mid = GIMBAL_MODE_RECOVER, .gimbal_sw_down = GIMBAL_MODE_RECOVER,
.gimbal_sw_down = GIMBAL_MODE_RELATIVE, // .gimbal_sw_down = GIMBAL_MODE_AI_CONTROL,
#endif #endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS #if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX, .balance_sw_up = CHASSIS_MODE_RELAX,
.balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, .balance_sw_mid = CHASSIS_MODE_RELAX,
.balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, .balance_sw_down = CHASSIS_MODE_RELAX,
// .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE,
// .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR,
#endif #endif
}, },
}, },

78
User/module/gimbal.c
View File

@ -63,17 +63,6 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
} }
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
/* 进入 RECOVER 时初始化双零点选择 */
if (mode == GIMBAL_MODE_RECOVER) {
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
float zero0 = g->param->mech_zero.yaw;
float zero1 = zero0 + M_PI;
if (zero1 >= M_2PI) zero1 -= M_2PI;
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
g->recover_yaw_reversed = (err1 < err0);
}
g->mode = mode; g->mode = mode;
return 0; return 0;
} }
@ -110,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, PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.pit_omega)); &(g->param->pid.pit_omega));
PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq, PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.yaw_angle)); &(g->param->pid.yaw_angle));
PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq, PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.yaw_omega)); &(g->param->pid.yaw_omega));
PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq, PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.pit_angle)); &(g->param->pid.pit_angle));
PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq, PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
&(g->param->pid.aimbot.pit_omega)); &(g->param->pid.pit_omega));
/* Recover模式PID（单环，编码器反馈） */ /* Recover模式PID（单环，编码器反馈） */
PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq, PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
@ -227,31 +216,26 @@ 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); CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) { if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL) {
/* AI 模式：直接从指令获取角度设定值（每周期刷新） */ if (g_cmd->setpoint_yaw == 0.0f && g_cmd->setpoint_pit == 0.0f) {
g->setpoint.eulr.yaw = g_cmd->setpoint_yaw; /* 自瞄丢失：切回ABSOLUTE，setpoint锁定当前IMU角度，停在原位 */
g->setpoint.eulr.pit = g_cmd->setpoint_pit; g_cmd->mode = GIMBAL_MODE_ABSOLUTE;
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
Gimbal_SetMode(g, GIMBAL_MODE_ABSOLUTE);
} else {
/* 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) { } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
/* 双零点选近：mech_zero 和 mech_zero+π，带迟滞避免临界点跳变 */ /* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle; float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
float zero0 = g->param->mech_zero.yaw; float zero0 = g->param->mech_zero.yaw;
float zero1 = zero0 + M_PI; float zero1 = zero0 + M_PI;
if (zero1 >= M_2PI) zero1 -= M_2PI; if (zero1 >= M_2PI) zero1 -= M_2PI;
float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI)); float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI)); float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
float hysteresis = 0.3f;
if (g->recover_yaw_reversed) {
/* 当前跟踪 zero1，只有 zero0 明显更近才切 */
if (err0 < err1 - hysteresis) {
g->recover_yaw_reversed = false;
}
} else {
/* 当前跟踪 zero0，只有 zero1 明显更近才切 */
if (err1 < err0 - hysteresis) {
g->recover_yaw_reversed = true;
}
}
g->setpoint.eulr.yaw = g->recover_yaw_reversed ? zero1 : zero0;
g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min); g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
} }
@ -277,25 +261,21 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
break; break;
case GIMBAL_MODE_AI_CONTROL: case GIMBAL_MODE_AI_CONTROL:
/* --- YAW --- */ /* --- YAW --- */
// 位置环: Kp * (pos_ref - pos_fdb) yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
yaw_omega_set_point = PID_Calc(&(g->pid.aimbot.yaw_angle),
g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt); g->feedback.imu.eulr.yaw, 0.0f, g->dt);
// 速度环: Kd * (vel_ref - vel_fdb)，用上位机下发的 yaw_vel 作为前馈参考 yaw_omega_set_point += g_cmd->ff_vel_yaw; /* 速度前馈叠加到角速度设定值 */
g->out.yaw = PID_Calc(&(g->pid.aimbot.yaw_omega), g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
yaw_omega_set_point + g_cmd->ff_vel_yaw, g->feedback.imu.gyro.z, 0.f, g->dt);
g->feedback.imu.gyro.z, 0.0f, g->dt) g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; /* 加速度前馈叠加到力矩输出 */
+ g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc
/* --- PITCH --- (反馈轴与 ABSOLUTE 模式一致: eulr.rol / gyro.y) */ /* --- PIT --- */
pit_omega_set_point = PID_Calc(&(g->pid.aimbot.pit_angle), pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit,
g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt); 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; /* 速度前馈 */
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.0f, g->dt) g->feedback.imu.gyro.y, 0.f, g->dt);
+ g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; /* 加速度前馈 */
break; break;
case GIMBAL_MODE_RECOVER: case GIMBAL_MODE_RECOVER:
g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw, g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt); g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);

3
User/module/gimbal.h
View File

@ -41,6 +41,7 @@ typedef struct {
typedef struct { typedef struct {
Gimbal_Mode_t mode; Gimbal_Mode_t mode;
Gimbal_Mode_t ai_mode; /* AI控制模式下的子模式，用于区分不同的AI控制策略 */
float delta_yaw; float delta_yaw;
float delta_pit; float delta_pit;
/* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */ /* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */
@ -171,8 +172,6 @@ typedef struct {
Gimbal_Output_t out; /* 云台输出 */ Gimbal_Output_t out; /* 云台输出 */
Gimbal_Feedback_t feedback; /* 反馈 */ Gimbal_Feedback_t feedback; /* 反馈 */
bool recover_yaw_reversed; /* RECOVER模式下跟踪的是第二个零点(+π) */
} Gimbal_t; } Gimbal_t;
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */

12
User/module/shoot.c
View File

@ -796,12 +796,12 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
} }
} }
/* 输出 */ /* 输出 */
MOTOR_RM_Ctrl(&s->param->motor.fric[0].param); // MOTOR_RM_Ctrl(&s->param->motor.fric[0].param);
if(s->param->basic.fric_num>4) // if(s->param->basic.fric_num>4)
{ // {
MOTOR_RM_Ctrl(&s->param->motor.fric[4].param); // MOTOR_RM_Ctrl(&s->param->motor.fric[4].param);
} // }
MOTOR_RM_Ctrl(&s->param->motor.trig); // MOTOR_RM_Ctrl(&s->param->motor.trig);
last_firecmd = cmd->firecmd; last_firecmd = cmd->firecmd;
return SHOOT_OK; return SHOOT_OK;
} }

2
User/task/ctrl_shoot.c
View File

@ -75,7 +75,7 @@ void Task_ctrl_shoot(void *argument) {
shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit; 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.Hcd = (float)shoot_ref.robot_status.shooter_barrel_cooling_value;
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat; shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
shoot.heatcontrol.Hgen = 100.0f; /* 42mm弹丸每发产生热量 */ shoot.heatcontrol.Hgen = 10.0f; /* 42mm弹丸每发产生热量 */
} }
Shoot_UpdateFeedback(&shoot); Shoot_UpdateFeedback(&shoot);
Shoot_SetMode(&shoot,shoot_cmd.mode); Shoot_SetMode(&shoot,shoot_cmd.mode);

4
User/task/init.c
View File

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