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

/******************************************************************

    积分分离的 PID 控制算法

    在普通 PID 控制中， 引入积分环节的目的， 主要是为了消除静差， 提高
    控制精度。 但是在启动、 结束或大幅度增减设定时， 短时间内系统输出有很大的
    偏差， 会造成 PID 运算的积分积累， 导致控制量超过执行机构可能允许的最大动
    作范围对应极限控制量， 从而引起较大的超调， 甚至是震荡， 这是绝对不允许的。
    为了克服这一问题， 引入了积分分离的概念， 其基本思路是 当被控量与设
    定值偏差较大时， 取消积分作用; 当被控量接近给定值时， 引入积分控制， 以消
    除静差， 提高精度。 其具体实现代码如下：

*******************************************************************/




typedef struct
{
    float SetSpeed;    //定义设定值
    float ActualSpeed; //定义实际值
    float err;         //定义偏差值
    float err_last;    //定义上一个偏差值
    float Kp,Ki,Kd;    //定义比例、 积分、 微分系数
    float voltage;     //定义电压值（控制执行器的变量）
    float integral;    //定义积分值
} PID ;

void PID_Init();
float PID_realize(float speed);

PID pid ;
FILE* out ;

int main()
{
    out = fopen("C:\\Users\\Administrator\\Desktop\\log.txt","w");
    if(out)
    {
        printf("File open err...\n");
    }

    printf("System begin \n");
    PID_Init();

    unsigned long  count=0;
    while(count<2000)
    {
        float speed=PID_realize(200.0);

        printf("%f\n",speed);
        fprintf(out,"%f\n",speed);
        fflush(out);

        count++;
    }

    fclose(out);

    return 0;
}



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.015;
    pid.Kd=0.2;
    printf("PID_init end \n");
}




float PID_realize(float speed)
{
    int index = 0 ;

    pid.SetSpeed=speed;
    pid.err=pid.SetSpeed-pid.ActualSpeed;
//    pid.integral+=pid.err;
//    pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);

    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;

}