PID算法

typedef struct PID
{
    int  SetPoint;     //設定目標 Desired Value
    long SumError;                //誤差累計
    double  Proportion;         //比例常數 Proportional Cons
    double  Integral;           //積分常數 Integral Const
    double  Derivative;         //微分常數 Derivative Const
    int LastError;               //Error[-1]
    int PrevError;               //Error[-2]
} PID;

/*******************************************************************************
* 函數名稱 : IncPIDCalc
* 函數描述 : 增量式 PID 控制計算
* 函數輸入 : int 當前位置
* 函數輸出 : 無
* 函數返回 : 增量式PID結果
*******************************************************************************/
int IncPIDCalc(int NextPoint)
{
    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);
}
/*******************************************************************************
* 函數名稱 : LocPIDCalc
* 函數描述 : 位置式 PID 控制計算
* 函數輸入 : int 當前位置
* 函數輸出 : 無
* 函數返回 : 位置式PID結果
*******************************************************************************/
int LocPIDCalc(int NextPoint)
{
    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);        //微分項
}