STM32G4-DRV8301-FOC/FOC_Related/CONTROL/control.c

#include "control.h"
#include "pid.h"

float Angle_Out; // <20>Ƕ<EFBFBD>PWM<57><4D><EFBFBD><EFBFBD>
float Speed_Out; // <20>ٶ<EFBFBD>PWM<57><4D><EFBFBD><EFBFBD>
float Current_Out; // <20><><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD>

/**********************
<EFBFBD>ǶȻ<EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD>

<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǶȻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
**********************/
float Angle_Control(float Angle_Err)
{
    int PWM_Out; // PWM<57><4D><EFBFBD><EFBFBD>

    pid_angle.err = Angle_Err; // <20><><EFBFBD>½ǶȻ<C7B6><C8BB>ĵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>

    pid_angle.integral += pid_angle.err; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

    // PID<49><44>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    PWM_Out = pid_angle.Kp * pid_angle.err + 
              pid_angle.Ki * pid_angle.integral + 
              pid_angle.Kd * (pid_angle.err - pid_angle.err_last);

    // <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    pid_angle.integral = pid_angle.integral > 2000 ? 2000 : 
                         (pid_angle.integral < (-2000) ? (-2000) : pid_angle.integral);

    pid_angle.err_last = pid_angle.err; // <20><><EFBFBD>浱ǰ<E6B5B1><C7B0><EFBFBD><EFBFBD>ֵ

    return PWM_Out; // <20><><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD>
}

/**********************
<EFBFBD>ٶȻ<EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD>

<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD>ٶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȻ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
**********************/
float Speed_Control(float Speed_Err)
{
    int PWM_Out; // PWM<57><4D><EFBFBD><EFBFBD>

    pid_speed.err = Speed_Err; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6><C8BB>ĵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>

    pid_speed.integral += pid_speed.err; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

    // PID<49><44>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    PWM_Out = pid_speed.Kp * pid_speed.err + 
              pid_speed.Ki * pid_speed.integral + 
              pid_speed.Kd * (pid_speed.err - pid_speed.err_last);

    // <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    pid_speed.integral = pid_speed.integral > 3000 ? 3000 : 
                         (pid_speed.integral < (-3000) ? (-3000) : pid_speed.integral);

    pid_speed.err_last = pid_speed.err; // <20><><EFBFBD>浱ǰ<E6B5B1><C7B0><EFBFBD><EFBFBD>ֵ

    return PWM_Out; // <20><><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD>
}

/**********************
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD>

<EFBFBD><EFBFBD><EFBFBD>룺<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
**********************/
float Current_Control(float Current_Err)
{
    int PWM_Out; // PWM<57><4D><EFBFBD><EFBFBD>

    pid_current.err = Current_Err; // <20><><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>

    pid_current.integral += pid_current.err; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

    // PID<49><44>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    PWM_Out = pid_current.Kp * pid_current.err + 
              pid_current.Ki * pid_current.integral + 
              pid_current.Kd * (pid_current.err - pid_current.err_last);

    // <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    pid_current.integral = pid_current.integral > 3000 ? 3000 : 
                           (pid_current.integral < (-3000) ? (-3000) : pid_current.integral);

    pid_current.err_last = pid_current.err; // <20><><EFBFBD>浱ǰ<E6B5B1><C7B0><EFBFBD><EFBFBD>ֵ

    return PWM_Out; // <20><><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD>
}

/**********************
<EFBFBD>޷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
**********************/
void Moment_limiting(void)
{
    if (Angle_Out > 6)
        Angle_Out = 6; // <20>ǶȻ<C7B6><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
    if (Angle_Out < -6)
        Angle_Out = -6;
    if (Speed_Out > 6)
        Speed_Out = 6; // <20>ٶȻ<D9B6><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
    if (Speed_Out < -6)
        Speed_Out = -6;
    if (Current_Out > 6)
        Current_Out = 6; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
    if (Current_Out < -6)
        Current_Out = -6;
}