typedef struct{
  volatile int SetPoint;
  volatile double Proportion;
  volatile double Integral;
  volatile double Derivative;
  volatile int LastError;
  volatile int PrevError;
}PID;

宏定义

#define P_DATA  3.2
#define I_DATA  1.1
#define D_DATA  -0.15

SetPoint设置目标参数，若是控制电机，就是目标速度。Proportion、Integral、Derivative分别是P、I、D参数。LastError上一次的偏差值。PrevError上上次的偏差值。

void IncPIDInit(void)
{
  sptr->LastError = 0;
  sptr->PrevError = 0;
  sptr->Proportion = P_DATA;
  sptr->Integral   = I_DATA;
  sptr->Derivative = D_DATA;
  sptr->SetPoint = 100;
}

int IncPIDCalc(int NextPoint)
{
  int iError;
  int iIncpid;
  iError = sptr->SetPoint - NextPoint;
  ilncpid = (sptr->Proportion*iError)
            -(sptr->Integral*sptr->LastError)
            +(sptr->Derivative*sptr->PrevError);
  sptr->PrevError = sptr->LastError;
  sptr->LastError = iError;
return (ilncpid);
}

  count = 当前值;
  para = lncPIDCalc(count);
  if((para<-3)||(para>3))  //避免误差较小的时候引起震荡
    PWM_Duty += para;

  if(PWM_Duty>899)
    PWM_Duty = 899;