From 4a3706db4c787b671485102c944ccf029ac097e6 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 17 Mar 2026 01:38:40 +0800 Subject: [PATCH] add_gimbal_recover --- User/module/config.c | 3 +++ User/module/gimbal.c | 29 ++++++++++++++++++++++++++++- User/module/gimbal.h | 16 +++++++++++----- 3 files changed, 42 insertions(+), 6 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index 65f3b20..a074bad 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -80,6 +80,9 @@ Config_RobotParam_t robot_config = { .yaw = -1.0f, .pit = 0.85f, }, + .recover = { + .yaw_mech_zero = 0.0f, /* yaw编码器机械零点，需要实测标定 */ + }, .low_pass_cutoff_freq = { .out = -1.0f, .gyro = 1000.0f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index bca9898..c811b6a 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -50,7 +50,6 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); MOTOR_DM_Enable(&(g->param->yaw_motor)); - MOTOR_DM_Enable(&(g->param->pit_motor)); AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ @@ -256,6 +255,34 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->feedback.imu.gyro.x, 0.0f, g->dt) + g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA; // 加速度前馈: J * acc break; + + case GIMBAL_MODE_RECOVER: { + /* --- YAW: 编码器双环PID，双零点选近 --- */ + float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle; + float zero0 = g->param->recover.yaw_mech_zero; + 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)); + float yaw_target = (err0 <= err1) ? zero0 : zero1; + + yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), yaw_target, + yaw_enc, 0.0f, g->dt); + g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point, + g->feedback.motor.yaw.rotor_speed, 0.0f, g->dt); + + /* --- PITCH: 编码器双环PID，锁定限位中心 --- */ + float pit_center = 0.5f * (g->limit.pit.max + g->limit.pit.min); + float pit_enc = g->feedback.motor.pit.rotor_abs_angle; + + pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), pit_center, + pit_enc, 0.0f, g->dt); + g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point, + g->feedback.motor.pit.rotor_speed, 0.0f, g->dt); + break; + } } /* 输出滤波 */ diff --git a/User/module/gimbal.h b/User/module/gimbal.h index 92ddd5f..fc3d6c2 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 /* 回中模式，使用编码器双环PID控制yaw回零点，pitch锁定限位中心 */ } Gimbal_Mode_t; /* UI 导出结构（供 referee 系统绘制） */ @@ -74,10 +75,10 @@ typedef struct { KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ 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 */ + KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */ + KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */ + KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */ + KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */ } aimbot; } pid; @@ -97,6 +98,11 @@ typedef struct { float pit; /* pitch轴机械限位行程 -1表示无限位*/ } travel; + /* Recover模式参数 */ + struct { + float yaw_mech_zero; /* yaw机械零点（编码器角度） */ + } recover; + } Gimbal_Params_t; /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */