main.c

//Date: 10th January, 2016
//Author: Megh Shukla
//Interrupts : 0 (Color Sensor), 1 (node), 4 (left motor),5 (right motor) used
//Timers : 1 (Servo), 3 (Buzzer and ssd freeze), 5 (PWM), 4 (SSD)
//Robot straight line: 255,253

// timeline 1: 0 x a b c d   e f g h
// ab: color   cd: size   efgh : house
// p = preorder, r = regular order
// pizza_shop: 0 x color size    red = 1, green = 2, blue = 3


/*
TEAM ID : PD#176
AUTHOR: MEGH SHUKLA. No one else. Certain parts of code from EYRC experiments folder
FILENAME : MAIN.C
THEME: Pizza Delivery
Functions: Only one .C contains program. contains lot of folders
GLOBAL VARIABLES: Declarations before first function are only global variables used
*/

//PLEASE NOTE: IN CASE OF PUSH_RELOAD ERROR, PLEASE USE DIFFERENT VERSION OF ATMEL STUDIO 6. 
//PLEASE MINIMIZE ALL FUNCTIONS FIRST


//VERY VERY IMPORTANT. The soul of code resides in LOGIC(); function, where robot takes time related decisions. Request to read that carefully 

#define __OPTIMIZE__ -O0
#define F_CPU 14745600   
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <math.h> //included to support power function
#include "lcd.h"
#include <stdlib.h>

volatile unsigned long int pulse = 0; //to keep the track of the number of pulses generated by the color sensor
volatile unsigned long int red;       // variable to store the pulse count when read_red function is called
volatile unsigned long int blue;      // variable to store the pulse count when read_blue function is called
volatile unsigned long int green;     // variable to store the pulse count when read_green function is called

unsigned char ADC_Conversion(unsigned char);
unsigned char ADC_Value;
unsigned char sharp;
unsigned int value;
volatile long int ShaftCountLeft = 0; //to keep track of left position encoder
volatile long int ShaftCountRight = 0; //to keep track of right position encoder
unsigned int Degrees; //to accept angle in degrees for turning
volatile long int time_ssd = 0;
volatile long int temp_time_ssd=0;
unsigned char lookup[10] = {0x03,0x9F,0x25,0x0D,0x99,0x49,0x41,0x1F,0x01,0x09};
volatile unsigned int cwl = 0, rwl = 0, lwl = 0;
volatile unsigned char a=230,b=230;																		//line following

unsigned char shop_node;                                                                           


unsigned char pizza_shop[10] = {0x99,0x99,0x99,0x99,0x99,0x99,0x99,0x99,0x99,0x99};                      //Elements define what pizza at individual; stop
unsigned char order_timeline_1[7] = {0x71,0xD8,0xA2,0x5B,0x7C,0x95,0xE5};														 //Order Timeline for pizza type and house
unsigned char order_track[7] = {0,0,0,0,0,0,0};			
unsigned char order_type[7] = {'r','r','p','p','r','p','p'};
unsigned int  order_time[7] = {40,100,115,200,250,350,350};
unsigned char timeline_count = 0;
unsigned char estimate_house[12] ={12,17,23,25,25,23,17,12,13,18};
unsigned char estimate_shop[10] = {21,18,16,14,12,12,14,16,18,21};	  

unsigned char buzzer_control_bit=0;
unsigned char ssd_freeze=0;
volatile long int fast_time=0;	
unsigned char attempt_two_delivery=0;

//initalize port to use encoder interrupts
void left_encoder_pin_config (void)																		 //Position Encoder
{
	DDRE  = DDRE & 0xEF;  //Set the direction of the PORTE 4 pin as input
	PORTE = PORTE | 0x10; //Enable internal pull-up for PORTE 4 pin
}
void right_encoder_pin_config (void)
{
	DDRE  = DDRE & 0xDF;  //Set the direction of the PORTE 5 pin as input
	PORTE = PORTE | 0x20; //Enable internal pull-up for PORTE 5 pin
}

//initalize servo motor pins
void servo1_pin_config (void)
{
	DDRB  = DDRB | 0x20;  //making PORTB 5 pin output
	PORTB = PORTB | 0x20; //setting PORTB 5 pin to logic 1
}
void servo2_pin_config (void)
{
	DDRB  = DDRB | 0x40;  //making PORTB 6 pin output
	PORTB = PORTB | 0x40; //setting PORTB 6 pin to logic 1
}
void servo3_pin_config (void)
{
	DDRB  = DDRB | 0x80;  //making PORTB 7 pin output
	PORTB = PORTB | 0x80; //setting PORTB 7 pin to logic 1
}

//initalize ADC and ADC CONVERSION to convert analog to digital values. Used from experiments
void lcd_port_config (void)																				 //ADC
{
	DDRC = DDRC | 0xF7;
	PORTC = PORTC & 0x80;
}
void adc_pin_config (void)
{
	DDRF = 0x00;
	PORTF = 0x00;
	DDRK = 0x00;
	PORTK = 0x00;
}
void adc_init()
{
	ADCSRA = 0x00;
	ADCSRB = 0x00;
	ADMUX = 0x20;
	ACSR = 0x80;
	ADCSRA = 0x86;
}
unsigned char ADC_Conversion(unsigned char Ch)
{
	unsigned char a;
	if(Ch>7)
	{
		ADCSRB = 0x08;
	}
	Ch = Ch & 0x07;
	ADMUX= 0x20| Ch;
	ADCSRA = ADCSRA | 0x40;		
	while((ADCSRA&0x10)==0);	
	a=ADCH;
	ADCSRA = ADCSRA|0x10; 
	ADCSRB = 0x00;
	return a;
}
void print_sensor(char row, char coloumn, unsigned char channel)
{
	ADC_Value = ADC_Conversion(channel);
	lcd_print(row, coloumn, ADC_Value, 3);
}

/*
FUNCTION NAME: SSD DISPLAY
INPUT: TIME VARIABLE
OUPTPUT : SSD
LOGIC: EACH DIGIT IS obtained by dividing by 100,10 and remainder. at one instant, one SSD selected. Display on that
EXAMPLE : ssd_display(time_ssd);
*/
void ssd_display(volatile long int time)
{
	unsigned char PortLrestore;
	PortLrestore = PINL;
	PortLrestore = PortLrestore | 0x04;//enabling one pin ( CA0) AT A TIME
	PortLrestore = PortLrestore & 0xBC;
	PORTL = PortLrestore;
	PORTJ = lookup[(time/100)];
	_delay_us(3000);
	PortLrestore = PINL;
	PortLrestore = PortLrestore | 0x02;
	PortLrestore = PortLrestore & 0xBA;
	PORTL = PortLrestore;
	PORTJ = lookup[((time%100)/10)];//DIVIDING THE internal timer count working
	_delay_us(3000);
	PortLrestore = PINL;
	PortLrestore = PortLrestore | 0x01;
	PortLrestore = PortLrestore & 0xB9;
	PORTL = PortLrestore;
	PORTJ = lookup[((time%100)%10)];
}
//switch buzzer on and off .experiment function
void buzzer_on (void)
{
	unsigned char port_restore = 0;
	port_restore = PINC;
	port_restore = port_restore | 0x08;
	PORTC = port_restore;
}
void buzzer_off (void)
{
	unsigned char port_restore = 0;
	port_restore = PINC;
	port_restore = port_restore & 0xF7;
	PORTC = port_restore;
}
/*
FUNCTION: DELIVERED CONTROL
INPUT: NONE
OUTPUT: Buzzer 1 second, timer freeze 5s
*/

void delivered_control(void)
{
	if(fast_time<=32)						//fast time = count of prescalar 8 timer   fast time=32 indicates 1s
	buzzer_on();
	else
	buzzer_off();
	if(fast_time<=160)
	ssd_display(temp_time_ssd);
	else
	{
		ssd_display(time_ssd);
		ssd_freeze=0;
		fast_time=0;
	}
}

void motion_pin_config (void)																			 //PWM_Motion
{
	DDRA = DDRA | 0x0F;
	PORTA = PORTA & 0xF0;
	DDRL = DDRL | 0x18;   //configuring the motors though port A
}
void timer5_init()
{
	TCCR5B = 0x00;	
	TCNT5H = 0xFF;	
	TCNT5L = 0x01;	
	OCR5AH = 0x00;	
	OCR5AL = 0xFF;	
	OCR5BH = 0x00;	
	OCR5BL = 0xFF;	
	OCR5CH = 0x00;	
	OCR5CL = 0xFF;	
	TCCR5A = 0xA9;	
	TCCR5B = 0x0B;	
}

//Ready made function for PWM
void velocity (unsigned char left_motor, unsigned char right_motor)
{
	OCR5AL = (unsigned char)left_motor;
	OCR5BL = (unsigned char)right_motor;
}//velocity function used for pwm-Pulse width modulation

//specify PORT A to change direction of motor rotation
void motion_set (unsigned char Direction)
{
	unsigned char PortARestore = 0;

	Direction &= 0x0F; 			
	PortARestore = PORTA; 			
	PortARestore &= 0xF0; 			
	PortARestore |= Direction; 	
	PORTA = PortARestore; 			
}
void stop (void)
{
	motion_set(0x00);
}
void forward (void) //both wheels forward
{
	motion_set(0x06);
}
void back (void) //both wheels backward
{
	motion_set(0x09);
}
//rotate left till black line
void left (void) //Left wheel backward, Right wheel forward
{
	EIMSK = EIMSK & 0xCF;                                                         //masking position encoder interrupts
	velocity(200,200);                                                            //works with 150 150
	motion_set(0x05);
	_delay_ms(700);
	while(1)
	{
		cwl=ADC_Conversion(2);
		lcd_print(1,6,cwl,3);
		if(cwl>=10)
		{
			stop();
			_delay_ms(5);
			EIMSK = EIMSK | 0x30;
			velocity(255,253);
			break;
		}
	}
}
//rotate right till black line
void right (void) //Left wheel forward, Right wheel backward
{
	EIMSK = EIMSK & 0xCF;                                                         //masking position encoder interrupts
	velocity(200,200);
	motion_set(0x0A);
	_delay_ms(700);
	while(1)
	{
		cwl=ADC_Conversion(2);
		lcd_print(1,6,cwl,3);
		if(cwl>=10)
		{
			stop();
			_delay_ms(5);
			EIMSK = EIMSK | 0x30;
			velocity(255,253);
			break;
		}
	}
}
void soft_left (void) //Left wheel stationary, Right wheel forward
{
	motion_set(0x04);
}
void soft_right (void) //Left wheel forward, Right wheel is stationary
{
	motion_set(0x02);
}
void soft_left_2 (void) //Left wheel backward, right wheel stationary
{
	motion_set(0x01);
}
void soft_right_2 (void) //Left wheel stationary, Right wheel backward
{
	motion_set(0x08);
}
void linear_distance_mm(unsigned int DistanceInMM)
{
	float ReqdShaftCount = 0;
	unsigned long int ReqdShaftCountInt = 0;

	ReqdShaftCount = DistanceInMM / 5.338; // division by resolution to get shaft count
	ReqdShaftCountInt = (unsigned long int) ReqdShaftCount;
	
	ShaftCountRight = 0;
	while(1)
	{
		lwl = ADC_Conversion(3);
		cwl = ADC_Conversion(2);
		rwl = ADC_Conversion(1);
		lcd_print(1,1,lwl,3);							//Prints value of White Line Sensor1 LEFT
		lcd_print(1,6,cwl,3);							//Prints Value of White Line Sensor2 CENTRE
		lcd_print(1,10,rwl,3);						//Prints Value of White Line Sensor3 RIGHT
		if(ShaftCountRight > ReqdShaftCountInt)
		{
			break;
		}
	}
	stop(); //Stop robot
}
//move a fixed distane, all from experiments folder
void forward_mm(unsigned int DistanceInMM)
{
	forward();
	linear_distance_mm(DistanceInMM);
}
void back_mm(unsigned int DistanceInMM)
{
	back();
	linear_distance_mm(DistanceInMM);
}
void left_position_encoder_interrupt_init (void) //Interrupt 4 enable
{
	cli(); //Clears the global interrupt
	EICRB = EICRB | 0x02; // INT4 is set to trigger with falling edge
	EIMSK = EIMSK | 0x10; // Enable Interrupt INT4 for left position encoder
	sei();   // Enables the global interrupt
}
void right_position_encoder_interrupt_init (void) //Interrupt 5 enable
{
	cli(); 
	EICRB = EICRB | 0x08; // INT5 is set to trigger with falling edge
	EIMSK = EIMSK | 0x20; // Enable Interrupt INT5 for right position encoder
	sei();   
}
void angle_rotate(unsigned int Degrees)
{
	float ReqdShaftCount = 0;
	unsigned long int ReqdShaftCountInt = 0;

	ReqdShaftCount = (float) Degrees/ 4.090; // division by resolution to get shaft count
	ReqdShaftCountInt = (unsigned int) ReqdShaftCount;
	ShaftCountRight = 0;
	ShaftCountLeft = 0;
	while (1)
	{
		if((ShaftCountRight >= ReqdShaftCountInt) | (ShaftCountLeft >= ReqdShaftCountInt))
		break;
	}
	stop(); //Stop robot
}
void left1 (void) //Left wheel backward, Right wheel forward
{
	motion_set(0x05);
}
void right1 (void) //Left wheel forward, Right wheel backward
{
	motion_set(0x0A);
}
void left_degrees(unsigned int Degrees)
{
	// 88 pulses for 360 degrees rotation 4.090 degrees per count
	left1(); //Turn left
	angle_rotate(Degrees);
}
void right_degrees(unsigned int Degrees)
{
	// 88 pulses for 360 degrees rotation 4.090 degrees per count
	right1(); //Turn right
	angle_rotate(Degrees);
}
void soft_left_degrees(unsigned int Degrees)
{
	// 176 pulses for 360 degrees rotation 2.045 degrees per count
	soft_left(); //Turn soft left
	Degrees=Degrees*2;
	angle_rotate(Degrees);
}
void soft_right_degrees(unsigned int Degrees)
{
	// 176 pulses for 360 degrees rotation 2.045 degrees per count
	soft_right();  //Turn soft right
	Degrees=Degrees*2;
	angle_rotate(Degrees);
}
void soft_left_2_degrees(unsigned int Degrees)
{
	// 176 pulses for 360 degrees rotation 2.045 degrees per count
	soft_left_2(); //Turn reverse soft left
	Degrees=Degrees*2;
	angle_rotate(Degrees);
}
void soft_right_2_degrees(unsigned int Degrees)
{
	// 176 pulses for 360 degrees rotation 2.045 degrees per count
	soft_right_2();  //Turn reverse soft right
	Degrees=Degrees*2;
	angle_rotate(Degrees);
}

void seven_segment_pin_config (void)
{
	DDRJ = DDRJ | 0xFF; //all Seven Segments pins set as output
	DDRL = DDRL | 0x47;
	PORTL = PORTL | 0x47;
	PORTJ = 0x01;
}
/*
FUNCTIONNAME: INterrupt trigger
INPUT: NONE
OUPut: Trigger interrupt level trigg
LOGIC: Set to 0 to trigger interrupt
*/
void interruptTrigger( void )
{
	if(cwl>40 && (rwl>20 || lwl>20))         //cwl>40 && (rwl>20 || lwl>20)
	{
		PORTD = PORTD & 0xFD;						//Interrupt
		PORTD = PORTD | 0x02;
		stop();
		_delay_ms(5);
		forward_mm(35);					//65mm distance
		_delay_ms(5);
		forward();
	}
}
/*
FUNCTION NAME: Line following
Input : none
Outpur: line follow
logic: if right white line showing greater value, turn right
elsle if left wlgreater, turn left. if no white line showing greater value, use previous values to search for black line
*/
void lineFollowing( void )
{
	lwl = ADC_Conversion(3);
	cwl = ADC_Conversion(2);
	rwl = ADC_Conversion(1);
	if(lwl>15 && cwl<9)
	{
		velocity(30,240);
		a=30;
		b=240;
	}									//90,240
	else if (rwl>13 && cwl<9)							//Normal Line Following
	{
		velocity(250,30);
		a=250;
		b=30;
	}									//250,70
	else if (cwl>10 && (rwl<9 || lwl<10))
	velocity(255,253);
	else if(lwl<10 && (cwl<9 && rwl<9))
	velocity(a,b);
}
void buzzer_pin_config (void)
{
	DDRC = DDRC | 0x08;		    //Setting PORTC 3 as outpt
	PORTC = PORTC & 0xF7;		//Setting PORTC 3 logic low to turnoff buzzer
}
void color_sensor_pin_config(void)
{
	DDRD  = DDRD | 0xFE; //set PD0 as input for color sensor output
	PORTD = PORTD | 0x01;//Enable internal pull-up for PORTD 0 pin
}
void led_pin_config(void)
{
	DDRH = DDRH | 0x70;    //PH 4,5,6 output
	PORTH = PORTH | 0x70;  //initially led off all high
}

//funtions used from colour sensor tutorial
//select a filter and count the pulses
void color_sensor_scaling()		//This function is used to select the scaled down version of the original frequency of the output generated by the color sensor, generally 20% scaling is preferable, though you can change the values as per your application by referring datasheet
{
	//Output Scaling 20% from datasheet
	//PORTD = PORTD & 0xEF;
	PORTD = PORTD | 0x10; //set S0 high
	//PORTD = PORTD & 0xDF; //set S1 low
	PORTD = PORTD | 0x20; //set S1 high
}
void filter_red(void)    //Used to select red filter
{
	//Filter Select - red filter
	PORTD = PORTD & 0xBF; //set S2 low
	PORTD = PORTD & 0x7F; //set S3 low
}
void filter_green(void)	//Used to select green filter
{
	//Filter Select - green filter
	PORTD = PORTD | 0x40; //set S2 High
	PORTD = PORTD | 0x80; //set S3 High
}
void filter_blue(void)	//Used to select blue filter
{
	//Filter Select - blue filter
	PORTD = PORTD & 0xBF; //set S2 low
	PORTD = PORTD | 0x80; //set S3 High
}
void red_read(void) // function to select red filter and display the count generated by the sensor on LCD. The count will be more if the color is red. The count will be very less if its blue or green.
{
	//Red
	EIMSK = EIMSK | 0x01;
	filter_red(); //select red filter
	pulse=0; //reset the count to 0
	_delay_ms(100); //capture the pulses for 100 ms or 0.1 second
	red = pulse;  //store the count in variable called red
	
	EIMSK = EIMSK & 0xFE;
}
void green_read(void) // function to select green filter and display the count generated by the sensor on LCD. The count will be more if the color is green. The count will be very less if its blue or red.
{
	//Green
	EIMSK = EIMSK | 0x01;
	filter_green(); //select green filter
	pulse=0; //reset the count to 0
	_delay_ms(100); //capture the pulses for 100 ms or 0.1 second
	green = pulse;  //store the count in variable called green
	
	EIMSK = EIMSK & 0xFE;
}
void blue_read(void) // function to select blue filter and display the count generated by the sensor on LCD. The count will be more if the color is blue. The count will be very less if its red or green.
{
	//Blue
	EIMSK = EIMSK | 0x01;
	filter_blue(); //select blue filter
	pulse=0; //reset the count to 0
	_delay_ms(100); //capture the pulses for 100 ms or 0.1 second
	blue = pulse;  //store the count in variable called blue
	
	EIMSK = EIMSK & 0xFE;
}

/*FUNCTION NAME:
MOVE SHOP
Input: current locaiotn at shop and next shop location to go to.
LOGIC:

If at pizza counter, specify by 's'. If going to 6,7,8,9,10, robot takes right and counts black boxes on flex it encounters before turning
similar for 1,2,3,4,5
If location is non start, then depending on if first shop is on one side and second shop on other side, interrupt is counted
black squares increment shop node
*/
void move_shop (unsigned char current, unsigned char next)
{
	if(current=='s')
	{
		if(next>5)																		//from start to right pizza
		{
			shop_node=5;
			right();
			_delay_ms(10);
			forward();
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==next)
				{
					stop();
					_delay_ms(20);
					forward_mm(40);
					forward_mm(60);
					_delay_ms(10);
					soft_right_2_degrees(95);
					break;
				}
				
			}
		}
		if(next<=5)																	//from start to left pizza
		{
			shop_node=next;
			left();
			right_degrees(10);
			_delay_ms(10);
			forward();
			while(1)
			{
				lineFollowing();
				interruptTrigger();
			    if(shop_node==6)
				{
                    stop();
                    _delay_ms(20);
                    forward_mm(40);
                    forward_mm(60);
                    _delay_ms(10);
					soft_left_2_degrees(92);
					break;
				}
			}
		}
	}
	else                                                                                    //inter node assumed robot facing houses
	{
		if((next-current)>0)																//from one pizza to another
		{
			
			shop_node=0;
			soft_left_degrees(90);
			back_mm(10);
			_delay_ms(10);
			forward();
			if((current>5&&next>5)||(current<=5&&next<=5))
			{
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==(next-current))
					{
						stop();
						_delay_ms(20);
						forward_mm(40);
						forward_mm(60);
						_delay_ms(10);
						soft_right_2_degrees(97);
						break;
					}
				}
			}
		    if((current<=5&&next>5))
			{
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==((next-current)+1))
					{
						stop();
						_delay_ms(20);
						forward_mm(40);
						forward_mm(60);
						_delay_ms(10);
						soft_right_2_degrees(97);
						break;
					}
				}
			}
		}
		if((next-current)<0)
		{
			shop_node=0;
			soft_right_degrees(90);
			back_mm(10);
			_delay_ms(10);
			forward();
			if((current>5&&next>5)||(current<=5&&next<=5))
			{
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==(current-next))
					{
						stop();
						_delay_ms(20);
						forward_mm(40);
						forward_mm(60);
						_delay_ms(10);
						soft_left_2_degrees(92);
						break;
					}
				}
			}
			if((current>5&&next<=5))
			{
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==((current-next)+1))
					{
						stop();
						_delay_ms(20);
						forward_mm(40);
						forward_mm(60);
						_delay_ms(10);
						soft_left_2_degrees(92);
						break;
					}
				}
			}
		}
	}
}

/*FINCTION : Return shop
Depending on location of shop, it takes left or right turn and travels towards pizza counter
black squares used to counter interrupt driven shop node variable. Logical equations determine if counter reached or no
*/  
void return_shop(unsigned char shop)
{
	if(shop<=5)
	{
		shop_node=0;													
		soft_left_degrees(90);
		back_mm(10);											//Before return robot should face the direction of the houses
		_delay_ms(5);
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==(6-shop))
			{
				stop();
				forward_mm(30);
				_delay_ms(5);
				left();
				_delay_ms(5);
				break;
			}
		}
	}
	if(shop>5)
	{
		shop_node=0;
		soft_right_degrees(90);
		back_mm(10);
		_delay_ms(5);
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==(shop-5))
			{
				stop();
				forward_mm(30);
				_delay_ms(5);
				right();
				_delay_ms(5);
				break;
			}
		}
	}
}

//Travel from one node to next node. IF CONdition specifies values of wl sensor reading at black sqaures 
void node_node( void )
{
	forward();
	while(1)
	{
		ssd_display(time_ssd);
		lineFollowing();
		if(cwl>40 && (rwl>20 || lwl>20))
		{
			stop();
			forward_mm(75);
			_delay_ms(5);
			break;
		}
	}
}

/*FUNCTION: OUTER RING
LOGIC:  Houses divided into outer road and inner road. outer=1,2,3,4,5,6,7,8
rest are inside.
depending on house value, robot takes left or right. Interrupts used to count number of black squares crossed to reach
particular destinatatioin which satisfies condition in IF
*/
//Input is one house to which to got to  
void outer_ring(unsigned char h1)
{
	shop_node=0;							//assume outer ring node
	if(h1<=4)								//bifurcation point between h1-h4 and h5-h8
	left();
	else
	right();
	forward();
	
		if((h1>2) && (h1<=4))
		{
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==(h1+1))
			    {
				   stop();
				   forward_mm(20);
				   _delay_ms(5);
				   left();
				   node_node();
				   break;
				}
			}
		}
		else if(h1<3)
		{
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==h1)
			    {
				  stop();
				  forward_mm(20);
				  _delay_ms(5);
				  left();
				  node_node();
				  break;
				}
			}
		}
		else if(h1>6)
		{
			shop_node=h1;
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==9)
			    {
				   stop();
				   forward_mm(20);
				   right();
				   node_node();
				   break;
				}
			}
		}
		else
		{
			shop_node=h1;
			while(1)
			{ 
				lineFollowing();
				interruptTrigger();
				if(shop_node==10)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					right();
					node_node();
					break;
				}
			}
		}
}
//Similar function to identify houses in the inner ring
void inner_ring(unsigned char h1)
{
	shop_node=0;
	if((h1==9) || (h1==10))
	{
		left();
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==1)
			{
				if(h1==9)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					break;
				}
				else
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					right();
					forward();
					break;
				}
			}
		}
		if(h1==10)
	    {
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==3)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					break;
				}
			}
		}
	}
	else
	{
		right();
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==1)
			{
				if(h1==12)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					break;
				}
				else
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					left();
					forward();
					break;
				}
			}
		}
		if(h1==11)
		{
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==3)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					break;
				}
			}
		}
	}
}
//Outer outer 
//travel between two houses in the outer loop 
//two inputs to identify house to reach from first house
//expressions determine if house reached or not
void outer_outer(unsigned char h1, unsigned char h2)
{
	shop_node=0;
	if(((h2-h1)<=4 && (h2-h1)>0)||((h2-h1)<=-4 && (h2-h1)>=-7))			//conditions robot takes left
	{
		left();
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if((h2-h1)<=4 && (h2-h1)>0)
		    {
			    if(h1%2==0)													//even house
			    {
					if(h2<=h1+2)									
					{
						if(shop_node==(h2-h1+1))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					else
					{
						if(shop_node==(h2-h1+2))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
			    }
				else                                                  //odd house
				{
					if(h2==h1+1)
					{
						if(shop_node==(h2-h1))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					else if(h2<=h1+3)
					{
						if(shop_node==(h2-h1+1))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					else
					{
						if(shop_node==(h2-h1+2))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
				}
		    }
			else
			{
				if(h1%2==0)
				{
					if(((h2-h1)==-7) || ((h2-h1)==-6))
					{
						if(shop_node==(9+(h2-h1)))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					if(((h2-h1)==-5) || ((h2-h1)==-4))
					{
						if(shop_node==(10+(h2-h1)))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
				}
				else
				{
					if(((h2-h1)==-6) || ((h2-h1)==-5))
					{
						if(shop_node==(9+h2-h1))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					if((h2-h1)==-4)
					{
						if(shop_node==(10+h2-h1))
						{
							stop();
							forward_mm(30);
							_delay_ms(5);
							break;
						}
					}
				}
			}
		}
		left();
		forward();
		while(1)
		{
			lineFollowing();
			if(cwl>40 && (rwl>20 || lwl>20))
			{
				stop();
				forward_mm(65);
				_delay_ms(5);
				break;
			}
		}
	}
	else
	{
		right();						
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(((h2-h1)<=-1)&&((h2-h1)>=-3))			//Condition robot takes right			
			{
				if(h1%2==0)
				{
					if(h2==h1-1)
					{
						if(shop_node==1)
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					else 
					{
						if(shop_node==(h1-h2+1))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
				}
				else
				{
					if((h1-h2)<=2)
					{
						if(shop_node==(h1-h2+1))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					else
					{
						if(shop_node==(h1-h2+2))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
				}
			}
			else
			{
				if(h1%2==0)
				{
					if(shop_node==(9-h2+h1))
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						break;
					}
				}
				else
				{
					if((h2-h1)>=6)
					{
						if(shop_node==(9-(h2-h1)))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
					else
					{
						if(shop_node==(10-(h2-h1)))
						{
							stop();
							forward_mm(20);
							_delay_ms(5);
							break;
						}
					}
				}
			}
		}
		right();
		forward();
		while(1)
		{
			lineFollowing();
			if(cwl>40 && (rwl>20 || lwl>20))
			{
				stop();
				forward_mm(65);
				_delay_ms(5);
				break;
			}
		}
	}
}
//travel between houses on the inner node
void inner_inner(unsigned char h1, unsigned char h2)
{
	if((h2-h1)==1||(h2-h1)==-3)
	{
		right();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				break;
			}
		}
	}
	if((h2-h1)==2 || (h2-h1)==-2)
	{
		right();
		_delay_ms(5);
		right();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				break;
			}
		}
	}
	if((h2-h1)==-1 || (h2-h1)==3)
	{
		left();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				break;
			}
		}
	}
}
//travel between houses one in outer and one in inner
//robot travels from outer house to point where inner house sqaure and straight line from pizza counter meet
//from that point, inner loop function is called
void outer_inner(unsigned char h1, unsigned char h2)
{
	if((h1%2)==0)
	{
	   left();
	   forward();
	   shop_node=0;
	   while(1)
	   {
		   lineFollowing();
		   interruptTrigger();
		   if(shop_node==1)
		   {
			   stop();
			   forward_mm(20);
			   _delay_ms(5);
			   right();
			   forward();
			   break;
		   }
	   }
	   while(1)
	   {
		   lineFollowing();
		   interruptTrigger();
		   if(shop_node==2)
		   {
			   stop();
			   forward_mm(20);
			   _delay_ms(5);
			   break;
		   }
	   }
	}
	else
	{
		right();
		forward();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==1)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				left();
				forward();
				break;
			}
		}
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				break;
			}
		}
	}
	if((h1==2) || (h1==3))
	{
		right();
		forward();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==1)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				left();
				forward();
				break;
			}
		}
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				break;
			}
		}
		left_degrees(90);
	}
	else if(h1==7|| h1==6)
	{
		left();
		forward();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==1)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				right();
				forward();
				break;
			}
		}
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				break;
			}
		}
		right_degrees(90);
	}
	else if((h1==4) || (h1==5))
	{
		left();
		forward();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==1)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				right();
				forward();
				break;
			}
		}
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==3)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				right();
				forward();
				break;
			}
		}
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==4)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				right_degrees(90);
				break;
			}
		}
	}
	else;   //nothing to do if 1 or 8
	inner_ring(h2);
} 
//similar logic fpr inner outer, but only in this case the rbot does not travel to outer loop intersection with straight line form pizza counter
//rathyer it moves to closest house on the outer loop to save time 
void inner_outer(unsigned char h1, unsigned char h2)
{
	right();
	forward();
	shop_node=0;
	while(1)
	{
		lineFollowing();
		interruptTrigger();
		if(shop_node==1)
		{
			stop();
			forward_mm(20);
			_delay_ms(5);
			left();
			forward();
			break;
		}
	}
	while(1)
	{
		lineFollowing();
		interruptTrigger();
		if(shop_node==2)
		{
			stop();
			forward_mm(20);
			_delay_ms(5);
			right();
			forward();
			break;
		}
	}
	while(1)
	{
		lineFollowing();
		interruptTrigger();
		if(shop_node==3)
		{
			stop();
			forward_mm(20);
			_delay_ms(5);
			right_degrees(90);
			break;
		}
	}
    if(h1==9)
	outer_outer(3,h2);
	else if(h1==10)
	outer_outer(5,h2);
	else if(h1==11)
	outer_outer(7,h2);
	else
	outer_outer(1,h2);
}
//returns from any house on the flex to the pizza counter. Depending on inner and outer loop houses, it retraces it's step back towards pizza counter
void return_house(unsigned char h1)
{
	if(h1<=4)
	{
		right();
		forward();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(h1>2)
			{
				if(shop_node==h1+1)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					right();
					break;
				}
			}
			else
			{
				if(shop_node==h1)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					right();
					break;
				}
			}
		}
		while(1)
		{
			lineFollowing();
			forward();
			if(cwl>40 && (rwl>20 || lwl>20))
			{
				stop();
				_delay_ms(5);
				forward_mm(65);
				right_degrees(180);
				break;
			}
		}
	}
	else if(h1>4 && h1<9)
	{
		shop_node=4;
		left();
		forward();
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(h1<7)
			{
				if(shop_node==10)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					left();
					break;
				}
			}
			else
			{
				if(shop_node==9)
				{
					stop();
					forward_mm(20);
					_delay_ms(5);
					left();
					break;
				}
			}
		}
		while(1)
		{
			lineFollowing();
			forward();
			if(cwl>40 && (rwl>20 || lwl>20))
			{
				stop();
				_delay_ms(5);
				forward_mm(65);
				right_degrees(180);
				break;
			}
		}
	}
	else
	{
		if(h1>10)
		{
			right();
			shop_node=0;
			forward();
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==1)
				{
					if(h1==12)
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						left();
						break;
					}
					else
					break;
				}
			}
			if(h1==11)
			{
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==2)
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						right();
						forward();
						break;
					}
				}
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==3)
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						left();
						break;
					}
				}
			}
		}
		else
		{
			left();
			shop_node=0;
			forward();
			while(1)
			{
				lineFollowing();
				interruptTrigger();
				if(shop_node==1)
				{
					if(h1==9)
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						right();
						break;
					}
					else
					break;
				}
			}
			if(h1==10)
			{
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==2)
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						left();
						forward();
						break;
					}
				}
				while(1)
				{
					lineFollowing();
					interruptTrigger();
					if(shop_node==3)
					{
						stop();
						forward_mm(20);
						_delay_ms(5);
						right();
						break;
					}
				}
			}
		}
		forward();
		shop_node=0;
		while(1)
		{
			lineFollowing();
			interruptTrigger();
			if(shop_node==2)
			{
				stop();
				forward_mm(20);
				_delay_ms(5);
				right_degrees(180);
				break;
			}
		}
	}
}

/*MOVE HOUSE
LOGIC: COMpares it's two house inputs to determine which function needs to be called
comparision is if house>9 or lesser than 9 
*/
void move_house(unsigned char house1, unsigned char house2)
{
	if(house1=='s')
	{
		node_node();
		if(house2<=8)
		outer_ring(house2);
		else
		{
			node_node();
			inner_ring(house2);
		}
	}
	else if(house1<9 && house2<9)
	outer_outer(house1, house2);
	else if(house1>8 && house2>8)
	inner_inner(house1, house2);
	else if(house1<9 && house2>8)
	outer_inner(house1, house2);
	else
	inner_outer(house1, house2);
}

//servo motor is given pwm values
//free is used to unlock servo by giving 100% duty ccycle
//from experiment folders
void servo_1(unsigned char degrees)
{
	float PositionPanServo = 0;
	PositionPanServo = ((float)degrees / 1.86) + 35.0;
	OCR1AH = 0x00;
	OCR1AL = (unsigned char) PositionPanServo;
}
void servo_2(unsigned char degrees)
{
	float PositionTiltServo = 0;
	PositionTiltServo = ((float)degrees / 1.86) + 35.0;
	OCR1BH = 0x00;
	OCR1BL = (unsigned char) PositionTiltServo;
}
void servo_3(unsigned char degrees)
{
	float PositionServo = 0;
	PositionServo = ((float)degrees / 1.86) + 35.0;
	OCR1CH = 0x00;
	OCR1CL = (unsigned char) PositionServo;
}
void servo_1_free (void) //makes servo 1 free rotating
{
	OCR1AH = 0x03;
	OCR1AL = 0xFF; //Servo 1 off
}
void servo_2_free (void) //makes servo 2 free rotating
{
	OCR1BH = 0x03;
	OCR1BL = 0xFF; //Servo 2 off
}
void servo_3_free (void) //makes servo 3 free rotating
{
	OCR1CH = 0x03;
	OCR1CL = 0xFF; //Servo 3 off
}

//function: sense
//raises robotic arm at a particular value
//uses sharp sensor to detect size
//used colour sensor to detect colour
void sense(unsigned char shop_current)
{
	volatile int sharpblock=0;
	_delay_ms(100);
	servo_1(75);
	servo_2(125);
	sharpblock=ADC_Conversion(11);
	if(sharpblock>106)
	pizza_shop[shop_current-1]=0x03;
	else if(sharpblock>86)
	pizza_shop[shop_current-1]=0x02;
	else if(sharpblock>65)
	pizza_shop[shop_current-1]=0x01;
	else
	pizza_shop[shop_current-1]=0x00;
	_delay_ms(200);
	if(pizza_shop[shop_current-1]!=0x00)
	{
		color_sensor_scaling();
		if(pizza_shop[shop_current-1]==0x01)
		{
			servo_1(112);
			servo_2(150);
		}
		else if(pizza_shop[shop_current-1]==0x02)
		{
			servo_1(100);
			servo_2(140);
		}
		else
		{
			servo_1(100);
			servo_2(140);
		}
		_delay_ms(200);
		red_read();
		green_read();
		blue_read();
		if(red>1.3*green && red>blue)
		{
			pizza_shop[shop_current-1]=pizza_shop[shop_current-1] | 0x10;
		}
		else if(blue>red && blue>green)
		{
			pizza_shop[shop_current-1]=pizza_shop[shop_current-1] | 0x30;
		}
		else
		pizza_shop[shop_current-1]=pizza_shop[shop_current-1] | 0x20;
		
	}
	servo_1(50);
	servo_2(110);
	_delay_ms(100);
}

ISR(INT5_vect)
{
	ShaftCountRight++;  
}
ISR(INT4_vect)
{
	ShaftCountLeft++;  
}
//increment variable time_ssd every second
ISR(TIMER4_OVF_vect)
{
	TCNT4H = 0x1F; 
	TCNT4L = 0x01; 
    time_ssd++;
}
//if ssd_freeze set, calls another function which sounds the buzzer for one second and freezes the SSD module
//else it refreshes ssd every 30ms
ISR(TIMER3_OVF_vect)
{
	//TIMER3 has overflowed
	sei();
	TCNT3H = 0x1F; //reload counter high value
	TCNT3L = 0x01; //reload counter low value
	if(ssd_freeze==0)
	ssd_display(time_ssd);
	else
	{
		fast_time++;
		delivered_control();
	} 
}
//increment variable everytime it passes black square
ISR(INT1_vect)
{
	sei();
	shop_node++;
}
ISR(INT0_vect)
{ 
	sei();
	pulse++; //increment on receiving pulse from the color sensor
}

//various initializations
void timer4_init(void)                                                                           //SSD_timer
{
	TCCR4B = 0x00; //stop
	TCNT4H = 0x1F; 
	TCNT4L = 0x01; 
	OCR4AH = 0x00; 
	OCR4AL = 0x00; 
	OCR4BH = 0x00; 
	OCR4BL = 0x00; 
	OCR4CH = 0x00; 
	OCR4CL = 0x00; 
	ICR4H  = 0x00; 
	ICR4L  = 0x00; 
	TCCR4A = 0x00;
	TCCR4C = 0x00;
	TCCR4B = 0x04; //start Timer
}
void timer1_init(void)                                                                           //SERVO Timer
{
 TCCR1B = 0x00; //stop
 TCNT1H = 0xFC; //Counter high value to which OCR1xH value is to be compared with
 TCNT1L = 0x01;	//Counter low value to which OCR1xH value is to be compared with
 OCR1AH = 0x03;	//Output compare Register high value for servo 1
 OCR1AL = 0xFF;	//Output Compare Register low Value For servo 1
 OCR1BH = 0x03;	//Output compare Register high value for servo 2
 OCR1BL = 0xFF;	//Output Compare Register low Value For servo 2
 OCR1CH = 0x03;	//Output compare Register high value for servo 3
 OCR1CL = 0xFF;	//Output Compare Register low Value For servo 3
 ICR1H  = 0x03;	
 ICR1L  = 0xFF;
 TCCR1A = 0xAB; /*{COM1A1=1, COM1A0=0; COM1B1=1, COM1B0=0; COM1C1=1 COM1C0=0}
 					For Overriding normal port functionality to OCRnA outputs.
				  {WGM11=1, WGM10=1} Along With WGM12 in TCCR1B for Selecting FAST PWM Mode*/
 TCCR1C = 0x00;
 TCCR1B = 0x0C; //WGM12=1; CS12=1, CS11=0, CS10=0 (Prescaler=256)
}																		
void timer3_init(void)
{
	TCCR3B = 0x00; //stop
	TCNT3H = 0x1F; //Counter higher 8 bit value
	TCNT3L = 0x01; //Counter lower 8 bit value
	OCR3AH = 0x00; //Output Compair Register (OCR)- Not used
	OCR3AL = 0x00; //Output Compair Register (OCR)- Not used
	OCR3BH = 0x00; //Output Compair Register (OCR)- Not used
	OCR3BL = 0x00; //Output Compair Register (OCR)- Not used
	OCR3CH = 0x00; //Output Compair Register (OCR)- Not used
	OCR3CL = 0x00; //Output Compair Register (OCR)- Not used
	ICR3H  = 0x00; //Input Capture Register (ICR)- Not used
	ICR3L  = 0x00; //Input Capture Register (ICR)- Not used
	TCCR3A = 0x00;
	TCCR3C = 0x00;
	TCCR3B = 0x02;// 8bit prescalar
}

void node_interrupt_init( void )
{
	cli();
	DDRD = DDRD | 0x02;    //Initialize PD1 as output
	PORTD = PORTD | 0x02;  //Initially pin held high
	EICRA = EICRA & 0xF3;  //INT1 low level
	EIMSK = EIMSK | 0x02;  //Enable Interrupt 1
	sei();
}
void color_sensor_pin_interrupt_init(void) //Interrupt 0 enable
{
	cli(); //Clears the global interrupt
	EICRA = EICRA | 0x02; // INT0 is set to trigger with falling edge
	sei(); // Enables the global interrupt
}
void port_init( void )
{
	buzzer_pin_config();
	color_sensor_pin_config();//color sensor pin configuration
	led_pin_config();
	lcd_port_config();
	adc_pin_config();
	motion_pin_config();
	left_encoder_pin_config(); 
	right_encoder_pin_config(); 
	seven_segment_pin_config();
	servo1_pin_config(); //Configure PORTB 5 pin for servo motor 1 operation
	servo2_pin_config(); //Configure PORTB 6 pin for servo motor 2 operation
	servo3_pin_config(); //Configure PORTB 7 pin for servo motor 3 operation
}
void init_devices (void)
{
	cli(); //Clears the global interrupts
	port_init();
	adc_init();
	timer1_init();
	timer5_init();
	timer4_init();
	TIMSK4 = 0x01; //timer4 overflow interrupt enable
	timer3_init();
	TIMSK3 = 0x01; //timer3 overflow interrupt enable
	left_position_encoder_interrupt_init();
	right_position_encoder_interrupt_init();
	node_interrupt_init();
	color_sensor_pin_interrupt_init();
	sei(); //Enables the global interrupts
}

//INPUT: SIZE AND LIFT
//depending on size the arm adjusts itseld. LIFT specifies if robot to be held in robot structure or lifted by arm
void pick_block(unsigned char sizes, unsigned char lift)
{
	short int i;
	_delay_ms(100);
	if(sizes=='s')
	{
		servo_1(110);
		servo_2(155);
		_delay_ms(100);
		servo_3(45);
		_delay_ms(100);
		if(lift==1)
		{
			
			for(i=0;i<=30;i++)
			{
				servo_1(112-i);
				servo_2(150+0.5*i); 
				_delay_ms(25);
			}
			for(i=0;i<=35;i++)
			{
				servo_1(82-i);
				servo_2(160-2.5*i);
				_delay_ms(25);
			}
			servo_3(0);
			_delay_ms(200);
			servo_1(50);
			servo_2(140);
		}
		else
		{
			servo_1(60);
			_delay_ms(100);
			servo_2(150);
			_delay_ms(100);
		}
	}
	else if (sizes=='m')
	{
		servo_1(110);
		servo_2(150);
		_delay_ms(100);
		servo_3(40);
		_delay_ms(100);
		if(lift==1)
		{
			for(i=0;i<=30;i++)
			{
				servo_1(110-i);										//servo1 should be 60 and servo2 120
				servo_2(150+0.3*i);
				_delay_ms(25);
			}
			for(i=0;i<=40;i++)
			{
				servo_1(80-i);
				servo_2(160-(2.5*i));
				_delay_ms(25);
			}
			servo_3(0);
			_delay_ms(200);
			servo_1(50);
			servo_2(140);
		}
		else
		{
			servo_1(60);
			_delay_ms(100);
			servo_2(150);
			_delay_ms(100);
		}
	}
	else
	{
		servo_1(100);
	
		servo_2(120);
		_delay_ms(100);
		servo_3(45);
		_delay_ms(100);
		if(lift==1)
		{
			for(i=0;i<=30;i++)
			{
				servo_1(100-i);
				servo_2(120+0.75*i);
				_delay_ms(25);													//servo1 be 50 and servo2 be 115
			}
			for(i=0;i<=40;i++)
			{
				servo_1(70-i);
				servo_2(140-(1.9*i));
				_delay_ms(25);
			}
			servo_3(0);
			_delay_ms(200);
			servo_1(50);
			servo_2(140);
		}
		else
		{
			servo_1(60);
			_delay_ms(100);
			servo_2(150);
		}
	}
	_delay_ms(100);
}
//drop the block corresponding to size and if the block was carried in upper level or by the ARM
void drop_block(unsigned char sizes, unsigned char lift)
{
	short int i;
	if(sizes=='s')
	{
		if(lift==1)
		{
			servo_1(65);
			_delay_ms(100);
			servo_2(82);
			_delay_ms(100);
			servo_3(45);
			_delay_ms(100);
			for(i=0;i<=20;i++)
			{
				servo_1(65+i);
				servo_2(82+(2.2*i));
				_delay_ms(25);
			}
			for(i=0;i<=20;i++)
			{
				servo_1(92+i);
				_delay_ms(25);
			}
			servo_3(0);
			_delay_ms(200);
			servo_1(40);
			servo_2(150);
		}
		else
		{
			servo_1(60);
			servo_2(40);
			_delay_ms(200);
			servo_3(0);
		}
	}
	else if (sizes=='m')
	{
			if(lift==1)
			{
				servo_1(30);
				servo_2(50);
				_delay_ms(300);
				servo_3(45);
				_delay_ms(100);
				for(i=0;i<=30;i++)
				{
					servo_1(30+i);
					servo_2(50+(1.8*i));
					_delay_ms(25);
				}
				for(i=0;i<=30;i++)
				{
					servo_1(60+i);
					_delay_ms(25);
				}
				servo_3(0);
				_delay_ms(200);
				servo_1(30);
				_delay_ms(100);
				servo_2(120);
			}
			else
			{
				servo_1(100);
				_delay_ms(100);
				servo_2(110);
				servo_3(0);
				_delay_ms(100);
			}
	}
	else
	{
		if(lift==1)
		{
			servo_1(30);
			servo_2(60);
			_delay_ms(300);
			servo_3(50);
			_delay_ms(100);
			for(i=0;i<=30;i++)
			{
				servo_1(30+i);
				servo_2(70+(i));
				_delay_ms(25);
			}
			for(i=0;i<=10;i++)
			{
				servo_1(60+i);
				servo_2(100+i);
				_delay_ms(25);
			}
			servo_3(0);
			_delay_ms(200);
			servo_1(70);
			servo_2(90);
		}
		else
		{
			servo_1(75);
			servo_2(110);
			servo_3(0);
			_delay_ms(100);	
		}
	}
	servo_1(30);
	servo_2(110);
}

//INPUT: clour obtained by masking timeline bits
//PORTH used to control LED
void led_color(unsigned char colour)
{
	unsigned char temp_led;
	temp_led=PINH;
	temp_led=temp_led & 0x8F;
	if(colour==0x01)
	{
		temp_led=temp_led | 0xEF;		//red    D for green //B for blue
		PORTH=temp_led;
	}
	else if (colour==0x02)
	{
		temp_led=temp_led | 0xDF;		//red    D for green //B for blue
		PORTH=temp_led;
	}
	else if (colour==0x03)
	{
		temp_led=temp_led | 0xBF;		//red    D for green //B for blue
		PORTH=temp_led;
	}
	else
	PORTH = PORTH | 0x7F;
}

//FUNCTION NAME: LOGIC
//SOUL OF THE CODE
//DIVIDED INTO:
//ORDER ACCEPT FROM TIMELINE
//PIZZA PICK
//PIZZA DROP

//PLEASE GO THROUGH THIS CODE with comments inside wherever required present
void logic (void)
{
	short int k;
	short int i;
	short int l=0;
	short int n=0;
	short int o=0;
	short int j=0;
	unsigned char houseconvert0;
	unsigned char houseconvert1;
	unsigned char houseconvert2;
	unsigned char houseconvert3;
	unsigned char pizzaconvert0;
	unsigned char pizzaconvert1;
	unsigned char pizzaconvert2;
	unsigned char pizza1size;
	unsigned char pizza2size;
	unsigned char pizza1colour;
	unsigned char pizza2colour;
	unsigned int order_process[3] = {0, 0, 0};
	unsigned int order_accepted[2] = {0, 0};
restart:	for(k=0;k<=7;k++)						//job scheduling. k=1 means corresponding order delivered. 0 indicates not delivered.
											//                k=2 means delivery later time (TIP time elapsed for example)
	{
		if(order_track[k]==0)
		{
			order_process[l]=k;							//passing of order numbers to different array, which holds orders
			l++;										//that need to be in consideration for delivery management
			if(l==3)									//at a time three orders considered
			break;										//THESE NUMBERS ARE USED TO DETERMINE HOUSE and PIZZA
		}
	}
	if(order_process[0]==0 && order_process[1]==0 && order_process[0]==0)      //0 all three places indicates order_track array as either 1 or 2.
	{																		   //initially, order track all 0
		for(k=0;k<=7;k++)
		{																	   //as robot keeps delivering, order_track changes value to 1 or 2
			if(order_track[k]==2)						                       // 1 indicate delivered and 2 means delivery later time
			{																   //if order track non zero, no new orders pending, we can deliver earlier orders
				order_process[l]=k;							                   //(denoted by 2) without affecting tip
				l++;															   
				if(l==3)
				break;
			}
		}
	}
	houseconvert0=order_timeline_1[order_process[0]] & 0x0F;
	houseconvert1=order_timeline_1[order_process[1]] & 0x0F;     //decode house from timeline value
	houseconvert2=order_timeline_1[order_process[2]] & 0x0F;	 //please refer to start of code for how order
	houseconvert3=order_timeline_1[order_process[2]+1] & 0x0F;   //stored in timeline array. Order process used to access various 
	pizzaconvert0=order_timeline_1[order_process[0]] & 0xF0;	 //timeline orders
	pizzaconvert1=order_timeline_1[order_process[1]] & 0xF0;
	pizzaconvert2=order_timeline_1[order_process[2]] & 0xF0;
	
	//ORDER DECIDING PHASE. WHICH ORDERS RoBoT should accept based on type of pizza 
	if(order_type[order_process[0]]=='r' && order_type[order_process[1]]=='r')
	{
		if((houseconvert0<0x04 && houseconvert1<0x04) || (((houseconvert0>0x04)&&(houseconvert0<=0x08)) && ((houseconvert1>0x04)&&(houseconvert1<=0x08))) || abs(houseconvert0-houseconvert1)== (1||7))
		//If order house lies on same side of flex or adjacent accross the sides. Eg H4 and H5
		{
			if(order_timeline_1[order_process[1]]-order_timeline_1[order_process[0]]<=10)  //difference between two orders is 10s
			{
				for(n=0;n<=9;n++)
				{
					if(pizza_shop[n]==pizzaconvert0)
					break;
				}
				for(o=0;o<=9;o++)
				{
					if(pizza_shop[o]==pizzaconvert1)
					break;
				}
				if(n!=10 && o!=10)    //if difference is less than 10 and pizza locations known 
				{
					attempt_two_delivery=1;
					order_accepted[0]=order_process[0];
					order_accepted[1]=order_process[1];
					order_track[order_process[0]]=1;
					order_track[order_process[1]]=1;
				}
			}
		}
		else         //attempt single delivery if above conditions not specified. For TWO REGULAR
		{
			attempt_two_delivery=0;
			order_accepted[0]=99;  //99 indicating only one order
			order_accepted[1]=order_process[0];
			order_track[order_process[0]]=1;
		}
	}
	else if(order_type[order_process[0]]=='r' && order_type[order_process[1]]=='r' && order_type[order_process[2]]=='p')
	{
		if((houseconvert0<0x04 && houseconvert1<0x04) || (((houseconvert0>0x04)&&(houseconvert0<=0x08)) && ((houseconvert1>0x04)&&(houseconvert1<=0x08))) || abs(houseconvert0-houseconvert1)== (1||7))
		{
			if(order_timeline_1[order_process[1]]-order_timeline_1[order_process[0]]<=10)
			{
				for(n=0;n<=9;n++)
				{
					if(pizza_shop[n]==pizzaconvert0)
					break;
				}
				for(o=0;o<=9;o++)
				{
					if(pizza_shop[o]==pizzaconvert1)
					break;
				}
				if(n!=10 && o!=10)
				{
					attempt_two_delivery=1;
					order_accepted[0]=order_process[0];
					order_accepted[1]=order_process[1];
					order_track[order_process[0]]=1;
					order_track[order_process[1]]=1;
				}
			}
		}
		//Special case, If the third order taken into consideration is preorder and time window lies somehwere near delivery time
		//of regular pizza. 
		else if((order_time[order_process[2]]<=order_time[order_process[0]]+55) && (houseconvert3!=houseconvert2))
		{
			for(n=0;n<=9;n++)
			{
				if(pizza_shop[n]==pizzaconvert0)
				break;
			}
			for(o=0;o<=9;o++)
			{
				if(pizza_shop[o]==pizzaconvert2)
				break;
			}
			if(n!=10 && o!=10)
			{
				attempt_two_delivery=1;
				order_accepted[0]=order_process[0];
				order_accepted[1]=order_process[2];
				order_track[order_process[0]]=1;
				order_track[order_process[2]]=1;
			}
			
		}
		else
		{
			attempt_two_delivery=0;
			order_accepted[0]=99;
			order_accepted[1]=order_process[0];
			order_track[order_process[0]]=1;
		}
	}
	else if(order_type[order_process[0]]=='r' && order_type[order_process[1]]=='p')
	{
		if((order_time[order_process[1]]<=order_time[order_process[0]]+60) && (houseconvert1!=houseconvert2))
		//the second condition in loop checks for if same house orders two pizzas. if it does, then preorder pizza is dropped
		//from consideration. better deliver the two pizzas in one go only rather than separate it.
		{
			for(n=0;n<=9;n++)
			{
				if(pizza_shop[n]==pizzaconvert0)
				break;
			}
			for(o=0;o<=9;o++)
			{
				if(pizza_shop[o]==pizzaconvert1)
				break;
			}
			if(n!=10 && o!=10)
			{
				attempt_two_delivery=1;
				order_accepted[0]=order_process[0];
				order_accepted[1]=order_process[1];
				order_track[order_process[0]]=1;
				order_track[order_process[1]]=1;
			}
			
		}
		else
		{
			attempt_two_delivery=0;
			order_accepted[0]=99;
			order_accepted[1]=order_process[0];
			order_track[order_process[0]]=1;
		}
	}
	else if(order_type[order_process[0]]=='p' && order_type[order_process[1]]=='r')
	//advantage of preorder lies on double time window. Halfway past the time window, it acts as regular order only.
	//hence similar condition to TWO REGULAR pizza orders.
	{
		if((houseconvert0<0x04 && houseconvert1<0x04) || (((houseconvert0>0x04)&&(houseconvert0<=0x08)) && ((houseconvert1>0x04)&&(houseconvert1<=0x08))) || abs(houseconvert0-houseconvert1)== (1||7))
		{
			if(order_timeline_1[order_process[1]]-order_timeline_1[order_process[0]]<=10)
			{
				for(n=0;n<=9;n++)
				{
					if(pizza_shop[n]==pizzaconvert0)
					break;
				}
				for(o=0;o<=9;o++)
				{
					if(pizza_shop[o]==pizzaconvert1)
					break;
				}
				if(n!=10 && o!=10)
				{
					attempt_two_delivery=1;
					order_accepted[0]=order_process[0];
					order_accepted[1]=order_process[1];
					order_track[order_process[0]]=1;
					order_track[order_process[1]]=1;
				}
			}
		}
		else
		{
			attempt_two_delivery=0;
			order_accepted[0]=99;
			order_accepted[1]=order_process[0];
			order_track[order_process[0]]=1;
		}
	}
	//if both preorders same house, we attempt delivery for both
	else if(order_type[order_process[0]]=='p' && order_type[order_process[1]]=='p')
	{
		if(houseconvert0==houseconvert1)
		{
			attempt_two_delivery=1;
			order_accepted[0]=order_process[0];
			order_accepted[1]=order_process[1];
			order_track[order_process[0]]=1;
			order_track[order_process[1]]=1;
		}
		//if different house, condition identical to ONE REGULAR AND PREORDER. Explained earlier
		else if((order_time[order_process[1]]<=order_time[order_process[0]]+55) && (houseconvert1!=houseconvert2))
		{
			for(n=0;n<=9;n++)
			{
				if(pizza_shop[n]==pizzaconvert0)
				break;
			}
			for(o=0;o<=9;o++)
			{
				if(pizza_shop[o]==pizzaconvert1)
				break;
			}
			if(n!=10 && o!=10)
			{
				attempt_two_delivery=1;
				order_accepted[0]=order_process[0];
				order_accepted[1]=order_process[1];
				order_track[order_process[0]]=1;
				order_track[order_process[1]]=1;
			}
			
		}
		else
		{
			attempt_two_delivery=0;
			order_accepted[0]=99;
			order_accepted[1]=order_process[0];
			order_track[order_process[0]]=1;
		}
	}
	
	//delivery not able to make it on time CONDITION
	if(time_ssd==order_time[order_process[0]]+20)
	{
		order_track[order_accepted[0]]=2;
		goto restart;
	}
	
	//PIZZA IDENTIFICATION PHASE
	if(attempt_two_delivery==0)          //only one delivery
	{									 //try to scan till particular time after order placed. If block not found, abort order and deliver at end.
		
		for(n=0;n<=9;n++)
		{
				if(pizza_shop[n]==(order_timeline_1[order_accepted[1]]&0xF0))
				break;
		}
		if(n==10)      //pizza location not known
		{
			loop1:  // SCAN shop while time is still enough to deliver pizza. If IF condition not satisfied, it implies robot didnt find pizza on time.
			if(order_time[order_accepted[0]]+40>=time_ssd+estimate_house[(int)(order_timeline_1[order_accepted[0]] & 0x0F)]) 
			{
										
				for(i=0;i<=9;i++)
				{
					if(pizza_shop[i]==0x99)
					break;
				}
				j=i;
				for(;j<=9;j++)
				{
					if(pizza_shop[j]==0x99)
					break;
				}
				move_shop('s',i);
				sense(i);
				if(pizza_shop[i]!=(order_timeline_1[order_accepted[1]]&0xF0))   //pizza not found
				{
					move_shop(i,j);
					sense(j);
					if(pizza_shop[j]!=(order_timeline_1[order_accepted[1]]&0xF0))
					{
						goto loop1;				//loop if pizza not found.
					}
				}
			}
			else      //if condition failed, time limit exceeded, abort order and mark it for later delivery (indicated by 2)
			{
				order_track[order_accepted[1]]=2;
				goto restart;
			}
		}
		//pizza found if it has not gone to loop1
		//scan while estimated delivery time is 35 seconds + order time. In previous IF after loop1:, we considered 40s as
		//if pizza found at 40, it can still deliver on time. However, scan pizza shop till 35s over order time  after 
		//locating pizza as robot needs time to come back to pizza counter. 5s time considered.
		while(order_time[order_accepted[0]]+35>=time_ssd+estimate_house[(int)(order_timeline_1[order_accepted[0]] & 0x0F)])        //scan shop till it is time to deliver pizza
		{
			for(i=0;i<=9;i++)
			{
				if(pizza_shop[i]==0x99)
				break;
			}
			j=i;
			for(;j<=9;j++)   //identify pizza shop where pizza still not identified
			{
				if(pizza_shop[j]==0x99)
				break;
			}
			if(j<=9)
			{
				move_shop(i,j);
				sense(j);
			}	
		}
		//exit while loop means time to deliver order (only one pizza carry condition specified by attempt_two_delivery variable
	}
	if(attempt_two_delivery==1)      //order accepted[0] indicates it needs to be delivered first. order accepted [1] indicates second delivery.
	{
		for(n=0;n<=9;n++)
		{
			if(pizza_shop[n]==(order_timeline_1[order_accepted[0]]&0xF0))
			break;
		}
		if(n==10)      //pizza location not known
		{
			loop2:  // SCAN shop while time is still enough to deliver pizza. If IF condition not satisfied, it implies robot didnt find pizza on time.
			if(order_time[order_accepted[0]]+40>=time_ssd+estimate_house[(int)(order_timeline_1[order_accepted[0]] & 0x0F)])
			{
				
				for(i=0;i<=9;i++)
				{
					if(pizza_shop[i]==0x99)
					break;
				}
				j=i;
				for(;j<=9;j++)
				{
					if(pizza_shop[j]==0x99)
					break;
				}
				move_shop('s',i);
				sense(i);
				if(pizza_shop[i]!=(order_timeline_1[order_accepted[0]]&0xF0))   //pizza not found
				{
					move_shop(i,j);
					sense(j);
					if(pizza_shop[j]!=(order_timeline_1[order_accepted[0]]&0xF0))
					{
						goto loop2;				//loop if pizza not found.
					}
				}
			}
			else      //if condition failed, time limit exceeded, abort order and mark it for later delivery.
			{
				order_track[order_accepted[0]]=2;
				goto restart;
			}
		} //for attempt_two_delivery==1, condition same as for 0. First priority pizza needs to be found. Indicated 
		  //by ORDER_ACCEPTED[0]. since that delivery window gets over earlier.
		
		//We found first pizza. now similar sequence for second pizza. 
		for(n=0;n<=9;n++)
		{
			if(pizza_shop[n]==(order_timeline_1[order_accepted[1]]&0xF0))
			break;
		}
		if(n==10)      //pizza location not known
		{
			loop3:  // SCAN shop while time is still enough to deliver pizza. If IF condition not satisfied, it implies robot didnt find pizza on time.
			if(order_time[order_accepted[1]]+40>=time_ssd+estimate_house[(int)(order_timeline_1[order_accepted[1]] & 0x0F)])
			{
				
				for(i=0;i<=9;i++)
				{
					if(pizza_shop[i]==0x99)
					break;
				}
				j=i;
				for(;j<=9;j++)
				{
					if(pizza_shop[j]==0x99)
					break;
				}
				move_shop('s',i);
				sense(i);
				if(pizza_shop[i]!=(order_timeline_1[order_accepted[1]]&0xF0))   //pizza not found
				{
					move_shop(i,j);
					sense(j);
					if(pizza_shop[j]!=(order_timeline_1[order_accepted[1]]&0xF0))
					{
						goto loop3;				//loop if pizza not found.
					}
				}
			}
			else      //if condition failed, time limit exceeded, abort order and mark it for later delivery.
			{
				order_track[order_accepted[1]]=2;
				attempt_two_delivery=0;       //second priority pizza not found, abort it, single delivery only
				order_accepted[1]=order_accepted[0];
				order_accepted[0]=99;               // for single delivery only order_accepted[1] used
			}
		}
	}
	
	//PIZZA PICKING PHASE
	if(attempt_two_delivery==0)
	{
		for(n=0;n<=9;n++)
		{
			if(pizza_shop[n]==(order_timeline_1[order_accepted[1]]&0xF0))
			break;
		}
		return_shop(j+1);  //robot is at Jth location during search. J+1 because return_shop uses 1 to 10 as reference and not 0 to 9
		move_shop('s',n+1);
		pizza1size=(order_timeline_1[order_accepted[1]] & 0x30);
		pizza1colour=(order_timeline_1[order_accepted[1]] & 0xC0);
		if(pizza1size==0x01)           //descriptions given at top of code
		{
			pick_block('s',1);
		}
		if(pizza1size==0x02)
		{
			pick_block('m',1);
		}
		if(pizza1size==0x03)
		{
			pick_block('l',1);
		}
		led_color(pizza1colour);
		return_shop(n+1);
	}
	else
	{
		for(n=0;n<=9;n++) //pick priority 2 pizza
		{
			if(pizza_shop[n]==(order_timeline_1[order_accepted[1]]&0xF0))
			break;
		}
		return_shop(j+1);  //robot is at Jth location during search. J+1 because return_shop uses 1 to 10 as reference and not 0 to 9
		move_shop('s',n+1);
		pizza1size=(order_timeline_1[order_accepted[1]] & 0x30);
		pizza1colour=(order_timeline_1[order_accepted[1]] & 0xC0);
		if(pizza1size==0x01)           //descriptions given at top of code
		{
			pick_block('s',1);
		}
		if(pizza1size==0x02)
		{
			pick_block('m',1);
		}
		if(pizza1size==0x03)
		{
			pick_block('l',1);
		}
		led_color(pizza1colour);
		
		//pick priority1 pizza
		for(o=0;o<=9;n++)
		{
			if(pizza_shop[o]==(order_timeline_1[order_accepted[0]&0xF0]))
			break;
		}
		move_shop(n+1,o+1);  //move_shop() can directly go from one node to next without going to pizza counter every time
		pizza2size=(order_timeline_1[order_accepted[0]] & 0x30);
		pizza2colour=(order_timeline_1[order_accepted[0]] & 0xC0);
		
		if(pizza2size==0x01)           //descriptions given at top of code
		{
			pick_block('s',0);
		}
		if(pizza2size==0x02)
		{
			pick_block('m',0);
		}
		if(pizza2size==0x03)
		{
			pick_block('l',0);
		}
		led_color(pizza2colour);
		return_shop(o+1);
	}
	
	//PIZZA DEPOSITION PHASE
	if(attempt_two_delivery==0)
	{
		move_house('s',(order_timeline_1[order_accepted[1]]&0x0F));  //masking size and colour
		if(pizza1size==0x01)           //descriptions given at top of code
		{
			drop_block('s',1);
		}
		if(pizza1size==0x02)
		{
			drop_block('m',1);
		}
		if(pizza1size==0x03)
		{
			drop_block('l',1);
		}
		ssd_freeze=1;     //sound buzzer and freeze display
		led_color(0);
		for (n=0;n<=7;n++)
		{
			if(order_track[n]==0||2)
			break;
		}
		if (n!=8)      //deliveries still remaining
		return_house(order_timeline_1[order_accepted[1]]&0x0F);
		else
		{
			buzzer_on();
			_delay_ms(5000);
			while(1);         //end of RUN
		
		}
	}
	if(attempt_two_delivery==1)
	{
		//deliver first block
		move_house('s',(order_timeline_1[order_accepted[0]]&0x0F));  //masking size and colour
		if(pizza2size==0x01)           //descriptions given at top of code
		{
			drop_block('s',1);
		}
		if(pizza2size==0x02)
		{
			drop_block('m',1);
		}
		if(pizza2size==0x03)
		{
			drop_block('l',1);
		}
		ssd_freeze=1;     //sound buzzer and freeze display
		led_color(pizza2colour);
		move_house((order_timeline_1[order_accepted[0]]&0x0F),(order_timeline_1[order_accepted[1]]&0x0F));
		if(pizza1size==0x01)           //descriptions given at top of code
		{
			drop_block('s',1);
		}
		if(pizza1size==0x02)
		{
			drop_block('m',1);
		}
		if(pizza1size==0x03)
		{
			drop_block('l',1);
		}
		ssd_freeze=1;
		led_color(0);
		for (n=0;n<=7;n++)
		{
			if(order_track[n]==0||2)
			break;
		}
		if (n!=8)      //deliveries still remaining
		return_house(order_timeline_1[order_accepted[1]]&0x0F);
		else
		{
			buzzer_on();
			_delay_ms(5000);
			while(1);         //end of RUN
		}
		goto restart;   //process repeats all over again!
	}
}
//MAIN FINTION
//Role is limited to calling LOGIC when it reaches pizza counter
void main()
{
	init_devices();
	velocity(255,253);
	servo_1(70);
	servo_2(90);
	servo_3(0);
	_delay_ms(100);
	node_node();	
	logic();
}