ref离线了
This commit is contained in:
parent
467567fae6
commit
d933c59ae8
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
/* 输出滤波 */
|
||||
|
||||
@ -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;
|
||||
|
||||
/* 低通滤波器截止频率 */
|
||||
@ -147,6 +152,10 @@ typedef struct {
|
||||
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 {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user