color+movement+sharp.c

/************************************************************************************
 LCD Connections:
 			  LCD	  Microcontroller Pins
 			  RS  --> PC0
			  RW  --> PC1
			  EN  --> PC2
			  DB7 --> PC7
			  DB6 --> PC6
			  DB5 --> PC5
			  DB4 --> PC4

 ADC Connection:
 			  ACD CH.	PORT	Sensor
			  0			PF0		Battery Voltage
			  1			PF1		White line sensor 3
			  2			PF2		White line sensor 2
			  3			PF3		White line sensor 1
			  4			PF4		IR Proximity analog sensor 1*****
			  5			PF5		IR Proximity analog sensor 2*****
			  6			PF6		IR Proximity analog sensor 3*****
			  7			PF7		IR Proximity analog sensor 4*****
			  8			PK0		IR Proximity analog sensor 5
			  9			PK1		Sharp IR range sensor 1
			  10		PK2		Sharp IR range sensor 2
			  11		PK3		Sharp IR range sensor 3
			  12		PK4		Sharp IR range sensor 4
			  13		PK5		Sharp IR range sensor 5
			  14		PK6		Servo Pod 1
			  15		PK7		Servo Pod 2

 ***** For using Analog IR proximity (1, 2, 3 and 4) sensors short the jumper J2. 
 	   To use JTAG via expansion slot of the microcontroller socket remove these jumpers.  
 
 Motion control Connection:
 			L-1---->PA0;		L-2---->PA1;
   			R-1---->PA2;		R-2---->PA3;
   			PL3 (OC5A) ----> PWM left; 	PL4 (OC5B) ----> PWM right; 
 

********************************************************************************/

#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 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;	   

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;

//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(); //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 < 10000) && (green < 10000) && (blue < 10000))
    {
	     black_show();
         _delay_ms(1000);
    }

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


void turn_right()
{
   
	right_degrees(30);
	adjust_right();
	turnR++;
    
}

void turn_left()
{
    
	 left_degrees(30);
	 adjust_left();
	 turnL++;

}


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);	   
		Front_IR_Sensor = ADC_Conversion(6);
		
		   /*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_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))
		{   
			forward();

            if(Left_white_line< 0x18 && Center_white_line <0x18 && Right_white_line <0x18)
		{
				 if( turnR == turnL )
            {
				turn_right();
				forward_mm(200); //Moves robot forward 200mm
			    stop();
			    _delay_ms(500);
				forward();
				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();
			   forward_mm(200); //Moves robot forward 200mm
		       stop();
		     
			   _delay_ms(500);
			   forward();
               if((Left_white_line> 0x18 && Center_white_line >0x18) || (Right_white_line >0x18 && Center_white_line >0x18))
			   turn_left();
             }
		

			 else 
             stop();

         }

			/*	if(Front_Sharp_Sensor>0x45 || Front_IR_Sensor<0xF0)	 
	   
			{	   
				stop();	   
				buzzer_on();
				_delay_ms(100);
				buzzer_off();
				   
					   
			}	  
			*/	

		}


		
}


//Main Function
int main()
{
	init_devices();
	lcd_set_4bit();
	lcd_init();
	color_sensor_scaling();
    velocity(250,242);
	
/*forward();*/
	while(1)
	{    
	    /*colorsensor();*/
		blackline_sharp(); 
       /* forward_mm(200); //Moves robot forward 200mm
		stop();
		_delay_ms(500);
		
		back_mm(400);   //Moves robot backward 100mm
		stop();
		_delay_ms(500);
		
		left_degrees(90); //Rotate robot left by 90 degrees
		stop();
		_delay_ms(500);
		
		right_degrees(90); //Rotate robot right by 90 degrees
		stop();
		_delay_ms(500);
		*/
		

	
		}
	
	servo_2(90);
 	
 

 _delay_ms(2000);
 servo_2_free();
 while(1);

}