#include <LiquidCrystal.h>
#include <Wire.h>
#include <Servo.h> 
#include <Adafruit_MotorShield.h>


Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *myMotorright = AFMS.getMotor(1);
Adafruit_DCMotor *myMotorleft = AFMS.getMotor(2);


int Green;
int Blue;
int Red;
int left=0;
int right=0;
int x=0;
int g=0;
int b=0;
int r=0;
int pos = 0; 
int i=0;
int PM=0;
const int rs = 5, en = 6, d4 = 7, d5 = 8, d6 = 4, d7 = 10;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
Servo servo1;

void setup() {
  pinMode(A0,INPUT);
    pinMode(A1,INPUT);
  pinMode(A2,INPUT);
     pinMode(A3,INPUT);
  pinMode(2,INPUT);
    pinMode(9,OUTPUT);
      pinMode(3,INPUT);



  AFMS.begin();
  lcd.begin(16, 2);
  lcd.print("Hello! This is");
  lcd.setCursor(0, 1);
  lcd.print("Warehouse Robot");
  //servo1.attach(9);

  //servo1.write(0);
}

void loop() {
  Green = digitalRead(3);
   Blue = digitalRead(A1);
   Red = digitalRead(A2);

   if(Green == 1){
    
    lcd.clear();
    lcd.print("Robot is heading");
  lcd.setCursor(0, 1);
  lcd.print("to green boxes");
  //servo1.write(0); 
  //PM= analogRead(A0);
  //PM= map(PM,0,1023,0,255);
  analogWrite(9,41);
  g=0;

  while(g==0){
    left=digitalRead(A3);
right=digitalRead(2);


if (left==0 && right==0){
   myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);}
  

  
  if (left==1 && right==1 && x==0){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

  delay(200);
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);
     myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);

  delay(180);
  myMotorleft->run(RELEASE);
  myMotorright->run(RELEASE);
  x=1;
  
  }

   if (left==1 && right==0){
    myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);
  myMotorleft->run(RELEASE);
  }

  left=digitalRead(A3);
right=digitalRead(2);


if (left==0 && right==1){
  
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);
  myMotorright->run(RELEASE);
  
  }


  if (left==1 && right==1 && x==1){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

   /*for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servo1.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 15ms for the servo to reach the position
  }*/

  for(i=0; i<=14; i++){
    analogWrite(9, i);
    delay(0.1);
  }

  delay(200);
 myMotorleft->setSpeed(130);
  myMotorleft->run(FORWARD);
  myMotorright->setSpeed(130);
  myMotorright->run(FORWARD);


  delay(80);
 
  myMotorleft->run(RELEASE);
   myMotorright->run(RELEASE);
  x=2;
  
  } 

  left=digitalRead(A3);
right=digitalRead(2);

  if (left==1 && right==1 && x==2){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

  delay(500);
x=0;

g=1;
lcd.clear();
    lcd.print("Hello again");
 lcd.setCursor(0, 1);
  lcd.print("Select the path");
  
  } 
  }
     
   }

      if(Blue == 1){
    
    lcd.clear();
    lcd.print("Robot is heading");
  lcd.setCursor(0, 1);
  lcd.print("to blue boxes");
    //servo1.write(0); 
analogWrite(9,41);
  b=0;

   while(b==0){

 left=digitalRead(A3);
right=digitalRead(2);


if (left==0 && right==0){
   myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);}
  

  
  if (left==1 && right==1 && x==0){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

  delay(200);
  myMotorleft->setSpeed(200);
  myMotorleft->run(FORWARD);
     myMotorright->setSpeed(100);
  myMotorright->run(FORWARD);

  delay(150);
  myMotorleft->run(RELEASE);
  myMotorright->run(RELEASE);
  x=1;
  
  }

   if (left==1 && right==0){
    myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);
  myMotorleft->run(RELEASE);
  }

   left=digitalRead(A3);
right=digitalRead(2);


if (left==0 && right==1){
  
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);
  myMotorright->run(RELEASE);
  
  }


  if (left==1 && right==1 && x==1){
   myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

 /* for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servo1.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 15ms for the servo to reach the position
  }*/

    for(i=0; i<=14; i++){
    analogWrite(9, i);
    delay(0.1);
  }

  delay(200);
 myMotorleft->setSpeed(130);
  myMotorleft->run(FORWARD);
  myMotorright->setSpeed(130);
  myMotorright->run(FORWARD);


  delay(40);
 
  myMotorleft->run(RELEASE);
   myMotorright->run(RELEASE);
  x=2;
  
  } 

    left=digitalRead(A3);
right=digitalRead(2);

  if (left==1 && right==1 && x==2){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

   delay(500);
x=0;

b=1;
lcd.clear();
    lcd.print("Hello again");
 lcd.setCursor(0, 1);
  lcd.print("Select the path");
  
  }  

    
   }
     
   }


      if(Red == 1){
    
    lcd.clear();
    lcd.print("Robot is heading");
  lcd.setCursor(0, 1);
  lcd.print("to red boxes");
    //servo1.write(0);
    analogWrite(9,41); 

r=0;

   while(r==0){

 left=digitalRead(A3);
right=digitalRead(2);


if (left==0 && right==0){
   myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);}
  

  
  if (left==1 && right==1 && x==0){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

  delay(200);
  myMotorright->setSpeed(200);
  myMotorright->run(FORWARD);
     myMotorleft->setSpeed(100);
  myMotorleft->run(FORWARD);

  delay(150);
  myMotorleft->run(RELEASE);
  myMotorright->run(RELEASE);
  x=1;
  
  }

   if (left==1 && right==0){
    myMotorright->setSpeed(150);
  myMotorright->run(FORWARD);
  myMotorleft->run(RELEASE);
  }

   left=digitalRead(A3);
right=digitalRead(2);


if (left==0 && right==1){
  
  myMotorleft->setSpeed(150);
  myMotorleft->run(FORWARD);
  myMotorright->run(RELEASE);
  
  }


  if (left==1 && right==1 && x==1){
   myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

  /*for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servo1.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 15ms for the servo to reach the position
  }*/

    for(i=0; i<=14; i++){
    analogWrite(9, i);
    delay(0.1);
  }

  delay(200);
 myMotorleft->setSpeed(130);
  myMotorleft->run(FORWARD);
  myMotorright->setSpeed(130);
  myMotorright->run(FORWARD);


  delay(40);
 
  myMotorleft->run(RELEASE);
   myMotorright->run(RELEASE);
  x=2;
  
  } 

    left=digitalRead(A3);
right=digitalRead(2);

  if (left==1 && right==1 && x==2){
  myMotorright->run(RELEASE);
  myMotorleft->run(RELEASE);

   delay(500);
x=0;

r=1;
lcd.clear();
    lcd.print("Hello again");
 lcd.setCursor(0, 1);
  lcd.print("Select the path");
  
  }  

    
   }



  
     
   }
  
 
}