l_pid.c

mega8 对电机控制。采用mega8对电机控制。然后再用mega16 来控制mega8

/*********************************************************************************
										左电机速度和位置PID控制器参数
*********************************************************************************/ 
#define L_V_Ka 690		//电机旋转速度PID控制器参数
#define L_V_Kb 90
#define L_V_Kc 30

#define L_S_Kp 30		//电机旋转位置PID控制器参数
#define L_S_Ki 10
#define L_S_Kd 100

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

#define VV_MAX 1023<<10 		//1024<<10 使控制信号扩大,以使过渡平顺
#define VV_MIN -1023<<10

#define SS_MAX 400<<10 		//1024<<10 使控制信号扩大,以使过渡平顺
#define SS_MIN -(400<<10)

#define VV_DEADLINE 0X08	//速度PID，设置死区范围

#define SS_DEADLINE 0		//位置PID，设置死区范围

#define SS_Imax 100000		//位置PID，积分上限
#define SS_Imin	-100000		//位置PID，积分下限

#define SS_ADJUST 500000	//位置PID调节使能域度

#define SS_ACCEL 200		//位置PID，加速度
#define SS_M_SPEED  1000		//位置PID，最大速度

/*********************************************************************************
									PID 结构体，包含速度和位置数据
*********************************************************************************/									
typedef struct PID			//定义数法核心数据
{
	signed int vi_Ref;		//速度PID，速度设定值
	signed int vi_FeedBack;		//速度PID，速度反馈值
	
	signed long si_Ref;		//位置PID，位移设定值
	signed long si_FeedBack;	//位置PID，位移反馈值,当前位置
	
	signed int vi_PreError;	  	//速度PID，前一次，速度误差,,vi_Ref - vi_FeedBack
	signed int vi_PreDerror;	//速度PID，前一次，速度误差之差，d_error-PreDerror;
	
	signed long si_PreError;	//位置PID，前一次，位移误差,ui_Ref - FeedBack
	signed long si_PreIntegral;	//位置PID，前一次，位移积分项，ui_PreIntegral+ui
	
	signed int v_Ka;		//速度PID，Ka = Kp
	signed int v_Kb;		//速度PID，Kb = Kp * ( T / Ti )
	signed int v_Kc;		//速度PID，
		
	signed int s_Kp;		//位置PID，比例系数	
	signed int s_Ki;		//位置PID，积分系数	
	signed int s_Kd;		//位置PID，微分系数
	
	signed int s_Accel;		//位置PID，加速度
	signed int M_Speed;		//位置PID，最大速度	
	
	signed long vl_PreU;		//电机控制输出值
		
}PID;

PID	 L_sPID;        		// 左右电机PID控制器变量		

void L_PIDInit ()
{	
	L_sPID.vi_Ref = 0 ;		//速度设定值
	L_sPID.vi_FeedBack = 0 ;		//速度反馈值
	
	L_sPID.si_Ref = 0 ;		//位移设定值
	L_sPID.si_FeedBack = 0 ;		//位移反馈值,当前位置
		
	L_sPID.vi_PreError = 0 ;	  	//前一次，速度误差,,vi_Ref - vi_FeedBack
	L_sPID.vi_PreDerror = 0 ;		//前一次，速度误差之差，d_error-PreDerror;
	
	L_sPID.si_PreError = 0 ;		//前一次，位移误差,ui_Ref - FeedBack
	L_sPID.si_PreIntegral = 0 ;	//前一次，位移积分项，ui_PreIntegral+ui
	
	L_sPID.v_Ka = L_V_Ka;
	L_sPID.v_Kb =  L_V_Kb;
	L_sPID.v_Kc =  L_V_Kc;
		
	L_sPID.s_Kp =  L_S_Kp;		 	
	L_sPID.s_Ki = L_S_Ki;			
	L_sPID.s_Kd = L_S_Kd;
	
	L_sPID.s_Accel = SS_ACCEL ;		//加速度
	L_sPID.M_Speed = SS_M_SPEED ;		//最大速度	
	
	L_sPID.vl_PreU = 0 ;		//电机控制输出值
}

/*********************************************************************************
								电机速度PID控制器
*********************************************************************************/							
signed int v_PIDCalc( PID *pp )
{
	signed long  error,d_error,dd_error;	
			
    error = (signed int)(pp->vi_Ref - pp->vi_FeedBack);	// 偏差计算			
    d_error = error - pp->vi_PreError;
    dd_error = d_error - pp->vi_PreDerror;
        
	pp->vi_PreError = error;		//存储当前偏差
	pp->vi_PreDerror = d_error;
	
	if( ( error < VV_DEADLINE ) && ( error > -VV_DEADLINE ) )	//设置调节死区
	{
	}
	else								//速度PID计算
		pp->vl_PreU += ( ( pp -> v_Ka * d_error + pp -> v_Kb * error ) + (pp->v_Kc*dd_error));
		
	if( pp->vl_PreU >= VV_MAX ) 		//速度PID，防止调节最高溢出
		pp->vl_PreU = VV_MAX;
		
	else if( pp->vl_PreU <= VV_MIN )	//速度PID，防止调节最低溢出 
		pp->vl_PreU = VV_MIN;			

	return ( pp->vl_PreU  >> 10 );		// 返回预调节占空比
	
}

/*********************************************************************************
								电机旋转位置PID控制器
*********************************************************************************/	
signed int s_PIDCalc( PID *pp )
{
    signed long  error,d_error;
    signed long  temp_comp;
    static unsigned char flag=0;
    
    pp->si_FeedBack += pp->vi_FeedBack;	//储存当前速速值
    error = pp->si_Ref - pp->si_FeedBack;		// 偏差	
       
    if( ( error < SS_DEADLINE ) && ( error > -SS_DEADLINE ) )	//设置调节死区
    {
    
    }						
    else					//执行位置PID调节
    {   
        d_error = error - pp->si_PreError;	//计算微分项偏差
        
        pp->si_PreIntegral += error;		//存储当前积分偏差
        pp->si_PreError = error;		//存储当前偏差	
        
        if(pp->si_PreIntegral > SS_Imax)	//积分修正,设定积分上下限，并于正负换向时清零
        	pp->si_PreIntegral = SS_Imax;        		
        else if(pp->si_PreIntegral < SS_Imin)       	
        	pp->si_PreIntegral = 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->vl_PreU = pp->s_Kp * error + pp->s_Ki * pp->si_PreIntegral + pp->s_Kd*d_error;	//位置PID算法
        
        if( pp->vl_PreU >= SS_MAX ) 		//防止调节溢出
        	pp->vl_PreU = SS_MAX;
        else if( pp->vl_PreU <= SS_MIN ) 
        	pp->vl_PreU = SS_MIN;	     			
    }       				
	
    return (  pp->vl_PreU  >> 10 );		// 返回预调节占空比
}

/*#define KAVALUE 100			//110,  Kp
#define KBVALUE 50      //50     Ki
#define KCVALUE 0			//Kd

#define U_MAX 1048570 //1024<<10 使控制信号扩大,以使过渡平顺
#define U_MIN -1023<<10

#define DEADLINE 0X08	//设置死区范围

//控制输入量为旋转编码盘在采样周期内的脉冲期望值
typedef struct PID			//定义数法核心数据
{
	signed int ui_Ref;		//设定值
	signed int ui_FeedBack;	//反馈值
	signed int Ka;		 	//Ka = Kp,比例系数
	signed int Kb;			//Kb = Kp * ( T / Ti )，积分系数
	signed int Kc;          //Kc = Kp*(Td/T)，微分系数
	signed int ui_PreError;	  	//前一次，误差,,ui_Ref - FeedBack
	signed int ui_PreDerror;	//前一次，误差之差，d_error-PreDerror;
	signed long ul_PreU;
}PID;
PID	 L_sPID;        		//  PID Control Structure,定义速度PID变量

signed int v_PIDCalc( PID *pp )
{
	signed long  error,d_error,dd_error;	
	
	static signed long val_preu = 0;
			
        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 < DEADLINE ) && ( error > -DEADLINE ) );	//设置调节死区
	else
		pp->ul_PreU += ( ( pp -> Ka * d_error + pp -> Kb * error ) + (pp->Kc*dd_error));
	
	val_preu = pp->ul_PreU;
		
	if( pp->ul_PreU >= U_MAX ) 		//防止调节最高溢出
		pp->ul_PreU = U_MAX;
		
	else if( pp->ul_PreU <= U_MIN )  	//防止调节最低溢出
		pp->ul_PreU = U_MIN;		
		
	val_preu = pp->ul_PreU;
				
	return ( (pp->ul_PreU) >> 10 );		// 返回预调节占空比
}

void PIDInit ( )
{
	L_sPID.ui_FeedBack = 0;
	L_sPID.ui_PreError = 0;	
	L_sPID.ui_PreDerror = 0;
	L_sPID.ul_PreU = 0;
	
	L_sPID.Ka = KAVALUE;
	L_sPID.Kb = KBVALUE;
	L_sPID.Kc = KCVALUE;
}*/