Chassis/project/code/Task.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;

/*速度环的输入，开启角度环后由角度环接管(范围-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 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);

/*Yaw角工具*/
void yaw_correct(float* yaw);
void Task_10ms(uint32 state, void *ptr)
{
    // 10ms 执行一次的任务代码(100Hz)
    motor_encoder_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();

    imu_update();
    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(0.01f);
    angle_control_init(0.02f);
    position_control_init(0.02f);

    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);
}

void motor_control_init(float T)
{
    PID_Init_Config_s config_right = {
        .Kp = 300.0f,
        .Ki = 150.0f,
        .Kd = 0.0f,
        .T = T,
        .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 = T,
        .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 T)
{
    PID_Init_Config_s config_yaw = {
        .Kp = 1.0f,
        .Ki = 0.0f,
        .Kd = 0.0f,
        .T = T,
        .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 T)
{
    PID_Init_Config_s config_position = {
        .Kp = 0.5f,
        .Ki = 0.0f,
        .Kd = 0.0f,
        .T = T,
        .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;
#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
/*位置环子任务 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);
    }
    //如果右转90度标志位启用，接管角度速率，固定速度右转
    if(position_turn_right_90_runing)
    {
        //检测角度，如果完成90度，则取消右转90度标志位
        float sub = position_turn_target_yaw - imu_angle.yaw;
        yaw_correct(&sub);
        if(sub<=0)
        {
            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;
    }
}