end.c

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

#include <math.h> //included to support power function
#include "lcd.c"
#include "servo.h"

void port_init();
void timer5_init();
void velocity(unsigned char, unsigned char);
void motors_delay();

unsigned char ADC_Conversion(unsigned char);
unsigned char ADC_Value;
unsigned char flag = 0;
unsigned char flag1 = 0;	   
unsigned char flag2 = 0;
unsigned char turnR = 0;
unsigned char turnL = 0;
unsigned char data;
unsigned char a = 0;
unsigned char b=0;
unsigned char node=0;
unsigned char r=0;
unsigned char SHARP_1=0;
unsigned char SHARP_2=0;
unsigned char SHARP_3=0;
unsigned char SHARP_4=0;
unsigned char SHARP_5=0;

	   
unsigned char Left_white_line = 0;
unsigned char Center_white_line = 0;
unsigned char Right_white_line = 0;
unsigned char Front_Sharp_Sensor=0;	   
unsigned char Front_IR_Sensor=0;
unsigned char IR1=0;
unsigned char IR2=0;
unsigned char IR4=0;
unsigned char IR5=0;	   

volatile unsigned long int ShaftCountLeft = 0; //to keep track of left position encoder
volatile unsigned long int ShaftCountRight = 0; //to keep track of right position encoder
volatile unsigned int Degrees; //to accept angle in degrees for turning

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

int freeze=0;

void uart0_init(void)
{
 UCSR0B = 0x00; //disable while setting baud rate
 UCSR0A = 0x00;
 UCSR0C = 0x06;
 UBRR0H = 0x00; //set baud rate hi
 UBRR0L = 0x07; //set baud rate lo
 UCSR0B = 0x98;
}


//SIGNAL(SIG_USART0_RECV) 		// ISR for receive complete interrupt
//{	data = UDR0; 				//making copy of data from UDR0 in 'data' variable 
//	UDR0 = data; 				//echo data back to PC
//}
void spi_init(void)
{
 SPCR = 0x53; //setup SPI
 SPSR = 0x00; //setup SPI
 SPDR = 0x00;
}

void transmit(uint8_t data)
{
 while(!(SPSR & (1<<SPIF)));
 //loop_until_bit_is_set(UCSR0A, UDRE0);
//while (!( UCSR0A & (1<<UDRE0))); 
 UDR0=data;

}

//Function to configure LCD port
void lcd_port_config (void)
{
 DDRC = DDRC | 0xF7; //all the LCD pin's direction set as output
 PORTC = PORTC & 0x80; // all the LCD pins are set to logic 0 except PORTC 7
}

void buzzer_pin_config (void)
{
 DDRC = DDRC | 0x08;		//Setting PORTC 3 as output
 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
}

//ADC pin configuration
void adc_pin_config (void)
{
 DDRF = 0x00; 
 PORTF = 0x00;
 DDRK = 0x00;
 PORTK = 0x00;
}

//Function to configure ports to enable robot's 

void motion_pin_config (void) 
{
 DDRA = DDRA | 0x0F;
 PORTA = PORTA & 0xF0;
 DDRL = DDRL | 0x18;   //Setting PL3 and PL4 ns as output for PWM generation
 PORTL = PORTL | 0x18; //PL3 and PL4 pins are for velocity control using PWM.
}

//Function to Initialize PORTS
void port_init()
{
	lcd_port_config();
	adc_pin_config();
	motion_pin_config();
	color_sensor_pin_config();//color sensor pin configuration
	buzzer_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 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
	EIMSK = EIMSK | 0x01; // Enable Interrupt INT0 for color sensor
	sei(); // Enables the global interrupt
}
// Timer 5 initialized in PWM mode for velocity control
// Prescale:256
// PWM 8bit fast, TOP=0x00FF
// Timer Frequency:225.000Hz
void timer5_init()
{
	TCCR5B = 0x00;	//Stop
	TCNT5H = 0xFF;	//Counter higher 8-bit value to which OCR5xH value is compared with
	TCNT5L = 0x01;	//Counter lower 8-bit value to which OCR5xH value is compared with
	OCR5AH = 0x00;	//Output compare register high value for Left Motor
	OCR5AL = 0xFF;	//Output compare register low value for Left Motor
	OCR5BH = 0x00;	//Output compare register high value for Right Motor
	OCR5BL = 0xFF;	//Output compare register low value for Right Motor
	OCR5CH = 0x00;	//Output compare register high value for Motor C1
	OCR5CL = 0xFF;	//Output compare register low value for Motor C1
	TCCR5A = 0xA9;	/*{COM5A1=1, COM5A0=0; COM5B1=1, COM5B0=0; COM5C1=1 COM5C0=0}
 					  For Overriding normal port functionality to OCRnA outputs.
				  	  {WGM51=0, WGM50=1} Along With WGM52 in TCCR5B for Selecting FAST PWM 8-bit Mode*/
	
	TCCR5B = 0x0B;	//WGM12=1; CS12=0, CS11=1, CS10=1 (Prescaler=64)
}

void adc_init()
{
	ADCSRA = 0x00;
	ADCSRB = 0x00;		//MUX5 = 0
	ADMUX = 0x20;		//Vref=5V external --- ADLAR=1 --- MUX4:0 = 0000
	ACSR = 0x80;
	ADCSRA = 0x86;		//ADEN=1 --- ADIE=1 --- ADPS2:0 = 1 1 0
}

//Function For ADC Conversion
unsigned char ADC_Conversion(unsigned char Ch) 
{
	unsigned char a;
	if(Ch>7)
	{
		ADCSRB = 0x08;
	}
	Ch = Ch & 0x07;  			
	ADMUX= 0x20| Ch;	   		
	ADCSRA = ADCSRA | 0x40;		//Set start conversion bit
	while((ADCSRA&0x10)==0);	//Wait for conversion to complete
	a=ADCH;
	ADCSRA = ADCSRA|0x10; //clear ADIF (ADC Interrupt Flag) by writing 1 to it
	ADCSRB = 0x00;
	return a;
}

//Function To Print Sesor Values At Desired Row And Coloumn Location on LCD
void print_sensor(char row, char coloumn,unsigned char channel)
{
	
	ADC_Value = ADC_Conversion(channel);
	lcd_print(row, coloumn, ADC_Value, 3);
}


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 for velocity control
void velocity (unsigned char left_motor, unsigned char right_motor)
{
	OCR5AL = (unsigned char)left_motor;
	OCR5BL = (unsigned char)right_motor;
}

//Function used for setting motor's direction
void motion_set (unsigned char Direction)
{
 unsigned char PortARestore = 0;

 Direction &= 0x0F; 		// removing upper nibbel for the protection
 PortARestore = PORTA; 		// reading the PORTA original status
 PortARestore &= 0xF0; 		// making lower direction nibbel to 0
 PortARestore |= Direction; // adding lower nibbel for forward command and restoring the PORTA status
 PORTA = PortARestore; 		// executing the command
}

void forward (void) 
{
  motion_set (0x06);

  
}

void stop (void)
{
  motion_set (0x00);
}

void left (void) //Left wheel backward, Right wheel forward
{
  motion_set(0x05);
}

void right (void) //Left wheel forward, Right wheel backward
{
  motion_set(0x0A);
}

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 back (void) //both wheels backward
{
  motion_set(0x09);
}

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 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(); //Clears the global interrupt
	EICRB = EICRB | 0x08; // INT5 is set to trigger with falling edge
	EIMSK = EIMSK | 0x20; // Enable Interrupt INT5 for right position encoder
	sei();   // Enables the global interrupt
}

//ISR for right position encoder
ISR(INT5_vect)
{
	ShaftCountRight++;  //increment right shaft position count
}


//ISR for left position encoder
ISR(INT4_vect)
{
	ShaftCountLeft++;  //increment left shaft position count
}

ISR(INT0_vect)
{
	pulse++; //increment on receiving pulse from the color sensor
}

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
}

//Function used for moving robot forward by specified distance


void black_line()
{
        Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
		Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
		Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor
        if(Left_white_line > 0x18 &&  Center_white_line <0x18 && Right_white_line <0x18)
		{
			stop();
			soft_left();	//soft left
		}
		
		
		if(Left_white_line<0x18 && Center_white_line > 0x18 && Right_white_line <0x18)
		{   

			forward();
		}
		
		if(Left_white_line< 0x18 && Center_white_line <0x18 && Right_white_line >0x18)
		{
			stop();
			soft_right();	//soft right
		}
}

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)
	{   if(ShaftCountRight < ReqdShaftCountInt)
        {
		   black_line();
		}
		else
		{
			break;
		}
	}
	stop(); //Stop robot
}



void forward_mm(unsigned int DistanceInMM)
{
	forward();
	linear_distance_mm(DistanceInMM);
}

void back_mm(unsigned int DistanceInMM)
{
	back();
	linear_distance_mm(DistanceInMM);
}

void left_degrees(unsigned int Degrees)
{
	// 88 pulses for 360 degrees rotation 4.090 degrees per count
	left(); //Turn left
	angle_rotate(Degrees);
}



void right_degrees(unsigned int Degrees)
{
	// 88 pulses for 360 degrees rotation 4.090 degrees per count
	right(); //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 init_devices (void)
{
 	cli(); //Clears the global interrupt
	port_init();  //Initializes all the ports
	left_position_encoder_interrupt_init();
	right_position_encoder_interrupt_init();
	color_sensor_pin_interrupt_init();
	adc_init();
    timer1_init();
	timer5_init();
	uart0_init(); //Initailize UART1 for serial communiaction
    spi_init(); //timer4 overflow interrupt enable
	sei();   // Enables the global interrupt
	
}

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 filter_clear(void)	//select no filter
{
	//Filter Select - no filter
	PORTD = PORTD | 0x40; //set S2 High
	PORTD = PORTD & 0x7F; //set S3 Low
}

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 color_read(void)
{
	//Color
	
	
	//Red
	filter_red(); //select red filter
	pulse=0; //reset the count to 0
	_delay_ms(1000); //capture the pulses for 100 ms or 0.1 second
	red = pulse;  //store the count in variable called red
	
	lcd_cursor(1,1);  //set the cursor on row 1, column 1
	lcd_string("R"); // Display "Red Pulses" on LCD
	lcd_print(1,3,red,5);  //Print the count on second row
	_delay_ms(100);// Display for 1000ms or 1 second
    //Clear the LCD
    
	
	//Green
	filter_green(); //select green filter
	pulse=0; //reset the count to 0
	_delay_ms(1000); //capture the pulses for 100 ms or 0.1 second
	green = pulse;  //store the count in variable called green
	
	lcd_cursor(1,9);  //set the cursor on row 1, column 1
	lcd_string("G"); // Display "Green Pulses" on LCD
	lcd_print(1,11,green,5);  //Print the count on second row
	_delay_ms(100);// Display for 1000ms or 1 second
   //Clear the LCD
	
	
	//Blue
	filter_blue(); //select blue filter
	pulse=0; //reset the count to 0
	_delay_ms(1000); //capture the pulses for 100 ms or 0.1 second
	blue = pulse;  //store the count in variable called blue
	
	lcd_cursor(2,1);  //set the cursor on row 1, column 1
	lcd_string("B"); // Display "Blue Pulses" on LCD
	lcd_print(2,3,blue,5);  //Print the count on second row
	_delay_ms(100);	// Display for 1000ms or 1 second
	 //Clear the LCD
}
void red_show()
{
	lcd_wr_command(0x01); //Clear the LCD
	lcd_cursor(1,4);  //set the cursor on row 1, column 4
	lcd_string("RED"); // Display " RED" on LCD
	_delay_ms(100);	// Display for 1000ms or 1 second
}
void green_show()
{
	lcd_wr_command(0x01); //Clear the LCD
	lcd_cursor(1,4);  //set the cursor on row 1, column 4
	lcd_string("GREEN"); // Display " GREEN" on LCD
	_delay_ms(100);	// Display for 1000ms or 1 second
}
void blue_show()
{
	lcd_wr_command(0x01); //Clear the LCD
	lcd_cursor(1,4);  //set the cursor on row 1, column 4
	lcd_string("BLUE"); // Display " BLUE" on LCD
	_delay_ms(100);	// Display for 1000ms or 1 second
}

void black_show()
{
	lcd_wr_command(0x01); //Clear the LCD
	lcd_cursor(1,4);  //set the cursor on row 1, column 4
	lcd_string("BLACK"); // Display " BLUE" on LCD
	_delay_ms(100);	// Display for 1000ms or 1 second
}

void adjust_left()
{
	while(1)
	{
		
		Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
		Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
		Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor

		if(Left_white_line<0x18 && Center_white_line>0x18 && Right_white_line<0x18) //this condition will check weather robot is on black line or not and if it is on black line than it will break the loop
		{
			break;
		}
		left();//this will take left turn until it find black line


	}
}
/*
* Function Name: adjust_right()
* Input		 : 
* Output		 : 
* Logic		 : it will rotate right until robot comes on the black line
* Example Call : adjust_right();
*/



void adjust_right()
{
	while(1)
	{
		

		Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
		Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
		Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor

		if(Left_white_line<0x18 && Center_white_line>0x18 && Right_white_line<0x18)//this condition will check weather robot is on black line or not and if it is on black line than it will break the loop
		{
			break;
		}
		right(); //it will take right turn until it find black line

	}
}

void colorsensor()
{  
    color_read();
	if ((red < 9000) && (green < 9000) && (blue < 9000))
    {
	     black_show();
         _delay_ms(1000);
		 transmit('B');
    }

	else if((red > green) && (red > blue))		//this is condition for detection red pizza 
	{
		red_show();
		_delay_ms(1000);
		transmit('R');
	}
	else  if((green > blue) && (green>red))		//this is condition for detection green pizza 
	{
	    green_show();
		_delay_ms(1000);
		transmit('G');
	}
	else if((blue > red)&& (blue > green))		//this is condition for detection blue pizza 
	{
		blue_show();
		_delay_ms(1000);
		transmit('B');
	//	transmit('p');
	}
}


void turn_right()
{
   
	right_degrees(30);
//	transmit('R');
    adjust_right();
	//if(a==1)
	//b++;
	//turnR++;
    
}

void turn_left()
{
    
	 left_degrees(30);
	 adjust_left();
//	 transmit('L');
//	 turnL++;

}

void turn_four()
{       
        colorsensor();
        forward_mm(50);
		stop();
		turn_right();
		forward_mm(280);
		colorsensor();
		forward_mm(50);
		turn_right();
		forward_mm(280);
		colorsensor();
		forward_mm(50);
		turn_right();
		 forward_mm(280);
		 colorsensor();
		 forward_mm(50);
		turn_right();
        blackline_sharp();


		
}


void blackline_sharp()
{
    	Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
		Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
		Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor
		Front_Sharp_Sensor = ADC_Conversion(11);
			 SHARP_1 = ADC_Conversion(9);
 //lcd_print(2,1,SHARP_1,3);
 
 SHARP_2 = ADC_Conversion(10);
 //lcd_print(2,5,SHARP_2,3);
 
 SHARP_4 = ADC_Conversion(12);
 //lcd_print(2,9,SHARP_4,3);
 
 SHARP_5 = ADC_Conversion(13);	   
		IR1 = ADC_Conversion(4); 
 
 		IR2 = ADC_Conversion(5);
 
  		Front_IR_Sensor = ADC_Conversion(6);

 		IR4 = ADC_Conversion(7);

 		IR5 = ADC_Conversion(8);
		   /*print_sensor(1,1,3);	//Prints value of White Line Sensor1	   
			print_sensor(1,5,2);	//Prints Value of White Line Sensor2	   
			print_sensor(1,9,1);	//Prints Value of White Line Sensor3	   
			print_sensor(2,4,11);	//Prints Value of Front Sharp Sensor	   
			print_sensor(2,8,6);	//Prints Value of Front IR Sensor	   
		*/

		forward();
		
		if(Left_white_line > 0x18 &&  Center_white_line <0x18 && Right_white_line <0x18)
		{
			soft_left();	//soft left
		}
		
		
		if(Left_white_line<0x18 && Center_white_line > 0x18 && Right_white_line <0x18)
		{   
		    forward();
			_delay_ms(100);
			transmit('F');
		/*	while(r==0)
		    { 
			transmit('F');
			stop();
			r++;
			}
*/
/*			if(flag==1 )
			  {  
			     if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
				     {
                        stop();
						_delay_ms(100);
						servo_2(50);
						colorsensor();
						flag++;
						turn_right();
				 	}		
			}

			if(flag==2 )
			  {  
			     if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
				     {
                      stop();
						_delay_ms(100);
						servo_2(50);
						colorsensor();
						flag++;
						turn_right();
				 	}		
			}

			if(flag==3 )
			  {  
			     if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
				     {
                      stop();
						_delay_ms(100);
						servo_2(50);
						colorsensor();
						flag++;
						turn_right();
				 	}		
			}
			  
			if(flag==4)
			  {  
			     if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
				     {
                      stop();
						_delay_ms(100);
						flag=0;
				 	}		
			}			  
			  */	}
       


		/*	forward_mm(200);
			                  //Moves robot forward 200mm
		    stop();
			_delay_ms(100);
			servo_2(1);
			colorsensor();
		    servo_2(179);
			colorsensor();

		  */  

		
		if(Left_white_line< 0x18 && Center_white_line <0x18 && Right_white_line >0x18)
		{
			soft_right();	//soft right
		}


        if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
		{   
	//	    if(b==1)
      //      turn_right();
	        r=0;
		    int w=0;
			node++;
          transmit('N');
          if(SHARP_1<0x110 && SHARP_1>80 )
			  {
			      transmit('1');
			  }
		  if(SHARP_1>0x110 )
		   { 
		     r++;
			 forward_mm(50);  
			}
			if(r==1)
			{
				turn_four();
						
			            

				}
				print_sensor(2,4,11);	//Prints Value of Front Sharp Sensor	   
			print_sensor(2,8,6);	//Prints Value of Front IR Sensor
  	if(Front_Sharp_Sensor>0x35)	 
	   
			{	stop();     	   
				turn_right();
					   
			}
			
		if(Front_Sharp_Sensor<0x35)	   
			{	   
				  	   
				forward();	   
				buzzer_off();	   
					   
			}	    
             forward_mm(50);
            if(node==1)
			 {
			 colorsensor();
			 turn_right();
			 }

             if(node==6)
			 turn_right();

             if(node==7)
			 turn_right();

			 if(node==12)
			 turn_left();
			 
			 if(node==13)
			 turn_left();

			 if(node==18)
			 turn_right();

             if(node==19)
			 turn_right();

			 
			 if(node==24)
			 turn_left();
			 
			 if(node==25)
			 turn_left();


			 if(node==30)
			 turn_right();

             if(node==31)
			 turn_right();
			        
			        

           // if(Left_white_line< 0x18 && Center_white_line <0x18 && Right_white_line <0x18)
		    
		     //forward();
			/*	 if( turnR == turnL )
            {  
			    a++;
				turn_right();
				if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
				turn_right();
			}
			
			else if ( turnR!=turnL)
			{
			   turn_left();
               if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
			   turn_left();
             }
		

			 else 
             stop();
			 */


         }
	
}


//Main Function
int main()
{
	init_devices();
	lcd_set_4bit();
	lcd_init();
	color_sensor_scaling();
    velocity(250,245);
	
/*forward();*/
	while(1)
	{  
	   // turn_four();
		 /*  transmit('F') ;
	     servo_1(50);
         // _delay_ms(1000);
		 // servo_2(50);
		  colorsensor();
		//  servo_3(50);
        //  _delay_ms(1000);
		  servo_1(150);
		 // _delay_ms(1000);
		//   servo_2(150);
		  colorsensor();
		 // servo_3(150);
          //_delay_ms(1000);
          transmit('N');
		  */


	 /*   SHARP_1 = ADC_Conversion(9);
		servo_2(50);
		print_sensor(1,4,9);
        _delay_ms(1000);
		servo_2(150);
	print_sensor(1,4,9);
		_delay_ms(1000);
			transmit('F');
			*/
  //   IR1 = ADC_Conversion(4); 
 
 	//	IR2 = ADC_Conversion(5);
 
  	//	Front_IR_Sensor = ADC_Conversion(6);

 	//	IR4 = ADC_Conversion(7);

 	//	IR5 = ADC_Conversion(8);

	//forward();  

	blackline_sharp(); 
    /*  	while(flag<4)
				{
				     
			            stop();
				        _delay_ms(100);
						servo_2(50);
						colorsensor();
						flag++;
						forward_mm(50);
						turn_right();
						
			            servo_2(155);
						colorsensor();
		                servo_2_free();
		if(Left_white_line > 0x18 &&  Center_white_line <0x18 && Right_white_line <0x18)
		{
			soft_left();	//soft left
		}
		
		
		if(Left_white_line<0x18 && Center_white_line > 0x18 && Right_white_line <0x18)
		{   
		    forward();
			transmit('F');
					 	 }
         	if(Left_white_line< 0x18 && Center_white_line <0x18 && Right_white_line >0x18)
		{
			soft_right();	//soft right
		}

				}
		
		//	servo_2(90);
 	
 
*/
// _delay_ms(2000);
 //servo_2_free();
 //while(1);
}
}