diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 2ca0392..655aa6a 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1042,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; } diff --git a/User/module/config.c b/User/module/config.c index 1ee870e..2f6c0be 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -73,9 +73,31 @@ Config_RobotParam_t robot_config = { .d_cutoff_freq = -1.0f, .range = M_2PI, }, + .recover = { + .yaw_angle = { + .k = 2.0f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 5.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI, + }, + .pit_angle = { + .k = 1.5f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 0.0f, + .out_limit = 5.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI, + }, + }, }, .mech_zero = { - .yaw = 0.0f, + .yaw = 2.96925735f, /* 机械零点 */ .pit = 3.23056364f, }, .travel = { @@ -500,7 +522,8 @@ Config_RobotParam_t robot_config = { .rc_mode_map = { #if CMD_ENABLE_MODULE_GIMBAL .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_RELATIVE, #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS diff --git a/User/module/gimbal.c b/User/module/gimbal.c index f251da4..f6f945c 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -46,6 +46,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { PID_Reset(&g->pid.aimbot.yaw_omega); PID_Reset(&g->pid.aimbot.pit_angle); PID_Reset(&g->pid.aimbot.pit_omega); + PID_Reset(&g->pid.recover.yaw_angle); + PID_Reset(&g->pid.recover.pit_angle); LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); @@ -105,6 +107,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq, &(g->param->pid.aimbot.pit_omega)); + /* Recover模式PID(单环,编码器反馈) */ + PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq, + &(g->param->pid.recover.yaw_angle)); + PID_Init(&(g->pid.recover.pit_angle), KPID_MODE_NO_D, target_freq, + &(g->param->pid.recover.pit_angle)); + LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, g->param->low_pass_cutoff_freq.out); LowPassFilter2p_Init(&g->filter_out.pit, target_freq, @@ -211,7 +219,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { /* 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) { + /* 双零点选近:mech_zero 和 mech_zero+π,选离当前编码器角度最近的 */ + 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->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1; + g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min); + } + /* 控制相关逻辑 */ float yaw_omega_set_point, pit_omega_set_point; switch (g->mode) { @@ -253,6 +272,12 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { 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, + g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt); + g->out.pit = PID_Calc(&(g->pid.recover.pit_angle), g->setpoint.eulr.pit, + g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt); + break; } /* 输出滤波 */ diff --git a/User/module/gimbal.h b/User/module/gimbal.h index 82d8534..79e3ca8 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -30,7 +30,8 @@ typedef enum { GIMBAL_MODE_RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */ GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制,控制相对于底盘的姿态 */ - GIMBAL_MODE_AI_CONTROL /* AI控制模式,直接接受AI下发的目标角度 */ + GIMBAL_MODE_AI_CONTROL, /* AI控制模式,直接接受AI下发的目标角度 */ + GIMBAL_MODE_RECOVER /* 自起模式,底盘翻倒后自动恢复到正常姿态 */ } Gimbal_Mode_t; /* UI 导出结构(供 referee 系统绘制) */ @@ -79,6 +80,10 @@ typedef struct { KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ } aimbot; + struct { + KPID_Params_t yaw_angle; /* recover yaw位置环PID参数 */ + KPID_Params_t pit_angle; /* recover pitch位置环PID参数 */ + } recover; } pid; /* 低通滤波器截止频率 */ @@ -141,12 +146,16 @@ typedef struct { KPID_t yaw_omega; /* yaw轴角速度环PID */ KPID_t pit_angle; /* pitch轴角位置环PID */ KPID_t pit_omega; /* pitch轴角速度环PID */ - struct { + struct { KPID_t yaw_angle; /* yaw轴角位置环PID */ KPID_t yaw_omega; /* yaw轴角速度环PID */ KPID_t pit_angle; /* pitch轴角位置环PID */ KPID_t pit_omega; /* pitch轴角速度环PID */ } aimbot; + struct { + KPID_t yaw_angle; /* recover yaw位置环PID */ + KPID_t pit_angle; /* recover pitch位置环PID */ + } recover; } pid; struct {