增量C语言,PID增量算法 c语言

#include

struct _pid{

float SetSpeed;            //

float ActualSpeed;        //

float err;                //

float err_next;            //

float err_last;            //

float Kp,Ki,Kd;

float last_speed;

}pid;

void PID_init(void){                                         //?????????

pid.SetSpeed=3.0;

pid.ActualSpeed=0.0;

//pid.last_Speed=0.0;

pid.err=0.0;

pid.err_next=0.0;

pid.err_last=0.0;

pid.Kp=0.2;

pid.Ki=0.15;

pid.Kd=0.2;

pid.last_speed=0.0;

}

float absu(float err)

{

if(err<0)

err=0-err;

return err;

}

void PID_realize(float speed)

{

float inSpeed=0;int index;

int i=0;

pid.SetSpeed=speed;

pid.err=pid.SetSpeed-pid.ActualSpeed;

while(pid.err>0.0001)

{

i++;

pid.err=pid.SetSpeed-pid.ActualSpeed;

if(absu(pid.err)>pid.SetSpeed-0.01)

{

index=0;

}

else

{

index=1;}

inSpeed = pid.Kp*(pid.err-pid.err_next)+index*pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);

pid.ActualSpeed+=inSpeed;

pid.err_last=pid.err_next;

pid.err_next=pid.err;

printf("no:%d,%f,%f\n",i,pid.ActualSpeed,inSpeed);

}

}

main()

{

PID_init();

PID_realize(200);

}