PID 寫法

回想以前寫的程式，和現在在公司被訓練的結果，果然還是有差

今天播空寫一下PID 的寫法，功能可能不太正常，不過大至上應該是正常的

typedef enum _PIDType_e
{
    CTR_TYPE_PID = 0,
    CTR_TYPE_P,
    CTR_TYPE_PD,
    CTR_TYPE_PI,
} PIDType_e;

 

typedef struct _PIDStruct
{
PIDType_e Type;
    int Kp;
    int Ki;
    int Kd;
    long Error;
    long Error_Differential;
    long Error_Last;
    long Error_Accumulate;
    long Error_Saturation;
    long Output;
    long Output_Saturation;
   
}PIDStruct;

#define PIDStruct_Size sizeof(PIDStruct)

void PID_Control(PIDStruct *PID_ST)
{
    PID_ST->Error_Differential = PID_ST->Error - PID_ST->Error_Last;
    PID_ST->Error_Accumulate += PID_ST->Error;
    SATURATION(PID_ST->Error_Accumulate,
        PID_ST->Error_Saturation,
        -PID_ST->Error_Saturation);
    PID_ST->Error_Last = PID_ST->Error;

    switch (PID_ST->Type)
    {
    case CTR_TYPE_PID:
        PID_ST->Output = PID_ST->Kp * PID_ST->Error +
                         PID_ST->Ki * PID_ST->Error_Accumulate +
                         PID_ST->Kd * PID_ST->Error_Differential;      
        break;
    case CTR_TYPE_P:
        PID_ST->Output = PID_ST->Kp * PID_ST->Error;
    break;
    case CTR_TYPE_PD:
        PID_ST->Output = PID_ST->Kp * PID_ST->Error +
                         PID_ST->Kd * PID_ST->Error_Differential;      
    break;
    case CTR_TYPE_PI:
        PID_ST->Output = PID_ST->Kp * PID_ST->Error +
                         PID_ST->Ki * PID_ST->Error_Accumulate;
    break;

    default:
        // Error
        break;
    }
   
    if (0 != PID_ST->Output_Saturation)
    {
        // do not judgement if saturation is zero.
        SATURATION(PID_ST->Output,
        PID_ST->Output_Saturation,
        -PID_ST->Output_Saturation);
    }

}

這樣在做應用的時後，如果整支程式重複的地方，就不需要重複的貼上PID的寫法，只要CALL 這支副程式就可以搞定了

用之前先初始化變數

PIDStruct Motor_CTR_L;

void PID_Init(void)
{
    memset(&Motor_CTR_L, 0, PIDStruct_Size);
    Motor_CTR_L.Type = CTR_TYPE_PID;
    Motor_CTR_L.Kp = 10;
    Motor_CTR_L.Ki = 1;
    Motor_CTR_L.Kd = 2;
    Motor_CTR_L.Output_Saturation = -4000;
    Motor_CTR_L.Error_Saturation = -3000;
   
}

把誤差丟進去 Motor_CTR_L.Error = -1000;

最後CALL 副程式

PID_Control(&Motor_CTR_L);

我只有貼比較重要的片段，不過也差不多是所有的程式碼了。

 

忙裡偷閒的練習寫自己有興趣的程式，還滿爽的！