//final program!!!

//Copyrighted on April 13th, 20:01
//by Vivek Manoharan


#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/delay.h>

#include <ctype.h>
#include <inttypes.h>
#include <string.h>
#include <math.h>

#define motora	OCR0
#define	motorb	OCR2
#define	servo	OCR3A
#define pickup_forward	OCR3B
#define pickup_backward	OCR3C
#define a 1
#define b 0
#define forward 1
#define backward 0

// Declare your global variables here       
//    PIN SETUP:
//      PORTC7 = D7
//      PORTC6 = D6
//      PORTC5 = D5
//      PORTC4 = D4  

//      PORTC3 = NC
//      PORTC2 = EN
//      PORTC1 = NC (grounded on the board), but PINC1 as input
//      PORTC0 = RS
                              

const int delaylen = 50;
const int max_balls = 3;

int uart_data[1000];
int recpos = 0;
int readpos = 0;
char uart_mean_data[9]; //to account for the :
int uart_track_data[11];

int vala = 0xDD; //from DD, from D5
int valb = 0xDD;

float sonar_val1;
float sonar_val2;

int mx = 0;
int my = 0;

int mx_backup = 0;
int my_backup = 0;

int s = 0;
int found = 0;

int ball_count = 0;

int slow_val = 0xA0;
int nogo = 0x00;
//INTERRUPTS!
//UART recieve interrupt

//DELAY fuctions
void delay_ms(int time)
{
	for(int i=0; i < time; i++)
			_delay_ms(1);
}

void delay_us(int time)
{
	for(int i=0; i < time; i++)
		_delay_us(1);
}

void enter_key() //located on PC1
{
	while ((PINA&0x80) == 0x00)
	;
	
	delay_ms(200); //software debounce
}

//LCD FUNCTIONS
void lcd_write(void)
{
	PORTC |= 0x04; //Set LCD enable high
	delay_us(100);
	PORTC &= ~(0x04); //set LCD enable low  
	delay_us(100);
}
                

void lcd_init(void)
{
		delay_ms(1000);

        _delay_ms(delaylen);
        PORTC = 0x30; 
        lcd_write();
        
        _delay_ms(delaylen);
        PORTC = 0x30;
        lcd_write();
        
        _delay_ms(delaylen);
        PORTC = 0x30;
        lcd_write();
                
        _delay_ms(delaylen);
        PORTC = 0x20;   
        lcd_write();
                        
//      ENTERING 4-BIT MODE     //                
                
        _delay_ms(delaylen);

        PORTC = 0x20;
        lcd_write();
        
        PORTC = 0x00; //changed from 80 to 00
        lcd_write();

                
        _delay_ms(delaylen);
        PORTC = 0x00;
        lcd_write();

        PORTC = 0xF0;   //display off, cursor off, blink off (changed from F to 8, worked with F)
        lcd_write();
                
        _delay_ms(delaylen);
        PORTC = 0x00;
        lcd_write();
                
        PORTC = 0x10;   //clear screen, cursor home
        lcd_write();        
        
        _delay_ms(delaylen);
        PORTC = 0x00;                   //was 00
        lcd_write();
        
        PORTC = 0x60;   //increment cursor to the right, dont shift screen (was 60
        lcd_write();
                
        _delay_ms(delaylen);    
        
        PORTC = 0x00;
        lcd_write();    
        PORTC = 0xF0;
        lcd_write();
        
        _delay_ms(delaylen);
        

        
//      INITIALIZATION DONE!    //        


}

void lcd_put_ascii(int input)
{
	
	//set up the nibbles
	int input_high_nib = input & 0xF0;//high nibble
	int input_low_nib = input <<= 4;	//low nibble

		
	//make sure RS = 1, data mode
	input_high_nib |= 0x01;
	input_low_nib |= 0x01;
	
	
	
	PORTC = input_high_nib;
    lcd_write(); 	
	PORTC = input_low_nib;
    lcd_write(); 
}


//puts a character onto the LCD display
void lcd_put_char(char data)
{
	lcd_put_ascii(toascii(data));
}

void lcd_puts(char string[])
{
	int i = 0;
    while(i<strlen(string))
	{ 
		lcd_put_char(string[i]);
		i++;
	}
}

void lcd_put_int(int data)
{
	int hundred, ten, one;
	int temp = data;
	
	hundred = temp / 100;
	temp = temp % 100;
	
	ten = temp / 10;
	temp = temp % 10;
	
	one = temp;
	
	hundred += 48;
	ten += 48;
	one += 48;
	
	if(hundred != 48)
		lcd_put_ascii(hundred);
	if(ten != 48)
		lcd_put_ascii(ten);
//	if(one != 48)
		lcd_put_ascii(one);

}

//clear LCD screen
void lcd_clear()
{
	PORTC &= 0xFE;		//set to command mode
	PORTC = 0x00;
    lcd_write(); 	
	PORTC = 0x10;
    lcd_write(); 
	
	PORTC = 0x00;
    lcd_write(); 	
	PORTC = 0xC0;
    lcd_write(); 
	
	PORTC |= 0x01;		//enable back to data write mode
	
	delay_ms(10);

}

//sets the direction of whichever motor is chosen
//motor:
//		0 - motor A, d-bit is PA4
//		1 - motor B, d-bit is PA5
//direction:
//		0 - backwards
//		1 - forward

//CHANGING TO PORTA!!

void direction_set(int motor, int dir)
{
	if(motor == 0) //motor A
	{
		if(dir == 0)
			PORTA &= 0xDF;
		if(dir == 1)
			PORTA |= 0x20;
	}
	else
		if(motor == 1) //motor B
		{
			if(dir == 0)
				PORTA &=  0xEF;
			if(dir == 1)
				PORTA |= 0x10;
		}

}


void sonar_test()
{ 
        //Requires Two Pins!  PortB(0:1)
        //PORTB0       Pulse Enable (DDR Output)
        //PINB1        Echo         (DDR Input)

		//changing it to timer1


		TCCR1B = 0x00;	//stop timer
		TCNT1 = 0x00;

		PORTB|=0x01;        //Set Pulse Enable High
        delay_us(10);       //10 uSec Delay
        PORTB&=0xfe;        //Set Pulse Low
        
	
	while ((PINB&0x02) == 0x00)   //Poll Echo For Rising Edge
        {       
        }; 
        
		TCCR1B = 0x05;	//start timer
		
    while ((PINB&0x02) == 0x02)   //Poll Echo For Falling Edge
        {       
        };        
        
        TCCR1B=0x00;         //Stop Timer
        sonar_val1=TCNT1*1.;       //Save Result  removed *1.     
        
        delay_ms(10);       //Mandatory Delay 
        sonar_val1 =  ((sonar_val1) * .432); //have to multiply it by 64/148    

//SONAR TWO///////////////////////////////////////

		TCCR1B = 0x00;	//stop timer
		TCNT1 = 0x00;

		PORTA|=0x01;        //Set Pulse Enable High
        delay_us(10);       //10 uSec Delay
        PORTA&=0xfe;        //Set Pulse Low
        
	
	while ((PINA&0x02) == 0x00)   //Poll Echo For Rising Edge
        {       
        }; 
        
		TCCR1B = 0x05;	//start timer
		
    while ((PINA&0x02) == 0x02)   //Poll Echo For Falling Edge
        {       
        };        
        
        TCCR1B=0x00;         //Stop Timer
        sonar_val2=TCNT1*1.;       //Save Result  removed *1.     
        
        delay_ms(10);       //Mandatory Delay 
        sonar_val2 =  ((sonar_val2) * .432); //have to multiply it by 64/148   
}

void turn_around()
{
		int dbit1, dbit2;
		
		lcd_clear();
		lcd_puts("Found Wall!");
		motorb = 0x00;		//stop
		motora = motorb;
		
		delay_ms(1000);
		
		direction_set(a, 0);	//reverse direction
		direction_set(b, 0);
		
		motorb = 0xA0;
		motora = 0xA0;
		
		delay_ms(2000);
		
		dbit1 = 1;
		dbit2 = 0;

		direction_set(a, dbit1);	//pivot in place 0
		direction_set(b, dbit2);	// 				 1
		
		sonar_test();
		
		while(! ((sonar_val1 > 25) && (sonar_val2 > 25)) )
			sonar_test();
		
		lcd_clear();
		lcd_puts("Full Speed Ahead!");
		direction_set(a, 1);	//forward
		direction_set(b, 1);
		
		motorb = valb;
		motora = vala;
}

//UART FUNCTIONS
void uart_write(unsigned char data)
{
	while ( !( UCSR0A & (1<<UDRE0)) )
	;
	
	UDR0 = data;
}
unsigned char uart_recv( void )
{
	/* Wait for data to be received */
	while ( !(UCSR0A & (1<<RXC0)) )
		;

	/* Get and return received data from buffer */
	return UDR0;
}

void uart_flush( void )
{
	unsigned char dummy;
	while ( UCSR0A & (1<<RXC0) ) 
		dummy = UDR0;
		
	recpos = 0;
	readpos = 0;
}

void uart_puts(char string[])
{
	int i = 0;
    while (i<strlen(string))
    {
        uart_write(string[i]);
		i++;
    };

}   

void uart_print()
{
	for(readpos; readpos < recpos; readpos++)
	{
		//if(!((uart_data[readpos] < 48) || ((uart_data[readpos] > 57) && 
		//	(uart_data[readpos] < 64)) || (uart_data[readpos] > 122)))
			lcd_put_char(uart_data[readpos]);
	}
	
	readpos = 0;
	recpos = 0;
}

void uart_print_int()
{
	int i = 0;

	for(i; i < recpos; i++)
	{
		lcd_put_int(uart_data[i]);
		lcd_puts(" ");
	}
	
	readpos = 0;
	recpos = 0;
}

void uart_receive()
{
	while ((UCSR0A&0x80))
	{
		lcd_put_ascii(uart_recv());
	}
}


//CMU CAMERA FUNCTIONS
void camera_init()
{
	lcd_clear();
	
	//UART SETUP
	UBRR0H = 0x00;
	UBRR0L = 0x08; //changing speeds to 08 from 33
	UCSR0B = 0x98;
	UCSR0C = 0x06;
	
	uart_puts("RS \r"); //resets cmu
	delay_ms(10);
	uart_puts("RM 3\r");	//sets raw mode output and acks disabled
	delay_ms(10);
	uart_puts("CR 18 44\r");//white balance on
	delay_ms(10);
	uart_puts("MM 1\r");	//middle mass on
	delay_ms(10);

	delay_ms(10);
	uart_print();
	
}

void drop_off()
{
	//make servo swing arm out
	
	TCCR3A = 0xA0; 	//1010 | 0000
	TCCR3B = 0x12; 	//0001 | 0010
	ICR3 = 20000;
	
	servo = 2500; //increase to adjust, from 500
	
	delay_ms(5000);
	
	servo = 1700; //decrease to adjust
	
	delay_ms(2000);
	
	TCCR3A = 0x00;
	TCCR3B = 0x00;

}

void find_cone()
{
	s = 0;
	cli();
	
	uart_puts("RS \r"); //resets cmu
	delay_ms(10);
	uart_puts("RM 3\r");	//sets raw mode output and acks disabled
	delay_ms(10);
	uart_puts("CR 18 44\r");//white balance on
	delay_ms(10);
	uart_puts("MM 1\r");	//middle mass on
	delay_ms(10);
	
	//set color for cone
	//
	uart_puts("TC 200 240 0 25 0 25\r");
	delay_ms(1000);

	uart_flush();
	
	lcd_clear();
	lcd_puts("find cone!");

	sei();
	s = 1;
}

void arm_down()
{

	lcd_clear();
	lcd_puts("arm moving down!");

	pickup_forward = 0xffff;
	pickup_backward = 0x00;
	
	delay_ms(8300);
	
	pickup_forward = 0x00;
	pickup_backward = 0x00;

}

void arm_up()
{
	lcd_clear();
	lcd_puts("arm moving up!");

	pickup_forward = 0x00;
	pickup_backward = 0xffff;
	
	delay_ms(8000);
	
	pickup_forward = 0x00;
	pickup_backward = 0x00;
	

}

void pick_up()
{
	
	lcd_clear();
	lcd_puts("pickup mechanism");
	delay_ms(3000);
	
	//timer3, ocr3b/c used for two pins
	TCNT3 = 0x00;
	
	TCCR3A = 0xAB;
	TCCR3B = 0x11;
	
	arm_down();
	arm_up();
	
	
	ball_count++;
	
	lcd_clear();
	lcd_puts("Picked up ");
	lcd_put_int(ball_count);
	lcd_puts(" balls!");
	
	delay_ms(5000);
	
	found = 0;
	
	mx = 0;
	my = 0;
	
	if(ball_count == max_balls)
		find_cone();
	
	
}

void line_up_cone()
{
	lcd_clear();
	lcd_puts("X: ");
	lcd_put_int(mx);
	lcd_puts(" Y: ");
	lcd_put_int(my);
	
	sonar_test();
	
	if((sonar_val1 < 20) && (sonar_val2 < 20))
	{
		motora = nogo;
		motorb = nogo;
		
		cli();
		ball_count++;
		
		drop_off();
	}
	
	if((mx > 20) && (mx < 58))
	{
		direction_set(a, 1);
		direction_set(b, 1);
		motora = slow_val;
		motorb = slow_val;
	}
	else
	{	
		if(mx > 58)
		{
			direction_set(a, 0);
			direction_set(b, 1);
			motora = slow_val;
			motorb = slow_val;
		}
	
		if(mx < 50)
		{
			direction_set(a, 1);
			direction_set(b, 0);
			motora = slow_val;
			motorb = slow_val;
		}
	}
	
}



void line_up_ball()
{
	lcd_clear();
	lcd_puts("X: ");
	lcd_put_int(mx);
	lcd_puts(" Y: ");
	lcd_put_int(my);
	
	if(my > 50)
	{
		if((mx > 20) && (mx < 58))
		{
			direction_set(a, 1);
			direction_set(b, 1);
			motora = slow_val;
			motorb = slow_val;
		}
		else
		{	
			if(mx > 58)
			{
				direction_set(a, 0);
				direction_set(b, 1);
				motora = slow_val;
				motorb = slow_val;
			}
		
			if(mx < 50)
			{
				direction_set(a, 1);
				direction_set(b, 0);
				motora = slow_val;
				motorb = slow_val;
			}
		}
	
	}
	else
	{
		if((my < 50) && (my > 20))
		{
			if((mx > 57) && (mx < 70))
			{
				direction_set(a, 1);
				direction_set(b, 1);
				motora = slow_val;
				motorb = slow_val;
			}
			else
			{	
				if(mx > 70)
				{
					direction_set(a, 0);
					direction_set(b, 1);
					motora = slow_val;
					motorb = slow_val;
				}
		
				if(mx < 50)
				{
					direction_set(a, 1);
					direction_set(b, 0);
					motora = slow_val;
					motorb = slow_val;
				}
			}
		}
		else //safe to assume its right underneath the arm mechanism
		{
			cli();
			lcd_clear();
			lcd_puts("arm time");
			
			motora = nogo;
			motorb = nogo;
			
			delay_ms(2000);
			
			pick_up();
			
			found = 0;
		}
	}
//	delay_ms(2000);
	
	mx_backup = mx;
	my_backup = my;

}

void avoid_obstacle()
{
	motora = vala;
	motorb = valb;
	
	sonar_test();
	
	if(sonar_val1 < 35)
	{
		motora = 0xFF; 
		while(sonar_val1 < 20)
		{
			motorb = 0x50;
			sonar_test();
			
			if((sonar_val1 < 20) && (sonar_val2 < 20))
				turn_around();
		}
	}
	
	sonar_test();
	
	if(sonar_val2 < 25) 
	{
		motorb = 0xFF; 
		while(sonar_val2 < 20)
		{
			motora = 0x50;
			sonar_test();
			
			if((sonar_val1 < 20) && (sonar_val2 < 20))
				turn_around();
		}
	}

}


SIGNAL(SIG_UART0_RECV)
{
	//get data
	if(recpos == 900)
	{
		recpos = 0;
		readpos = 0;
	}
	unsigned int foo=UDR0;
	
	if(uart_data[(recpos-2)] == 77)
	{
		mx = uart_data[(recpos-1)];
		my = foo;
		if(s)
		{
			if(mx || found)
			{
				if(!found)
				{
					//cli();
					motora = 0x00;
					motorb = 0x00;
					lcd_clear();
					lcd_puts("found object!");
					delay_ms(2000);
					found = 1;

				}
				else
				{
					if(mx == 0)
						mx = mx_backup;
					if(my == 0)
						my = my_backup;
					
					if(ball_count == max_balls)
						line_up_cone();
					else
						line_up_ball();
				}
		
			}
		}
		else
			lcd_clear();
	}
	
	uart_data[recpos++]=foo;
}

//MAIN FUNCTION
int main(void)
{
	delay_ms(500);
	
	DDRC = 0xFF;  //bits: O O O O | O O I O
	DDRB = 0x91;
	DDRE = 0x3A; //bits: I I O O | O I O I
	DDRA = 0x31;
	
	sei();
	
	direction_set(0, forward);
	direction_set(1, forward);
	
	
	
	lcd_init();
	lcd_puts("BallMan v1.0");
	delay_ms(2000);
	lcd_clear();
	lcd_puts("by Vivek Manoharan");
	
	enter_key();
	
	lcd_clear();
	
	///////////////////////////////
	//set up the motor pwms
	TCNT2=0x00;
	TCCR2=0x61; //0x61 for 00 being off, FF being full speed

	TCNT0=0x00;
	TCCR0=0x61;
	/////////////////////////////////
	
	camera_init();
	enter_key();
	uart_flush();
	
	lcd_clear();
	lcd_puts("place object in window!");
	cli();
	enter_key();
	
	lcd_clear();
	uart_puts("TW\r");
	delay_ms(5000);
	sei();
	
	motora = nogo; //vala
	motorb = nogo; //valb
	
	delay_ms(1000);
	
	uart_flush();
	
	s = 1;

	while(ball_count <= max_balls) //adding the = to for testing
	{	
		if(!found)	//commented out to test ranges of x and y allowable
		{	
			avoid_obstacle();
		}
		
	};
	
	
	cli();
	lcd_clear();
	lcd_puts("done!");
	
}