/***************************PID算法有關(guān)**********************************************/
/**************************定義PID的三個(gè)參數(shù)*************************/
#define VV_KAVALUE  0x30
#define VV_KBVALUE  0x10
#define VV_KCVALUE  0x10
// Ka, Kc偏大， 從10以內(nèi)開始較好

/******************************************************************************
設(shè)置相應(yīng)的PID常量:
控制死區(qū);上下限;
*******************************************************************************/


typedef	struct 	//定義運(yùn)算數(shù)據(jù)
{
	signed int vi_Ref;		  	 //速度PID，速度設(shè)定值
	signed int vi_FeedBack;		 //速度PID，速度反饋值
	
	signed int vi_PreError;	  	//速度PID，前一次，速度誤差, vi_Ref - vi_FeedBack
	signed int vi_PreDerror;	//速度PID，前一次，速度誤差之差， d_error - PreDerror ;
	
	signed int v_Ka;			//速度PID，Ka = Kp
	signed int v_Kb;			//速度PID，Kb = ( T / Ti )
	signed int v_Kc;			//速度PID，kc =  ( Td / T )
	
	signed long vi_PreU;		//電機(jī)控制輸出值
		
} PID;
PID	 sPID;        		//  PID Control Structure		


/**********************************************************************
PID參數(shù)初始化
**********************************************************************/
void PIDInit ()
{	
	sPID.vi_Ref = 0 ;			//速度設(shè)定值
	sPID.vi_FeedBack = 0 ;		//速度反饋值
	
	sPID.vi_PreError = 0 ;	  	//前一次，速度誤差,,vi_Ref - vi_FeedBack
	sPID.vi_PreDerror = 0 ;		//前一次，速度誤差之差，PreDerror - d_error
	
	sPID.v_Ka = VV_KAVALUE;
	sPID.v_Kb = VV_KBVALUE;
	sPID.v_Kc = VV_KCVALUE;
	
	sPID.vi_PreU = 0 ;		//電機(jī)控制輸出值
}


/**********************************************************************
簡單PID算法
**********************************************************************/
signed int v_PIDCalc( PID *pp )
{

	  int ek, d_error;
	  unsigned char beta;

	  ek = pp->vi_Ref - pp->vi_FeedBack;				// 本次速度誤差

	  // 遇限削弱積分法，如果進(jìn)入飽和區(qū)，并且積分增加，則不進(jìn)行積分運(yùn)算，盡快離開飽和。
	  // 用beta作為積分運(yùn)算與否的標(biāo)志
	  if  ( ( pp->vi_PreU >=540 && ek>0 ) || ( pp->vi_PreU <= 0 && ek<0 ) )
		     beta=0;
	  else beta = 1;
		
	  d_error = ek - pp->vi_PreError;							// 本次速度誤差與前次誤差之差
	  pp->vi_PreDerror = d_error - pp->vi_PreDerror;		// 更新前一次速度誤差之差

	  // 增量式PID控制, 更新電機(jī)控制輸出值
	  pp->vi_PreU+= ( pp->v_Ka *  ( d_error +  beta * pp->v_Kb * ek + pp->v_Kc * pp->vi_PreDerror ) ) >>5;	
		// 增量的范圍（0~30*(1+10+10)*256=161280）轉(zhuǎn)換為（0~550），需要嘗試改變移位數(shù), 在4~7之間效果好

	  pp->vi_PreError = ek;													// 更新前一次速度誤差
		
	  // 處理飽和效應(yīng)
	  if ( pp->vi_PreU >550 ) 
		  pp->vi_PreU = 550;
	  else if ( pp->vi_PreU <0 )
	     pp->vi_PreU = 0;

     return      ( pp->vi_PreU );

//return 	((sPID.vi_Ref << 1)+ 50 );  // 開環(huán)返回值
}