pid_advanced.c

PID算法 做了一些改进 增量式的 希望能够有用

#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_Slow	
*
*参数：			CtrlError:			误差	
* 
*功能描述：		pid增量式控制，慢速版本，坐标每两次更新才进行控制输出
*
*返回值：		调整量AmendV
*
*创建人：		horizon ，2008年12月17日
*
*版本号：		beta1.0
*
*修改记录：
*   
=================================================================== 
*/
int16 PIDControl_Slow(uint8 dir,fp32 CtrlError,fp32 PROP,fp32 Ti,fp32 Td)
{
	fp32  a0 = 0.0;
	fp32  a1 = 0.0;
	fp32  a2 = 0.0;
	fp32  Vd;
	uint8 flag = 0;
       
	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 > 0x500)
		{
			AmendV = 0x500;
		}
		if(AmendV < -0x500)
		{
			AmendV = -0x500;
		}
	}//限幅
	if(dir==Back)
	{      
	    if(AmendV > 0x400)
		{
			AmendV = 0x400;
		}
		if(AmendV < -0x400)
		{
			AmendV = -0x400;
		}
	}//限幅
  
	EP2 = EP1;
	EP1 = CtrlError; 

	return AmendV;  	
}




/*
==================================================================
* 
*函数名：		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;  	
}

uint8 PIDSet(fp32 P, fp32 I, fp32 D)
{
	PROP	= P;			// =15;
	Ti		= I;			//=7000;
	Td		= D;			//=0.02;

	return TRUE ;
}

/*
==================================================================
* 
*函数名：		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 ;
}