PID计算用于计算电动机输出速度。在此使用的是位置式PID算法。程序中首先根据采样值和给定值计算误差，然后计算积分误差，当然必须对积分误差进行限幅，接下来求得微分误差，最后根据PID离散化公式求得输出值。PID控制程序如下：

#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>

//数据结构
typedef struct PID
{
  int SetPoint;//设定目标Desired Value
 
  long SumError;//误差累计
 
  double Proportion;//比例常数Proportional Const
  double Integral;//积分常数Integral Const
  double Derivative;//微分常数Derivative Const
 
  int LastError;//Error[-1]
  int PrevError;//Error[-2]
 
}PID;

static PID sPID;
static PID*sptr=&sPID;

//PID参数初始化
void IncPIDInit(void)
{
    sptr->SumError=0;
   
    sptr->LastError=0;//Error[-1]
   
    sptr->PrevError=0;//Error[-2]
   
    sptr->Proportion=0;//比例常数Proportional Const
   
    sptr->Integral=0;//积分常数Integral Const
   
    sptr->Derivative=0;//微分常数Derivative Const
   
    sptr->SetPoint=0;
}
//增量式PID控制设计
int IncPIDCalc(int NextPoint)
{
    register int iError,iIncpid;
   
    iError=sptr->SetPoint-NextPoint;   //当前误差
   
    //增量计算
    iIncpid=sptr->Proportion*iError//E[k]项
   
    -sptr->Integral*sptr->LastError//E[k－1]项
   
    +sptr->Derivative*sptr->PrevError;//E[k－2]项
   
   
    //存储误差，用于下次计算
    sptr->PrevError=sptr->LastError;
   
    sptr->LastError=iError;
   
    //返回增量值
    return(iIncpid);
}
//位置式PID控制设计
unsigned int LocPIDCalc(int NextPoint)
{
    register int iError,dError;
   
    iError=sptr->SetPoint-NextPoint;//偏差
   
    sptr->SumError+=iError;//积分
   
    dError=iError-sptr->LastError;//微分
   
    sptr->LastError=iError;
   
    return(sptr->Proportion*iError//比例项
   
    +sptr->Integral*sptr->SumError//积分项
   
    +sptr->Derivative*dError);//微分项
   
}