#include <Servo.

h>
#define S0 47
#define S1 49
#define S2 51
#define S3 53
#define sensorOut 45
int w,x,y,z;
int frequency = 0;

Servo leftServo;
Servo rightServo;
int countright=0;
int countleft=0;
int right_read=0;
int left_read=0;
int exleft_read=0;
int exright_read=0;
int centre_read=0;

int count=0;
int check=0;
int counter=0;
int exright=A8;
int exleft=A13;
int centre=A10;
//int centre=A11;
int left=A12;
int right=A9;
int colorchk=0;
//motors input and output
int motor_leftpwm=3;
int motor_rightpwm=8;
int motor_leftlogic1=4;
int motor_leftlogic2=5;
int motor_rightlogic1=9;
int motor_rightlogic2=10;
int encoderright=19;
int encoderleft=18;

void resetEncoder()
{
countright=0;
countleft=0;
}
void sharpright(int enc)//right
{
// Serial.println("right Turn");
motoroff();
resetEncoder();
while(countright<enc)
{
digitalWrite(motor_leftlogic1,HIGH);
digitalWrite(motor_leftlogic2,LOW);
digitalWrite(motor_rightlogic2,HIGH);
digitalWrite(motor_rightlogic1,LOW);
analogWrite(motor_leftpwm,150);
analogWrite(motor_rightpwm,150);
 Serial.println("countleft");
Serial.println(countleft);
Serial.println("countright");
Serial.println(countright);

}
motoroff();
delay(300);

//90 sharp left turn

void sharpleft(int en)//left
{
// Serial.println("Left Turn");
motoroff();
resetEncoder();
while(countleft<en)
{Serial.println("Left Turn");
digitalWrite(motor_leftlogic1,LOW);
digitalWrite(motor_leftlogic2,HIGH);
digitalWrite(motor_rightlogic2,LOW);
digitalWrite(motor_rightlogic1,HIGH);
analogWrite(motor_leftpwm,150);
analogWrite(motor_rightpwm,150);
// Serial.println("countleft");
// Serial.println(countleft);
// Serial.println("countright");
// Serial.println(countright);
}
motoroff();
delay(300);
}

//counter right
void counter_right(void)
{
countright++;
}

// counter left
void counter_left(void)
{
countleft++;
}

//Sensor Value
int sensorvalue(int a)
{
int z=0;
for (int i=0;i<=5;i++)
{
z=z+analogRead(a);
delay(1);}
z=z/5;
return z;
}
void motorforwardslow()
{
digitalWrite(motor_leftlogic1,HIGH);
digitalWrite(motor_leftlogic2,LOW);
digitalWrite(motor_rightlogic1,HIGH);
digitalWrite(motor_rightlogic2,LOW);
analogWrite(motor_leftpwm,20);//80,40
analogWrite(motor_rightpwm,20);
}
void motorforwardfast()
{
digitalWrite(motor_leftlogic1,HIGH);
digitalWrite(motor_leftlogic2,LOW);
digitalWrite(motor_rightlogic1,HIGH);
digitalWrite(motor_rightlogic2,LOW);
analogWrite(motor_leftpwm,100);//80,40
analogWrite(motor_rightpwm,100);
}

//Slow left turn

void turnleft()//left
{

digitalWrite(motor_leftlogic2,HIGH); //high for forwrd

digitalWrite(motor_leftlogic1,LOW); //low for forward
digitalWrite(motor_rightlogic1,HIGH);
digitalWrite(motor_rightlogic2,LOW);
analogWrite(motor_leftpwm,30);//110
analogWrite(motor_rightpwm,80);
}

//Slow right turn

void turnright()//right
{

digitalWrite(motor_leftlogic2,LOW);
digitalWrite(motor_leftlogic1,HIGH);
digitalWrite(motor_rightlogic1,LOW);//low
digitalWrite(motor_rightlogic2,HIGH);//high
analogWrite(motor_leftpwm,80);//140
analogWrite(motor_rightpwm,30);
}

void motorrev()
{

digitalWrite(motor_leftlogic1,LOW);
digitalWrite(motor_leftlogic2,HIGH);

digitalWrite(motor_rightlogic1,LOW);
digitalWrite(motor_rightlogic2,HIGH);
analogWrite(motor_rightpwm,60);
analogWrite(motor_leftpwm,60);
}

void motorforward()
{
digitalWrite(motor_leftlogic1,HIGH);
digitalWrite(motor_leftlogic2,LOW);
digitalWrite(motor_rightlogic1,HIGH);
digitalWrite(motor_rightlogic2,LOW);
analogWrite(motor_leftpwm,50);//80,40
analogWrite(motor_rightpwm,50);
Serial.println("countleft");
Serial.println(countleft);
Serial.println("countright");
Serial.println(countright);
}

void motorrevwall()
{

digitalWrite(motor_leftlogic1,LOW);
digitalWrite(motor_leftlogic2,HIGH);

digitalWrite(motor_rightlogic1,LOW);
digitalWrite(motor_rightlogic2,HIGH);
analogWrite(motor_rightpwm,100);
analogWrite(motor_leftpwm,100);
}

void motoroff()
{
digitalWrite(motor_leftlogic2,HIGH);
digitalWrite(motor_leftlogic1,LOW);
digitalWrite(motor_rightlogic1,HIGH);
digitalWrite(motor_rightlogic2,LOW);
analogWrite(motor_leftpwm,0);
analogWrite(motor_rightpwm,0);
}
void motorbrake()
{
digitalWrite(motor_leftlogic2,HIGH);
digitalWrite(motor_leftlogic1,HIGH);
digitalWrite(motor_rightlogic1,HIGH);
digitalWrite(motor_rightlogic2,HIGH);
analogWrite(motor_leftpwm,0);
analogWrite(motor_rightpwm,0);
}

void deg180(){
// motorbrake();
//delay(200);
sharpright(290);
delay(100);
motorforward();
delay(150);
sharpright(290);
motorbrake();
delay(50);
}

void takecolor(){
digitalWrite(S2,LOW);
 digitalWrite(S3,LOW);
// Reading the output frequency
frequency = pulseIn(sensorOut, LOW);
//Remaping the value of the frequency to the RGB Model of 0 to 255
frequency = map(frequency, 25,72,255,0);
// Printing the value on the serial monitor
w=frequency;
/*Serial.print("R= ");//printing name
Serial.print(frequency);//printing RED color frequency
Serial.print(" ");*/
delay(100);
// Setting Green filtered photodiodes to be read
digitalWrite(S2,HIGH);
digitalWrite(S3,HIGH);
// Reading the output frequency
frequency = pulseIn(sensorOut, LOW);
//Remaping the value of the frequency to the RGB Model of 0 to 255
frequency = map(frequency, 30,90,255,0);
// Printing the value on the serial monitor
x=frequency;
/*Serial.print("G= ");//printing name
Serial.print(frequency);//printing RED color frequency
Serial.print(" ");*/
delay(100);
// Setting Blue filtered photodiodes to be read
digitalWrite(S2,LOW);
digitalWrite(S3,HIGH);
// Reading the output frequency
frequency = pulseIn(sensorOut, LOW);
//Remaping the value of the frequency to the RGB Model of 0 to 255
frequency = map(frequency, 25,70,255,0);
// Printing the value on the serial monitor
y=frequency;
/*Serial.print("B= ");//printing name
Serial.print(frequency);//printing RED color frequency
Serial.println(" ");*/
delay(100);
z=w+x+y;
Serial.println("valueeeee");
Serial.println(z);
}

void action() {

while(colorchk==0)
{delay(100);
takecolor();
Serial.println(z);

if(z<=-50)
{ Serial.print("RED");
rightServo.write(0);
delay(2000);
rightServo.write(90);
colorchk=1;}

else if(z>=0 && z<=400 )

{Serial.print("blue");
leftServo.write(165);
delay(2000);
leftServo.write(90);
colorchk=1;}

else if(z>=450)
{Serial.print("yellow");
rightServo.write(165);
delay(2000);
rightServo.write(90);
colorchk=1;}

}
}

void setup() {
pinMode(S0, OUTPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
pinMode(S3, OUTPUT);
pinMode(sensorOut, INPUT);

// Setting frequency-scaling to 20%

digitalWrite(S0,HIGH);
digitalWrite(S1,LOW);
pinMode(centre,INPUT);
pinMode(exright,INPUT);
pinMode(exleft,INPUT);
pinMode(left,INPUT);
pinMode(right,INPUT);
pinMode(motor_rightpwm,OUTPUT);
pinMode(motor_leftpwm,OUTPUT);
pinMode(motor_rightlogic1,OUTPUT);
pinMode(motor_rightlogic2,OUTPUT);
pinMode(motor_leftlogic1,OUTPUT);
pinMode(motor_leftlogic2,OUTPUT);
pinMode(encoderright,INPUT_PULLUP);
pinMode(encoderleft,INPUT_PULLUP);
attachInterrupt(4, counter_right, CHANGE);
attachInterrupt(5, counter_left, CHANGE);
Serial.begin(9600);
pinMode(11,OUTPUT);
leftServo.attach(7);
rightServo.attach(2);
rightServo.write(90);
leftServo.write(90);
}

void loop() {
// put your main code here, to run repeatedly:
// digitalWrite(motor_leftlogic2,LOW);
//digitalWrite(motor_leftlogic1,HIGH);
//digitalWrite(motor_rightlogic1,HIGH);
 //digitalWrite(motor_rightlogic2,LOW);
//analogWrite(motor_leftpwm,100);//140
//analogWrite(motor_rightpwm,100);

centre_read=digitalRead(centre);
right_read=digitalRead(right);
exright_read=digitalRead(exright);
exleft_read=digitalRead(exleft);
left_read=digitalRead(left);

/*Serial.println("countright");
Serial.println(countright);
delay(100);
Serial.println("countleft");
Serial.println(countleft);
*/

if(centre_read==1 && left_read==0 && right_read==0 )

{

motorforward();
}

else if(centre_read==0 && (exleft_read==1 || left_read==1) && right_read==0 &&

exright_read==0 )
{
sharpleft(20);
//turnleft();
}

else if(centre_read==0 && left_read==0 && (right_read==1 || exright_read==1) )

{
sharpright(20);
//turnright();
}

else if((centre_read==1 || exright_read==1 ) && (right_read==1) )//exleft..left

{
// motorbrake();
//delay(200);
motorforward();
delay(30);

left_read=digitalRead(left);
exleft_read=digitalRead(exleft);

if(check==1)
{check=2;
// motorrev();
// delay(100);
motorbrake();
delay(500);
action();
// rightServo.write(35);
//delay(1500);
//rightServo.write(90);

delay(200);//150
motorrev();
delay(400);
deg180();}

else if( check==2)

{motorrev();
delay(100);
motorbrake();
delay(200);
check=3;
sharpright(290);
motorforward();
delay(100);
}

else if( check==3)

{motorrev();
delay(100);
motorbrake();
delay(200);
check=4;
sharpright(290);
motorforward();
delay(200);}
else if(check==4 )
{check=6;
motorrev();
delay(20);
colorchk=0;
motorbrake();
delay(500);
action();
// rightServo.write(135);
//delay(1500);
//rightServo.write(90);
motorrev();
motorrev();
delay(400);
motorbrake();
delay(5000);
}

else if(check==0 )
{
check=1;
motorrev();
 delay(100);
motorbrake();
delay(150);
sharpright(290);
motorforward();
delay(300);}
}
else if(centre_read==0 && left_read==0 && right_read==0 && exright_read==0 &&
exleft_read==0)
{

motorforward();
}
else if(centre_read==1 && left_read==1 && right_read==0 && exright_read==0 &&
exleft_read==0)
{

motorforward();
}