#include <pic.h>
__CONFIG(XT & DEBUGDIS & WDTDIS & LVPDIS);
#define 	_XTAL_FREQ 10000000
#define	PORTBIT(adr, bit)	((unsigned)(&adr)*8+(bit))

#define 	LIR		0			//ADC PORT/CHANNEL RA0 
#define 	RIR		1			//ADC PORT/CHANNEL RA1	
#define 	CIR		3			//ADC PORT/CHANNEL RA3
#define 	LFT_PWM		CCPR1L	//PWM CCP1 RC2
#define 	RGT_PWM		CCPR2L	//PWM CCP2 RC1
#define 	MD_STP		0		//Motor Direction Stop
#define 	MD_FWD		1		//Motor Direction Forward
#define 	MD_REV		2		//Motor Direction Reverse
#define 	MD_LFT 		3		//Motor Direction Left
#define 	MD_RGT 		4		//Motor Direction Right
#define 	MD_BRK 		5		//Motor Direction Brake
#define 	MD_SRC		6		//Motor Direction Forward
#define 	SEE_CENTER	100		//IR Port Sensitivity Center
#define 	SEE_SIDES	120		//IR Port Sensitivity Sides
#define		LFT_TOPSPD	127		//Speed Calibration Top  Speed Left
#define		RGT_TOPSPD	127		//Speed Calibration Top  Speed Right
#define		LFT_SRCSPD	80		//Speed Calibration Top  Speed Left
#define		RGT_SRCSPD	55		//Speed Calibration Top  Speed Right
#define		LFT_TURNSPD	50		//Speed Calibration Turn Speed Left
#define		RGT_TURNSPD	50		//Speed Calibration Turn Speed Right
#define		SEARCH		0		//Action Search
#define		ATTACK		1		//Action Attack
#define		TURNLEFT	2		//Action Turn Left
#define		TURNRIGHT	3		//Action Turn Right
#define		BACKUP		4		//Action Backup

static bit LED1 	@ PORTBIT(PORTB, 0);
static bit LED2 	@ PORTBIT(PORTB, 1);
static bit LED3 	@ PORTBIT(PORTB, 2);
static bit LED4 	@ PORTBIT(PORTB, 3);
static bit GO_BTN 	@ PORTBIT(PORTB, 4);
static bit BORDER 	@ PORTBIT(PORTA, 2);
static bit INPUT1 	@ PORTBIT(PORTC, 4);
static bit INPUT2 	@ PORTBIT(PORTC, 5);
static bit INPUT3 	@ PORTBIT(PORTC, 0);
static bit INPUT4 	@ PORTBIT(PORTC, 3);

unsigned int ldist;		//ADC value of Left IR Sensor
unsigned int rdist;		//ADC value of Right IR Sensor
unsigned int cdist;		//ADC value of Center IR Sensor
unsigned int running;	//Are we running or waiting for start?
unsigned int mode;		//Current action mode
unsigned int curdir;	//Current direction of the motors
unsigned int backcnt;	//Backup Turn Cycle Count
unsigned int nextdir;	//Last Turn Direction

unsigned int ReadADC(unsigned char ADC_Channel){
	volatile unsigned int ADC_VALUE;

	/* Selecting ADC channel */
	ADCON0	= (ADC_Channel << 3) + 1;
	ADIE 	= 0;
	ADIF 	= 0;
	ADRESL 	= 0;
	ADRESH 	= 0;
	ADGO		= 1;			
  	while(!ADIF) continue;
	ADC_VALUE = ADRESL;
	ADC_VALUE += (ADRESH << 8);
  	return (ADC_VALUE);     /* Return the value of the ADC process */
}  

void pause(unsigned int i) {
	unsigned int j;

	for(j=0; j<i; j++) {
		__delay_ms(50);
	}
}

motor_ch_dir(unsigned int dir) {
	if (dir != curdir) {
		curdir=dir;
		if (dir==MD_STP) {
			LFT_PWM 	= LFT_TOPSPD;
			RGT_PWM 	= RGT_TOPSPD;
			INPUT1 	= 0;	//Turn right motor OFF
			INPUT2 	= 0;
			INPUT3 	= 0;	//Turn left motor OFF
			INPUT4 	= 0;
		}
		if (dir==MD_FWD) {
			LFT_PWM 	= LFT_TOPSPD;
			RGT_PWM 	= RGT_TOPSPD;
			INPUT1 	= 0;	//Turn right motor FWD
			INPUT2 	= 1;
			INPUT3 	= 0;	//Turn left motor FWD
			INPUT4 	= 1;
		} 
		if (dir==MD_SRC) {
			LFT_PWM 	= LFT_SRCSPD;
			RGT_PWM 	= RGT_SRCSPD;
			INPUT1 	= 0;	//Turn right motor FWD
			INPUT2 	= 1;
			INPUT3 	= 0;	//Turn left motor FWD
			INPUT4 	= 1;
		} 
		if (dir==MD_REV) {
			LFT_PWM 	= LFT_TOPSPD;
			RGT_PWM 	= RGT_TOPSPD;
			INPUT1 	= 1;	//Turn right motor REV
			INPUT2 	= 0;
			INPUT3 	= 1;	//Turn left motor REV
			INPUT4 	= 0;
		} 
		if (dir==MD_LFT) {
			LFT_PWM 	= LFT_TURNSPD;
			RGT_PWM 	= RGT_TURNSPD;
			INPUT1 	= 1;	//Turn right motor FWD
			INPUT2 	= 0;
			INPUT3 	= 0;	//Turn left motor REV
			INPUT4 	= 1;
		} 
		if (dir==MD_RGT) {
			LFT_PWM 	= LFT_TURNSPD;
			RGT_PWM 	= RGT_TURNSPD;
			INPUT1 	= 0;	//Turn right motor REV
			INPUT2 	= 1;
			INPUT3 	= 1;	//Turn left motor FWD
			INPUT4 	= 0;
		} 
		if (dir==MD_BRK) {
			LFT_PWM 	= LFT_TOPSPD;
			RGT_PWM 	= RGT_TOPSPD;
			INPUT1 	= 1;	//Turn right motor REV
			INPUT2 	= 1;
			INPUT3 	= 1;	//Turn left motor FWD
			INPUT4 	= 1;
		}
	}
}

void disp(int d){
	if (d<2) {
		LED1=d;
		LED2=d;
		LED3=d;
		LED4=d;
	 } else {
		LED1=!BORDER;
		LED2=(ldist>SEE_SIDES);
		LED3=(rdist>SEE_SIDES);
		LED4=(cdist>SEE_CENTER);
	}
}

void init(void){
	unsigned int i;

	TRISB 	= 0b00010000;	//PortB pin 4 input for GO_BTN, all other output for LEDS
	TRISC 	= 0;			//PortC all outputs
	PORTB 	= 0;			//PortB all low
	PORTC 	= 0;			//PortC all low

	CCP1CON = 0b00001111; 	//Enable PWM mode on RC2
	CCP2CON = 0b00001111; 	//Enable PWM mode on RC1
	PR2 	= 126;		//Top value for PWM
	T2CON 	= 0b00000011; 	//Prescaler=16
	TMR2ON 	= 1;			//Enable TIMER2 for PWM
	ADCON1 	= 0b10000100;	//Set up ADC ports

	LFT_PWM = 0;
	RGT_PWM = 0;
	curdir 	= MD_STP;
	running	= 0;
	mode	= SEARCH;
	nextdir	= MD_LFT;

	for(i=0; i<5; i++) {
		LED1=1;
		LED2=0;
		LED3=1;
		LED4=0;
		pause(5);
		LED1=0;
		LED2=1;
		LED3=0;
		LED4=1;
		pause(5);
	}
	LED1=0;
	LED2=0;
	LED3=0;
	LED4=0;
}

void startdelay(void) {
	//delay 5s, flashing lights
	unsigned int j;

	for(j=0; j<10; j++) {
		disp(0);
		pause(5);
		disp(1);
		pause(5);
	}	
}

void sumo(void) {
	if (!BORDER) {	//BORDER IS ACTIVE LOW
		mode=BACKUP;
		backcnt=0;
	}
	cdist = ReadADC(CIR);
	ldist = ReadADC(LIR);
	rdist = ReadADC(RIR);
	disp(2);						//show status on LED's
	
	if ((mode==SEARCH) || ((mode==BACKUP) && (backcnt > 8))) {	//If we're in search mode, TURN around..
		if (ldist > SEE_SIDES)			//attack if there's something in front
			mode=TURNLEFT;			//or turn if something's on the side
		if (rdist > SEE_SIDES)
			mode=TURNRIGHT;
		if (cdist > SEE_CENTER) {
			mode=ATTACK;
			motor_ch_dir(MD_FWD); } 		//just sit still for now & wait to go somewhere
		if (mode==SEARCH)			//If still searching, keep turning until I see something
			motor_ch_dir(MD_SRC); 		//just sit still for now & wait to go somewhere
	}
	
	if (mode==ATTACK) {
		if (cdist > SEE_CENTER)
			motor_ch_dir(MD_FWD);
		else
			mode=SEARCH;
	}

	if (mode==TURNLEFT) {
		motor_ch_dir(MD_LFT);
		if (cdist > SEE_CENTER)
			mode=ATTACK;
		if (ldist <= SEE_SIDES)
			mode=SEARCH;
	}

	if (mode==TURNRIGHT) {
		motor_ch_dir(MD_RGT);
		if (cdist > SEE_CENTER)
			mode=ATTACK;
		if (rdist <= SEE_SIDES)
			mode=SEARCH;
	}

	if (mode==BACKUP) {
		if (backcnt==0) {
			motor_ch_dir(MD_REV);
			pause(10);
			backcnt++;
		} else {
			if (backcnt==30) {
				mode=SEARCH;
				if (nextdir == MD_RGT)
					nextdir = MD_LFT;
				else
					nextdir = MD_RGT;
			} else {
				motor_ch_dir(nextdir);
				pause(1);
				backcnt++;
			}
		}
	}
}

void main(void){
	init();

	for(;;) {
		if (running)
			sumo();
		else
			disp(1);
		
		if (!GO_BTN) {
			while(!GO_BTN){}
			if (running) {
				motor_ch_dir(MD_STP);
				running	= 0;
				mode	= SEARCH;
				nextdir	= MD_LFT;
			}
			else {
				startdelay();
				running=1;
			}
		}
	}
}