#include "app.h"

#define P_center	180    			// 角度偏差與速度偏差系數之比,
 									// 將弧度誤差轉換為位置 1度對應2.6毫米
fp32   PROP=10;
fp32   Ti=10000;
fp32   Td=0;

static fp32  	EP1 = 0.0;
static fp32  	EP2 = 0.0;
static int16 	AmendV = 0;

/*
==================================================================
* 
*函數名：		PIDControl	
*
*參數：			CtrlError:			誤差	
* 
*功能描述：		pid增量式控制
*
*返回值：		調整量AmendV
*
*創建人：		g s，2007年3月30日
*
*版本號：		beta1.0
*
*修改記錄：
*   
=================================================================== 
*/
int16 PIDControl(uint8 dir,fp32 CtrlError,fp32 PROP,fp32 Ti,fp32 Td)
{
	fp32  a0 = 0.0;
	fp32  a1 = 0.0;
	fp32  a2 = 0.0;
	fp32  Vd;
       
	a0 = ( PROP * ( 1 + 1 / Ti + Td ));     
	a1 = ( -PROP * ( 1 + 2 * Td ));         
	a2 = ( PROP * Td );    	
   
	Vd = a0 * CtrlError + a1 * EP1 + a2 * EP2;  //增量式pid
   
	AmendV = AmendV + Vd;
	
    if(Int16Abs(AmendV)<2)
      AmendV = 0;
      
	if(dir==Front)
	{
	    if(AmendV > 0x1000)
		{
			AmendV = 0x1000;
		}
		if(AmendV < -0x1000)
		{
			AmendV = -0x1000;
		}//限幅
	}
	if(dir==Back)
	{      
	    if(AmendV > 0x400)
		{
			AmendV = 0x400;
		}
		if(AmendV < -0x400)
		{
			AmendV = -0x400;
		}//限幅
	}
  
	EP2 = EP1;
	EP1 = CtrlError; 

	return AmendV;  	
}

/*
==================================================================
* 
*函數名：		PIDClear	
*
*參數：			none
* 
*功能描述：		清空本文件靜態全局變量EP1 ，EP2 ，amend_V ，
*				應該在一段PID控制結束后調用此函數，以備下一個PID控制用。
*				
*
*返回值：		成功返回TRUE
*
*創建人：		rc，2008年1月11日
*
*版本號：		beta1.0
*
*修改記錄：
*   
=================================================================== 
*/
uint8 PIDClear( void )
{
	EP1 = 0.0 ;
	EP2 = 0.0 ;
	AmendV = 0 ;
	
	return TRUE ;
}

/*
==================================================================
* 
*函數名：		CombineError	
*
*參數：			AngleError:				角度誤差	
*				CenterError:		   	位置誤差
* 
*功能描述：		將角度誤差和位置誤差綜合成一個，便于校正
*
*返回值：		綜合誤差error
*
*創建人：		rc，2008年1月11日
*
*版本號：		1.0
*
*修改記錄：
*   
=================================================================== 
*/
fp32 CombineError( fp32 AngleError , fp32 CenterError )
{
	return CenterError+ AngleError * P_center ;
}