增量式PID控制器C语言

//  增量式PID控制器

#include
struct pid
{
    float Setspeed;
    float Actualspeed;
    float Err;
    float Err_Last;
    float Err_Next;
    float Kp,Ki,Kd;
    float Voltage;
    float Integral;
} pid;

void PID_Init(void);
float PID_Cal(float Speed);

int main()
{
    struct pid;
    int count=0;
    printf("SYSTEM BEGIN!\n");
    PID_Init();
    while(count<200)
    {
        float speed=PID_Cal(200.0);
        printf("-%d  %f\n",count,speed);
        count++;
    }
    return 0;
}

float PID_Cal(float Speed)
{
    float incrementspeed;
    pid.Setspeed         =  Speed;
    pid.Err              =  pid.Setspeed - pid.Actualspeed;
    incrementspeed       =  pid.Kp*(pid.Err-pid.Err_Next)+pid.Ki*pid.Err+pid.Kd*(pid.Err-2*pid.Err_Next+pid.Err_Last);
    pid.Actualspeed      += incrementspeed;
    pid.Err_Last         =  pid.Err_Next;
    pid.Err_Next         =  pid.Err;
    return pid.Actualspeed;

}

void PID_Init(void)
{
    printf("PID_Init begin!\n");
    pid.Setspeed=       0;
    pid.Actualspeed=    0;
    pid.Err=            0;
    pid.Err_Last=       0;
    pid.Kp=             0.2;
    pid.Ki=             0.15;
    pid.Kd=             0.2;
    pid.Voltage=        0;
    pid.Integral=       0;
    printf("PID_Init end!\n");
}