seek_line_pid.c

自动小车 寻找白线 程序。 采用后轮驱动mega系列单片机控制

/***************************PID算数有关**********************************************/

//unsigned int L_kp ;	
//unsigned int A_kp ;	
#define ANGLE_KPVALUE 1000
#define ANGLE_KIVALUE 0  
//2
#define ANGLE_KDVALUE 0
//60
  
  
  
#define ANGLE_MAX 800<<10 		//最大调节量输出为正负400。
#define ANGLE_MIN -(800<<10)

#define ANGLE_Imax ( 10<<10 )		//位置PID，积分上限
#define ANGLE_Imin -( 10<<10 )		 //位置PID，积分下限

#define ANGLE_DEADLINE 0X00	//速度PID，设置死区范围
//////////////////////////////////////////////////////////////////////////////////


#define LOCA_KPVALUE 1600
#define LOCA_KIVALUE 3
#define LOCA_KDVALUE 60 
  


#define LOCA_MAX 400<<10 		//最大调节量输出为正负200。
#define LOCA_MIN -(400<<10)

#define LOCA_Imax ( 10<<10 )		//位置PID，积分上限
#define LOCA_Imin -( 10<<10 )		//位置PID，积分下限

#define LOCA_DEADLINE 0X00	//速度PID，设置死区范围
///////////////////////////////////////////////////////////////////////


typedef struct PID			//定义数法核心数据
{

	
	signed long angle_Ref;		//位置PID，位移设定值
	signed long angle_FeedBack;	//位置PID，位移反馈值,当前位置

	signed long angle_PreError;	//位置PID，前一次，位移误差,ui_Ref - FeedBack
	signed long angle_PreIntegral;	//位置PID，前一次，位移积分项，ui_PreIntegral+ui
			
	signed int angle_Kp;		//位置PID，比例系数	
	signed int angle_Ki;		//位置PID，积分系数	
	signed int angle_Kd;		//位置PID，微分系数
	
	signed long angle_PreU;		//电机控制输出值
	
	
	signed long loca_Ref;		//位置PID，位移设定值
	signed long loca_FeedBack;	//位置PID，位移反馈值,当前位置

	signed long loca_PreError;	//位置PID，前一次，位移误差,ui_Ref - FeedBack
	signed long loca_PreIntegral;	//位置PID，前一次，位移积分项，ui_PreIntegral+ui
			
	signed int loca_Kp;		//位置PID，比例系数	
	signed int loca_Ki;		//位置PID，积分系数	
	signed int loca_Kd;		//位置PID，微分系数
	
	signed long loca_PreU;		//电机控制输出值
				
}PID;
PID	 skPID;        		//  PID Control Structure		



void SEEK_PIDInit (unsigned char seek_speed)
{	
	switch ( seek_speed )
	{
		case 0x00 :
		{
			skPID.angle_Kp = ANGLE_KPVALUE;//800; //		 	
			skPID.angle_Ki = ANGLE_KIVALUE;//20; //			
			skPID.angle_Kd = ANGLE_KDVALUE;//60;//
			
			skPID.loca_Kp = LOCA_KPVALUE;//300;		 	
			skPID.loca_Ki = LOCA_KIVALUE;//2;			
			skPID.loca_Kd = LOCA_KDVALUE;//60;
			
			break;
		}
		case 0x01 :
		{
			skPID.angle_Kp = 200;		 	
			skPID.angle_Ki = 2;			
			skPID.angle_Kd = 100;
			
			skPID.loca_Kp = 200;		 	
			skPID.loca_Ki = 2;			
			skPID.loca_Kd = 100;
			
			break;
		}
		case 0x02 :
		{
			skPID.angle_Kp = 150;		 	
			skPID.angle_Ki = 2;			
			skPID.angle_Kd = 100;
			
			skPID.loca_Kp = 200;		 	
			skPID.loca_Ki = 2;			
			skPID.loca_Kd = 50;
			
			break;
		}
		case 0x03 :
		{
			skPID.angle_Kp = 120;		 	
			skPID.angle_Ki = 2;			
			skPID.angle_Kd = 40;
			
			skPID.loca_Kp = 120;		 	
			skPID.loca_Ki = 2;			
			skPID.loca_Kd = 40;
			
			break;
		}
		case 0x04 :
		{
			skPID.angle_Kp = 100;		 	
			skPID.angle_Ki = 2;			
			skPID.angle_Kd = 30;
			
			skPID.loca_Kp = 100;		 	
			skPID.loca_Ki = 2;			
			skPID.loca_Kd = 30;
			
			break;
		}	
			
	}
	
	skPID.angle_Ref = 0 ;			//位移设定值	
	skPID.angle_FeedBack = 0;		//位移反馈值,当前位置
			
	skPID.angle_PreError = 0 ;		//前一次，位移误差,ui_Ref - FeedBack
	skPID.angle_PreIntegral = 0 ;		//前一次，位移积分项，ui_PreIntegral+ui
		
	skPID.angle_PreU = 0 ;	
	
	skPID.loca_Ref = 0 ;			//位移设定值	
	skPID.loca_FeedBack = 0;		//位移反馈值,当前位置
			
	skPID.loca_PreError = 0 ;		//前一次，位移误差,ui_Ref - FeedBack		
	skPID.loca_PreIntegral = 0 ;		//前一次，位移积分项，ui_PreIntegral+ui
		
	skPID.loca_PreU = 0 ;	
	
}

//PID参数设定
void SeekPIDPara (unsigned char seek_speed)
{	
	switch ( seek_speed )
	{
		case 0x00 :
		{
			skPID.angle_Kp = 3000;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 1000;		 	
			skPID.loca_Ki = 3;			
			skPID.loca_Kd = 60;
			
			break;
		}
		
		case 0x01 :
		{
			skPID.angle_Kp = 3500;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 1000;		 	
			skPID.loca_Ki = 3;			
			skPID.loca_Kd = 60;
			
			break;
		}
		
		case 0x02:
		{
			skPID.angle_Kp = 3500;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 1600;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		
		case 0x03 :
		{
			skPID.angle_Kp = 3500;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd =60;
			
			skPID.loca_Kp = 2000;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		case 0x04:
		{
			skPID.angle_Kp =5000;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 4000;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		case 0x05:
		{
			skPID.angle_Kp =5000;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 4000;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		case 0x06:
		{
			skPID.angle_Kp =5000;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 4000;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		case 0x07:
		{
			skPID.angle_Kp =5000;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 4000;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		case 0x08:
		{
			skPID.angle_Kp =5000;		 	
			skPID.angle_Ki = 10;			
			skPID.angle_Kd = 60;
			
			skPID.loca_Kp = 4000;		 	
			skPID.loca_Ki = 6;			
			skPID.loca_Kd = 60;
			
			break;
		}	
		default :
		{
			skPID.angle_Kp = 4000;		 	
			skPID.angle_Ki = 2;			
			skPID.angle_Kd = 30;
			
			skPID.loca_Kp = 1600;		 	
			skPID.loca_Ki = 2;			
			skPID.loca_Kd = 30;
		}break;	
			
	}
}


//****************************************************************************************


signed int angle_PIDCalc( PID *pp )
{
	signed long angle_error = 0,angle_derror = 0;
	static long angle_Ditem = 0;
	static unsigned int d_count = 100;
	
	d_count++;
		
    	angle_error = pp->angle_Ref - pp->angle_FeedBack;		// 偏差	
		        
	if( ( angle_error < ANGLE_DEADLINE ) && ( angle_error > -ANGLE_DEADLINE ) )	//设置调节死区
        {
        		
        }						
        else					//执行位置PID调节
        {   			           	
        	angle_derror = angle_error - pp->angle_PreError;	//计算微分项偏差
        	
        	pp->angle_PreIntegral += angle_error;		//存储当前积分偏差
        	pp->angle_PreError = angle_error;		//存储当前偏差	
        
        	if(pp->angle_PreIntegral > ANGLE_Imax)		//积分修正,设定积分上下限，
        		pp->angle_PreIntegral = ANGLE_Imax;        		
        	else if(pp->angle_PreIntegral < ANGLE_Imin)       	
        		pp->angle_PreIntegral = ANGLE_Imin; 
        /*		       		      		
        	else if( pp->angle_PreIntegral>0 && angle_error <0 )	//并于正负换向时清零
        		pp->angle_PreIntegral=0;       		
        	else if( pp->angle_PreIntegral<0 && angle_error >0 )
        		pp->angle_PreIntegral=0;
        */		
        	else if( pp->angle_PreIntegral>0 && angle_derror <0 )	//变化趋势改变，积分减半
        		pp->angle_PreIntegral >>= 1;       		
        	else if( pp->angle_PreIntegral<0 && angle_derror >0 )
        		pp->angle_PreIntegral >>= 1;
        	
        	angle_Ditem *= 90;		//加延时因子
        	angle_Ditem /= 100;
        		
        	if(angle_derror != 0)
        	{
        		angle_Ditem += (angle_derror * pp->angle_Kd)/d_count;
        		d_count = 0;
        	}	
        	 
        	pp->angle_PreU = pp->angle_Kp * angle_error + pp->angle_Ki * pp->angle_PreIntegral + angle_Ditem;	//位置PID算法
		
		if( pp->angle_PreU >= ANGLE_MAX ) 		//防止调节溢出
			pp->angle_PreU = ANGLE_MAX;

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



//****************************************************************************************

                  
signed int loca_PIDCalc( PID *pp )
{
	signed long loca_error = 0,loca_derror = 0;
	static long loca_Ditem = 0;
	static unsigned int d_count = 100;
	
	d_count++;
		
    	loca_error = pp->loca_Ref - pp->loca_FeedBack;		// 偏差	
		        
	if( ( loca_error < LOCA_DEADLINE ) && ( loca_error > -LOCA_DEADLINE ) )	//设置调节死区
        {
        		
        }						
        else					//执行位置PID调节
        {   			           	
        	loca_derror = loca_error - pp->loca_PreError;	//计算微分项偏差
        	
        	pp->loca_PreIntegral += loca_error;		//存储当前积分偏差
        	pp->loca_PreError = loca_error;		//存储当前偏差	
        
        	if(pp->loca_PreIntegral > LOCA_Imax)		//积分修正,设定积分上下限，
        		pp->loca_PreIntegral = LOCA_Imax;        		
        	else if(pp->loca_PreIntegral < LOCA_Imin)       	
        		pp->loca_PreIntegral = LOCA_Imin; 
        /*		       		      		
        	else if( pp->loca_PreIntegral>0 && loca_error <0 )	//并于正负换向时清零
        		pp->loca_PreIntegral=0;       		
        	else if( pp->loca_PreIntegral<0 && loca_error >0 )
        		pp->loca_PreIntegral=0;
        */	
        	else if( pp->loca_PreIntegral>0 && loca_derror <0 )	//变化趋势改变，积分减半
        		pp->loca_PreIntegral >>= 1;       		
        	else if( pp->loca_PreIntegral<0 && loca_derror >0 )
        		pp->loca_PreIntegral >>= 1;
        		
        	loca_Ditem *= 90;		//加延时因子
        	loca_Ditem /= 100;
        		
        	if(loca_derror != 0)
        	{
        		loca_Ditem += (loca_derror * pp->loca_Kd)/d_count;
        		d_count = 0;
        	}	
			
        	 
        	pp->loca_PreU = pp->loca_Kp * loca_error + pp->loca_Ki * pp->loca_PreIntegral + loca_Ditem;	//位置PID算法
		
		if( pp->loca_PreU >= LOCA_MAX ) 		//防止调节溢出
			pp->loca_PreU = LOCA_MAX;

		else if( pp->loca_PreU <= LOCA_MIN ) 
			 pp->loca_PreU = LOCA_MIN;	     			
       	}       				
		
				
	
	return (  pp->loca_PreU  >> 10 );
}