/**************************************************************************************
 * MAIN 
 * Created By: Gorang Gandhi
 * 4/24/06
 * Atmega 128
 ************************************************************************************** 
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/pgmspace.h>
#include <stdio.h>
#include <stdlib.h>

//=====================================================================================
// PORTC- LCD (pin0-6)
// PORTF- IR Range finders (pin0,6)
// PORTA- PHOTO (pin0-3)
// PORTE- Output Compares (pin3-5)
// PORTB- Motors (pin1-4), Speaker (pin5)
// PORTD- IR Can (pin0)
// PORTG- Bump Switches (pin0-2)
// PORTD- UART1- AVRCam (pin3,2)
//=====================================================================================

// LCD Screen =========================================================================
#define LCD PORTC
#define LCD_DDR DDRC

// IR ================================================================================
#define IR PORTF
#define IR_DDR DDRF

// Photoreflector ====================================================================
#define PHOTO PORTA
#define PHOTO_DDR DDRA
#define PHOTO_PIN PINA&0x0F // Only least significant nibble

// Motors=============================================================================
// PortB- B0B1B2B3 - A+A-B+B- - A is left motor, B is right motor
#define MOTOR PORTB 
#define MOTOR_DDR DDRB
#define MOTOR_L OCR3A	// For PWM of Aenable of motor driver- left motor- pin3 port E
#define MOTOR_R OCR3B	// For PWM of Aenable of motor driver- right motor- pin4 port E
#define FORWARD 0x14	// 0001 0100
#define BACKWARD 0X0A	// 0000 1010
#define LEFT 0x0C		// 0000 1100
#define RIGHT 0x12		// 0001 0010
#define STOP 0x00		// 0000 0000
#define SLOW 5000
#define MEDIUM 10000 
#define FAST 15000

// Servo===============================================================================
#define SERVO OCR3C		// For PWM of servo motor- pin5 port E
#define S_CLOSE 1370	// Closes servo gripper
#define S_OPEN 2000  // Fully opens servo gripper
#define S_MIDDLE 1700 // middle servo gripper position

// Bump Switches=======================================================================
#define BUMP PORTG // Bump switches will bo on PORTG
#define BUMP_DDR DDRG 
#define BUMP_PIN PING

// IR Can==============================================================================
#define IRCAN PORTD // Pin0
#define IRCAN_DDR DDRD
#define IRCAN_PIN PIND

// Speaker=============================================================================
#define SPEAKER OCR1A // Only pin5 of PORTB
#define SPEAKER_DDR DDRB
#define SPEAKER_F ICR1


volatile uint16_t ms_count; 
volatile uint8_t IR_flag;
volatile uint8_t BUMP_flag;
volatile uint8_t PHOTO_flag; 
volatile uint8_t IR_output_d;                                
volatile uint8_t BUMP_output_d;
volatile uint8_t PHOTO_output_d;
volatile uint8_t IR_output_s;
volatile uint16_t BUMP_output_s;
volatile uint16_t PHOTO_output_s;
volatile uint8_t motor_input_d;
volatile uint16_t motor_input_s;
volatile uint8_t MODE; // MODE = 0 going to/from soda pickup; MODE = 1 picking up soda
volatile uint8_t SODA; // 1-Coke, 2-Pepsi, 3-Sprite
volatile uint8_t FIRST_PASS; // 0 if first pass, 1 if not
volatile uint8_t AVRCAM_AVG;
volatile uint8_t CAM_counter;
volatile uint8_t IR_val;
volatile uint8_t PATH; // 1-Left path, 2 Center Path, 3 Right Path
						

// TIMER ==================================================================================
//=========================================================================================

void ms_sleep(uint16_t ms)
// ms_sleep() - delay for specified number of milliseconds
{
	  TCNT0  = 0;
	  ms_count = 0;
	  while (ms_count != ms)
		;
}

SIGNAL(SIG_OUTPUT_COMPARE0)
/* 
 * millisecond counter interrupt vector 
 */
{
	ms_count++;
}

void init_timer(void)
{
  /*
   * Initialize timer0 to generate an output compare interrupt, and
   * set the output compare register so that we get that interrupt
   * every millisecond.
   */
	  TIFR  |= _BV(OCIE0); // Clears Interupt flag
	  TCCR0  = _BV(WGM01)|_BV(CS02)|_BV(CS00); /* CTC, prescale = 128 */
	  TCNT0  = 0;
	  TIMSK |= _BV(OCIE0);    /* enable output compare interrupt */
	  OCR0   = 125;          /* match in 1 ms */
}

// LCD ====================================================================================
//=========================================================================================
void lcd_init(void)
{
/*   Intializes the LCD
 */
 
	 ms_sleep(20);  
	 LCD = 0x13;
	 LCD = 0x03;
	 ms_sleep(5);
	 LCD = 0x13;
	 LCD = 0x03;
	 ms_sleep(1);
	 LCD = 0x13;
	 LCD = 0x03;
	 ms_sleep(5);
	 LCD = 0x12;
	 LCD = 0x02; // 4 Bit Operation
	 ms_sleep(1);
	 
	 LCD = 0x12;
	 LCD = 0x02; // $28- Two Line
	 LCD = 0x18;
	 LCD = 0x08;
	 ms_sleep(2);
	 
	 LCD = 0x10; 
	 LCD = 0x00; // $0F- Display, Cursor, Blink
	 LCD = 0x1F;
	 LCD = 0x0F;
	 ms_sleep(2);
	 
	 LCD = 0x10;
	 LCD = 0x00; // $01- Clear screen, cursor home
	 LCD = 0x11;
	 LCD = 0x01;
	 ms_sleep(2);
	 
	 LCD = 0x10;
	 LCD = 0x00; // $06- Increment cursor to right
	 LCD = 0x16;
	 LCD = 0x06;
	 ms_sleep(2);
 
 }
 
int lcd_char(char character)
 // Displays a character to the LCD
 {
	uint8_t MS_Nibble;
	uint8_t LS_Nibble;
	
	MS_Nibble = (character&0xF0)>>4; // 0000xxxx- MS Nibble
	LS_Nibble = character&0x0F; // 0000xxxx- LS Nibble
	
	LCD = MS_Nibble|0x50;  	// RS,E = 1
	LCD = MS_Nibble|0x40;  // RS = 1, E = 0
	ms_sleep(2);
	LCD = LS_Nibble|0x50;
	LCD = LS_Nibble|0x40;
	ms_sleep(2);
	
	return character;
 }
 
 void lcd_string(char message[])
// Displays a string to the LCD
{
	int k;
	
	for(k=0;k<21;k++)
	{
		if(message[k] == '\0' || message[k] == '\n')	
		{
			break;
		}
		
		lcd_char(message[k]);
	}

}

void lcd_clear_display(void)
// Clears the entire LCD display
{
	 LCD = 0x10;
	 LCD = 0x00; // $01- Clear screen, cursor home
	 LCD = 0x11;
	 LCD = 0x01;
	 ms_sleep(2);
}

void lcd_goto_line(int line)
// Goes to line 1 or 2 specified by line
{
	switch(line)
	{
		case(1):
		{
			LCD = 0x10;
			LCD = 0x00; // $02- cursor home
			LCD = 0x12;
			LCD = 0x02;
			ms_sleep(2);
		}
		case(2):
		{
			LCD = 0x1C;
			LCD = 0x0C; // $C0- second row
			LCD = 0x10;
			LCD = 0x00;
			ms_sleep(1);
		}
	}
}

void lcd_goto_pos(int pos)
// Goes to horizontal position. 0-19
{
	int i;
	
	 LCD = 0x10;
	 LCD = 0x00; // $02- cursor home
	 LCD = 0x12;
	 LCD = 0x02;
	 ms_sleep(2);
	
	for(i=0;i<pos;i++)
	{
		LCD = 0x11;
		LCD = 0x01; // $14- shift cursor right
		LCD = 0x14;
		LCD = 0x04;
		ms_sleep(1);
	}
}

void banner(void)
// Displays a beggining banner
{
	lcd_clear_display();
	
	printf("BARGHLES!!!!");
	
	ms_sleep(3000);
	
}

// ADC==========================================================================================
// =============================================================================================
void adc_init(void)
 /* initialize A/D converter
 *
 * Initialize A/D converter to free running, start conversion, use
 * internal 5.0V reference, pre-scale ADC clock to 125 kHz (assuming
 * 16 MHz MCU clock) 
 */
{
  IR = 0x00;
  
  ADMUX = _BV(REFS0);
  ADCSR = _BV(ADEN)|_BV(ADSC)|_BV(ADFR) | _BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
}


void adc_chsel(uint8_t channel)
 /*A/D Channel Select
 *
 * Select the specified A/D channel for the next conversion
 */
{
  ADMUX = (ADMUX & 0xe0) | (channel & 0x07);
}

void adc_wait(void)
 /* adc_wait() - A/D Wait for conversion
 *
 * Wait for conversion complete.
 */
{
  /* wait for last conversion to complete */
  while ((ADCSR & _BV(ADIF)) == 0)
    ;
}


void adc_start(void)
 /* adc_start() - A/D start conversion
 *
 * Start an A/D conversion on the selected channel
 */
{
  
  ADCSR |= _BV(ADIF); /* clear conversion, start another conversion */
}

uint16_t adc_read(void)
/* adc_read() - A/D Converter - read channel
 *
 * Read the currently selected A/D Converter channel.
 */
{
  return ADC;
}


uint16_t adc_readn(uint8_t channel, uint8_t num)
/*
 * adc_readn() - A/D Converter, read multiple times and average
 *
 * Read the specified A/D channel 'n' times and return the average of
 * the samples 
 */
{
  uint16_t t;
  uint8_t i;

  adc_chsel(channel);
  adc_start();
  adc_wait();

  adc_start();

  /* sample selected channel n times, take the average */
  t = 0;
  for (i=0; i<num; i++) 
  {
    adc_wait();
    t += adc_read();
    adc_start();
  }

  /* return the average of n samples */
  return t / num;
}

// Motor===============================================================================================
//=====================================================================================================

void motor_init(void)
// Initializes motors: Enable OC3A, OC3B, OC3C.  3 Mhz frequency.  
// ICR3 will be the TOP.  clk(io)/8.
{
	TCCR3A = 0xA8; //10101000- Clear OCA,B,C on compare match- non-inv PWM
	TCCR3B = 0x12; //00010010- Mode 8 - PWM Phase/Freq Correct, clk(io)/8
	ICR3 = 20000; // PWM = 50 Hz. 1/(16 MHz/8)*2*20000 = 1/50
	TCNT3 = 0x00; // Used to be TCNT2?

	MOTOR_L = 0; // Not moving
	MOTOR_R = 0;
	SERVO = S_MIDDLE; // sets OC3C to 1700
	
	PORTE = 0x00;
}

void motor_move(uint8_t direction, uint16_t speed)
// Moves in a certain direction at a certian speed. 
// Directions- 1-FW,2-BW,3-L,4-R, Speeds- 0 to 20000
{
	int increment_l = 0;
	int increment_r = 0;
		
	if(MOTOR_L > speed)
	{
		increment_l = -1;
	}
	if(MOTOR_L < speed)
	{
		increment_l = 1;
	}
	if(MOTOR_L == speed)
	{
		increment_l = 0;
	}
	if(MOTOR_R > speed)
	{
		increment_r = -1;
	}
	if(MOTOR_R < speed)
	{
		increment_r = 1;
	}
	if(MOTOR_R == speed)
	{
		increment_r = 0;
	}
		
	if(direction == 1)
	{
		MOTOR = FORWARD;		
	}
	if(direction == 2)
	{
		MOTOR = BACKWARD;	
	}
	if(direction == 3)
	{
		MOTOR = LEFT;
	}
	if(direction == 4)
	{
		MOTOR = RIGHT;
	}
	
	while(MOTOR_L != speed || MOTOR_R !=speed)
	{
		if(MOTOR_L != speed)
		{
			MOTOR_L = MOTOR_L + increment_l;
		}
			
		if(MOTOR_R != speed)
		{
			MOTOR_R = MOTOR_R + increment_r;
		}
				
		ms_sleep(1); // 3 too long
	}

}


// Speaker ===========================================================================================
//====================================================================================================

void speaker_init(void)
// Initializes speakers: Enable OC1A. clk(io)/8.
{

	TCCR1A = 0x54; //01010100- Toggle OCA on compare match
	TCCR1B = 0x1A; //00011010- Mode 12 - CTC- Clear counter on match w/ ICR1, clk(io)/8
	TCNT1 = 0x00;
	SPEAKER_F = 0x00;	// Sets ICR1 to 0
}


// IR Can==============================================================================================
//=====================================================================================================

void falling_edge(void)
// Waits for a falling edge on pin0 of PORTD
{
	int i;
	
	while(1)				// Loops till pin is high
	{
		if(IRCAN_PIN&0x01)
		{
			break;
		}
	}
	
	for(i=0;i<3;i++);
	
	while(1)				// Loops till pin is low
	{
		if(!(IRCAN_PIN&0x01))
		{
			break;
		}
	}
	
	//printf("falling edge");
}

void rising_edge(void)
// waits for a rising edge on pin0 of PORTD
{
	int i;
	
	while(1)				// Loops till pin is high
	{
		if(IRCAN_PIN&0x01)
		{
			break;
		}
	}
	
	for(i= 0;i<3;i++);
	
	//printf("rising edge");
}

uint8_t remote(void)
// Reads in a remote signal. Returns zero when a key is pressed.
{
	uint16_t time; // Holds the time of the pulse
	uint8_t value; // Holds the 4 bit data number
	uint8_t adder;
	int n;
	
	value = 0;
	adder = 0x01;
	
		do					// Gets start bit which is b/t 2 - 4 ms
		{
			falling_edge();
			TCNT0 = 0;
			ms_count = 0;
			rising_edge();
			time = ms_count;
			
		}while((time<2)||(time>4));
		
		for(n=0;n<4;n++)		// Get the rest of the data bits
		{
			falling_edge();
			TCNT0 = 0;
			ms_count = 0;
			rising_edge();
			time = ms_count;
			
			if(time>0)		// If time >= 1 ms, then place a 1
			{
				value += adder; 
			}
			
			adder = adder<<1; // Shifts 1 to the left
		}
		
	value += 0x01; // The key 1 on the remote sends binary zero, so I must offset by 1.
	
	return value;
		
}

uint8_t remote_avg(void)
// Reads in a remote signal. Returns zero when a key is pressed.
{
	uint8_t k;
	IR_val = 0;
	
	lcd_clear_display();
	
	printf("Press a remote key:");
	
	for(k=0;k<10;k++)
	{
		IR_val = remote() + IR_val;
	}
		
	if(IR_val <= 15)
	{
		lcd_goto_line(2);
		printf("1 key: Coke");
		
		SPEAKER_F = 1136; // freq= 1760 Hz. 1/(16 MHz/8)*1136 = 1/1760
		ms_sleep(1000);
		SPEAKER_F = 0;
		
		SODA = 1; 
		return 0;
	}
	
	else if(IR_val >= 16 && IR_val <= 25)
	{
		lcd_goto_line(2);
		printf("2 key: Pepsi");
	
		SPEAKER_F = 2025; // freq= 987.7 Hz. 1/(16 MHz/8)*2025 = 1/987.7
		ms_sleep(1000);
		SPEAKER_F = 0;
	
		SODA = 2;
		return 0;
	}
	
	else if(IR_val >= 26)
	{
		lcd_goto_line(2);
		printf("3 key: Mt. Dew");
	
		SPEAKER_F = 956; // freq= 2093 Hz. 1/(16 MHz/8)*956 = 1/2093
		ms_sleep(1000);
		SPEAKER_F = 0;
		
		SODA = 3;
		return 0;
	}
	
	return 1;
}
	
//===================================================================================================
// AVRCAM
//===================================================================================================

void UART1_init(void)
{
	UCSR1A |= 0x02; // Reduces divisor of baud rate to 8- doubles transfer rate, U2X = 1
	UCSR1B = 0x18; // Enables the Reciever and Transmitter
	UCSR1C = 0x06; // Asychronous operation; No parity; 1 stop bit; 8 bits
	UBRR1H = 0x00;
	UBRR1L = 0x10; // UBRR = 16, 115.2 kbps baud rate
}

void UART1_transmit(char data[])
// Transmits a char array over UART1
{
	int k;
	
	for(k=0;k<5;k++)
	{

		while( !(UCSR1A & (1<<UDRE1)) ) // Wait for a empty transmit buffer
			;
			
		UDR1 = data[k];
	}
	
}

unsigned char UART1_receive(void)
// Receives data overy UART1 and returns it
{
	while( !(UCSR1A & (1<<RXC1)) ) // Wait for data to be recieved
		;
		
	return UDR1;
}

void AVRCAM_track_on(void)
// Turns the AVRCam on
{
	UART1_transmit("ET\r"); // Enable Color Tracking mode
}

void AVRCAM_track_off(void)
// Turns the AVRCam off
{
	UART1_transmit("DT\r"); // Disable Color Tracking mode
}

void AVRCAM_data(void)
// Displays the color found- byte 2- 00-07.
// Make averaging better....
{
	unsigned char AVRCAMdata1[13];
	int k;
	
	unsigned char temp[50];
	int i;
	
	lcd_clear_display();
	
	for(i=0;i<50;i++)	// Reads in everything, including ACK/r
	{
		temp[i] = UART1_receive();
		
	}

	k = 0;
	
	while(temp[k] != 0x0A) // Increments till the beginning of the packet stream
	{
		k++;
	}
	
	if(temp[k+1] != 0xFF)	// If dont have temp[]= 0A,FF,0A,...
	{
		for(i=0;i<13;i++)
		{
			AVRCAMdata1[i] = temp[k+i];
		}
		

		if(AVRCAMdata1[2] <= 3)		// If a valid color
		{
			CAM_counter++;
		
			if(AVRCAMdata1[2] == 0x00)
			// If color 1 displays it to LCD
			{
				AVRCAM_AVG = AVRCAM_AVG + 0;		
			}
			else if(AVRCAMdata1[2] == 0x01) 
			// If color 2 displays it to LCD
			{
				AVRCAM_AVG = AVRCAM_AVG + 1;
			}
			else if(AVRCAMdata1[2] == 0x02)
			// If color 3 displays it to LCD
			{
				AVRCAM_AVG = AVRCAM_AVG + 2;
			}
		}
	
	} // Ends if(temp[k+1] != 0xFF)

}

uint8_t AVRCAM_data_avg(void)
// Returns an average AVRCAM color reading. Prints it to LCD and returns 1 when done.
{

	AVRCAM_AVG = 0;
	CAM_counter = 0;
		
	while(CAM_counter < 20)	// Takes 20 samples
	{
		AVRCAM_data();	
	}
		
	lcd_clear_display();
	
	printf("Color Avg = %i",AVRCAM_AVG);	
	lcd_goto_line(2);
	
	ms_sleep(2000);
	
	lcd_clear_display();
		
	if(AVRCAM_AVG <= 6)
	{
		printf("Color1: Coke");
		
		if(SODA == 1) // If Coke was selected by remote
		{
			return 1; // In pick up soda mode
		}
		
		else
		{
			lcd_goto_line(2);
			printf("Not this one");
			return 0;
		}
	}
	else if(AVRCAM_AVG >= 7 && AVRCAM_AVG <= 31)
	{
		printf("Color2: Pepsi");
		
		if(SODA == 2) // If Pepsi was selected by remote
		{
			return 1; // In pick up soda mode
		}
		
		else
		{
			lcd_goto_line(2);
			printf("Not this one");
			return 0;
		}
	}
	else if(AVRCAM_AVG >= 32 && AVRCAM_AVG <= 40)
	{
		printf("Color3: Mt. Dew");
		
		if(SODA == 3) // If Sprite was selected by remote
		{
			return 1; // In pick up soda mode
		}
		
		else
		{
			lcd_goto_line(2);
			printf("Not this one");
			return 0;
		}
	}
	
	printf("No Color Match");
	return 0; // If not in any of the ranges	
}

//===========================================================================================
// BEHAVIORS
//===========================================================================================

void avoid_ir(void)
// Obstacle Avoidance
{
	uint16_t ir_l;
	uint16_t ir_r;
	
	if(IR_flag == 1) // If flag was set already
	{
		ms_sleep(3000); // waits 3 seconds
		lcd_clear_display();
	}
	
	
	ir_l = adc_readn(0,20); // pin0, 5 samples, takes the average
	
	ir_r = adc_readn(6,20); // pin6. pin7 does not work.
		
	if(ir_l > 400 || ir_r > 400) // 450 works, may be too high
	{
		if(IR_flag == 0) // If the message was not already displayed
		{
			lcd_goto_line(2);
			printf("IR: Remove object"); 
		}
			
		IR_output_d = 2; // Backward
		IR_output_s = 0;
		IR_flag = 1;
		
		SPEAKER_F = 1136; // freq= 1760 Hz. 1/(16 MHz/8)*1136 = 1/1760
		ms_sleep(500);
		SPEAKER_F = 0;
		ms_sleep(100);
		
	}
	
	else
	{
		IR_flag = 0;
	}
}

void avoid_bump(void)
// Bump ring obstacle advoidance
{	
	if(BUMP_flag == 1) // If the bump was hit last time in this fnc
	{
		ms_sleep(3000); // waits 3 seconds
		
		lcd_clear_display();
	}
	
	if((BUMP_PIN & 0x01) == 0 || (BUMP_PIN & 0x02) == 0 || (BUMP_PIN & 0x04) == 0)
	{	
		BUMP_flag = 1;
		BUMP_output_d = 2; // Backward
		BUMP_output_s = 0;
		
		lcd_goto_line(2);
		printf("B: Remove object");
		
	}
	
	else
	{
		BUMP_flag = 0;
	}
	
}

unsigned char line_tracking(void)
// 2200 slow, 2400 medium speed
{
		switch(PHOTO_PIN)
		{
			case 0x0F: 		// 1111
			{
				//Do nothing. Turning messes it up. May try stop/forward/back.
				
				break;
			}
			case 0x0E: 		// 1110
			{
			
				PHOTO_output_d = 4;			// Right
				PHOTO_output_s = 2400;		//	2500 may be too high
				PHOTO_flag = 1;
			
				break;
			}
			case 0x0D: 		// 1101
			{
				PHOTO_output_d = 4;		// Right
				PHOTO_output_s = 2200;	// good for now
				PHOTO_flag = 1;
				
				break;
			}
			case 0x0C: 		// 1100
			{
				
				PHOTO_output_d = 4;		// Right
				PHOTO_output_s = 2400;	// Good for now
				PHOTO_flag = 1;
				
				break;
			}
			case 0x0B: 		// 1011
			{
				PHOTO_output_d = 3;		// Left
				PHOTO_output_s = 2200;	// 2200 is good
				PHOTO_flag = 1;
				
				break;
			}
			case 0x0A: 		// 1010
			{
				// Do nothing. 
				
				break;
			}
			case 0x09: 		// 1001
			{
				PHOTO_output_d = 1;		// Forward
				PHOTO_output_s = 2400;	// 2500 may be too fast
				PHOTO_flag = 1;
				
				break;
			}
			case 0x08: 		// 1000
			{
				PHOTO_output_d = 4;		// Right
				PHOTO_output_s = 2400;	// 
				PHOTO_flag = 1;
				
				break;
			}
			case 0x07: 		// 0111
			{
			
				PHOTO_output_d = 3;		// Left
				PHOTO_output_s = 2400;	// 2500 may be too high
				PHOTO_flag = 1;
			
				break;
			}
			case 0x06: 		// 0110
			{
				// Do nothing
				
				break;
			}
			case 0x05: 		// 0101
			{
			
				// Do nothing. 
				
				break;
			}
			case 0x04: 		// 0100
			{
				// Do nothing. 
				
				break;
			}
			case 0x03: 		// 0011
			{
	
				PHOTO_output_d = 3;		// Left
				PHOTO_output_s = 2400;	// 
				PHOTO_flag = 1;
				
				break;
			}
			case 0x02: 		// 0010
			{
				// Do nothing. 
				
				break;
			}
			case 0x01: 		// 0001
			{
				PHOTO_output_d = 3;		// Left
				PHOTO_output_s = 2400;	// 
				PHOTO_flag = 1;
				
				break;
			}
			case 0x00: 		// 0000
			{
				// At intersection
				
				PHOTO_flag = 1;
				
				break;
			}
		}

	return PHOTO_PIN;	
}

void turn_left_line()
// Turns left until over a line
{
	motor_move(3,2200);		 // Turns left, 2200 too slow
	
	while(PHOTO_PIN&0x02); // Breaks once right inner sensor over line
	
	MOTOR = STOP;
	
}

void turn_right_line()
// Turns right until over a line
{
	motor_move(4,2200);		 // Turns right, 2200 too slow
	
	while(PHOTO_PIN&0x04); // Breaks once left inner sensor over line
	
	MOTOR = STOP;
	
}

void forward_line()
// Move forward untill on a line
{
	motor_move(1,2200);
	
	while(PHOTO_PIN&0x06);	// Breaks once both inner sensors over a line
	
	MOTOR = STOP;
}

void turn_left_off_line()
// Turns left until off a line
{
	motor_move(3,2200);		 // Turns left, 2200 too slow
	
	while(1) // Breaks once none of the sensors are over the line
	{	
		if((PHOTO_PIN&0x0F) == 0x0F)
		{
			break;
		}
	}
	
	MOTOR = STOP;
	
}

void turn_right_off_line()
// Turns right until off a line
{
	motor_move(4,2200);		 // Turns right, 2200 too slow. 2500 too fast
	
	while(1) // Breaks once none of the sensors are over the line
	{	
		if((PHOTO_PIN&0x0F) == 0x0F)
		{
			break;
		}
	}
	
	MOTOR = STOP;
	
}

void pickupsoda_front(void)
// Moves robot to pick up the front soda can and returns robot to return line
{
	lcd_clear_display();
	printf("OFF1");
	
	motor_move(1,2500);			// Move over first intersection
	ms_sleep(750); 			// 500 too short
	MOTOR = STOP;	
	
	while(line_tracking() != 0) // While not at a intersection, track to get to soda can
	{
		printf("LT");
		line_tracking();
		
		motor_move(PHOTO_output_d,PHOTO_output_s);
	}
	
	MOTOR = STOP;	// Stop at intersection
	
	ms_sleep(1000);		// Close claw
	SERVO = S_CLOSE;
	ms_sleep(1000);
	
	lcd_clear_display();
	printf("OFF2");
	
	//turn_right_off_line();	// Return robot to return line
	motor_move(4,2200);		// Right
	ms_sleep(1500);
	turn_right_line();	
}

void pickupsoda_right(void)
// Moves robot to pick up the right soda can and returns robot to return line
{
	while(line_tracking() != 0) // While not at a intersection, track to get to soda can
	{
		printf("LT");
		line_tracking();
		
		motor_move(PHOTO_output_d,PHOTO_output_s);
	}
	
	MOTOR = STOP;	// Stop at intersection
	
	ms_sleep(1000);		// Close claw
	SERVO = S_CLOSE;
	ms_sleep(1000);
	
	lcd_clear_display();
	printf("OFF2");
	
	ms_sleep(1000);
	
	motor_move(2,2400); // Backward
	ms_sleep(1200);		// Used to be 2000
	
	motor_move(4,2200);		 // Turns Right
	ms_sleep(500);
	turn_right_off_line();	// Return robot to return line
	turn_right_line();	
}

void pickupsoda_left(void)
// Moves robot to pick up the left soda can and returns robot to return line
{
	while(line_tracking() != 0) // While not at a intersection, track to get to soda can
	{
		printf("LT");
		line_tracking();
		
		motor_move(PHOTO_output_d,PHOTO_output_s);
	}
	
	MOTOR = STOP;	// Stop at intersection
	
	ms_sleep(1000);		// Close claw
	SERVO = S_CLOSE;
	ms_sleep(1000);
	
	lcd_clear_display();
	printf("OFF2");
	
	ms_sleep(1000);
	
	motor_move(2,2400); // Backward
	ms_sleep(1000);		// Used to be 2000
	
	motor_move(3,2200);		 // Turns left
	ms_sleep(500);
	turn_left_off_line();	// Return robot to return line
	turn_left_line();	
}

void arbitrate()
// Behavior arbitrator
{			
	if(PHOTO_flag == 1)
	{
		motor_input_d = PHOTO_output_d;
		motor_input_s = PHOTO_output_s;
		
	}
	
	if(IR_flag == 1)
	{
		
		motor_input_d = IR_output_d;
		motor_input_s = IR_output_s;
		
	}
	
	if(BUMP_flag == 1) // Highest priority
	{
		
		motor_input_d = BUMP_output_d;
		motor_input_s = BUMP_output_s;
		
	}
	
}

//===================================================================================================
// MAIN
//===================================================================================================
	
int main(void)
{	
	int k;
	LCD_DDR= 0xFF; // Set all bits of port C for output
	IR_DDR= 0x00; // Set all bits of port F for input
	PHOTO_DDR = 0x00; // Set all bits of port A for input
	MOTOR_DDR = 0x0F; // Set all bits of port B for output
	SPEAKER_DDR = 0XF0; 
	IRCAN_DDR = 0x00; // Set for input- IR can
	DDRE = 0x38; //00111000- Sets OCA,B,C to outputs
	BUMP_DDR= 0x00; // Set bump switches to inputs
	
	BUMP = 0xFF; // Enables internal pull up resistors of port
	
	IR_flag = 0;
	BUMP_flag = 0;
	PHOTO_flag = 0;
	MODE = 0; 		// To/From soda pickup
	FIRST_PASS = 0;
	SODA = 0;
	PATH = 0;

	init_timer();
	
	sei();  // enable interrupts 
	
	lcd_init();	
	adc_init(); 	// For IR rangefinders
	UART1_init(); 	// Initializes UART1
	motor_init();
	speaker_init();
	
	fdevopen(lcd_char,NULL,0); 	// Sets up to print to lcd using printf


	while(1)
	{
	
		if(FIRST_PASS == 0 && MODE == 0)
		{
			banner(); // displays banner

			while(remote_avg()); // Loops till a key is pressed
	
			ms_sleep(1000);
		}
	
		if(MODE == 0) // If going to/from soda pickup
		{
			lcd_clear_display();
			printf("MODE 0");
			
			while(line_tracking() != 0) // Break out once hit a intersection
			{
				avoid_bump();
				avoid_ir();
				line_tracking();
				arbitrate();
				
				motor_move(motor_input_d,motor_input_s);	
			}
			
			MOTOR = STOP;	// Stop b/c at intersection
			
			if(FIRST_PASS == 0)
			{
				FIRST_PASS = 1;
				MODE = 1;	// Go to soda detection mode
			}
			
			else	// Coming back with soda can
			{
				lcd_clear_display();	// Sequence is done
				printf("Done!");
				
				SERVO = S_MIDDLE; 
				
				for(k=0;k<5;k++)
				{
					SPEAKER_F = 2025; // freq= 987.7 Hz. 1/(16 MHz/8)*2025 = 1/987.7
					ms_sleep(750);
					SPEAKER_F = 0;
					ms_sleep(250);
				}
				
				// Turn around robot before restarting:
				if(PATH == 1) // Left Path
				{
					turn_right_off_line();
					forward_line();
				}
				if(PATH == 2) // Center Path
				{
					//turn_right_off_line();
					motor_move(4,2200); // right
					ms_sleep(2500);
					turn_right_line();
				}
				if(PATH == 3) // Right Path
				{
					turn_left_off_line();
					forward_line();
				}
	
				FIRST_PASS = 0; 	// Restart cycle
			}
			
		}	// Ends if MODE == 0
	
		if(MODE == 1) // If in soda pick up mode
		{
			lcd_clear_display();
			printf("MODE 1");
			ms_sleep(5000);
			
			AVRCAM_track_on();
	
			if(AVRCAM_data_avg() == 1) // If the soda that is being looked at is the one desired
			{
				ms_sleep(2000);
				
				AVRCAM_track_off();
				
				pickupsoda_front();
				
				PATH = 2; // Center path
				
				MODE = 0; // Back to to/from soda pick up mode
			}
			
			else
			{
				turn_right_off_line();	// Move to check right soda can
				turn_right_line();
			
				ms_sleep(1000);
			
				if(AVRCAM_data_avg() == 1) // If the soda that is being looked at is the one desired
				{
					ms_sleep(2000);
				
					AVRCAM_track_off();
				
					pickupsoda_right();
				
					PATH = 1; // Left path
					
					MODE = 0;	// Back to to/from soda pick up mode
				}
				
				else
				{
			
					turn_left_off_line();	// Move to check left soda can
					turn_left_line();
					turn_left_off_line();
					turn_left_line();
			
					ms_sleep(1000);
			
					if(AVRCAM_data_avg() == 1) // If the soda that is being looked at is the one desired
					{
						ms_sleep(2000);
				
						AVRCAM_track_off();
				
						pickupsoda_left();
						
						PATH = 3; // Right path
				
						MODE = 0;	// Back to to/from soda pick up mode
					}
					
					else
					{
						ms_sleep(3000);
						
						lcd_clear_display();
						printf("None detected"); // Displays if none of the 3 cans is the one desired
			
						AVRCAM_track_off();
			
						turn_left_off_line();	// Move return home w/ no soda can. :(
						turn_left_line();
						
						PATH = 3; // Right path
						
						MODE = 0;
			
					}	// Ends else3
				} // Ends else2
			} // Ends else1
			
		} // End if Mode = 1
	
	} // Ends main while loop


} // Ends main()