robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

+正弦

Browse Source
This commit is contained in:
Robofish 2026-03-15 18:52:18 +08:00
parent 513254e56f
commit 6b59d5922d
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
User/module/balance_chassis.c
View File

@ -820,8 +820,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== Yaw轴控制 ==================== */
if (c_cmd->mode == CHASSIS_MODE_BALANCE_ROTOR || c->mode == CHASSIS_MODE_BALANCE_ROTOR) {
/* 小陀螺模式：自动以max_spin_speed恒速旋转 */
float target_spin = c->param->rotor_ctrl.max_spin_speed;
/* 小陀螺模式：旋转速度在5~8 rad/s之间以0.5Hz正弦变化 */
float t = (float)BSP_TIME_Get_us() / 1000000.0f; /* 秒 */
float target_spin = 6.5f + 1.5f * sinf(2.0f * M_PI * 0.5f * t);
if (c->rotor_state.exiting) {
/* 退出过渡：目标角速度为0，用减速斜坡 */