#include<stdio.h>
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);
}
|
