From 9992ffba2dfaacae027048b81b985b50967c2347 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 15 Mar 2026 11:26:01 +0800 Subject: [PATCH] fix rotor --- .DS_Store | Bin 8196 -> 12292 bytes User/module/balance_chassis.c | 97 +++++++++++++++++++++++++++------- User/module/balance_chassis.h | 17 ++++++ User/module/config.c | 19 +++++++ 4 files changed, 115 insertions(+), 18 deletions(-) diff --git a/.DS_Store b/.DS_Store index 1fed1b622b6f443fc2ff7a4544e9e83ebacb753a..49bb25a6296f3160217e20210e3afbfdf44bc507 100644 GIT binary patch literal 12292 zcmeHNU5pb|6h61RxGf92-9P?GDr+Kb!N{dhgEA zh&~=NBF1R8FOu2L?}l_e87*8#qFYsEeAwT*M%Rppswo*kpvD!T?07=e;`(@>KB~pD zf;MD!czs^quzyA_w|M!ojzCXW_u-B}Zu#P_jzI6y-ouA|-nRBdk8ByvYJ%eVC@JZY?odomrOjt}KU~&MNYQWXNy`}+c=kPlA`+H3d zuDq@WTn$vv0QV0892^EMm^Yzt9XRn2n)mY%S~OgXI1@0y9{>g|m^UGwpfQsa&?IG6 zior~BJP%lSK?~+hXp$2XGh-ZUW}dk4#>DJ^0our3tSVm7K2lcFKsJgaohnU?^QUlr^9W!zcHm7-4cB_)f0+!K)w?#6U zh?Y?L4(?1wqDpeJ)~6eWo(ijwf^#mW45cG6RZFz5x@xM`*B<1`MDtU-d4ak~YvIa* zto@6`NpU@->6!wfL9?elxWPj4(gIpdA=*LvDNiTp9r}R2q>J<`{Xy5*R5p#xV+&XZ zTf&yHAY0A0uWy~py9G}ju>0xXLQ$E!{zN(F+(DT&X^F;aSezf}nFoggt1Dr}{{@y@j%Zvd9JPa=u~0@!>RJLba4G6&6Us6hXd(2mg*FW&^_ayAC+-8% zQ`bbpT^V+Ce1Z~=_ZF6Ld}ax!yJsm%IK5prRl+Hg>PC!n*6dl8jZW22vb=` zEK$0SaoU!3I1PmyNUTqGX)5aZ-B8IICmH#z~_A&aT7!9L8A>EvOX7FNvD( zI7LuDRVZsWQg#gEye%1N6)Kyov-WdWRs~a*d9eRj!68+}F3EmP$<5`P0 z-VbTlqIR01xLraz>u^JDltp!wYr~*XO4G2oPf-sorV(0(a}OQBud_^7OCWWpq^nkJ z0z;UWQ?9ot!?X?~K1v(GQwUExp?wEOo;JrgUV0q3t3g>&0yfVfK`rpJypC^|lpO({ zPvWnLC%M{B3K7F?tCr8qRktXYedKg(R*cxW&0pp1%kBPjVsgf*r;vxVe{3 ziy&+rqKFv$PlPml8+Jq!BF*|;vhWi2C}Kaw?hgcZLv?JM-y*ijHoF*4_FZk4yZ=67 zdM3xLvj44)W%FTXVX^rA+rc-F;$$g{crwSer`kKfaj{S4HNWay=1gIiu)eX&T(*1~ zPCNT${%S9M%hw&3)BHX^f_))s=aRkb*byAqGOzec#z6UaGRJd&9Dlmgu~4MUdVQttKdYED^Y90yiOg6ddy?gO0rNfgCu(l(|wRdU=&r#*jI!pPt7A;53HS7D4vzT$k^nx6{UH5mV-rXwd`qab!+E zo;zAtA<|4TrT^NSg(hdI4k1WbmaN-Vc-?VkTtY7yyiFST}u6>byun1rk$Fr z7@AtZp8?7Gz$MSM<0umH9C;Xd?neANp)s9eb}5A&HAqrA%8zzz`Wjj-Vc%u>KXf>W zD-!?xpEz`yb1XToJNoW3Q|Bp3h1}`0~QQgFmFPW{6_u+AnnR_Z_nKQ Zzv8}b*OI&cmqoZdbCf?r-XZn>e*i<+)jt3L delta 253 zcmZokXmOBWU|?W$DortDU;r^WfEYvza8E20o2Vx_*+7Ry637P&G6KaJ81fj>8B#oR z@{^NtHVZO+W8JL4w3cx*I|mB~qr_w$1;fd|6*MMCD$SZaSy^v#wF>v-7!}3Ie^oRl zFIClMV>k*lYu@B*s-=?;s})VYA|$wZyU=^4$!21%o3+Kc8Tpu)fbOaL4+abjle?sK zCyOyX7Gwsw0q6>DAmIvf9?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; - 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; + 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; diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h index 5fd3f0e..51c7a0a 100644 --- a/User/module/balance_chassis.h +++ b/User/module/balance_chassis.h @@ -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]; diff --git a/User/module/config.c b/User/module/config.c index 35e95c3..cddf13f 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -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)