voypid.cpp

足球机器人功能demo

#include "stdafx.h"
#include "VoyPID.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


/***************************d_PID算数有关**********************************************/
signed int DDPIDCalc( DDPID *pp )
{
	signed int Error,d_error,dd_error;	
			
	Error = (signed int)(pp->ui_Ref - pp->ui_FeedBack);	// 偏差计数			
	d_error = Error - pp->ui_PreError;
	dd_error=d_error-pp->ui_PreDerror;
        
	pp->ui_PreError = Error;		//存储当前偏差
	pp->ui_PreDerror = d_error;
	
	if( ( Error < pp ->PLUS_DEADLINE ) && ( Error > pp ->MINUS_DEADLINE ) );		//设置调节死区
	else
		pp->ul_PreU += ( ( pp -> Ka * d_error + pp -> Kb * Error ) + (pp->Kc*dd_error));
		
	if( pp->ul_PreU >= pp->U_MAX ) 		//防止调节最高溢出
		pp->ul_PreU = pp->U_MAX;
		
	else if( pp->ul_PreU <= pp->U_MIN )  	//防止调节最低溢出
		pp->ul_PreU = pp->U_MIN;		
				
	return ( (pp->ul_PreU) >> 10 );		// 返回预调节占空比
}



signed int SSPIDCalc( SSPID *pp )
{
	signed long  error,d_error;		
	error = pp->si_Ref - pp->si_FeedBack;		// 偏差	
        
	if( ( error <  pp ->PLUS_DEADLINE ) && ( error >pp ->MINUS_DEADLINE ) )	//设置调节死区
	{
		pp->ul_PreU = 0;	
	}						
	else					//执行位置PID调节
	{   	           	
    	d_error = error - pp->si_PreError;	//计算微分项偏差
    	
    	pp->si_PreIntegral += error;		//存储当前积分偏差
    	pp->si_PreError = error;		//存储当前偏差	
    
    	if(pp->si_PreIntegral >  pp->SS_Imax)	//积分修正,设定积分上下限，并于正负换向时清零
    		pp->si_PreIntegral =  pp->SS_Imax;        		
    	else if(pp->si_PreIntegral <  pp->SS_Imin)       	
    		pp->si_PreIntegral =  pp->SS_Imin;        		      		
    	else if( pp->si_PreIntegral>0 && error <0 )
    		pp->si_PreIntegral=0;       		
    	else if( pp->si_PreIntegral<0 && error >0 )
    		pp->si_PreIntegral=0;	
    	 
    	pp->ul_PreU = pp->si_Kp * error + pp->si_Ki * pp->si_PreIntegral + pp->si_Kd*d_error;	//位置PID算法
		
	if( pp->ul_PreU >= pp->SS_MAX ) 		//防止调节溢出
		pp->ul_PreU = pp->SS_MAX;

	else if( pp->ul_PreU <= pp->SS_MIN ) 
		 pp->ul_PreU = pp->SS_MIN;	     			
   	}       				
		
	return (  pp->ul_PreU  >> 10 );		// 返回预调节占空比
}