v_pid.h

使用ICCAVR编辑

/***************************PID算法有关**********************************************/
/**************************定义PID的三个参数*************************/
#define VV_KAVALUE  0x30
#define VV_KBVALUE  0x10
#define VV_KCVALUE  0x10
// Ka, Kc偏大， 从10以内开始较好

/******************************************************************************
设置相应的PID常量:
控制死区;上下限;
*******************************************************************************/


typedef	struct 	//定义运算数据
{
	signed int vi_Ref;		  	 //速度PID，速度设定值
	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;		//电机控制输出值
		
} PID;
PID	 sPID;        		//  PID Control Structure		


/**********************************************************************
PID参数初始化
**********************************************************************/
void PIDInit ()
{	
	sPID.vi_Ref = 0 ;			//速度设定值
	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 ;		//电机控制输出值
}


/**********************************************************************
简单PID算法
**********************************************************************/
signed int v_PIDCalc( PID *pp )
{

	  int ek, d_error;
	  unsigned char beta;

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

	  // 遇限削弱积分法，如果进入饱和区，并且积分增加，则不进行积分运算，尽快离开饱和。
	  // 用beta作为积分运算与否的标志
	  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控制, 更新电机控制输出值
	  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）转换为（0~550），需要尝试改变移位数, 在4~7之间效果好

	  pp->vi_PreError = ek;													// 更新前一次速度误差
		
	  // 处理饱和效应
	  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 );  // 开环返回值
}