fix rotor
This commit is contained in:
parent
5ad9ef8f49
commit
9992ffba2d
@ -77,6 +77,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
|
||||
PID_Reset(&c->pid.leg_length[0]);
|
||||
PID_Reset(&c->pid.leg_length[1]);
|
||||
PID_Reset(&c->pid.yaw);
|
||||
PID_Reset(&c->pid.rotor);
|
||||
PID_Reset(&c->pid.roll);
|
||||
PID_Reset(&c->pid.roll_force);
|
||||
PID_Reset(&c->pid.tp[0]);
|
||||
@ -143,6 +144,13 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
/* ROTOR -> BALANCE：启动退出过渡（减速→对齐），不立即切换 */
|
||||
if (c->mode == CHASSIS_MODE_BALANCE_ROTOR &&
|
||||
mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
|
||||
c->rotor_state.exiting = true;
|
||||
return CHASSIS_OK; /* 保持在 ROTOR 模式，由控制循环处理退出 */
|
||||
}
|
||||
|
||||
Chassis_MotorEnable(c);
|
||||
Chassis_ResetControllers(c);
|
||||
Chassis_SelectYawZeroPoint(c);
|
||||
@ -171,6 +179,12 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
|
||||
c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
|
||||
}
|
||||
|
||||
/* 进入小陀螺时重置旋转状态 */
|
||||
if (mode == CHASSIS_MODE_BALANCE_ROTOR) {
|
||||
c->rotor_state.current_spin_speed = 0.0f;
|
||||
c->rotor_state.exiting = false;
|
||||
}
|
||||
|
||||
c->mode = mode;
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
@ -454,6 +468,7 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
|
||||
PID_Init(&c->pid.leg_length[0], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||||
PID_Init(&c->pid.leg_length[1], KPID_MODE_CALC_D, target_freq, ¶m->leg_length);
|
||||
PID_Init(&c->pid.yaw, KPID_MODE_CALC_D, target_freq, ¶m->yaw);
|
||||
PID_Init(&c->pid.rotor, KPID_MODE_CALC_D, target_freq, ¶m->rotor);
|
||||
PID_Init(&c->pid.roll, KPID_MODE_CALC_D, target_freq, ¶m->roll);
|
||||
PID_Init(&c->pid.roll_force, KPID_MODE_CALC_D, target_freq, ¶m->roll_force);
|
||||
PID_Init(&c->pid.tp[0], KPID_MODE_CALC_D, target_freq, ¶m->tp);
|
||||
@ -545,6 +560,10 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
|
||||
/* 初始化电机离线检测 */
|
||||
c->motor_status.any_offline = false;
|
||||
|
||||
/* 初始化小陀螺状态 */
|
||||
c->rotor_state.current_spin_speed = 0.0f;
|
||||
c->rotor_state.exiting = false;
|
||||
|
||||
return CHASSIS_OK;
|
||||
}
|
||||
|
||||
@ -696,8 +715,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
break;
|
||||
case CHASSIS_MODE_BALANCE_ROTOR:
|
||||
Chassis_LQRControl(c, c_cmd);
|
||||
c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
|
||||
c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
|
||||
Chassis_VMCControl(c, c_cmd);
|
||||
Chassis_Output(c);
|
||||
break;
|
||||
@ -753,21 +770,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
if (c == NULL || c_cmd == NULL) return CHASSIS_ERR_NULL;
|
||||
|
||||
/* ==================== 速度控制 ==================== */
|
||||
// float raw_vx = c_cmd->move_vec.vx * 2.0f;
|
||||
// float desired_velocity = c->yaw_control.is_reversed ? -raw_vx : raw_vx;
|
||||
|
||||
// /* 加速度限制 */
|
||||
// float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
|
||||
// float max_velocity_change = c->param->motion.max_acceleration * c->dt;
|
||||
// velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);
|
||||
|
||||
// float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
|
||||
// c->chassis_state.target_velocity_x = target_dot_x;
|
||||
// c->chassis_state.last_target_velocity_x = target_dot_x;
|
||||
// c->chassis_state.target_x += target_dot_x * c->dt;
|
||||
if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR) {
|
||||
/* 小陀螺平移：将世界坐标系期望速度投影到机体坐标系 */
|
||||
float yaw_angle = c->feedback.imu.euler.yaw;
|
||||
float world_vx = c_cmd->move_vec.vx * c->param->rotor_ctrl.max_trans_speed;
|
||||
c->chassis_state.target_velocity_x = world_vx * cosf(yaw_angle);
|
||||
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
|
||||
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
|
||||
} else {
|
||||
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.target_x += c->chassis_state.target_velocity_x * c->dt;
|
||||
}
|
||||
|
||||
|
||||
/* ==================== 状态更新 ==================== */
|
||||
@ -801,7 +815,54 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
||||
};
|
||||
|
||||
/* ==================== Yaw轴控制 ==================== */
|
||||
if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) {
|
||||
if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) {
|
||||
/* 小陀螺模式：PID跟踪目标角速度，带速度斜坡 */
|
||||
float target_spin = c_cmd->move_vec.vy * c->param->rotor_ctrl.max_spin_speed;
|
||||
|
||||
if (c->rotor_state.exiting) {
|
||||
/* 退出过渡：目标角速度为0，用减速斜坡 */
|
||||
target_spin = 0.0f;
|
||||
float decel_step = c->param->rotor_ctrl.spin_decel * c->dt;
|
||||
if (c->rotor_state.current_spin_speed > decel_step) {
|
||||
c->rotor_state.current_spin_speed -= decel_step;
|
||||
} else if (c->rotor_state.current_spin_speed < -decel_step) {
|
||||
c->rotor_state.current_spin_speed += decel_step;
|
||||
} else {
|
||||
/* 减速完成，检查yaw对齐 */
|
||||
c->rotor_state.current_spin_speed = 0.0f;
|
||||
Chassis_SelectYawZeroPoint(c);
|
||||
float yaw_error = CircleError(c->yaw_control.target_yaw,
|
||||
c->feedback.yaw.rotor_abs_angle, M_2PI);
|
||||
if (fabsf(yaw_error) < c->param->rotor_ctrl.align_threshold) {
|
||||
/* 对齐完成，切换到平衡模式 */
|
||||
c->rotor_state.exiting = false;
|
||||
c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
||||
Chassis_ResetControllers(c);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
/* 正常旋转：平滑加速斜坡 */
|
||||
float accel_step = c->param->rotor_ctrl.spin_accel * c->dt;
|
||||
if (c->rotor_state.current_spin_speed < target_spin - accel_step) {
|
||||
c->rotor_state.current_spin_speed += accel_step;
|
||||
} else if (c->rotor_state.current_spin_speed > target_spin + accel_step) {
|
||||
c->rotor_state.current_spin_speed -= accel_step;
|
||||
} else {
|
||||
c->rotor_state.current_spin_speed = target_spin;
|
||||
}
|
||||
}
|
||||
|
||||
/* 退出过渡减速完成后，用yaw PID对齐零点 */
|
||||
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->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
|
||||
} else {
|
||||
float current_yaw_omega = c->feedback.imu.gyro.z;
|
||||
c->yaw_control.yaw_force = PID_Calc(&c->pid.rotor, c->rotor_state.current_spin_speed,
|
||||
current_yaw_omega, 0.0f, c->dt);
|
||||
}
|
||||
c->yaw_control.is_reversed = false;
|
||||
} else {
|
||||
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||||
|
||||
float manual_offset = c_cmd->move_vec.vy * M_PI_2;
|
||||
|
||||
@ -103,6 +103,7 @@ typedef struct {
|
||||
LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
|
||||
|
||||
KPID_Params_t yaw; /* yaw轴PID控制参数 */
|
||||
KPID_Params_t rotor; /* 小陀螺角速度PID */
|
||||
KPID_Params_t roll; /* roll轴腿长补偿PID */
|
||||
KPID_Params_t roll_force; /* roll轴力补偿PID */
|
||||
KPID_Params_t tp; /* 摆力矩PID */
|
||||
@ -146,6 +147,15 @@ typedef struct {
|
||||
float non_contact_theta; /* 离地时摆角目标 (rad) */
|
||||
} motion;
|
||||
|
||||
/* 小陀螺参数 */
|
||||
struct {
|
||||
float max_spin_speed; /* 最大旋转角速度 (rad/s) */
|
||||
float max_trans_speed; /* 小陀螺时最大平移速度 (m/s) */
|
||||
float spin_accel; /* 旋转加速度 (rad/s²) */
|
||||
float spin_decel; /* 退出减速度 (rad/s²) */
|
||||
float align_threshold; /* 退出对齐阈值 (rad)，yaw误差小于此值时回到BALANCE模式 */
|
||||
} rotor_ctrl;
|
||||
|
||||
/* LQR 目标状态偏移 */
|
||||
struct {
|
||||
float theta;
|
||||
@ -200,6 +210,12 @@ typedef struct {
|
||||
bool is_reversed; /* 是否使用反转的零点（180度零点），影响前后方向 */
|
||||
} yaw_control;
|
||||
|
||||
/* 小陀螺状态 */
|
||||
struct {
|
||||
float current_spin_speed; /* 当前旋转速度 (rad/s)，含斜坡 */
|
||||
bool exiting; /* 正在退出小陀螺 */
|
||||
} rotor_state;
|
||||
|
||||
/* 跳跃控制相关 */
|
||||
struct {
|
||||
Jump_State_t state; /* 跳跃状态 */
|
||||
@ -228,6 +244,7 @@ typedef struct {
|
||||
/* 反馈控制用的PID */
|
||||
struct {
|
||||
KPID_t yaw; /* 跟随云台用的PID */
|
||||
KPID_t rotor; /* 小陀螺角速度PID */
|
||||
KPID_t roll; /* 横滚角腿长补偿PID */
|
||||
KPID_t roll_force; /* 横滚角力补偿PID */
|
||||
KPID_t tp[2];
|
||||
|
||||
@ -243,6 +243,17 @@ Config_RobotParam_t robot_config = {
|
||||
.range=M_2PI, /* 2*PI，用于处理角度循环误差 */
|
||||
},
|
||||
|
||||
.rotor={
|
||||
.k=1.0f,
|
||||
.p=0.5f,
|
||||
.i=0.0f,
|
||||
.d=0.01f,
|
||||
.i_limit=0.0f,
|
||||
.out_limit=1.0f,
|
||||
.d_cutoff_freq=30.0f,
|
||||
.range=-1.0f, /* 角速度不需要循环误差处理 */
|
||||
},
|
||||
|
||||
.roll={
|
||||
.k=0.15f,
|
||||
.p=0.3f, /* 横滚角腿长补偿比例系数 (降低避免抖动) */
|
||||
@ -383,6 +394,14 @@ Config_RobotParam_t robot_config = {
|
||||
.non_contact_theta = -0.01f, /* 离地摆角目标 (rad) */
|
||||
},
|
||||
|
||||
.rotor_ctrl = {
|
||||
.max_spin_speed = 0.05236f, /* 最大旋转角速度 (rad/s)，0.5rpm */
|
||||
.max_trans_speed = 1.5f, /* 小陀螺时最大平移速度 (m/s) */
|
||||
.spin_accel = 5.0f, /* 旋转加速度 (rad/s²) */
|
||||
.spin_decel = 8.0f, /* 退出减速度 (rad/s²)，比加速更快 */
|
||||
.align_threshold = 0.15f, /* 退出对齐阈值 (rad)，约8.6° */
|
||||
},
|
||||
|
||||
.vmc_param = {
|
||||
{ // 左腿
|
||||
.leg_1 = 0.215f, // 后髋连杆长度 (L1) (m)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user