#include<stdio.h>
struct _pid
{
float Setspeed;//定义设定值
float ActualSpeed;//定义实际值
float err;//定义偏差值
float err_last;//定义上上个偏差值
float Kp,Ki,Kd;//定义比例、积分、微分系数
float err_next;//定义上一个偏差值
}pid;
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.err_next=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
printf("PID_init end\n");
}
float PID_realize(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()
{
int count;
printf("System begin\n");
PID_init();
count=0;
while(count<1000)
{
float speed=PID_realize(50);
printf("%f\t",speed);
count++;
}
getchar();
return 0;
}
| 
楼主
标题置顶
标题高亮
