/**************************************************************************
*  函數庫說明：PID 調節函數                                               *
*  版本：      v1.0                                                       *
*  作者：      田超                                                       *
*  創建日期：  2006年11月6日                                              *
* ----------------------------------------------------------------------- *
*  [說明]                                                                 *
*         1、支持完整的P（比例）、I（積分）、D（微分）調節。              *
*         2、為提高AVR的運算速度和減少數據占用空間P、I、D參數均為char型。 *
*            使用時根據實際被控系統調節Proportion、Integral、Derivative   *
*            參數已達到控制要求。                                         *
*         3、FeedbackDistance、FeedbackSpeed為反饋值，AimDistance、       *
*            AimSpeed為目標值。調用PIDOut()時根據控制類型給出反饋值和目標 *
*            值。                                                         *
**************************************************************************/
/*-----------------------------PID參數定義-------------------------------*/
const char  Proportion = 0;                                                //  比例常數
//const char  Integral = 5;                                                  //  積分常數
//const char  Derivative = 1;                                                //  微分常數

         //int  dError    = 0;                                               //微分偏差
		 int  Error     = 0;                                               //當前偏差
         //int  LastError = 0;                                               //Error[-1]
         //int  PrevError = 0;                                               //Error[-2]
         //long SumError  = 0;                                               //Sums of Errors
         long   AimDistance          = 0;                                  //目標位置
         long   FeedbackDistance     = 0;                                  //反饋位置
signed	 char   AimSpeed             = 0;                                  //目標速度
//signed	 char   FeedbackSpeed        = 0;                                  //反饋速度
/**************************************************************************
*  函數名稱 : PIDOut                                                      *
*  輸入：     AimDistance(輸入目標位置)                                   *
*  輸出：     PID調節                                                     *
**************************************************************************/		 
int PIDOut(void)
{			  
		Error = AimDistance -  FeedbackDistance;                           // 偏差
        //SumError += Error;                                                 // 積分
        //dError = LastError - PrevError;                                    // 當前微分
        //PrevError = LastError;
        //LastError = Error;
        return  (  Error >> Proportion                                     // 比例項
            //+    SumError >> Integral                                      // 積分項
            //+    dError >> Derivative                                      // 微分項
        );
}