#include <string.h>
#include<stdio.h>

typedef struct PID{
 double Command; //输入指令
 double Proportion;  //比例系数
 double Integral;   //积分系数
 double Derivative;  //微分系数
 double preErr;    //前一拍误差
 double sumErr;    //误差累积
}PID;

double PIDCale(PID *p,double feedback)
{
 double dErr,Err;
 Err=p->Command-feedback;  //当前误差
 p->sumErr+=Err;      //误差累加
 dErr=Err-p->preErr;   //误差微分
 p->preErr=Err;    
 return(p->Proportion*Err  //比例项
  +p->Derivative*dErr   //微分项
  +p->Integral*p->sumErr);  //积分项
}

void PIDInit(PID *p)
{
 memset(p,0,sizeof(PID));  //初始化
}

typedef struct motor{
 double lastY;
 double preY;
 double lastU;
 double preU;
}motor;

void motorInit(motor *m)
{
 memset(m,0,sizeof(motor));
}

double motorCal(motor *m,double u)
{
 double y=1.9753*m->lastY-0.9753*m->preY+0.00003284*u+0.00006568*m->lastU+0.00003284*m->preU;//二阶系统
 m->preY=m->lastY;
 m->lastY=y;
 m->preU=m->lastU;
 m->lastU=u;
 return y;
}

void main()
{
 FILE *fp=fopen("data.txt","w+");
 PID sPID;
  motor m_motor;
   int k=0;
 double u;
 double y=0;
 PIDInit(&sPID);
 sPID.Proportion=2;
 sPID.Derivative=1;
 sPID.Integral=0.00001;
 sPID.Command=10;

 motorInit(&m_motor);

 while(k<=1000)
 {
•	  fprintf(fp,"%d   设定值=%f   被控量=%f   偏差=%f   控制量=%f\n",k,sPID.Command,y,sPID.Command-y,u);

  u=PIDCale(&sPID,y);
  y=motorCal(&m_motor,u);
  k++;
 }
 printf("%f\n",y);
 fclose(fp);
}