/*================================================================================================
 
 CMtec RoboMSI (MasterSlaveInterface)	copyright 2005			  	          v1.0 (2006-02-21)
 
 Name: Christoffer Martinsson
 E-mail: cm@cmtec.se 

	
================================================================================================*/
//#define F_CPU 8000000UL //internal 8MHz
//#define F_CPU 16000000UL

#include <util/delay.h>
#include <string.h>
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>

#include <lcdDOG.h>
#include <rfID.h>
#include <roboMSP.h>
#include <ping.h>

// Time defenitions for servo generator
#define SERVO_TIMER 		0xEE
#define SERVO_POS_RIGHT_90 	67 //31
#define SERVO_POS_RIGHT_45 	103 //49
#define SERVO_POS_CENTER 	140 //68
#define SERVO_POS_LEFT_45 	174 //86
#define SERVO_POS_LEFT_90 	208 //104

#define SERVO_POS_MOTOR_STOP 	119 //68
#define SERVO_POS_MOTOR_FW  	133//130 //68
#define SERVO_POS_MOTOR_RE  	108//111 //68
#define SERVO_POS_MOTOR_FW_SOFT  	127 //68
#define SERVO_POS_MOTOR_RE_SOFT  	114 //68
#define SERVO_POS_MOTOR_FW_FAST  	134//139//133 //68
#define SERVO_POS_MOTOR_RE_FAST  	104//105//108 //68

// Formula = X * 13  (X = distance in mm)
#define DISTANCE_TO_STOP 5500UL
#define DISTANCE_ONE_METER 6000UL
#define DISTANCE_TWO_METER 26400UL

// Port defenitions for servo generator
#define SERVO_PING PC1
#define SERVO_MOTOR_LEFT PC0
#define SERVO_MOTOR_RIGHT PC2
#define SERVO_PORT PORTC
// Variables for servo generator
uint8_t servoPing = SERVO_POS_CENTER;
uint8_t servoMotorLeft = SERVO_POS_MOTOR_STOP;
uint8_t servoMotorRight = SERVO_POS_MOTOR_STOP;
uint8_t servoCounter = 0;

uint8_t servoMotorLeftReg = SERVO_POS_MOTOR_STOP;
uint8_t servoMotorRightReg = SERVO_POS_MOTOR_STOP;

// Time definitions for Program Timer
#define PROG_TIMER 0xBFFF // 14ms   //0xB1DF // 20ms
// Variables for Program Timer
uint8_t progCounterPing = 0;
uint8_t progCounterPPM = 0;

uint8_t progCounter = 0;
uint8_t progTimer = 0;
uint8_t progPosition = 0;
uint8_t progDoUpdate = 0;

uint16_t tempDistance = 0;
uint16_t tempDistanceToWall = 0;
uint16_t tempLastDistanceToWall = 0;
uint16_t tempLeftDistance = 0;
uint16_t tempRightDistance = 0;

// Variables for main program
uint8_t roboActive = 0;
uint8_t rfEnable = 0;

uint8_t switchOneCheck = 0;
uint8_t switchTwoCheck = 0;

uint8_t regulatorMode = 0;
uint8_t regulatorTimer = 0;
uint8_t regulatorCounter = 0;
uint8_t regulatorMeasureCounter = 0;

const char menuTable[][49] PROGMEM = 
{
	"     CMtec      " 
  	"roboCrawler v1.1" 
  	"                ",

	"     mode 1:    "
	" Follow wall at "
	" a distance(1m) ",

	"     mode 2:    "
	" Drive and void "
	"   any walls    ",

	"     mode 3:    "
	"Measure distance"
	"    forward     ",

	"    mode RF:    "
	" RF actived and " 
	"waiting for com.",
};

void checkCommunication(void);
void showDistanceOnLCD(uint16_t distance, uint8_t mode, uint8_t position);
/*================================================================================================
 Functions
================================================================================================*/
/*------------------------------------------------------------------------------------------------
 initIO
 
 Description: Initialise I/O
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
void initIO (void){
	
	TCCR1B = (1<<CS11); 	// Timer1 prescaler = fc/8
	
	TCNT1H = (uint8_t)(PROG_TIMER>>8);
	TCNT1L = (uint8_t)(PROG_TIMER);

	TCNT2 = SERVO_TIMER;

	TIMSK |= (1<<TOIE1)|(1<<TOIE2);	// Timer1 interrupt enable
	DDRC |= (1<<PC0)|(1<<PC1)|(1<<PC2);

	DDRA &= ~((1<<PA4)|(1<<PA5)|(1<<PA6)|(1<<PA7)); // Configuration button pins as input
	PORTA |= (1<<PA4)|(1<<PA5)|(1<<PA6)|(1<<PA7); // Pull-up on configuration button pins
	
	progPosition = 0;
}
/*================================================================================================
 Interrupt
================================================================================================*/
/*------------------------------------------------------------------------------------------------
 SIG_OVERFLOW1
 
 Description: Timer1 overflow interrupt. Used as program timer
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
ISR (TIMER1_OVF_vect)
{	
	// Program for update PPM pulses to all servos
	if (++progCounterPPM >= 2)
	{
		// Start for servo PPM pulses
		servoCounter = 0;
		SERVO_PORT |= (1<<SERVO_PING)|(1<<SERVO_MOTOR_LEFT)|(1<<SERVO_MOTOR_RIGHT);
		TCCR2 |= (1<<CS21);		// Timer2 start (prescaler = fc/8)
		progCounterPPM = 0;	
	}
	
	// Trigg program routine
	progDoUpdate = 1;

	// Set timer value
	TCNT1H = (uint8_t)(PROG_TIMER>>8);
	TCNT1L = (uint8_t)(PROG_TIMER);
}
/*------------------------------------------------------------------------------------------------
 SIG_OVERFLOW2
 
 Description: Timer1 overflow interrupt. Used as servo (PPM) timer
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
ISR (TIMER2_OVF_vect)
{
	uint8_t done = 1;

	// Check each servo output if pulselength is reached
	if(servoCounter >= servoPing)
		SERVO_PORT &= ~(1<<SERVO_PING);
	else
	{
		done = 0;
		asm volatile("NOP");		// Correction for constant interrupt length
	}
			                 
	if(servoCounter >= servoMotorLeft)
		SERVO_PORT &= ~(1<<SERVO_MOTOR_LEFT);
	else
	{
		done = 0;
		asm volatile("NOP");		// Correction for constant interrupt length
	}
		
	if(servoCounter >= servoMotorRight)
		SERVO_PORT &= ~(1<<SERVO_MOTOR_RIGHT);
	else
	{
		done = 0;
		asm volatile("NOP");		// Correction for constant interrupt length
	}

	if(done)
		TCCR2 &= ~(1<<CS21);		// Timer1 stop
	else
	{	
		servoCounter++;
		asm volatile("NOP");		// Correction for constant interrupt length
		asm volatile("NOP");
	}
	TCNT2 = SERVO_TIMER;
}
/*------------------------------------------------------------------------------------------------
 activateRobot
 
 Description:
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
void activateRobot(uint8_t mode)
{
	lcdSetLayout(0);
	lcdWriteStringP(menuTable[mode]);

	progTimer = 200;

	roboActive = mode;
}
/*------------------------------------------------------------------------------------------------
 deactivateRobot
 
 Description: 
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
void deactivateRobot(void)
{	
	roboActive = 0;
	
	servoMotorLeft = SERVO_POS_MOTOR_STOP;
	servoMotorRight = SERVO_POS_MOTOR_STOP;
	servoPing = SERVO_POS_CENTER;
	
	rfIdDisable();
}


/*------------------------------------------------------------------------------------------------
 updateProgram
 
 Description: 
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
void updateProgram(void)
{
	// Check radio communication
	if(rfEnable)
		checkCommunication();

	// Program for robot AI	
	if (roboActive == 1)
	{	
		// Program(2) for random RFID serach
		if (++progCounter >= progTimer)
		{
			switch (progPosition)
			{
				case 0:					
					// Kör frammåt		
					lcdClearDisplay();
					lcdSetLayout(1);
					lcdSetPos(0x00);
					lcdWriteString("Drive: FORWARD  ");	

					servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
					servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
			
					progTimer = 20;
					progPosition++;

				break;

				case 1:
					// Kör frammåt till dess att avståndet mot väggen är 1meter
					servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
					servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;					
					
					pingSendPing();		
					progTimer = 4;
					
					if(pingGetReady())
					{
						
						tempDistance = pingGetDistance();						

						if(tempDistance < DISTANCE_ONE_METER)
						{ 
							showDistanceOnLCD(0,0,2);
							progTimer = 1;
							progPosition++;
						}
						else
						{
							showDistanceOnLCD(tempDistance-DISTANCE_ONE_METER,0,2);
						}
					}	
					
				break;
				
				case 2:
					// Bromsa (backa något)
					lcdSetPos(0x00);
					lcdWriteString("Drive: STOP     ");	
					// Stop and breake
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;
					servoMotorLeft = SERVO_POS_MOTOR_RE;
					servoMotorRight = SERVO_POS_MOTOR_RE;

					progTimer = 40;
					progPosition++;
		
				break;

				case 3:
					// Stanna
					
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;

					progTimer = 1;
					regulatorMeasureCounter = 0;
					progPosition++;
		
				break;
				
				case 4:
					// Vrid servo 45grader vänster (från centrum)
					servoPing = SERVO_POS_LEFT_45;
					progTimer = 20;
					progPosition++;
		
				break;
				
				case 5:
					// Mät och spara avstånd
					pingSendPing();
					progTimer = 4;
					
					if(pingGetReady())
					{
						tempLeftDistance = pingGetDistance();
						progTimer = 1;
						progPosition++;
					}
		
				break;

				case 6:
					// Vrid servo 45grader höger (från centrum)
					servoPing = SERVO_POS_RIGHT_45;
					progTimer = 40;
					progPosition++;
		
				break;
				
				case 7:
					// Mät och spara avstånd
					pingSendPing();
					progTimer = 4;
					
					if(pingGetReady())
					{
						tempRightDistance = pingGetDistance();
						progTimer = 1;
						progPosition++;
					}
		
				break;

				case 8:
					
					servoPing = SERVO_POS_CENTER;						
					
					if(tempLeftDistance > tempRightDistance)
					{
						regulatorMode = 0; // Mäter åt höger	
					}
					else
					{
						regulatorMode = 1; // Mäter åt vänster
					}
					
					progTimer = 50;
					progPosition++;	
					
				break;	
				
				case 9:
					// Mät och spara avstånd
					pingSendPing();
					progTimer = 4;
					
					if(pingGetReady())
					{
						lcdSetPos(0x00);
						lcdWriteString("Align to wall   ");	
						tempLastDistanceToWall = pingGetDistance();
						tempDistanceToWall = tempLastDistanceToWall;
						progTimer = 1;
						progPosition++;
					}
		
				break;
				
				case 10:
					// Reglera in sig vinkerät mot väggen
					// Vrider sig ett steg
					if (regulatorMode == 0) // Tittar höger
					{	
						// sväng höger
						servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
						servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
						progTimer = 7;
					}	
					else if (regulatorMode == 1) // Tittar vänster
					{
						// sväng vänster	
						servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
						servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
						progTimer = 7;
					}
					
					progPosition++;
					
				break;
				
				case 11:	
					// Reglera in sig vinkerät mot väggen
					// Kontrollerar avstånd
					pingSendPing();		
					progTimer = 4;
					
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;
					
					if(pingGetReady())
					{
						
						tempDistance = pingGetDistance();
						
						if(tempDistance > tempLastDistanceToWall)
						{	
							lcdSetPos(0x00);
							lcdWriteString("  ^-- (1m) --^  ");	

							progTimer = 1;
							progPosition++;
						}
						else
							progPosition--;
							
						tempLastDistanceToWall = tempDistance;
					}	
					
				break;
					
				case 12:	
					// Reglera in position 1meter från väggen
					pingSendPing();		
					progTimer = 4;
					
					if(pingGetReady())
					{
						
						tempDistance = pingGetDistance();
						
						if(tempDistance > DISTANCE_ONE_METER)
							showDistanceOnLCD((tempDistance-DISTANCE_ONE_METER),0,2);
						else
							showDistanceOnLCD((DISTANCE_ONE_METER-tempDistance),1,2);

						if(tempDistance < (DISTANCE_ONE_METER - 200))
						{	
							// Kör frammåt (öka hastigheten långsamt)
							if(servoMotorLeft > SERVO_POS_MOTOR_RE_SOFT)
							{
								servoMotorLeft--;
								servoMotorRight--;
							}
								
							regulatorMeasureCounter = 0;
						}
						else if(tempDistance > (DISTANCE_ONE_METER + 200))
						{
							// Kör bakåt (öka hastigheten långsamt)
							if(servoMotorLeft < SERVO_POS_MOTOR_FW_SOFT)
							{
								servoMotorLeft++;
								servoMotorRight++;
								
							}
							
							regulatorMeasureCounter = 0;
						}
						else
						{
							servoMotorLeft = SERVO_POS_MOTOR_STOP;
							servoMotorRight = SERVO_POS_MOTOR_STOP;
							
							if(++regulatorMeasureCounter == 5)
							{	
								lcdSetPos(0x00);
								lcdWriteString("Posistion locked");	
								
								servoMotorLeft = SERVO_POS_MOTOR_STOP;
								servoMotorRight = SERVO_POS_MOTOR_STOP;
								tempDistanceToWall = tempDistance;
								progTimer = 50;
								regulatorMeasureCounter = 0;
								progPosition++;
							}
						}
					}	
					
				break;

				case 13:
					// Vrider 90grader
					servoPing = SERVO_POS_CENTER;						

					if(tempLeftDistance > tempRightDistance)
					{		
						lcdSetPos(0x00);
						lcdWriteString("Drive: Turn < ");	
								
						servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
						servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
						servoPing = SERVO_POS_RIGHT_90;
						regulatorMode = 0; // Mäter åt höger
						progTimer = 50;
					}
					else
					{
						lcdSetPos(0x00);
						lcdWriteString("Drive: Turn > ");	

						servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
						servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
						servoPing = SERVO_POS_LEFT_90;
						regulatorMode = 1; // Mäter åt vänster
						progTimer = 50;
					}
						
					progPosition++;
					
				break;	

				case 14:
					// Stanna 
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;

					progTimer = 10;
					regulatorTimer = 2;
					progPosition++;
					progPosition++;
					progPosition++;
		
				break;

				case 15:					
					// Kör frammåt	
					lcdSetPos(0x00);
					lcdWriteString("Drive: FORWARD  ");	

					servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
					servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
			
					progTimer = 20;
					progPosition++;

				break;
				
				case 16:						
					// Kör frammåt
					servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
					servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
			
					progTimer = 10;
					progPosition++;

				break;

				case 17:
						// Mäter avstånd
						pingSendPing();		
						progTimer = 3;				

						if(pingGetReady())
						{
						
							tempDistance = pingGetDistance();
							progTimer = 1;
							progPosition++;					
							
						}				
					
				break;

				case 18:
					// Reglerar 1meter från väggen
					if(++regulatorCounter >= regulatorTimer)
					{
						progPosition++;
						progTimer = 1;
						regulatorCounter = 0;
					}
					else
					{	
						uint8_t calculateProgTimer = 0;
						if (regulatorMode == 0) // Tittar höger
						{
							lcdSetPos(0x00);
							lcdWriteString(" ]--- (1m) --->|");						

							if(tempDistance > DISTANCE_ONE_METER)
								showDistanceOnLCD((tempDistance-DISTANCE_ONE_METER),3,2);
							else
								showDistanceOnLCD((DISTANCE_ONE_METER-tempDistance),2,2);

							if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER -1000)))
							{	
								// sväng vänster	
								servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
								servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
								
								calculateProgTimer = ((DISTANCE_ONE_METER - tempDistance)/40);
								
								if(calculateProgTimer < 15)
									progTimer = calculateProgTimer;
								else
									progTimer = 15;
								
							}
							else if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER -10)))
							{	
								// sväng vänster	
								servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
								servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
								
								progTimer = 5;
								
							}
							else  if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER+1000)))
							{	
								// sväng höger
								servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
								servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
								
								calculateProgTimer = ((tempDistance - DISTANCE_ONE_METER)/40);
								
								if(calculateProgTimer < 15)
									progTimer = calculateProgTimer;
								else
									progTimer = 15;
								
							}
							else  if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER+10)))
							{	
								// sväng höger
								servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
								servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
								
								progTimer = 5;
								
							}
							else
							{
								servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
								servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
								progTimer = 4;
							}
						}
						
						else if (regulatorMode == 1) // Tittar vänster
						{
							lcdSetPos(0x00);
							lcdWriteString("|<--- (1m) ---[ ");

							if(tempDistance > DISTANCE_ONE_METER)
								showDistanceOnLCD((tempDistance-DISTANCE_ONE_METER),2,2);
							else
								showDistanceOnLCD((DISTANCE_ONE_METER-tempDistance),3,2);

							if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER-1000)))
							{		
								// sväng höger
								servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
								servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
								
								calculateProgTimer = ((DISTANCE_ONE_METER - tempDistance)/40);
								
								if(calculateProgTimer < 15)
									progTimer = calculateProgTimer;
								else
									progTimer = 15;
							
							}
							else if((tempDistance < tempLastDistanceToWall) && (tempDistance < (DISTANCE_ONE_METER-10)))
							{		
								// sväng höger
								servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
								servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
								
								progTimer = 5;
							
							}
							else  if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER +1000)))
							{
								// sväng vänster	
								servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
								servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
								
								calculateProgTimer = ((tempDistance - DISTANCE_ONE_METER)/40);
								
								if(calculateProgTimer < 15)
									progTimer = calculateProgTimer;
								else
									progTimer = 15;
							}	
							else  if((tempDistance > tempLastDistanceToWall) && (tempDistance > (DISTANCE_ONE_METER +10)))
							{
								// sväng vänster	
								servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
								servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
								
								progTimer = 5;
							}	
							else
							{
								servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
								servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;
								progTimer = 4;
							}
						}
						tempLastDistanceToWall = tempDistance;
						progPosition--;
						progPosition--;
					}
	
				break;

				case 19:					

					servoPing = SERVO_POS_CENTER;
					
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;
					
					progTimer = 35;
					progPosition++;

				break;

				case 20:				
							
					progTimer = 3;
					pingSendPing();	

					if(pingGetReady())
					{
						
						tempDistance = pingGetDistance();		

						if(tempDistance < DISTANCE_ONE_METER)
						{
							showDistanceOnLCD(0,0,1);
							progPosition = 2;
						}
						else if (tempDistance < (DISTANCE_ONE_METER*2))
						{	
							showDistanceOnLCD(tempDistance-DISTANCE_ONE_METER,0,1);
							regulatorTimer = 2;
							progPosition++;
						}
						else 
						{	
							showDistanceOnLCD(tempDistance-DISTANCE_ONE_METER,0,1);
							regulatorTimer = 10;
							progPosition++;
						}
					}	
					
				break;

				case 21:
					
					if (regulatorMode == 0) // Tittar höger
					{
						servoPing = SERVO_POS_RIGHT_90;
					}
					else
					{
						servoPing = SERVO_POS_LEFT_90;
					}
					
					progTimer = 5;
					progPosition = 15;
		
				break;
	
			}

			progCounter = 0;
		}
	}

	// Program for robot AI	
	if (roboActive == 2)
	{	
		// Program(2) for random RFID serach
		if (++progCounter >= progTimer)
		{
			
			switch (progPosition)
			{
				case 0:					

					lcdClearDisplay();
					lcdSetLayout(1);

					progTimer = 1;
					progPosition++;

				break;
				
				case 1:					

					lcdSetPos(0x00);
					lcdWriteString("Drive: FORWARD  ");			

					servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
					servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
			
					progTimer = 20;
					progPosition++;

				break;

				case 2:
					
					servoMotorLeft = SERVO_POS_MOTOR_FW_SOFT;
					servoMotorRight = SERVO_POS_MOTOR_FW_SOFT;					
					
					pingSendPing();		
					progTimer = 3;
					
					if(pingGetReady())
					{
						
						tempDistance = pingGetDistance();
						
						if(tempDistance < DISTANCE_TO_STOP)
						{				 
							showDistanceOnLCD(0,0,2);
							progPosition++;
						}
						else
						{
							showDistanceOnLCD(tempDistance-DISTANCE_TO_STOP,0,2);
						}
						
					}	
					
				break;

				case 3:
					
					lcdSetPos(0x00);
					lcdWriteString("Drive: STOP     ");	
			
					// Stop and breake
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;
					servoMotorLeft = SERVO_POS_MOTOR_RE;
					servoMotorRight = SERVO_POS_MOTOR_RE;

					progTimer = 50;

					progPosition++;
		
				break;

				case 4:
					
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;

					progTimer = 2;

					progPosition++;
		
				break;

				case 5:

					servoPing = SERVO_POS_LEFT_45;

					progTimer = 20;

					progPosition++;
		
				break;

				case 6:
					
					pingSendPing();
					progTimer = 4;
					
					if(pingGetReady())
					{
						tempLeftDistance = pingGetDistance();
						progTimer = 20;
						progPosition++;
					}
		

				break;

				case 7:
					
					servoPing = SERVO_POS_RIGHT_45;

					progTimer = 40;

					progPosition++;
		
				break;

				case 8:
					
					pingSendPing();
					progTimer = 4;
					
					if(pingGetReady())
					{
						tempRightDistance = pingGetDistance();

						servoPing = SERVO_POS_CENTER;						

						if((tempLeftDistance < DISTANCE_TO_STOP ) 
							&& (tempRightDistance < DISTANCE_TO_STOP))
						{
							lcdSetPos(0x00);
							lcdWriteString("Drive: Turn <<< ");				

							servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
							servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
							progTimer = 70;
						}
						else if(tempLeftDistance > tempRightDistance)
						{
							lcdSetPos(0x00);
							lcdWriteString("Drive: Turn <   ");				
							
							servoMotorLeft = SERVO_POS_MOTOR_RE_FAST;
							servoMotorRight = SERVO_POS_MOTOR_FW_FAST;
							progTimer = 40;
						}
						else
						{
							lcdSetPos(0x00);
							lcdWriteString("Drive: Turn >   ");

							servoMotorLeft = SERVO_POS_MOTOR_FW_FAST;
							servoMotorRight = SERVO_POS_MOTOR_RE_FAST;
							progTimer = 40;
						}
						
						progPosition++;
					}
		
				break;

				case 9:
					
					servoMotorLeft = SERVO_POS_MOTOR_STOP;
					servoMotorRight = SERVO_POS_MOTOR_STOP;
					
					progTimer = 10;
					progPosition = 1;
		
				break;
			}

			progCounter = 0;
		}
	}
	
	if (roboActive == 3)
	{	
		// Program(2) for random RFID serach
		if (++progCounter >= progTimer)
		{
			
			switch (progPosition)
			{
				case 0:

					lcdClearDisplay();
					lcdSetLayout(1);
					lcdSetPos(0x00);
					lcdWriteString("Servo: forward ");
					lcdSetPos(0x10);
					lcdWriteString("Distance: ");

					progTimer = 1;
					progPosition++;

				break;

				case 1:
						
					pingSendPing();		
					progTimer = 10;
					
					if(pingGetReady())
					{
						
						tempDistance = pingGetDistance();
						lcdSetPos(0x1A);
						lcdWriteHexAsDecimal(tempDistance);			
						
					}	
					
				break;	
			}

			progCounter = 0;
		}
	}
}
/*------------------------------------------------------------------------------------------------
 showDistanceOnLCD
 
 Description: 
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
void showDistanceOnLCD(uint16_t distance, uint8_t mode, uint8_t position)
{
	if(position == 1)
		lcdSetPos(0x00);
	else if(position == 2)
		lcdSetPos(0x10);
	else if(position == 3)
		lcdSetPos(0x20);

	if(mode == 0) // Both arrow pointing in
	{
		if(distance > 32000)
		{
			lcdWriteString("                "); 
		}
		else if(distance > 16000)
		{
			lcdWriteString(">              <"); 
		}
		else if(distance > 8000)
		{
			lcdWriteString(">>            <<"); 
		}
		else if(distance > 4000)
		{
			lcdWriteString(">>>          <<<"); 
		}
		else if(distance > 2000)
		{
			lcdWriteString(">>>>        <<<<"); 
		}
		else if(distance > 1000)
		{
			lcdWriteString(">>>>>      <<<<<"); 
		}
		else if(distance > 500)
		{
			lcdWriteString(">>>>>>    <<<<<<"); 
		}
		else if(distance > 100)
		{
			lcdWriteString(">>>>>>>  <<<<<<<"); 
		}
		else 
		{
			lcdWriteString(">>>>>>>[]<<<<<<<"); 
		}
	}
	else if(mode == 1) // Both arrow pointing Out
	{
		if(distance > 32000)
		{
			lcdWriteString("                "); 
		}
		else if(distance > 16000)
		{
			lcdWriteString("      <  >      "); 
		}
		else if(distance > 8000)
		{
			lcdWriteString("     <<  >>     "); 
		}
		else if(distance > 4000)
		{
			lcdWriteString("    <<<  >>>    "); 
		}
		else if(distance > 2000)
		{
			lcdWriteString("   <<<<  >>>>   "); 
		}
		else if(distance > 1000)
		{
			lcdWriteString("  <<<<<  >>>>>  "); 
		}
		else if(distance > 500)
		{
			lcdWriteString(" <<<<<<  >>>>>> "); 
		}
		else if(distance > 100)
		{
			lcdWriteString("<<<<<<<  >>>>>>>"); 
		}
		else 
		{
			lcdWriteString("<<<<<<<[]>>>>>>>"); 
		}
	}
	else if(mode == 2) // Left arrow pointing out.
	{
		if(distance > 6400)
		{
			lcdWriteString("<<<<<<<         "); 
		}
		else if(distance > 3200)
		{
			lcdWriteString(" <<<<<<         "); 
		}
		else if(distance > 1600)
		{
			lcdWriteString("  <<<<<         "); 
		}
		else if(distance > 800)
		{
			lcdWriteString("   <<<<         "); 
		}
		else if(distance > 400)
		{
			lcdWriteString("    <<<         "); 
		}
		else if(distance > 200)
		{
			lcdWriteString("     <<         "); 
		}
		else if(distance > 30)
		{
			lcdWriteString("      <         "); 
		}
		else 
		{
			lcdWriteString("       []       "); 
		}
	}
	else if(mode == 3) // Right arrow pointing out.
	{
		if(distance > 6400)
		{
			lcdWriteString("         >>>>>>>"); 
		}
		else if(distance > 3200)
		{
			lcdWriteString("         >>>>>> "); 
		}
		else if(distance > 1600)
		{
			lcdWriteString("         >>>>>  "); 
		}
		else if(distance > 800)
		{
			lcdWriteString("         >>>>   "); 
		}
		else if(distance > 400)
		{
			lcdWriteString("         >>>    "); 
		}
		else if(distance > 200)
		{
			lcdWriteString("         >>     "); 
		}
		else if(distance > 30)
		{
			lcdWriteString("         >      "); 
		}
		else 
		{
			lcdWriteString("       []       "); 
		}
	}
}
/*------------------------------------------------------------------------------------------------
 checkCommunication
 
 Description: Check for incoming data packets.
 Input: -
 Output: -
------------------------------------------------------------------------------------------------*/
void checkCommunication(void){


	if (rfIdGetTagPresent())
	{	
		uint8_t *tempPointer = rfIdGetTag();
		
		lcdSetPos(0x13);
		lcdWriteString(tempPointer);
		roboMSPSetData(tempPointer);
		
	}

	switch (roboMSPGetActiveStatus())
	{
		case ACTIVATE_P1:
			activateRobot(1);
			rfIdEnable();
		break;	
		
		case ACTIVATE_P2:
			activateRobot(2);
			rfIdEnable();
		break;
			
		case DEACTIVATE:
			deactivateRobot();
		break;
			
		case ACK:
			rfIdClearBuffer();
		break;
	}
	
}
/*================================================================================================
 Main
================================================================================================*/
int main (void){
	
	cli();					// Disable global interrupt  
	
	initIO();
	lcdInit();
	rfIdInit();
	roboMSPInit();
	pingInit();
	
	lcdSetIntensity(50);
	lcdSetLayout(2);
	lcdWriteStringP(menuTable[0]);
		
	rfIdDisable();
	roboMSPDisable();
	
	sei();					// Enable global interrupt
	
	while(1){
	
		if(progDoUpdate)
		{
			progDoUpdate = 0;
			updateProgram();
		}		

		// Active-mode 1 (left switch lower position)
		if (!bit_is_set(PINA,PA4))
		{	
			if(switchOneCheck)
			{
				activateRobot(3);
				switchOneCheck = 0;
			}
		}
		// Active-mode 2 (left switch middle position)
		else if ((bit_is_set(PINA,PA5)) && (bit_is_set(PINA,PA4)))
		{
			if(!switchOneCheck)
			{
				deactivateRobot();
				rfEnable = 0;
				switchOneCheck = 1;
				main();
			}
		}
		// Active-mode 3 (left switch upper position)
		else if (!bit_is_set(PINA,PA5))
		{	
			if(switchOneCheck)
			{
				lcdSetLayout(0);
				lcdWriteStringP(menuTable[4]);
				rfEnable = 1;
				rfIdEnable();
				roboMSPEnable();
				switchOneCheck = 0;
			}
		}

		// Active-mode 1 (left switch lower position)
		if (!bit_is_set(PINA,PA6))
		{	
			if(switchTwoCheck)
			{
				activateRobot(1);
				switchTwoCheck = 0;
			}
		}
		// Active-mode 2 (left switch middle position)
		else if ((bit_is_set(PINA,PA7)) && (bit_is_set(PINA,PA6)))
		{
			if(!switchTwoCheck)
			{
				deactivateRobot();
				rfEnable = 0;
				switchTwoCheck = 1;
				main();
			}
		}
		// Active-mode 3 (left switch upper position)
		else if (!bit_is_set(PINA,PA7))
		{	
			if(switchTwoCheck)
			{
				activateRobot(2);
				switchTwoCheck = 0;
			}
		}
		
	}
	
	
    return (0);
}
/*================================================================================================
 End
================================================================================================*/