/********************************************************************/	
/*   																*/
/*			PID Robot Speed Control Example program    		 		*/
/*				for CUHK ARM board version 9.3							*/
/*														            */
/********************************************************************/
/*                                                                  */
/*			ARM CPU: Philips LPC2131								*/
/*			Crystal clock: 11.0592 MHz								*/
/*			Serial port baudrate: 57600								*/
/*			Programming tool: KEIL uVision3 RealView for ARM 		*/			
/*																	*/
/********************************************************************/
#include <lpc21xx.h>

extern void init_timer(void);

#define DeadBand	1

#define PWM_FREQ	276000		//set PWM frequency to 50 Hz
#define	L_DIR		0x00010000	//set p0.16 as left motor direction
#define	L_DIRinv	0x00020000	//set p0.17 as inverted left motor direction
#define	R_DIR		0x00040000	//set p0.18 as right motor direction
#define	R_DIRinv	0x00080000	//set p0.19 as inverted right motor direction
#define TEST_PIN	0x00010000	//set p1.16 as Test pin  
#define LWheelSen	0x00000040	//set p0.6 as left wheel sensor input
#define RWheelSen	0x00400000	//set p0.22 as right wheel sensor input
//#define RWheelSen	0x00000008	//set p0.3 as right wheel sensor input
#define SW1			0x00000100	//define p0.8 as SW1 input
#define SW2			0x00000200	//define p0.9 as SW2 input

#define NEWLINE sendchar(0x0a); sendchar(0x0d)

//define global variables
int ms;
long intcount,lefttimeval,righttimeval,leftPWM,rightPWM,tmpSW;
int lcur,rcur,lold,rold,lcount,rcount,SendRPM;
int leftRPM,leftRPMset,leftErr,leftlastErr,leftaccErr,leftP,leftI,leftD;
int rightRPM,rightRPMset,rightErr,rightlastErr,rightaccErr,rightP,rightI,rightD;
int Pgain,Igain,Dgain;
int current_leftRPM, current_rightRPM;
long current_leftPWM, current_rightPWM;

//===============================
void  Init_Serial_A(void) {  
	U0LCR = 0x83;	//8 bit length ,DLAB=1	           
   	U0DLL = 0x0F;	//Pclk/(16*baudrate)=(11059200 x 5)/(4 x 16 x 57600);
   	U0DLM = 0x00;
   	U0LCR = 0x03;    //DLAB=0
}

char getchar(void) {
	volatile char ch = '0';

  	while ((U0LSR&0x1)==0) // wait until  receive a byte
    ;
  	ch = U0RBR;         // receive character

  	return ch;
}

void sendchar(char ch) {
	while( (U0LSR&0x40)==0 );
    
 	U0THR  = ch;                     // Transmit next character
}

int  print(char *p) { 
	while(*p!='\0') {
      sendchar(*p++);
   }
   return(0);
}

void putint(int count) { 
	sendchar('0' + count/10);
	sendchar('0' + count % 10);
}

void putlong(int count) { 
	sendchar('0' + count/100000);
	sendchar('0' + (count/10000) % 10);
	sendchar('0' + (count/1000) % 10);
	sendchar('0' + (count/100) % 10);
	sendchar('0' + (count/10) % 10);
	sendchar('0' + count % 10);
}

char atob(char a) {
	int b;

	b = a - 0x30;

	return b;
}

int getint(void) {
	char a0,a1,a2,a3,a4;
	int d;

	a4 = getchar();
	a3 = getchar();
	a2 = getchar();
	a1 = getchar();
	a0 = getchar();

	d = 10000*atob(a4)+1000*atob(a3)+100*atob(a2)+10*atob(a1)+atob(a0);

	return d;
}

void forward(int Lstep,int Rstep,int Lspeed,int Rspeed) {

	leftRPMset = Lspeed;	//set desire left speed
	leftErr = 0;			//reset all varialbles
  	leftlastErr = 0;
  	leftaccErr = 0;
	leftRPM=0;
	
	rightRPMset = Rspeed; 	//set desire right speed
	rightErr = 0;			//reset all variables
  	rightlastErr = 0;
  	rightaccErr = 0;
	rightRPM=0;

	IO0SET|=L_DIR;			//set motors direction
 	IO0CLR|=L_DIRinv;
 	IO0SET|=R_DIRinv;
 	IO0CLR|=R_DIR;

	leftPWM=0;		//initial speed
  	rightPWM=0;

	lcount=0;  			//reset left step count
	rcount=0;			//reset right step count
	//stop when stepcount reach desire steps
	while((lcount<=Lstep)||(rcount<=Rstep)) {
		if(lcount>=Lstep) {
			IO0CLR|=L_DIRinv;	//stop left motor
 			IO0CLR|=L_DIR;
			lcount=0xffff;
		}
		if(rcount>=Rstep) {
			IO0CLR|=R_DIRinv;	//stop right motor
 			IO0CLR|=R_DIR;
			rcount=0xffff;
		}
	}
}

/*
void backward(int Lstep,int Rstep,int Lspeed,int Rspeed) {

	leftRPMset = Lspeed;	//set desire left speed
	leftErr = 0;			//reset all varialbles
  	leftlastErr = 0;
  	leftaccErr = 0;
	leftRPM=0;
	
	rightRPMset = Rspeed; 	//set desire right speed
	rightErr = 0;			//reset all variables
  	rightlastErr = 0;
  	rightaccErr = 0;
	rightRPM=0;

	IO0CLR|=L_DIRinv;		//set motors direction
 	IO0SET|=L_DIR;
 	IO0CLR|=R_DIR;
 	IO0SET|=R_DIRinv;

	leftPWM=PWM_FREQ/3;		//initial speed = half full speed
  	rightPWM=PWM_FREQ/3;

	lcount=0;  			//reset left step count
	rcount=0;			//reset right step count
	//stop when stepcount reach desire steps
	while((lcount<=Lstep)||(rcount<=Rstep)) {
		if(lcount>=Lstep) {
			IO0CLR|=L_DIRinv;	//stop left motor
 			IO0CLR|=L_DIR;
			lcount=0xffff;
		}
		if(rcount>=Rstep) {
			IO0CLR|=R_DIRinv;	//stop right motor
 			IO0CLR|=R_DIR;
			rcount=0xffff;
		}
	}
}

void turn_left(int speed,int step,int radius) {		// speed in RPM; radius in mm
	int Lstep, Rstep;
	
	leftRPMset = speed - (speed*44)/radius;	// set desire left speed; 
											// leftRPMset = speed + (speed*D)/(2*radius); where D = 44mm 
	leftErr = 0;			//reset all varialbles
  	leftlastErr = 0;
  	leftaccErr = 0;
	leftRPM=0;

	rightRPMset = speed + (speed*44)/radius;
	rightErr = 0;			//reset all variables
  	rightlastErr = 0;
  	rightaccErr = 0;
	rightRPM=0;

	IO0CLR|=L_DIRinv;		//set motors direction
 	IO0SET|=L_DIR;
 	IO0CLR|=R_DIR;
 	IO0SET|=R_DIRinv;

	leftPWM=PWM_FREQ/4;		//initial speed = half full speed
  	rightPWM=PWM_FREQ/4;

	Lstep = step - (step*44)/radius;
	Rstep = step + (step*44)/radius;

	lcount=0;  			//reset left step count
	rcount=0;			//reset right step count
	//stop when stepcount reach desire steps
	while((lcount<=Lstep)||(rcount<=Rstep)) {
		if(lcount>=Lstep) {
			IO0CLR|=L_DIRinv;	//stop left motor
 			IO0CLR|=L_DIR;
			lcount=0xffff;
		}
		if(rcount>=Rstep) {
			IO0CLR|=R_DIRinv;	//stop right motor
 			IO0CLR|=R_DIR;
			rcount=0xffff;
		}
	}
} 

void turn_right(int speed,int step,int radius) {		// speed in RPM; radius in mm
	int Lstep, Rstep;
	
	leftRPMset = speed + (speed*44)/radius;	// set desire left speed; 
											// leftRPMset = speed + (speed*D)/(2*radius); where D = 44mm 
	leftErr = 0;			//reset all varialbles
  	leftlastErr = 0;
  	leftaccErr = 0;
	leftRPM=0;

	rightRPMset = speed - (speed*44)/radius;
	rightErr = 0;			//reset all variables
  	rightlastErr = 0;
  	rightaccErr = 0;
	rightRPM=0;

	IO0CLR|=L_DIRinv;		//set motors direction
 	IO0SET|=L_DIR;
 	IO0CLR|=R_DIR;
 	IO0SET|=R_DIRinv;

	leftPWM=PWM_FREQ/4;		//initial speed = half full speed
  	rightPWM=PWM_FREQ/4;

	Lstep = step + (step*44)/radius;
	Rstep = step - (step*44)/radius;

	lcount=0;  			//reset left step count
	rcount=0;			//reset right step count
	//stop when stepcount reach desire steps
	while((lcount<=Lstep)||(rcount<=Rstep)) {
		if(lcount>=Lstep) {
			IO0CLR|=L_DIRinv;	//stop left motor
 			IO0CLR|=L_DIR;
			lcount=0xffff;
		}
		if(rcount>=Rstep) {
			IO0CLR|=R_DIRinv;	//stop right motor
 			IO0CLR|=R_DIR;
			rcount=0xffff;
		}
	}
}
*/

void stop(void) {
	IO0CLR|=L_DIR;			//set motors direction
 	IO0CLR|=L_DIRinv;
 	IO0CLR|=R_DIRinv;
 	IO0CLR|=R_DIR;
}

void delay(int t) {
	ms = 0;
	while(ms<t) {}
} 

int main(void)
{
 int leftDesiredRPM,rightDesiredRPM;
 int k;

 PINSEL0 = 0x00000005;		// set p0.0 to TXD0, p0.1 to RXD0 and the rest to GPIO
 PINSEL1 = 0x00000400;		// set p0.21 to PWM5  
 PINSEL0 |= 0x00008000;		// set p0.7 to PWM2     

 SendRPM = 0;	
 
 Init_Serial_A();		// Init COM port
 init_timer();			// Init TIMER 0
 
 //NEWLINE;
 //print("======================================================================="); NEWLINE;
 //print("*                                                                     *"); NEWLINE;
 //print("*         CUHK Computer Science and Engineering Department            *"); NEWLINE;
 //print("*                  LPC2131 ARM Board (v9.3)                           *"); NEWLINE;
 //print("*               PID Motor Speed Control Example                       *"); NEWLINE;
 //print("*                                                                     *"); NEWLINE;
 //print("======================================================================="); NEWLINE;
 //NEWLINE;
 
 PWMPCR=0x2000;             // enable pwm5
 PWMPCR|=0x0400;             // enable pwm2
 PWMMCR=0x0002;
 PWMMR0 = PWM_FREQ;    	//set PWM frequency to 50 Hz

 //set robot to full speed
 leftPWM=PWM_FREQ;
 rightPWM=PWM_FREQ;
 PWMMR2 = leftPWM;		//set left motor PWM width to full speed
 PWMMR5 = rightPWM;		//set right motor PWM width to full speed		
 PWMLER = 0x25;			//enable match 0,2,5 latch to effective	
 PWMTCR=0x09;
 
 //set p0.16-p0.19 as output
 IO0DIR|=L_DIR;
 IO0DIR|=L_DIRinv;
 IO0DIR|=R_DIR;
 IO0DIR|=R_DIRinv;

  while(1) {

  	lcur=lold=IO0PIN & LWheelSen;
 	rcur=rold=IO0PIN & RWheelSen;

  	Pgain = 8000;
  	Igain = 6000;
  	Dgain = 5000;

	leftDesiredRPM = 30;
	rightDesiredRPM = 30;

	print("Waiting PID and Speed parameters");

	Pgain = getint();
	putlong(Pgain);
	print(";");

	Igain = getint();
	putlong(Igain);
	print(";");

	Dgain = getint();
	putlong(Dgain);
	print(";");

	leftDesiredRPM = getint();
	putlong(leftDesiredRPM);
	print(";");

	rightDesiredRPM = getint();
	putlong(rightDesiredRPM);
	print(";");

	print("Press SW1 to run demo program ...... ");
 	NEWLINE;
	tmpSW=IO0PIN & SW1;
	while(tmpSW!=0) tmpSW=IO0PIN & SW1;
 	print("Start!");
 	NEWLINE;
 	NEWLINE;


	//////////////////////////////////////////////////////
	// Show the current RPM and PWM value (for experiment)
	//////////////////////////////////////////////////////
	leftRPMset = leftDesiredRPM*88/240;	//set left speed = 30 RPM
	leftErr = 0;		//reset all varialbles
  	leftlastErr = 0;
	leftaccErr = 0;
	leftRPM=0;
	
	rightRPMset = rightDesiredRPM*88/240; 	//set right speed = 30 RPM
	rightErr = 0;		//reset all variables
  	rightlastErr = 0;
	rightaccErr = 0;
	rightRPM=0;
	 
	IO0SET|=L_DIRinv;		//set motors direction
 	IO0CLR|=L_DIR;
 	IO0SET|=R_DIR;
 	IO0CLR|=R_DIRinv;

	leftPWM=0;		//initial speed = half full speed
  	rightPWM=0;

	print("Y");
	for(k=0;k<200;k++) {
	  
	  putint(current_leftRPM);
	  delay(50);
	  
	  //putint(current_rightRPM);
	  //delay(50);

	}
	stop();
	// End of show the current RPM and PWM value (for experiment) 
	
	//forward(200,200,30,30);		// straight forward for 200 steps with speed of 30 RPM
  }
}

 void __irq  IRQ_Exception()
{
	ms++;
	intcount++;
 
	//get the current wheel sensor values
	lcur=IO0PIN & LWheelSen;
	rcur=IO0PIN & RWheelSen;

	if(lcur!=lold) {		// increment left counter
		lcount++;
		lold=lcur;
		lefttimeval++;
	}

	if(rcur!=rold) {		// increment right counter
		rcount++;
		rold=rcur;
		righttimeval++;
	}

	//update the motor speed by PID feed back control in every 1/4 seconds					  
	if(intcount==250) {
		//caculate the speed of left motor in RPM
		leftRPM=lefttimeval;			

		leftErr =  leftRPMset - leftRPM;	//caculate left error
		if((leftErr<DeadBand*(-1))||(leftErr>DeadBand)) {				//if |left error| > deadband
			leftP = Pgain * leftErr;		//calculate P for left motor
			leftI = Igain * leftaccErr;		//calculate I for left motor
			leftD = Dgain * (leftErr - leftlastErr);	//calculate D for left motor
			leftPWM += (leftP + leftI + leftD);		//update the left motor PWM using PID
			if(leftPWM>PWM_FREQ) leftPWM=PWM_FREQ;	//prevent over range (max. = PWM_FREQ)
			if(leftPWM<0) leftPWM = 0;				// (min. = 0)
			leftaccErr += leftErr;					// accumulate error
			leftlastErr = leftErr;					//update left last error
		}
		
		lefttimeval = 0;		
		
		//the process repeat for right motor
		rightRPM=righttimeval;	//convert to RPM (88 steps = one revolution)
		
		rightErr =  rightRPMset - rightRPM;
		if((rightErr<DeadBand*(-1))||(rightErr>DeadBand)) {
			rightP = Pgain * rightErr;
			rightI = Igain * rightaccErr;
			rightD = Dgain * (rightErr - rightlastErr);
			rightPWM += (rightP + rightI + rightD);
			if(rightPWM>PWM_FREQ) rightPWM=PWM_FREQ;
			if(rightPWM<0) rightPWM = 0;
			rightaccErr += rightErr; 
			rightlastErr = rightErr;
		}


		righttimeval = 0;
		intcount = 0;

		current_leftRPM = leftRPM*240/88;
		current_rightRPM = rightRPM*240/88;
		current_leftPWM = leftPWM;
		current_rightPWM = rightPWM;
		}
	
	PWMMR2=leftPWM;
	PWMMR5=rightPWM;
	PWMLER = 0x24;			//enable match 2,5 latch to effective	

    T0IR = 1;                              	// Clear interrupt flag
    VICVectAddr = 0;                       	// Acknowledge Interrupt
}

/* Setup the Timer Counter 0 Interrupt */
void init_timer (void) {
	T0PR = 0;								// set prescaler to 0
	T0MR0 =13824;							// set interrupt interval to 1mS
											// Pclk/1KHz = (11059200 x 5)/(4 x 1000)
    T0MCR = 3;                             	// Interrupt and Reset on MR0
    T0TCR = 1;                             	// Timer0 Enable
	VICVectAddr0 = (unsigned long)IRQ_Exception;  	// set interrupt vector in 0
    VICVectCntl0 = 0x20 | 4;                    // use it for Timer 0 Interrupt
    VICIntEnable = 0x00000010;                  // Enable Timer0 Interrupt
}