#include <AFMotor.h>

AF_DCMotor motorR1(1); //Motor Front-Right
AF_DCMotor motorR2(2); //Motor Back-Right
AF_DCMotor motorL1(3); //Motor Front-Left
AF_DCMotor motorL2(4); //Motor Back-Left

int tcrt1Pin = A14;
int tcrt2Pin = A15;

void setup()
{ 
  delay(2000);  
  Serial.begin(9600);
  pinMode(52,INPUT); //Sensor tcrt1 OUTPUT PIN
  pinMode(53,INPUT); //Sensor tcrt2 OUTPUT PIN
  pinMode(51,OUTPUT); //BLUE LED OUTPUT PIN
  pinMode(31,OUTPUT); //RED LED OUTPUT PIN



}

void loop()
{
  int tcrt1 = analogRead(tcrt1Pin); //Sensor tcrt1
  int tcrt2 = analogRead(tcrt2Pin); //Sensor tcrt2
  analogWrite(52,tcrt1/4); //Sensor tcrt1
  analogWrite(53,tcrt2/4); //Sensor tcrt2

  Serial.println(tcrt1);
  Serial.println(tcrt2);

  if ((tcrt1<=500)&&(tcrt2<=500))
  {
    forward();//FORWARD DIRECTION
  }

  if ((tcrt1>=500)&&(tcrt2<=500))
  {
    turnright();  //TURN RIGHT DIRECTION
    
  }

  if ((tcrt1<=500)&&(tcrt2>=500)) 
  {
    turnleft();//TURN LEFT DIRECTION
  }

  else if((tcrt1>=500)&&(tcrt2>=500))
  {
    motorstop();
  }

  delay(150);
}

void forward()
{
  motorR1.setSpeed(100);
  motorR2.setSpeed(100);
  motorL1.setSpeed(100);
  motorL2.setSpeed(100);

  motorR1.run(FORWARD);
  motorR2.run(FORWARD);
  motorL1.run(FORWARD);
  motorL2.run(FORWARD);
  digitalWrite(51,HIGH);
  digitalWrite(31,HIGH);
  delay(100);
}

void turnright()
{             
  motorR1.setSpeed(200);
  motorR2.setSpeed(0);
  motorL1.setSpeed(0);
  motorL2.setSpeed(200);

  motorR1.run(FORWARD); ////// TCRT1 SENSING BLACK COLOR.OUTPUT IS HIGH.//////
  motorR2.run(RELEASE); //////////////////////////////////////////////////////
  motorL1.run(RELEASE);
  motorL2.run(FORWARD);
  digitalWrite(31,HIGH);
  delay(100);
}

void turnleft() 
{
  motorR1.setSpeed(0);
  motorR2.setSpeed(200);
  motorL1.setSpeed(200);
  motorL2.setSpeed(0);

  motorR1.run(RELEASE);
  motorR2.run(FORWARD);
  motorL1.run(FORWARD);
  motorL2.run(RELEASE);
  digitalWrite(51,HIGH);
  delay(100);
}

void motorstop()
{
  motorR1.run(RELEASE);
  motorR2.run(RELEASE);
  motorL1.run(RELEASE);
  motorL2.run(RELEASE);
  delay(0);
  digitalWrite(51,LOW);
  digitalWrite(31,LOW);
}