/************************************************************
系统的PID部分：主要负责左右底盘电机的PID调速

注：接线相关设定：
          左电机                              右电机
      正转（向前），码盘输出高         正转（向前），码盘输出低
**************************************************************/
#include <iom16v.h>
#include <macros.h>

//PID宏
#define KAVALUE_L 250  //110
#define KBVALUE_L 100     //50
#define KCVALUE_L 0
#define KAVALUE_R 250  //110
#define KBVALUE_R 100     //50
#define KCVALUE_R 0

#define U_MAX	(1023<<10)  //ICP的最高值		   
#define U_MIN   (-1023<<10)           //(-1023<<10)

#define DEADLINE 8	//设置死区范围

#define dirl  (PINB&0x80)                 //read direction
#define dirr  !(PINB&0x40)                 //read direction

int counter_l=0;//左电机码盘脉冲存储变量
int counter_r=0;//右电机码盘脉冲存储变量
unsigned char pid_enablel=1;//左电机PID调节使能
unsigned char pid_enabler=1;//左电机PID调节使能

typedef struct 		//定义数法核心数据/*
{
	signed int vi_Ref;		//设定值
	signed int vi_FeedBack;	//反馈值
	signed int Ka;		 	//Ka = Kp
	signed int Kb;			//Kb = Kp * ( T / Ti )
	signed int Kc;
	signed int vi_PreError;	    //前一次，误差,ui_Ref - FeedBack
	signed int vi_PreDerror;	//前一次，误差的变化率，d_error-PreDerror;
	signed long vl_PreU;
}PID;
PID	 L_sPID,R_sPID;        		//定义速度PID变量,左右电机的结构体


/*************************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; //e-e(n-1)
        dd_error=d_error-pp->vi_PreDerror; //
        
	pp->vi_PreError = error;		//存储当前偏差
	pp->vi_PreDerror = d_error;
	
	if( ( error < DEADLINE ) && ( error > -DEADLINE ) );	//设置调节死区
	else
		pp->vl_PreU += ( ( pp -> Ka * d_error + pp -> Kb * error ) + (pp->Kc*dd_error));
		
	if( pp->vl_PreU >=U_MAX ) 		//防止调节最高溢出
		pp->vl_PreU = U_MAX;
	if( pp->vl_PreU <= U_MIN )  	//防止调节最低溢出
		pp->vl_PreU = U_MIN;						
	return (pp->vl_PreU>>10);		// 返回预调节占空比
}

void L_PIDInit ()
{	
	L_sPID.vi_Ref = 0 ;		        //速度设定值
	L_sPID.vi_FeedBack = 0 ;		//速度反馈值
	
		
	L_sPID.vi_PreError = 0 ;	  	//前一次，速度误差,,vi_Ref - vi_FeedBack
	L_sPID.vi_PreDerror = 0 ;		//前一次，速度误差之差，d_error-PreDerror;
	
	
	L_sPID.Ka = KAVALUE_L;
	L_sPID.Kb = KBVALUE_L;
	L_sPID.Kc = KCVALUE_L;
		
	L_sPID.vl_PreU=0;
}

void R_PIDInit ()
{	
	R_sPID.vi_Ref = 0 ;		        //速度设定值
	R_sPID.vi_FeedBack = 0 ;		//速度反馈值
	
		
	R_sPID.vi_PreError = 0 ;	  	//前一次，速度误差,,vi_Ref - vi_FeedBack
	R_sPID.vi_PreDerror = 0 ;		//前一次，速度误差之差，d_error-PreDerror;
	
	
	R_sPID.Ka = KAVALUE_R;
	R_sPID.Kb = KBVALUE_R;
	R_sPID.Kc = KCVALUE_R;
		
	R_sPID.vl_PreU=0;
}


/**********************外部中断计数***********************/
#pragma interrupt_handler int0_isr:2
void int0_isr(void)
{	 if(dirl) counter_l++;
	 else counter_l--;
}

/**********************外部中断计数***********************/
#pragma interrupt_handler int1_isr:3
void int1_isr(void)
{	 if(dirr) counter_r++;
	 else counter_r--;
}

//TIMER2 initialize - prescale:1024
// WGM: Normal
// desired value: 10mSec
// actual value:  9.984mSec (0.2%)
void timer2_init(void)
{
 TCCR2 = 0x00; //stop
 ASSR  = 0x00; //set async mode
 TCNT2 = 0xD9; //setup
 OCR2  = 0x27;
 TCCR2 = 0x07; //start
}

/********************10ms采样中断一次********************/
#pragma interrupt_handler timer2_ovf_isr:5
void timer2_ovf_isr(void)
{static int temp=0; 

 
 /*************左电机PID调节********************/
if(pid_enablel) 
{L_sPID.vi_FeedBack=counter_l;
 counter_l=0;
 temp=v_PIDCalc(&L_sPID);
 if(temp>=0){setmotor('l','f',temp);}
 else {setmotor('l','b',0-temp);}
} 
 
 /*************右电机PID调节*********************/
if(pid_enabler) 
{R_sPID.vi_FeedBack=counter_r;
 counter_r=0;
 temp=v_PIDCalc(&R_sPID);
 if(temp>=0){setmotor('r','f',temp);}
 else {setmotor('r','b',0-temp);}
} 
 
 TCNT2 = 0xD9; //reload counter_l value

}