main.c

pic24 PID for motor control

#include <p24fxxxx.h>
#include <string.h>
#include "LCD.h"
_CONFIG1( JTAGEN_OFF & GCP_OFF & GWRP_OFF & COE_OFF & FWDTEN_OFF & ICS_PGx2) 
_CONFIG2( FCKSM_CSDCMD & OSCIOFNC_OFF & POSCMOD_HS & FNOSC_PRI)

#define DELAY 16000

void initTimer1(void);
void initTimer2(void);
void initTimer4(void);
void initTimer5(void);
//void __attribute__((__interrupt__, __shadow__)) _T1Interrupt(void);
//void __attribute__((__interrupt__, __shadow__)) _T2Interrupt(void);
void _ISRFAST _T1Interrupt(void);
void _ISRFAST _T2Interrupt(void);

void initPWM1(void);
void initPWM2(void);
void _ISRFAST _T3Interrupt(void);

void initADC( int amask);
int readADC( int ch);
#define POT     5       // 10k potentiometer connected to AN5 input
#define AINPUTS 0xFF00  //(AN0 a AN7) Analog inputs for Explorer16 POT and TSENS 


char buf1[20];
char buf2[20];
unsigned int val;
int velRef;
int encoder1, encoder2;
int print;
unsigned int temp;
double pos;


int	 main(){
	int i,j;
	
	
	TRISA=0xFF00;
	TRISBbits.TRISB5=1;
	//TRISD=0xFF;
	LATAbits.LATA5=0;
	
	encoder1=0;encoder2=0;print=0;pos=0;
	
	 //////////////////////////////////////////////////////////////////////////////////////// init LCD
    initLCD();
    initTimer1(); /// Actualizar o LCD
    initTimer2(); // Base de Tempo para o Contador
    initTimer4(); // contador do encoder Canal A
	initTimer5(); // contador do encoder Canal B
    initPWM1();
    //initPWM2(); 
 	initADC( AINPUTS);  // initialize the ADC for the Explorer16 analog inputs
    
    // put a title on the first line    
   putsLCD( "LED MONITOR");

	setLCDC(0x40);
	putsLCD("Hello World");
	
	LATAbits.LATA5=0;
	LATAbits.LATA6=0;
	
	while(PORTDbits.RD13); //Start
	
	LATAbits.LATA5=1;
	LATAbits.LATA6=0;	
	
	clrLCD();
	val=20;velRef=50;
	encoder1=0;
	while(1){
		//while(!encoder1);
		//val=((unsigned int)(TMR3<<8))+((unsigned int)TMR2);
		//val=(unsigned int)(TMR2);
		//sprintf( buf, "%x,%d",TMR3,val);
		clrLCD();
		//memset(buf,0,sizeof(buf)*20);
		homeLCD();
	//	sprintf( buf, "%x",TMR2);
		
		if(print){
		
			//sprintf( buf1, "PWM x%4X,x%4d",OC1RS,val);
			//sprintf( buf1, "v%d,vR%u,t%u",val,velRef,temp);
			sprintf( buf1, "p=%f",pos);
			//dVal=(double)(encoder1)/100; //100ms
			sprintf( buf2, "E1=%d,E2=%d",encoder1,encoder2); 
			print=0;
			encoder1=0;
		}
		putsLCD(buf1);
		setLCDC(0x40);
		putsLCD(buf2);
	//	clrLCD();
		
		velRef = (int)100.0*((double)readADC( POT))/1023.0;  // select the POT input and convert 	
		
		//velRef = readADC( POT);  // select the POT input and convert 
/*		if(PORTDbits.RD6)
			velRef--;
		if(PORTDbits.RD7)
			velRef++;
*/		
		if(!PORTDbits.RD13){
			LATAbits.LATA5^=1;
			LATAbits.LATA6^=1;
			for(i=0;i<30000;i++){};
			
		}
		
		for(i=0;i<30000;i++){};
		//encoder1=0;
	};
	
	return 0;
	
}






void initTimer1(void){
	/* The following code example will enable Timer1 interrupts, load the Timer1
	Period register and start Timer1.
	When a Timer1 period match interrupt occurs, the interrupt service
	routine must clear the Timer1 interrupt status flag in software.
	*/
	T1CON = 0x00; //Stops the Timer1 and reset control reg.
	TMR1 = 0x00; //Clear contents of the timer register
	T1CONbits.TCKPS=0b11;
	PR1 = 0x30D4 - 1; //Load the Period register with the value 0xFFFF  // Para 1s 0x1E84 ;) 1kHz ->0x07C5; // 1khz0x9C40
	//0x0FA0
	_T1IP = 1; //Setup Timer1 interrupt for desired priority level
	_T1IF = 0; //Clear the Timer1 interrupt status flag
	_T1IE = 1; //Enable Timer1 interrupts
	T1CONbits.TON = 1; //Start Timer1 with prescaler settings at 1:1 and
	//clock source set to the internal instruction cycle
}



void initTimer2(void){
	
	
	/* The following code example will enable Timer1 interrupts, load the Timer1
	Period register and start Timer1.
	When a Timer1 period match interrupt occurs, the interrupt service
	routine must clear the Timer1 interrupt status flag in software.
	*/
	T2CON = 0x00; //Stops the Timer1 and reset control reg.
	TMR2 = 0x00; //Clear contents of the timer register
	T2CONbits.TCKPS=0b10;
	PR2 = 0x1D4C - 1; //Load the Period register with the value 0xFFFF  // Para 1s 0x1E84 ;) 1kHz ->0x07C5; // 1khz0x9C40
	//0x0FA0
	_T2IP = 1; //Setup Timer1 interrupt for desired priority level
	_T2IF = 0; //Clear the Timer1 interrupt status flag
	_T2IE = 1; //Enable Timer1 interrupts
	T2CONbits.TON = 1; //Start Timer1 with prescaler settings at 1:1 and
	//clock source set to the internal instruction cycle
}


void initTimer4(void){
	
	
	/* The following code example will enable Timer2 interrupts, load the Timer2
	Period register and start Timer2 using an internal clock and an external
	gate signal. On the falling edge of the gate signal a Timer2 interrupt
	occurs. The interrupt service routine must clear the Timer2 interrupt
	status flag in software .
	*/
	T4CON = 0x00; //Stops the Timer2 and reset control reg.
	TMR4 = 0x00; //Clear contents of the timer register
	//PR2 = 0x0FFF; //Load the Period register with the value 0xFFFF
	
	//_T4IP = 2; //Setup Timer2 interrupt for desired priority level
	// (this example assigns level 1 priority)
	//_T4IF = 0; //Clear the Timer2 interrupt status flag
	//_T4IE = 1; //Enable Timer2 interrupts
	T4CONbits.TCS = 1; 		//Timer2 Clock Source Select bit
	//T4CONbits.TCKPS = 0b00;	//Timer2 Input Clock Prescale Select bits
	T4CONbits.TON = 1; 		//Start Timer2
}




void initTimer5(void){
	
	
	/* The following code example will enable Timer2 interrupts, load the Timer2
	Period register and start Timer2 using an internal clock and an external
	gate signal. On the falling edge of the gate signal a Timer2 interrupt
	occurs. The interrupt service routine must clear the Timer2 interrupt
	status flag in software .
	*/
	T5CON = 0x00; //Stops the Timer2 and reset control reg.
	TMR5 = 0x00; //Clear contents of the timer register
	//PR2 = 0x0FFF; //Load the Period register with the value 0xFFFF
	
	//_T5IP = 2; //Setup Timer2 interrupt for desired priority level
	// (this example assigns level 1 priority)
	//_T5IF = 0; //Clear the Timer2 interrupt status flag
	//_T5IE = 1; //Enable Timer2 interrupts
	T5CONbits.TCS = 1; 		//Timer2 Clock Source Select bit
	//T5CONbits.TCKPS = 0b00;	//Timer2 Input Clock Prescale Select bits
	T5CONbits.TON = 1; 		//Start Timer2
}


/* Example code for Timer1 ISR*/
void _ISRFAST _T1Interrupt(void)
{
	//int temp;
	/* Interrupt Service Routine code goes here */
	LATAbits.LATA0^=1;
	//clrLCD();
//	encoder1=TMR4;
//	TMR4=0x00;


	
	print=1;
	_T1IF = 0; //Reset Timer1 interrupt flag and Return from ISR
}

/* Example code for Timer2 ISR*/
void _ISRFAST _T2Interrupt(void)
{
	/* Interrupt Service Routine code goes here */
	LATAbits.LATA1^=1;
	//clrLCD();
	encoder1=TMR4;
	TMR4=0x00;
	encoder2=TMR5;
	TMR5=0x00;


	if(LATAbits.LATA5==1 &&	LATAbits.LATA6==0)
		temp=(unsigned int)(5.0*((double)encoder2)/217.0);
	else
		temp=(unsigned int)(5.0*((double)encoder1)/217.0);
	if(temp<velRef)
		val++;
	if (temp>velRef)
		val--;
	if(val<0)
		val=0;
	if(val>100)
		val=100;
	
	pos+=(double)(encoder1-encoder2);

	
//	print=1;
	_T2IF = 0; //Reset Timer1 interrupt flag and Return from ISR
	
}

/* Example code for Timer2 ISR*/
void _ISRFAST _T4Interrupt(void)
{
	/* Interrupt Service Routine code goes here */
	LATAbits.LATA2^=1;
	_T4IF = 0; //Reset Timer1 interrupt flag and Return from ISR
	
}

void initPWM1(void){
	
	
	
#if 0		
	// The following code example will set the Output Compare 1 module
	// for interrupts on the single pulse event and select Timer 2
	// as the clock source for the compare time-base. It is assumed
	// that Timer 2 and Period Register 2 are properly initialized.
	// Timer 2 will be enabled here.
	OC1CON = 0x0000; // Turn off Output Compare 1 Module
	OC1CON = 0x000C; // Load new compare mode to OC1CON
	OC1R = 0x3000; // Initialize Compare Register1 with 0x3000
	OC1RS = 0x3003; // Initialize Secondary Compare Register1 with 0x3003
	IPC0bits.OC1IP0 = 1; // Setup Output Compare 1 interrupt for
	IPC0bits.OC1IP1 = 3; // desired priority level
	IPC0bits.OC1IP2 = 3; // (this example assigns level 1 priority)
	IFS0bits.OC1IF = 0; // Clear Output Compare 1 interrupt flag
	IEC0bits.OC1IE = 1; // Enable Output Compare 1 interrupts
	T3CONbits.TON = 1; // Start Timer2 with assumed settings

#endif	
	T3CON= 0x8000; // eneble Timer3, prescale 1:1, internal clock
	PR3=400-1; //set period for the giving bitrate
	
	//_T3IP = 0; //Setup Timer3 interrupt for desired priority level
	
	_T3IF = 0;	//clear Timer 3 interrupt flag
	_T3IE=1;	//enable TMR3 interrupt

	// init PWM
	//Set de initial duty cycle 
	OC1R = OC1RS = 200; // Initialize Compare at 50%
	
	OC1CON = 0x000E; // Activate the PWM module

	
}


void initPWM2(void){
	
	
#if 0	
	
	T3CON= 0x8000; // eneble Timer3, prescale 1:1, internal clock
	PR3=400-1; //set period for the giving bitrate
	
	//_T3IP = 0; //Setup Timer3 interrupt for desired priority level
	
	_T3IF = 0;	//clear Timer 3 interrupt flag
	_T3IE=1;	//enable TMR3 interrupt
#endif

	// init PWM
	//Set de initial duty cycle 
	OC2R = OC2RS = 200; // Initialize Compare at 50%
	
	OC2CON = 0x000E; // Activate the PWM module

	
}


/* Example code for Timer3 ISR*/
void _ISRFAST _T3Interrupt(void){
	unsigned int limit;
	//LATAbits.LATA2^=1;
	//if(PORTDbits.RD6)
		//OC1RS=OC1RS+1;
	//if(PORTDbits.RD7)
	//OC1RS=val*1;
	
	//OC1RS=(int)val*400/100;
	limit=val*4;
	if (limit>PR3)
		limit =PR3;
	if (limit<1)
		limit =1;

	OC1RS=limit;
	
	//clear Timer 3 interrupt flag
	_T3IF = 0;
}

#if 0
/* Example code for Timer3 ISR*/
void _ISRFAST _OC1Interrupt(void){
	//LATAbits.LATA2^=1;
	
	//clear Timer 3 interrupt flag
	_OC1IF = 0;
}
#endif



// initialize the ADC for single conversion, select Analog input pins
void initADC( int amask) 
{
    AD1PCFG = amask;    // select analog input pins
    AD1CON1 = 0x00E0;   // auto convert after end of sampling 
    AD1CSSL = 0;        // no scanning required 
	AD1CON2 = 0;        // use MUXA, AVss and AVdd are used as Vref+/-
    AD1CON3 = 0x1F02;   // max sample time = 31Tad, Tad = 2 x Tcy = 125ns >75ns
    AD1CON1bits.ADON = 1; // turn on the ADC
} //initADC


int readADC( int ch)
{
    AD1CHS  = ch;               // select analog input channel
    AD1CON1bits.SAMP = 1;       // start sampling, automatic conversion will follow
    while (!AD1CON1bits.DONE);   // wait to complete the conversion
    return ADC1BUF0;            // read the conversion result
} // readADC