diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index cbab103..2ca0392 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -1050,7 +1050,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { * 无速度指令时:位移+速度双环控制,积分保持位置锁定 * 位移误差钳位到±2m,防止积分漂移过大 */ if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) { - c->chassis_state.target_x = c->chassis_state.position_x; + c->chassis_state.target_x = c->chassis_state.position_x+0.8f; /* 直接锁定在当前位置,消除位移误差 */ } else { c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt; } diff --git a/User/module/config.c b/User/module/config.c index 897f95c..1ee870e 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -5,6 +5,7 @@ /* Includes ----------------------------------------------------------------- */ #include "module/config.h" #include "module/balance_chassis.h" +#include "module/gimbal.h" #include @@ -81,9 +82,6 @@ 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, @@ -502,13 +500,14 @@ Config_RobotParam_t robot_config = { .rc_mode_map = { #if CMD_ENABLE_MODULE_GIMBAL .gimbal_sw_up = GIMBAL_MODE_RELAX, - .gimbal_sw_mid = GIMBAL_MODE_RELATIVE, + .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, .gimbal_sw_down = GIMBAL_MODE_RELATIVE, #endif #if CMD_ENABLE_MODULE_BALANCE_CHASSIS .balance_sw_up = CHASSIS_MODE_RELAX, - .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, - .balance_sw_down = CHASSIS_MODE_BALANCE_ROTOR, + // .balance_sw_mid = CHASSIS_MODE_WHELL_LEG_BALANCE, + .balance_sw_mid = CHASSIS_MODE_RELAX, // 先关闭轮腿平衡,避免不熟悉操作导致摔倒 + .balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE, #endif }, }, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 2b37b3d..f251da4 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -218,7 +218,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { case GIMBAL_MODE_RELAX: g->out.yaw = 0.0f; g->out.pit = 0.0f; - /* fallthrough - AI控制模式也需要执行PID计算 */ break; case GIMBAL_MODE_ABSOLUTE: case GIMBAL_MODE_RELATIVE: @@ -233,7 +232,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->feedback.imu.gyro.y, 0.f, g->dt); break; - break; case GIMBAL_MODE_AI_CONTROL: /* --- YAW --- */ // 位置环: Kp * (pos_ref - pos_fdb) @@ -246,43 +244,15 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) { g->feedback.imu.gyro.z, 0.0f, g->dt) + g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA; // 加速度前馈: J * acc - /* --- PITCH --- */ + /* --- 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.pit, 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, - g->feedback.imu.gyro.x, 0.0f, g->dt) + g->feedback.imu.gyro.y, 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 fc3d6c2..82d8534 100644 --- a/User/module/gimbal.h +++ b/User/module/gimbal.h @@ -30,8 +30,7 @@ typedef enum { GIMBAL_MODE_RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */ GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制,控制相对于底盘的姿态 */ - GIMBAL_MODE_AI_CONTROL, /* AI控制模式,直接接受AI下发的目标角度 */ - GIMBAL_MODE_RECOVER /* 回中模式,使用编码器双环PID控制yaw回零点,pitch锁定限位中心 */ + GIMBAL_MODE_AI_CONTROL /* AI控制模式,直接接受AI下发的目标角度 */ } Gimbal_Mode_t; /* UI 导出结构(供 referee 系统绘制) */ @@ -98,11 +97,6 @@ typedef struct { float pit; /* pitch轴机械限位行程 -1表示无限位*/ } travel; - /* Recover模式参数 */ - struct { - float yaw_mech_zero; /* yaw机械零点(编码器角度) */ - } recover; - } Gimbal_Params_t; /* 云台反馈数据的结构体,包含反馈控制用的反馈数据 */