From 7260fe9fba44fa6bb2bee0041dd9a02c0dabb5dc Mon Sep 17 00:00:00 2001 From: SkyLi Date: Sun, 7 Jun 2026 20:10:05 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8DIMU=E9=87=87=E6=A0=B7?= =?UTF-8?q?=E9=A2=91=E7=8E=87=E4=B8=8D=E5=8C=B9=E9=85=8D=E5=AF=BC=E8=87=B4?= =?UTF-8?q?yaw=E8=A7=92=E7=A7=AF=E5=88=86=E5=81=8F=E5=B0=8F4=E5=80=8D?= =?UTF-8?q?=E7=9A=84=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 根因：imu_init(200) 声明200Hz，但imu_update实际在50Hz任务中调用， Mahony积分dt差4倍，导致转90度只测出22度。 修复：imu_update移至10ms任务(100Hz)，传感器采集与控制频率对齐。 同时将任务周期全部参数化： - Task.h 统一定义 TASK_10MS/20MS/50MS_FREQ_HZ - 各子系统init改为传入频率(Hz)，内部算周期(dt) - PIT定时器周期由存储的dt自动计算，消除硬编码 --- project/code/Command.c | 3 ++ project/code/Task.c | 72 ++++++++++++++++++++++++++--------------- project/code/Task.h | 18 +++++++++-- project/user/src/main.c | 9 ++++-- 4 files changed, 70 insertions(+), 32 deletions(-) diff --git a/project/code/Command.c b/project/code/Command.c index e394768..dd4445b 100644 --- a/project/code/Command.c +++ b/project/code/Command.c @@ -158,6 +158,9 @@ void SwitchToAutoMode() //设定基础直行速度 motor_speed_base=AUTO_MODE_BASE_SPEED; + //清理子任务标志 + position_turn_right_90_runing=0; + /* 开启控制任务 */ motor_control_task_enable=1; angle_control_task_enable=1; diff --git a/project/code/Task.c b/project/code/Task.c index c452ccb..2dfa00c 100644 --- a/project/code/Task.c +++ b/project/code/Task.c @@ -14,6 +14,16 @@ volatile uint8_t motor_control_task_enable = 0; volatile uint8_t angle_control_task_enable = 0; volatile uint8_t position_control_task_enable = 0; +//================================================================================================== +// 子系统频率/周期存储（由 *_init 函数填入，其他函数按需读取） +//================================================================================================== +static float mc_freq = 0.0f; // 电机控制频率 (Hz) +static float mc_dt = 0.0f; // 电机控制周期 (s) +static float ac_freq = 0.0f; // 角度控制频率 (Hz) +static float ac_dt = 0.0f; // 角度控制周期 (s) +static float pc_freq = 0.0f; // 位置控制频率 (Hz) +static float pc_dt = 0.0f; // 位置控制周期 (s) + /*速度环的输入，开启角度环后由角度环接管(范围-50到+50)*/ volatile int16_t motor_speed_right_ref = 0; volatile int16_t motor_speed_left_ref = 0; @@ -30,9 +40,6 @@ volatile PIDInstance pid_yaw = {0}; volatile PIDInstance pid_position = {0}; -void motor_control_init(float T); -void angle_control_init(float T); -void position_control_init(float T); void angle_control_task(void); void position_control_task(void); void motor_control_task(void); @@ -43,9 +50,10 @@ void Task_10ms(uint32 state, void *ptr) { // 10ms 执行一次的任务代码(100Hz) motor_encoder_update(); + imu_update(); if(motor_control_task_enable) motor_control_task(); - + } void Task_20ms(uint32 state, void *ptr) { @@ -54,7 +62,6 @@ void Task_20ms(uint32 state, void *ptr) if(position_control_task_enable) position_control_task(); - imu_update(); if(angle_control_task_enable) angle_control_task(); } @@ -67,25 +74,28 @@ void Task_50ms(uint32 state, void *ptr) } void task_init(void) { - motor_control_init(0.01f); - angle_control_init(0.02f); - position_control_init(0.02f); + motor_control_init(TASK_10MS_FREQ_HZ); + angle_control_init(TASK_20MS_FREQ_HZ); + position_control_init(TASK_20MS_FREQ_HZ); - pit_ms_init(PIT_TIM_A0,10,Task_10ms,NULL); - interrupt_set_priority(TIMA0_INT_IRQn,1); - pit_ms_init(PIT_TIM_A1,20,Task_20ms,NULL); - interrupt_set_priority(TIMA1_INT_IRQn,1); - pit_ms_init(PIT_TIM_G6,50,Task_50ms,NULL); - interrupt_set_priority(TIMG6_INT_IRQn,7); + pit_ms_init(PIT_TIM_A0, (uint32_t)(mc_dt * 1000.0f), Task_10ms, NULL); + interrupt_set_priority(TIMA0_INT_IRQn, 1); + pit_ms_init(PIT_TIM_A1, (uint32_t)(ac_dt * 1000.0f), Task_20ms, NULL); + interrupt_set_priority(TIMA1_INT_IRQn, 1); + pit_ms_init(PIT_TIM_G6, (uint32_t)(1000.0f / TASK_50MS_FREQ_HZ), Task_50ms, NULL); + interrupt_set_priority(TIMG6_INT_IRQn, 7); } -void motor_control_init(float T) +void motor_control_init(float freq_hz) { + mc_freq = freq_hz; + mc_dt = 1.0f / freq_hz; + PID_Init_Config_s config_right = { .Kp = 300.0f, .Ki = 150.0f, .Kd = 0.0f, - .T = T, + .T = mc_dt, .MaxOut = MOTOR_MAX }; PIDInit((PIDInstance *)&pid_speed_right, &config_right); @@ -93,7 +103,7 @@ void motor_control_init(float T) .Kp = 300.0f, .Ki = 150.0f, .Kd = 0.0f, - .T = T, + .T = mc_dt, .MaxOut = MOTOR_MAX }; PIDInit((PIDInstance *)&pid_speed_left, &config_left); @@ -106,13 +116,16 @@ void motor_control_task(void) motor_set_pwm_right(pwm_right); motor_set_pwm_left(pwm_left); } -void angle_control_init(float T) +void angle_control_init(float freq_hz) { + ac_freq = freq_hz; + ac_dt = 1.0f / freq_hz; + PID_Init_Config_s config_yaw = { .Kp = 1.0f, .Ki = 0.0f, .Kd = 0.0f, - .T = T, + .T = ac_dt, .MaxOut = 25, .Improve = PID_Yaw_Error_Corretion | PID_Derivative_On_Measurement }; @@ -127,24 +140,28 @@ void angle_control_task(void) motor_speed_left_ref = motor_speed_base - speed_dif; motor_speed_right_ref = motor_speed_base + speed_dif; } -void position_control_init(float T) +void position_control_init(float freq_hz) { + pc_freq = freq_hz; + pc_dt = 1.0f / freq_hz; + PID_Init_Config_s config_position = { .Kp = 0.5f, .Ki = 0.0f, .Kd = 0.0f, - .T = T, + .T = pc_dt, .MaxOut = 90, }; PIDInit((PIDInstance *)&pid_position, &config_position); } /*位置环子任务 BEGIN*/ //右转90度，按需开启，开启后自动检测条件结束 -volatile static uint8_t position_turn_right_90_runing = 0; -volatile static float position_turn_target_yaw = 0; +volatile uint8_t position_turn_right_90_runing = 0; +volatile float position_turn_target_yaw = 0; #define check_turn_right_90() (GS_IS_ON_BLACK(GS_R4) && GS_IS_ON_BLACK(GS_R3) && GS_IS_ON_BLACK(GS_R2)) -#define POSITION_TURN_ANGLE_VELOCITY 37 -#define POSITION_TURN_ANGLE_INCREASE 85 +#define POSITION_TURN_ANGLE_VELOCITY (-57) +#define POSITION_TURN_ANGLE_INCREASE (-85) +#define POSITION_TURN_ANGLE_INCREASE_CHECK_THRESHOLD (5.0f) /*位置环子任务 END*/ void position_control_task(void) { @@ -156,6 +173,7 @@ void position_control_task(void) //记录要到达的角度 position_turn_target_yaw = imu_angle.yaw + POSITION_TURN_ANGLE_INCREASE; yaw_correct((float *)&position_turn_target_yaw); + beep_on_time(100); } //如果右转90度标志位启用，接管角度速率，固定速度右转 if(position_turn_right_90_runing) @@ -163,8 +181,9 @@ void position_control_task(void) //检测角度，如果完成90度，则取消右转90度标志位 float sub = position_turn_target_yaw - imu_angle.yaw; yaw_correct(&sub); - if(sub<=0) + if(-sub