PID算法的C语言实现四 增量型PID的C语言实现

有位置型编程可以知道这个编程也很简单，戒指接贴出代码了：

#include "stdio.h"

struct _pid{
float SetSpeed;
float ActualSpeed;
float Err;
float Err_Last;
float Err_Next;
    float Kp,Ki,Kd;
}pid;

void PID_Init(void)
{
printf("PID_Init begin! \n");

pid.SetSpeed      = 0;
pid.ActualSpeed   = 0;
pid.Err           = 0;
pid.Err_Last      = 0;
        pid.Err_Next      = 0;
pid.Kp            = 0.2;
pid.Ki            = 0.015;
pid.Kd            = 0.2;

printf("PID_Init end! \n");
}

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;  
}

int main(void)
{
int count = 0 ;
printf("SYSTEM BEGIN! \n");
PID_Init();

while(count<1000) 
{
float speed = PID_Cal(200.0);
printf("-%d-%f-",count,speed);
count++;
}
   return 0;
}

运行结果：