向东博客 专注WEB应用 构架之美 --- 构架之美，在于尽态极妍 | 应用之美，在于药到病除|赢在IT，Playin' with IT,Focus on Killer Application,Marketing Meets Technology.

#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;
double u;
double y=0;
PIDInit(&sPID);
sPID.Proportion=2;
sPID.Derivative=10;
sPID.Integral=0.00001;
sPID.Command=10;
motor m_motor;
motorInit(&m_motor);
int k=0;
while(k<=1000)
{
  fprintf(fp,"%f %fn",y,sPID.Command);
  u=PIDCale(&sPID,y);
  y=motorCal(&m_motor,u);
  k++;
}
printf("%fn",y);
fclose(fp);
}