#define  TRUE                   1
#define  FALSE                  0
typedef  unsigned char       BOOL;
typedef  unsigned char       BYTE;

#define  LEFT_MOTOR  0
#define  SERVO       2
#define  RIGHT_MOTOR 3

#define  LED_RED 	4
#define  LED_GREEN 	5
#define  LED_BLUE 	6

#define  H_LEFT		8
#define  H_RIGHT	9

#define BUMP_LEFT (10)
#define BUMP_CENTER (11)
#define BUMP_RIGHT (12)
#define BUTTON (13)

#define  STATUS_LED            (14)
#define COMPASS	(15)
//#define MOTION_SENSOR 19

#define LCD_E	(20)
#define LCD_RW	(21)
#define LCD_RS	(22)
#define CONTRAST	(23)

#define LCD_DB4	(24)
#define LCD_DB5	(25)
#define LCD_DB6	(26)
#define LCD_DB7	(27)

#define LCD_DB0	(28)
#define LCD_DB1	(29)
#define LCD_DB2	(30)
#define LCD_DB3	(31)


/* Analog/digital ports */
#define  IR_LEFT    7
#define  IR_CENTER  6
//#define  IR_RIGHT   5
#define  MOTION_SENSOR   5
#define  CDS_BASE	4

#define  PORT_MASK(nBit) (1<<nBit)

#define  STATE_INIT 	0
#define  STATE_CRUISING 1
#define  STATE_DEBUG 	2
#define  STATE_SNAP 	3
#define  STATE_SNEAK 	4
#define  STATE_POUNCE 	5
#define  STATE_TURNING 	6
#define  STATE_BUMPED 	7
//#define  STATE_REVERSE  8

#define  SURFACE_FLOOR 	0
#define  SURFACE_SNAP  	1
#define  SURFACE_POUNCE 2
#define  SURFACE_SNEAK 	3
#define  SURFACE_TURN   4

#define ANGLE_MAX 110000 // calibrated compass values
#define ANGLE_MIN   3000 // 1 degree = 297
#define NINETY_DEGREES 26750


int leftSpeed, RightSpeed;


int currentDirection;
int desiredDirection;
int currentState;

int reflectivity;
int redReflectivity;
int greenReflectivity;
int blueReflectivity;

int lastreflectivity;
int lastredReflectivity;
int lastgreenReflectivity;
int lastblueReflectivity;


int RrunningSurface;
int RpictureSurface;
int RpounceSurface;
int RsnapSurface;
int RturnSurface;
int RsneakSurface;

int GrunningSurface;
int GpictureSurface;
int GpounceSurface;
int GsnapSurface;
int GturnSurface;
int GsneakSurface;

int BrunningSurface;
int BpictureSurface;
int BpounceSurface;
int BsnapSurface;
int BturnSurface;
int BsneakSurface;

int motionSensor;

int currentSurface;
int lastSurface;

long programTimer;

//right turn increments


#include "AT91SAM7S64.h"
#include <math.h>

extern void isrIqr(void);
extern void isrFiq(void);
extern void iseSpur(void);


int leftSpeed = 0;
int rightSpeed = 0;

int currentLeftSpeed = 0;
int currentRightSpeed = 0;

int leftDirection = 0;
int rightDirection = 0;

int leftReading;
int centerReading;
int rightReading;



// ------------------------------------------------------------
// Init() initializes the Flash, the watchdog timer, the Power 
// Management Controller (PMC), and the PIO port.
// See the steps outlined in the section titled "Programming 
// Sequence" of the Atmel AT91SAM7S data sheet (Section 26.7 )
// to program the PMC.
// ------------------------------------------------------------

//void writeIntToLCD(int number);
//void writeIntToLCD(int number);
int abs(int n)
{
	if (n > 0)
	return n;
	else
	return -n;
	
	
}


void Init(void)

{
AT91PS_PMC   pPMC;
AT91PS_PIO   pPIO;
unsigned int u;
unsigned int u2;

// --- set flash memory parameters ---
// MCK will be set to 47.92 MHz (approximately 48MHz) below
// so set FMCN=48.  Add minimal wait states since we are above 40 MHz.
 AT91C_BASE_MC->MC_FMR = ((AT91C_MC_FMCN)&(48 <<16)) | AT91C_MC_FWS_1FWS ;

// --- disable the watchdog timer ---
 AT91C_BASE_WDTC->WDTC_WDMR=AT91C_WDTC_WDDIS;

// --- program the Power Management Controller (PMC) ---
// steps are as given in Section 26.7 "Programming Sequence"

 pPMC=AT91C_BASE_PMC;

// 1. start the "main oscillator" (Section 25.3.3)
// slow clock frequency = 32.768 KHz
// slow clock period = 30.5176 microseconds
// Start up time = 8*7*30.5176 = 1708.984375 microseconds or 1.709 milliseconds
 pPMC->PMC_MOR = (AT91C_CKGR_OSCOUNT & (0x07 <<8)) | AT91C_CKGR_MOSCEN;

// 2. check the main oscillator frequency (optional)
// skipped

// wait the master clock to settle
 while(!(pPMC->PMC_SR & AT91C_PMC_MOSCS));

// 3. set the PLL and the divider
// main clock or (crystal) frequency = 18.432MHz)
// use: USBDIV=1, DIV=5, MUL=25, and OUT=0, which yields,
// (18.432/5)*(25+1)=95,8464MHz
// for a LOCK time of 1000 microseconds (1 milliseconds),
// set PLLCOUNT to 1000/30.5176=33
 pPMC->PMC_PLLR = ((AT91C_CKGR_DIV & 5) | (AT91C_CKGR_PLLCOUNT & (33<<8)) | (AT91C_CKGR_MUL & (25<<16)) | (AT91C_CKGR_USBDIV & (1<<28)) );
// wait for the PLL to stabalize
 while(!(pPMC->PMC_SR&AT91C_PMC_LOCK));
// wait for the "master clock ready" flag
 while(!(pPMC->PMC_SR&AT91C_PMC_MCKRDY));
 
// 4. select the master clock source and the prescalar
// source    = the PLL clock
// prescalar = 2
 pPMC->PMC_MCKR=AT91C_PMC_CSS_PLL_CLK|AT91C_PMC_PRES_CLK_2;
 while(!(pPMC->PMC_SR & AT91C_PMC_MCKRDY));

// 5. select programmable clocks
// (programmable clocks are not used in this application)

// 6. enable the peripheral clock used by PIOA and ADC
 pPMC->PMC_PCER=(1<<AT91C_ID_PIOA)|(1<<AT91C_ID_ADC)|(1<<AT91C_ID_PWMC);
//pPMC->PMC_PCER=(1<<AT91C_ID_PIOA)|(1<<AT91C_ID_PWMC);

// initializ the port defined as PORT_LED as an output port.

//configure the PIO register used by the LED
 pPIO=AT91C_BASE_PIOA;
 
 u=PORT_MASK(LCD_RW)|PORT_MASK(LCD_RS)|PORT_MASK(LCD_E)|PORT_MASK(CONTRAST)|
   PORT_MASK(LCD_DB0)|PORT_MASK(LCD_DB1)|PORT_MASK(LCD_DB2)|PORT_MASK(LCD_DB3)|
   PORT_MASK(LCD_DB4)|PORT_MASK(LCD_DB5)|PORT_MASK(LCD_DB6)|PORT_MASK(LCD_DB7)|
   PORT_MASK(STATUS_LED)|PORT_MASK(H_LEFT)|PORT_MASK(H_RIGHT)|
   PORT_MASK(LED_RED)|PORT_MASK(LED_GREEN)|PORT_MASK(LED_BLUE)|
   PORT_MASK(LEFT_MOTOR)|PORT_MASK(RIGHT_MOTOR);
 
 
 u2 = PORT_MASK(COMPASS)|PORT_MASK(BUMP_LEFT)|
 	 PORT_MASK(BUMP_CENTER)|PORT_MASK(BUMP_RIGHT)|
 	 PORT_MASK(BUTTON);//|PORT_MASK(MOTION_SENSOR);
 
 
 pPIO->PIO_PER  = u|u2;   // enable ports
 pPIO->PIO_OER  = u;   // configure ports as an outputs
 pPIO->PIO_CODR = u;   // clearing the output ports

//pPIO->PIO_ODR = u2;
 	 
//pPIO->PIO_PPUER = PORT_MASK(COMPASS)|PORT_MASK(BUMP_LEFT)|
 //	 PORT_MASK(BUMP_CENTER)|PORT_MASK(BUMP_RIGHT);//|
 //pPIO->PIO_IFER = PORT_MASK(COMPASS)|PORT_MASK(BUMP_LEFT)|
 	// PORT_MASK(BUMP_CENTER)|PORT_MASK(BUMP_RIGHT);//|
 
 pPIO->PIO_ODR = u2;
 pPIO->PIO_PPUER = u2;
 pPIO->PIO_IFER = u2;

// PWM port is controlled by the peripheral
//pPIO->PIO_PDR = (PORT_MASK(LEFT_MOTOR)|PORT_MASK(RIGHT_MOTOR));
pPIO->PIO_PDR = PORT_MASK(SERVO);
// PWM port is peripheral A (multiplexed with PA1)
//pPIO->PIO_ASR = (PORT_MASK(LEFT_MOTOR)|PORT_MASK(RIGHT_MOTOR));
pPIO->PIO_ASR = PORT_MASK(SERVO);
 

}
////////////////////////////////////////////////////////
//Read a value from digital I/O						  //
////////////////////////////////////////////////////////


// ------------------------------------------------------------
BOOL GetPort(BYTE bPort)
{
AT91PS_PIO pPio;

 pPio=AT91C_BASE_PIOA;
 return((pPio->PIO_PDSR & PORT_MASK(bPort))?TRUE:FALSE);  // return BOOL
}
// ------------------------------------------------------------

////////////////////////////////////////////////////////
//Write a value to digital I/O						  //
////////////////////////////////////////////////////////

void SetPort(BYTE bPort, BOOL bState)
{
AT91PS_PIO pPio;

 pPio=AT91C_BASE_PIOA;
 if(bState) pPio->PIO_SODR = PORT_MASK(bPort);
    else pPio->PIO_CODR = PORT_MASK(bPort);
}


  
// ------------------------------------------------------------

////////////////////////////////////////////////////////
//Delay function									  //
////////////////////////////////////////////////////////

void sleep()
{
	volatile int a;
	for (a = 0; a <= 0x4500; a++);
}
	
////////////////////////////////////////////////////////
//Read value from compass by measuring pulse		  //
////////////////////////////////////////////////////////	
	
long readCompass()
{
	//volatile int value = 0;
	
	while (GetPort(COMPASS) == 1);
	
	while (GetPort(COMPASS) == 0);
	
	volatile long value = 0;
	while (GetPort(COMPASS) == 1)
	{
		value++;
	}
	
	return value;
}

////////////////////////////////////////////////////////
//Used to write data to the LCD display				  //
////////////////////////////////////////////////////////
	
void writeDataToLCD(unsigned int data)
{
	SetPort(LCD_RW, 0);
	SetPort(LCD_RS, 1);
	SetPort(LCD_E, 1);
	
	SetPort(LCD_DB0, data%2);
	data >>= 1;
	SetPort(LCD_DB1, data%2);
	data >>= 1;
	SetPort(LCD_DB2, data%2);
	data >>= 1;
	SetPort(LCD_DB3, data%2);
	data >>= 1;
	SetPort(LCD_DB4, data%2);
	data >>= 1;
	SetPort(LCD_DB5, data%2);
	data >>= 1;
	SetPort(LCD_DB6, data%2);
	data >>= 1;
	SetPort(LCD_DB7, data%2);
	
	sleep();
	
	SetPort(LCD_E, 0);

	sleep();


}

////////////////////////////////////////////////////////
//used to write control commands to LCD				  //
////////////////////////////////////////////////////////

void writeCommandToLCD(unsigned int data)
{
	SetPort(LCD_RW, 0);
	SetPort(LCD_RS, 0);
	SetPort(LCD_E, 1);
	
	SetPort(LCD_DB0, data%2);
	data >>= 1;
	SetPort(LCD_DB1, data%2);
	data >>= 1;
	SetPort(LCD_DB2, data%2);
	data >>= 1;
	SetPort(LCD_DB3, data%2);
	data >>= 1;
	SetPort(LCD_DB4, data%2);
	data >>= 1;
	SetPort(LCD_DB5, data%2);
	data >>= 1;
	SetPort(LCD_DB6, data%2);
	data >>= 1;
	SetPort(LCD_DB7, data%2);
	
	sleep();
	
	SetPort(LCD_E, 0);

	sleep();
}

////////////////////////////////////////////////////////
//Output a long integer to the LCD display			  //
////////////////////////////////////////////////////////

void writeNumToLCD(unsigned long number)
{
	if (number >= 10)
	{
		writeNumToLCD(((number - number % 10) / 10));
	}
	
	int num = (number % 10) + 48;

	//writeCommandToLCD((number % 10) + 0x30);
writeDataToLCD(num);
//writeCommandToLCD(0x38); //00111000
}

////////////////////////////////////////////////////////
//LCD Init Routine							  //
////////////////////////////////////////////////////////

void LCD_init()
{

writeCommandToLCD(0xC0); //00110000
writeCommandToLCD(0xC0); //
writeCommandToLCD(0xC0);
writeCommandToLCD(0x38); //00111000

writeCommandToLCD(0x08); //00001000
writeCommandToLCD(0x01); //00000001

writeCommandToLCD(0x6);  //00000110
writeCommandToLCD(0xF);  //00001111
writeCommandToLCD(0x38); //00111000

	SetPort(STATUS_LED, 0);
}

////////////////////////////////////////////////////////
//function to write a 'C string' to the LCD			  //
////////////////////////////////////////////////////////

void writeStringToLCD(char * str)
{
	volatile int pos = 0;
	while (str[pos] != '\0')
	{
		writeDataToLCD(str[pos]);
			pos++;
	}	
}



 AT91PS_PIO pPIO;
AT91PS_PWMC pPwmc;

AT91PS_PWMC_CH pPwmCh2;


AT91PS_ADC   pADC;


////////////////////////////////////////////////////////
//Read values from ADC								  //
////////////////////////////////////////////////////////

void getADC()
{
	int nADC = 0;
	volatile int n;
			SetPort(LED_RED, 1);
			SetPort(LED_GREEN, 1);
			SetPort(LED_BLUE, 1);
	for(n=0; n<0x1000; n++); 
	
	 if((pADC->ADC_SR)&(1<<IR_LEFT))  // check if adc channel is ready
    {
     nADC=pADC->ADC_CDR7;           // read conversion value
     pADC->ADC_CR=2;                // start ADC
         
     
  //   SetPort(STATUS_LED, (nADC>>6)%2);
     /*
     char status[16] = "IR Sensor ";
	 writeNumToLCD(1);
	 writeDataToLCD(':');
	 
     writeNumToLCD(nADC);
   	 writeDataToLCD(' ');
   	 */
   	 leftReading = nADC;
    }
      for(n=0; n<0x1000; n++); 
      
      if((pADC->ADC_SR)&(1<<IR_CENTER))  // check if adc channel is ready
    {
     nADC=pADC->ADC_CDR6;           // read conversion value
     pADC->ADC_CR=2;                // start ADC
   	 centerReading = nADC;
    }
    
    
      for(n=0; n<0x1000; n++); 
    
          if((pADC->ADC_SR)&(1<<CDS_BASE))  // check if adc channel is ready
    {
     nADC=pADC->ADC_CDR4;           // read conversion value
     pADC->ADC_CR=2;                // start ADC
     
  //   SetPort(STATUS_LED, (nADC>>6)%2);
     
    // char status[16] = "Base sensor ";
	// writeNumToLCD(3);
	 writeDataToLCD(':');     
    
     writeNumToLCD(nADC);
   	 writeDataToLCD(' ');
   	 //rightReading = nADC;
   	 reflectivity = nADC;
   	 
   	 			SetPort(LED_RED, 0);
			SetPort(LED_GREEN, 0);
			SetPort(LED_BLUE, 0);
   	 
   	 
    }
        else
    {
   		writeDataToLCD('&');
   		reflectivity = 0;
	}
    
  //     for(n=0; n<0x10000; n++); 
    
     if((pADC->ADC_SR)&(1<<MOTION_SENSOR))  // check if adc channel is ready
    {
     nADC=pADC->ADC_CDR5;           // read conversion value
     pADC->ADC_CR=2;                // start ADC
   	 motionSensor = nADC;
    }
    else
    {
    	motionSensor = 0;
    	writeDataToLCD('#');  //error occured
    }
    
//	 writeDataToLCD('@');     
//	 writeNumToLCD(motionSensor);
	  
    if (motionSensor > 800)
    {
    	writeDataToLCD('*');
    }
    else
    {
    	writeDataToLCD('z');
    }
    
    
  //    for(n=0; n<0x10000; n++); 
    
    
}

////////////////////////////////////////////////////////
//scan surface and analyze it						  //
////////////////////////////////////////////////////////

int checkSurface()
{
	int nADC;
	volatile unsigned int n;


			SetPort(LED_RED, 1);
			SetPort(LED_GREEN, 1);
			SetPort(LED_BLUE, 1);

for(n=0; n<(1<<19); n++); 
	
	
	
	
   for(n=0; n<0x1000; n++);
    if((pADC->ADC_SR)&(1<<CDS_BASE))  // check if adc channel is ready
    {
     nADC=pADC->ADC_CDR4;           // read conversion value
     pADC->ADC_CR=2;                // start ADC
     
 	 writeDataToLCD('F');     
    
     writeNumToLCD(nADC);
   	 writeDataToLCD(' ');
   	
   	 reflectivity = nADC;
   	  	 
    }
    else
    {
   		writeDataToLCD('f');
   		reflectivity = 0;
	}
   
   //if change is small enough, value has stabilized, so analyze
   //otherwise, program should measure color values
   
	if (abs(lastreflectivity - reflectivity) > 2)
	{
			lastreflectivity = reflectivity;
			lastredReflectivity = redReflectivity;
			lastgreenReflectivity = greenReflectivity;
			lastblueReflectivity = blueReflectivity;
	
		//major change?
			SetPort(LED_RED, 1);
			SetPort(LED_GREEN, 0);
			SetPort(LED_BLUE, 0);
			for(n=0; n<(3<<19); n++); 
    		if((pADC->ADC_SR)&(1<<CDS_BASE))  // check if adc channel is ready
    		{
     		nADC=pADC->ADC_CDR4;           // read conversion value
     		pADC->ADC_CR=2;                // start ADC
   	 		redReflectivity = nADC;
   	 		writeDataToLCD('R');
   	 		writeDataToLCD(':');
   	 		writeNumToLCD(redReflectivity);
   	 		
    		}
    		else
    		{
   	 		writeDataToLCD('r');
   	 		redReflectivity = 0;
		    }
		    
		    SetPort(LED_RED, 0);
			SetPort(LED_GREEN, 1);
			SetPort(LED_BLUE, 0);
			
			for(n=0; n<(3<<19); n++); 
			
    		if((pADC->ADC_SR)&(1<<CDS_BASE))  // check if adc channel is ready
    		{
     		nADC=pADC->ADC_CDR4;           // read conversion value
     		pADC->ADC_CR=2;                // start ADC
   	 		greenReflectivity = nADC;
   	 		writeDataToLCD('G');
   	 		writeDataToLCD(':');
   	 		writeNumToLCD(greenReflectivity);
		    }
		    else
    		{
   	 		writeDataToLCD('g');
   	 		greenReflectivity = 0;
		    }

			SetPort(LED_RED, 0);
			SetPort(LED_GREEN, 0);
			SetPort(LED_BLUE, 1);
			
			for(n=0; n<(3<<19); n++); 
    		
    		if((pADC->ADC_SR)&(1<<CDS_BASE))  // check if adc channel is ready
    		{
     		nADC=pADC->ADC_CDR4;           // read conversion value
     		pADC->ADC_CR=2;                // start ADC
   	 		blueReflectivity = nADC;
   	 		
   	 		  	 		writeDataToLCD('B');
   	 		writeDataToLCD(':');
   	 		writeNumToLCD(blueReflectivity);
   	 		
		    }
		        		else
    		{
   	 		writeDataToLCD('b');
   	 		blueReflectivity = 0;
   	 		
		    }
		
			SetPort(LED_RED, 0);
			SetPort(LED_GREEN, 0);
			SetPort(LED_BLUE, 0);
			
			writeDataToLCD('_');
			writeDataToLCD('_');
			writeDataToLCD('_');
		return 0;
	}
	
				SetPort(LED_RED, 0);
			SetPort(LED_GREEN, 0);
			SetPort(LED_BLUE, 0);
			
			
	lastreflectivity = reflectivity;
	lastredReflectivity = redReflectivity;
	lastgreenReflectivity = greenReflectivity;
	lastblueReflectivity = blueReflectivity;
	
	
/*
#define  SURFACE_FLOOR 	0
#define  SURFACE_SNAP  	1
#define  SURFACE_POUNCE 2
#define  SURFACE_SNEAK 	3
#define  SURFACE_TURN   4	


	*/
	
	
	int floor = abs(redReflectivity - RrunningSurface) + 
				abs(greenReflectivity - GrunningSurface) + 
				abs(blueReflectivity - BrunningSurface);
	int snap = abs(redReflectivity - RsnapSurface) + 
				abs(greenReflectivity - GsnapSurface) + 
				abs(blueReflectivity - BsnapSurface);
	int pounce = abs(redReflectivity - RpounceSurface) + 
				abs(greenReflectivity - GpounceSurface) + 
				abs(blueReflectivity - BpounceSurface);
	int sneak = abs(redReflectivity - RsneakSurface) + 
				abs(greenReflectivity - GsneakSurface) + 
				abs(blueReflectivity - BsneakSurface);
	int turn = abs(redReflectivity - RturnSurface) + 
				abs(greenReflectivity - GturnSurface) + 
				abs(blueReflectivity - BturnSurface);
				
	int lowest;
	int newSurface;
/*	
	writeDataToLCD('A');
	writeDataToLCD(':');  
	writeNumToLCD(floor);
	
	writeDataToLCD('B');
	writeDataToLCD(':');  
	writeNumToLCD(snap);
	
	writeDataToLCD('C');
	writeDataToLCD(':');  
	writeNumToLCD(pounce);
	
	writeDataToLCD('D');
	writeDataToLCD(':');  
	writeNumToLCD(sneak);
	
	writeDataToLCD('E');
	writeDataToLCD(':');  
	writeNumToLCD(turn);
	writeDataToLCD('#'); 
	*/
	
	if (floor < snap)
	{
		lowest = floor;
		newSurface = SURFACE_FLOOR;
		writeDataToLCD('F'); 
	}
	else
	{
		lowest = snap;
		newSurface = SURFACE_SNAP;
		writeDataToLCD('N'); 
	}
	
	if (pounce < lowest)
	{
		lowest = pounce;
		newSurface = SURFACE_POUNCE;
		writeDataToLCD('P'); 
	}
	
	if (sneak < lowest)
	{
		lowest = sneak;
		newSurface = SURFACE_SNEAK;
		writeDataToLCD('K'); 
	}
	
	if (turn < lowest)
	{
		lowest = turn;
		newSurface = SURFACE_TURN;
		writeDataToLCD('T'); 
	}
	
	if ((currentSurface != newSurface) && (lowest < 100))
	{
		currentSurface = newSurface;
		writeDataToLCD('!');
		return 1;
	}
	else
	{
		writeDataToLCD('_');
		return 0;
	}
	
	
}

void motor(int left, int right)
{
	if (left > 0)
	{
		SetPort(H_LEFT, 0);	
		SetPort(LEFT_MOTOR, 1);
		
		if (!GetPort(BUMP_LEFT) || !GetPort(BUMP_CENTER) || !GetPort(BUMP_RIGHT))
		{
			currentState = STATE_BUMPED;
			SetPort(LEFT_MOTOR, 0);
			SetPort(RIGHT_MOTOR, 0);
		}
		
	}
	else if (left < 0)
	{
		SetPort(H_LEFT, 1);	
		SetPort(LEFT_MOTOR, 1);
				
	}
	else
	{
		SetPort(LEFT_MOTOR, 0);
		
	}
	
	if (right > 0)
	{
		SetPort(H_RIGHT, 0);	
		SetPort(RIGHT_MOTOR, 1);
		
				if (!GetPort(BUMP_LEFT) || !GetPort(BUMP_CENTER) || !GetPort(BUMP_RIGHT))
		{
			currentState = STATE_BUMPED;
			SetPort(LEFT_MOTOR, 0);
			SetPort(RIGHT_MOTOR, 0);
					}
	}
	else if (right < 0)
	{
		SetPort(H_RIGHT, 1);	
		SetPort(RIGHT_MOTOR, 1);
				
	}
	else
	{
		SetPort(RIGHT_MOTOR, 0);
		
	}
}

void cruise()
{
	  		if (!GetPort(BUMP_LEFT) || !GetPort(BUMP_CENTER) || !GetPort(BUMP_RIGHT))
  		{
  			currentState = STATE_BUMPED;
  			
  			motor(-1, -1);
  	//		SetPort(H_LEFT, 0);
	//		SetPort(H_RIGHT, 0);
	
			//SetPort(LEFT_MOTOR, 0);
			//SetPort(RIGHT_MOTOR, 0);
  			
  		}
  		else
  		{


 
 
if (leftReading > 700 && centerReading > 700)
{
		writeDataToLCD('X');

currentState = STATE_BUMPED;
//	pPwmCh0->PWMC_CUPDR=0xFD; // duty cycle
	//pPwmCh1->PWMC_CUPDR=0xFD; // duty cycle.


//		leftSpeed = 0xFF;
//	rightSpeed = 0xFF;

	motor(-1, -1);
		//SetPort(H_LEFT, 0);
	//SetPort(H_RIGHT, 0);
	
		//SetPort(LEFT_MOTOR, 0);
	//SetPort(RIGHT_MOTOR, 0);
}
else if (leftReading > 700)
{
	writeDataToLCD('>');
	//pPwmCh0->PWMC_CUPDR=0x0; // duty cycle
	//pPwmCh1->PWMC_CUPDR=0xFD; // duty cycle
		leftSpeed = 0;
	rightSpeed = 0xFF;
	
	motor (1, -1);
	//	SetPort(LEFT_MOTOR, 1);
	//SetPort(RIGHT_MOTOR, 0);
	
	//SetPort(H_LEFT, 0);
	//SetPort(H_RIGHT, 0);
	}
else if (centerReading > 700)
{
	writeDataToLCD('<');
		leftSpeed = 0xFF;
	rightSpeed = 0;
//	pPwmCh0->PWMC_CUPDR=0xFD; // duty cycle
//	pPwmCh1->PWMC_CUPDR=0x0; // duty cycle

motor (-1, 1);
	//SetPort(LEFT_MOTOR, 0);
	//SetPort(RIGHT_MOTOR, 1);
	//SetPort(H_LEFT, 0);
	//SetPort(H_RIGHT, 0);
	}
	
else
{
	writeDataToLCD('^');
	leftSpeed = 0;
	rightSpeed = 0;
	motor (1, 1);

	



	
//	PWM();
	

writeDataToLCD(' ');
  			} //end if
  		} //end if
}


void turn(int amount)
{
	
	int a;
for (a = 0; a < abs(amount); a++)
{
	if (a < 0)
	{
		motor(-1, 1); // left turn
	}
	else
	{
		motor(1, -1);	
	}
	
	if (!GetPort(BUMP_LEFT) || !GetPort(BUMP_CENTER) || !GetPort(BUMP_RIGHT))
	{
		currentState = STATE_BUMPED;
		a = abs(amount);
	}
	
	sleep();
	
}
	
}

void reverse(int amount)
{
	int a;
for (a = 0; a < abs(amount); a++)
{
		motor(-1, -1);	
		
	if (!GetPort(BUMP_LEFT))
	{
		turn(50);
	}
	else if(!GetPort(BUMP_RIGHT))
	{
		turn(-50);
	}
	
	sleep();	
}

	
}


////////////////////////////////////////////////////////
/////////////////////////////main///////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
//main function begin here							  //
////////////////////////////////////////////////////////

int main(void){

redReflectivity = 0;
greenReflectivity = 0;
blueReflectivity = 0;

volatile int n, nADC;
currentState = STATE_INIT;
desiredDirection = 10000;

// perform initialization tasks
 Init();

// initialize the ADC
// track-an-hold : 600 nS   --> SHTIM   =  2
// start up time :  20 uS   --> STARTUP = 12
// 10-bit adc clk:   5 MHz  --> PRESCAL =  3
 pADC=AT91C_BASE_ADC;
 pADC->ADC_MR=0x020C0300;       // SHTIM:STARTUP:PRESCAL:00
   // enable adc channel
  pADC->ADC_CHER=(1<<IR_LEFT)|(1<<IR_CENTER)|(1<<CDS_BASE)|(1<<MOTION_SENSOR);
 pADC->ADC_CR=2;               // start ADC
 
 
 //pwm init

volatile int nDC;
 pPIO=AT91C_BASE_PIOA;

// PWM port is controlled by the peripheral
//pPIO->PIO_PDR = PORT_MASK(LEFT_MOTOR)|PORT_MASK(RIGHT_MOTOR);
pPIO->PIO_PDR = PORT_MASK(SERVO);
// PWM port is peripheral A (multiplexed with PA1)
//pPIO->PIO_ASR = PORT_MASK(LEFT_MOTOR)|PORT_MASK(RIGHT_MOTOR);
pPIO->PIO_ASR = PORT_MASK(SERVO);

pPwmc=AT91C_BASE_PWMC;
pPwmc->PWMC_ENA=4; // enable pwm channel 
//pPwmc->PWMC_ENA=1; // enable pwm channel 0

pPwmCh2=AT91C_BASE_PWMC_CH2;
pPwmCh2->PWMC_CMR=0x020A; // polarity 1, PWM_CLK = MCK/1024
//pPwmCh0->PWMC_CMR=0x000A; // polarity 0, PWM_CLK = MCK/1024
pPwmCh2->PWMC_CPRDR=0x100; // period
nDC=0xFF;
pPwmCh2->PWMC_CDTYR=nDC; // duty cycle
/*
pPwmCh1=AT91C_BASE_PWMC_CH1;
//pPwmCh1->PWMC_CMR=0x020A; // polarity 1, PWM_CLK = MCK/1024
pPwmCh1->PWMC_CMR=0x000A; // polarity 0, PWM_CLK = MCK/1024
pPwmCh1->PWMC_CPRDR=0x100; // period
nDC=0xFF;
pPwmCh1->PWMC_CDTYR=nDC; // duty cycle
// end PWM init
*/
 
 nADC=0;


pPwmCh2->PWMC_CUPDR=0x01;
LCD_init();

int testval;
char calibrate1[16] = "Regular ^";
char calibrate2[16] = "Pounce <";
char calibrate3[16] = "Sneak ^";
char calibrate4[16] = "Rotate <";
char calibrate5[16] = "Snap ^";
char calibrate6[16] = "Start >";



while (GetPort(BUMP_CENTER) == 1)
{
	 writeCommandToLCD(0x2);  //00000010
	 writeStringToLCD(calibrate1);
	 checkSurface();       
	 writeNumToLCD(reflectivity);
	 sleep();
	 sleep();
	 sleep();
	 sleep();
	
}

RrunningSurface = redReflectivity;
GrunningSurface = greenReflectivity;
BrunningSurface = blueReflectivity;


while (GetPort(BUMP_LEFT) == 1)
{
	 writeCommandToLCD(0x2);  //00000010
	 writeStringToLCD(calibrate2);
	  checkSurface();       
	 writeNumToLCD(reflectivity);
	 sleep();
	 sleep();
	 sleep();
	 sleep();
	
}
RpounceSurface = redReflectivity;
GpounceSurface = greenReflectivity;
BpounceSurface = blueReflectivity;

while (GetPort(BUMP_CENTER) == 1)
{
	 writeCommandToLCD(0x2);  //00000010
	 writeStringToLCD(calibrate3);
	  checkSurface();        
	 writeNumToLCD(reflectivity);
	 sleep();
	 sleep();
	 sleep();
	 sleep();
	
}
RsneakSurface = redReflectivity;
GsneakSurface = greenReflectivity;
BsneakSurface = blueReflectivity;

/*
while (GetPort(BUMP_LEFT) == 1)
{
	 writeCommandToLCD(0x2);  //00000010
	 writeStringToLCD(calibrate4);
	  checkSurface();        
	 writeNumToLCD(redReflectivity);
	 sleep();
	 sleep();
	 sleep();
	 sleep();
	
}
*/
RturnSurface = 0;
GturnSurface = 0;
BturnSurface = 0;
/*
while (GetPort(BUMP_CENTER) == 1)
{
	 writeCommandToLCD(0x2);  //00000010
	 writeStringToLCD(calibrate5);
	  checkSurface();       
	 writeNumToLCD(redReflectivity);
	 sleep();
	 sleep();
	 sleep();
	 sleep();
	
}
*/
RsnapSurface = 0;
GsnapSurface = 0;
BsnapSurface = 0;


 writeCommandToLCD(0x2);  //00000010
writeStringToLCD(calibrate6);

writeDataToLCD('|');
writeNumToLCD(RrunningSurface);
writeDataToLCD('|');
writeNumToLCD(RpounceSurface);
writeDataToLCD('|');
writeNumToLCD(RsneakSurface);
writeDataToLCD('|');
writeNumToLCD(RturnSurface);
writeDataToLCD('|');
writeNumToLCD(RsnapSurface);
writeDataToLCD('|');

while (GetPort(BUMP_RIGHT) == 1);


//writeStringToLCD(powerUp);
//sleep();
//sleep();
//writeStringToLCD(warning);
//writeStringToLCD(aaa);

//writeNumToLCD(1337);

//#define  STATE_INIT 	0
//#define  STATE_CRUISING 1
//#define  STATE_DEBUG 	2
//#define  STATE_SNAPPING 3
//#define  STATE_PANORAMIC 4
//#define  STATE_TURNING 5

//#define  SURFACE_FLOOR 	0
//#define  SURFACE_SNAP  	1
//#define  SURFACE_POUNCE 2
//#define  SURFACE_SNEAK 	3
//#define  SURFACE_TURN   4


checkSurface();

pPwmCh2->PWMC_CUPDR=0x80;
 while(TRUE)
  {
  	  	getADC(); //update AD sensors
            writeCommandToLCD(0x2);  //00000010 	
  	writeNumToLCD(currentState);
  	
  	switch (currentState)
  	{
  		case STATE_INIT:
  			currentState = STATE_CRUISING;
  			writeDataToLCD('C');
  			break;
  		case STATE_CRUISING:
  			pPwmCh2->PWMC_CUPDR=0x00;
  		
  			if (checkSurface() == 1)
	  		{
	  			programTimer = 0;
	  			
  				switch (currentSurface)
  				{
  				case SURFACE_FLOOR:
  				currentState = STATE_CRUISING;
  				writeDataToLCD('C');
  				break;
  				
  				case SURFACE_SNAP:
  				currentState = STATE_SNAP;
  				writeDataToLCD('N');
  				break;
  				
  				case SURFACE_POUNCE:
  				currentState = STATE_POUNCE;
  				writeDataToLCD('P');
  				break;
  				
  				case SURFACE_SNEAK:
  				currentState = STATE_SNEAK;
  				writeDataToLCD('K');
				break;
  				case SURFACE_TURN:
  				currentState = STATE_TURNING;
  				writeDataToLCD('T');
  			
  				break;
  			
  			} // end switch
  		} // end if
  		
  		
  		   for(n=0; n<0x10000; n++);        // delay

  		getADC();
  		
		cruise(); 		
  	
  	break;
  	case STATE_SNEAK:
  	//enable motors unless motion detected

  	if (motionSensor > 900)
  	{
  		//stop
  		motor(0, 0);
  	}
  	else
  	{
  		cruise();
  		
  	}
		if (checkSurface() == 1)
		{
  				switch (currentSurface)
  				{
  				case SURFACE_FLOOR:
  				currentState = STATE_CRUISING;
  				break;
  				
  				case SURFACE_SNAP:
  				currentState = STATE_SNAP;
  				break;
  				
  				case SURFACE_POUNCE:
  				currentState = STATE_POUNCE;
  				break;
  				
  				case SURFACE_SNEAK:
  				currentState = STATE_SNEAK;
				break;
  				case SURFACE_TURN:
  				currentState = STATE_TURNING;
  			
  				break;
  			
  			} // end switch
		}
		
  	
  	break;
  	case STATE_POUNCE:
  	//stay still until motion detected, then snap camera
motor(0, 0);

	for(n=0; n<(1<<21); n++); 
	
  	while (motionSensor > 900)
  	{
  		getADC();
  	}
  	
  	
  	currentState = STATE_SNAP;
  	
  	break;
  	case STATE_SNAP:
  	//trigger camera
  	pPwmCh2->PWMC_CUPDR=0xFF;
  	volatile int u;
  	for(u=0; u<(3<<22); u++){}
  	
  	pPwmCh2->PWMC_CUPDR=0x80;
  	
  	currentState = STATE_CRUISING;
  	
  	break;
  	case STATE_BUMPED:
  	//fire motors in reverse
  	
  	if (GetPort(BUMP_LEFT) == 0 && GetPort(BUMP_RIGHT))
  	{   //manual override
  		currentState = STATE_CRUISING;
  	}
  	else
  	{
  		
  	//	programTimer = 0;
  	//	currentState = STATE_REVERSE;
  		reverse(1000);
  		turn(2000);
  		
  		currentState = STATE_SNAP;
  		cruise(); 
  		
  	}
  	
  	
  	break;
  	
 // 	case STATE_REVERSE:
  	
  	
  	
  	
  	case STATE_TURNING:
  		turn(1000);
  		
  		currentState = STATE_CRUISING;
  		cruise(); 
  		
  		
  	break;			
  				
  }//end switch
  
  
  }//end while
} //end function

  

// -------------------------------------------------------------
// end of module main
// -------------------------------------------------------------



/*
void PWM()
{
	if (currentLeftSpeed != leftSpeed || currentRightSpeed != rightSpeed)
	{
		pPwmc=AT91C_BASE_PWMC;
pPwmc->PWMC_ENA=3; // enable pwm channel 0
//pPwmc->PWMC_ENA=1; // enable pwm channel 0

pPwmCh0=AT91C_BASE_PWMC_CH0;
pPwmCh0->PWMC_CMR=0x020A; // pol
arity 1, PWM_CLK = MCK/1024
//pPwmCh0->PWMC_CMR=0x000A; // polarity 0, PWM_CLK = MCK/1024
pPwmCh0->PWMC_CPRDR=0x100; // period

pPwmCh0->PWMC_CDTYR=0xFF; // duty cycle

pPwmCh1=AT91C_BASE_PWMC_CH1;
//pPwmCh1->PWMC_CMR=0x020A; // polarity 1, PWM_CLK = MCK/1024
pPwmCh1->PWMC_CMR=0x000A; // polarity 0, PWM_CLK = MCK/1024
pPwmCh1->PWMC_CPRDR=0x100; // period

pPwmCh1->PWMC_CDTYR=0xFF; // duty cycle


currentLeftSpeed = leftSpeed;
	currentRightSpeed = rightSpeed;
	
	
	pPwmCh0->PWMC_CUPDR=currentLeftSpeed; // duty cycle
	pPwmCh1->PWMC_CUPDR=currentRightSpeed; // duty cycle
			

	}
	
}

*/