SkyLiCoder
7260fe9fba
根因：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自动计算，消除硬编码
221 lines
6.7 KiB
C
221 lines
6.7 KiB
C
#include "Task.h"
|
|
#include "zf_driver_pit.h"
|
|
#include "zf_device_key.h"
|
|
#include "zf_common_interrupt.h"
|
|
#include "IMU.h"
|
|
#include "Beep.h"
|
|
#include "GrayscaleSensor.h"
|
|
#include "Motor.h"
|
|
#include "MotorEncoder.h"
|
|
#include "Command.h"
|
|
|
|
|
|
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;
|
|
/*角度环接管速度之后的基础直行速度*/
|
|
volatile int16_t motor_speed_base = 0;
|
|
/*角度环参考值*/
|
|
volatile float yaw_ref = 0;
|
|
/*角度速率控制，开启位置环后由位置环接管*/
|
|
volatile float angle_velocity = 0;
|
|
|
|
volatile PIDInstance pid_speed_right={0};
|
|
volatile PIDInstance pid_speed_left={0};
|
|
volatile PIDInstance pid_yaw = {0};
|
|
volatile PIDInstance pid_position = {0};
|
|
|
|
|
|
void angle_control_task(void);
|
|
void position_control_task(void);
|
|
void motor_control_task(void);
|
|
|
|
/*Yaw角工具*/
|
|
void yaw_correct(float* yaw);
|
|
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)
|
|
{
|
|
// 20ms 执行一次的任务代码(50Hz)
|
|
grayscale_sensor_scan();
|
|
if(position_control_task_enable)
|
|
position_control_task();
|
|
|
|
if(angle_control_task_enable)
|
|
angle_control_task();
|
|
}
|
|
void Task_50ms(uint32 state, void *ptr)
|
|
{
|
|
// 50ms 执行一次的任务代码(20Hz)
|
|
key_scanner();
|
|
beep_actuator();
|
|
command_task();
|
|
}
|
|
void task_init(void)
|
|
{
|
|
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, (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 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 = mc_dt,
|
|
.MaxOut = MOTOR_MAX
|
|
};
|
|
PIDInit((PIDInstance *)&pid_speed_right, &config_right);
|
|
PID_Init_Config_s config_left = {
|
|
.Kp = 300.0f,
|
|
.Ki = 150.0f,
|
|
.Kd = 0.0f,
|
|
.T = mc_dt,
|
|
.MaxOut = MOTOR_MAX
|
|
};
|
|
PIDInit((PIDInstance *)&pid_speed_left, &config_left);
|
|
}
|
|
void motor_control_task(void)
|
|
{
|
|
float pwm_right = PIDCalculate((PIDInstance *)&pid_speed_right,motor_speed_right.value,motor_speed_right_ref);
|
|
float pwm_left = PIDCalculate((PIDInstance *)&pid_speed_left,motor_speed_left.value,motor_speed_left_ref);
|
|
|
|
motor_set_pwm_right(pwm_right);
|
|
motor_set_pwm_left(pwm_left);
|
|
}
|
|
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 = ac_dt,
|
|
.MaxOut = 25,
|
|
.Improve = PID_Yaw_Error_Corretion | PID_Derivative_On_Measurement
|
|
};
|
|
PIDInit((PIDInstance *)&pid_yaw, &config_yaw);
|
|
}
|
|
void angle_control_task(void)
|
|
{
|
|
//角度速率控制
|
|
set_yaw_ref(yaw_ref + angle_velocity*pid_yaw.dt);
|
|
|
|
float speed_dif = PIDCalculate((PIDInstance *)&pid_yaw,imu_angle.yaw,yaw_ref);
|
|
motor_speed_left_ref = motor_speed_base - speed_dif;
|
|
motor_speed_right_ref = motor_speed_base + speed_dif;
|
|
}
|
|
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 = pc_dt,
|
|
.MaxOut = 90,
|
|
};
|
|
PIDInit((PIDInstance *)&pid_position, &config_position);
|
|
}
|
|
/*位置环子任务 BEGIN*/
|
|
//右转90度，按需开启，开启后自动检测条件结束
|
|
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 (-57)
|
|
#define POSITION_TURN_ANGLE_INCREASE (-85)
|
|
#define POSITION_TURN_ANGLE_INCREASE_CHECK_THRESHOLD (5.0f)
|
|
/*位置环子任务 END*/
|
|
void position_control_task(void)
|
|
{
|
|
//检测到需要右转90度
|
|
if(!position_turn_right_90_runing && check_turn_right_90())
|
|
{
|
|
//开启右转90度标志位
|
|
position_turn_right_90_runing=1;
|
|
//记录要到达的角度
|
|
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)
|
|
{
|
|
//检测角度，如果完成90度，则取消右转90度标志位
|
|
float sub = position_turn_target_yaw - imu_angle.yaw;
|
|
yaw_correct(&sub);
|
|
if(-sub<POSITION_TURN_ANGLE_INCREASE_CHECK_THRESHOLD)
|
|
{
|
|
beep_on_time(100);
|
|
position_turn_right_90_runing=0;
|
|
//取消后立即进行一次计算
|
|
angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
|
|
}else{
|
|
//接管角度速率
|
|
angle_velocity=POSITION_TURN_ANGLE_VELOCITY;
|
|
}
|
|
}
|
|
else{
|
|
//角度速率（单位度/秒）
|
|
angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
|
|
}
|
|
//发送给云台，角度速率前馈
|
|
command_send_angle_velocity((float*)&angle_velocity);
|
|
|
|
}
|
|
|
|
void set_yaw_ref(float set_value)
|
|
{
|
|
yaw_correct(&set_value);
|
|
yaw_ref = set_value;
|
|
}
|
|
void yaw_correct(float* yaw)
|
|
{
|
|
if(*yaw>180)
|
|
{
|
|
while(*yaw > 180)
|
|
*yaw -= 360;
|
|
}
|
|
else if(*yaw<-180)
|
|
{
|
|
while(*yaw < -180)
|
|
*yaw += 360;
|
|
}
|
|
} |