PID--抗積分飽和法--004

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/***********************************************************************
    所謂的積分飽和現象是指如果系統存在一個方向的偏差， PID 控制器的輸
    出由於積分作用的不斷累加而加大， 從而導致執行機構達到極限位置， 若控制器
    輸出 U(k)繼續增大， 執行器開度不可能再增大， 此時計算機輸出控制量超出了
    正常運行范圍而進入飽和區。 一旦系統出現反向偏差， u(k)逐漸從飽和區退出。
    進入飽和區越深則退出飽和區時間越長。 在這段時間里， 執行機構仍然停留在極
    限位置而不隨偏差反向而立即做出相應的改變， 這時系統就像失控一樣， 造成控
    制性能惡化， 這種現象稱為積分飽和現象或積分失控現象。
    防止積分飽和的方法之一就是抗積分飽和法， 該方法的思路是在計算
    u(k)時， 首先判斷上一時刻的控制量 u(k-1)是否已經超出了極限范圍： 如果
    u(k-1)>umax， 則只累加負偏差;
    如果 u(k-1)<umin， 則只累加正偏差。 從而避
    免控制量長時間停留在飽和區。 直接貼出代碼， 不懂的看看前面幾節的介紹。
****************************************************************************/
struct _pid
{
    float SetSpeed; //定義設定值
    float ActualSpeed; //定義實際值
    float err; //定義偏差值
    float err_last; //定義上一個偏差值
    float Kp,Ki,Kd; //定義比例、 積分、 微分系數
    float voltage; //定義電壓值（ 控制執行器的變量）
    float integral; //定義積分值
    float umax;
    float umin;
} pid;

void PID_init()
{
    printf("PID_init begin \n");
    pid.SetSpeed=0.0;
    pid.ActualSpeed=0.0;
    pid.err=0.0;
    pid.err_last=0.0;
    pid.voltage=0.0;
    pid.integral=0.0;
    pid.Kp=0.2;
    pid.Ki=0.1; //注意， 和上幾次相比， 這里加大了積分環節的值
    pid.Kd=0.2;
    pid.umax=400;
    pid.umin=-200;
    printf("PID_init end \n");
}

float PID_realize(float speed)
{
    int index;
    pid.SetSpeed=speed;
    pid.err=pid.SetSpeed-pid.ActualSpeed;
    if(pid.ActualSpeed>pid.umax) //灰色底色表示抗積分飽和的實現
    {
        if(abs(pid.err)>200) //藍色標注為積分分離過程
        {
            index=0;
        }
        else
        {
            index=1;
            if(pid.err<0)
            {
                pid.integral+=pid.err;
            }
        }
    }
    else if(pid.ActualSpeed<pid.umin)
    {
        if(abs(pid.err)>200) //積分分離過程
        {
            index=0;
        }
        else
        {
            index=1;
            if(pid.err>0)
            {
                pid.integral+=pid.err;
            }
        }
    }
    else
    {
        if(abs(pid.err)>200)
        //積分分離過程
        {
            index=0;
        }
        else
        {
            index=1;
            pid.integral+=pid.err;
        }
    }
    pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
    pid.err_last=pid.err;
    pid.ActualSpeed=pid.voltage*1.0;
    return pid.ActualSpeed;
}


int main()
{
    PID_init();
    int count=0;
    while(count<1000)
    {
        float speed=PID_realize(200.0);
        printf("%f\n",speed);
        count++;
    }
    return 0;
}