//US
int TrigPinL = 13;
int TrigPinF = 12;
int TrigPinR = 2;
int EchoPinL = 5;
int EchoPinF = 4;
int EchoPinR = 3;

long durationL, distanceL;
long durationF, distanceF;
long durationR, distanceR;

//L298N Motor and DC Motor (LEDs)
int MotorLforward = 10;
int MotorLbackward = 9;
int MotorRforward = 8;
int MotorRbackward = 7;


void setup() {
  // put your setup code here, to run once:

//US
pinMode(TrigPinL, OUTPUT);
pinMode(TrigPinF, OUTPUT);
pinMode(TrigPinR, OUTPUT);
pinMode(EchoPinL, INPUT);
pinMode(EchoPinF, INPUT);
pinMode(EchoPinR, INPUT);

//L298N Motor and DC Motor (LEDs)
pinMode(MotorLforward, OUTPUT);
pinMode(MotorLbackward, OUTPUT);
pinMode(MotorRforward, OUTPUT);
pinMode(MotorRbackward, OUTPUT);
Serial.begin(9600);

}

void loop() {
  // put your main code here, to run repeatedly:
//activate US first

//Left US
digitalWrite(TrigPinL,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinL,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinL,LOW);

durationL = pulseIn(EchoPinL,HIGH);
distanceL = durationL*0.034/2;

//Front US
digitalWrite(TrigPinF,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinF,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinF,LOW);

durationF = pulseIn(EchoPinF,HIGH);
distanceF = durationF*0.034/2;

//Right US
digitalWrite(TrigPinR,LOW);
delayMicroseconds(2);
digitalWrite(TrigPinR,HIGH);
delayMicroseconds(10);
digitalWrite(TrigPinR,LOW);

durationR = pulseIn(EchoPinR,HIGH);
distanceR = durationR*0.034/2;

//DISTANCE 
 //Forward
if (distanceF>16) {
  digitalWrite(MotorLforward,HIGH);
  digitalWrite(MotorLbackward,LOW);
  digitalWrite(MotorRforward,HIGH);
  digitalWrite(MotorRbackward,LOW);

  Serial.print (distanceL);
  Serial.print ("--");
  Serial.print (distanceF);
  Serial.print ("--");
  Serial.print (distanceR);
  Serial.print (":");
  Serial.println ("Moving Forward");
}

 //Turn Left
else if ((distanceF<=16)&((distanceL)>(distanceR))) {
digitalWrite(MotorLforward,LOW);
digitalWrite(MotorLbackward,HIGH);
digitalWrite(MotorRforward,HIGH);
digitalWrite(MotorRbackward,LOW);

Serial.print (distanceL);
Serial.print ("--");
Serial.print (distanceF);
Serial.print ("--");
Serial.print (distanceR);
Serial.print (":");
Serial.println ("Turning Left");
}

 //Turn Right
 else if ((distanceF<=16)&((distanceL)<(distanceR))) {
   digitalWrite(MotorLforward,HIGH);
   digitalWrite(MotorLbackward,LOW);
   digitalWrite(MotorRforward,LOW);
   digitalWrite(MotorRbackward,HIGH);

   Serial.print (distanceL);
   Serial.print ("--");
   Serial.print (distanceF);
   Serial.print ("--");
   Serial.print (distanceR);
   Serial.print (":");
   Serial.println ("Turning Right");
 }

  //Backwards
  else if (distanceF<=32 && distanceL<=16 && distanceR<=32) {
    digitalWrite(MotorLforward,LOW);
    digitalWrite(MotorLbackward,HIGH);
    digitalWrite(MotorRforward,LOW);
    digitalWrite(MotorRbackward,HIGH);

    Serial.print (distanceL);
    Serial.print ("--");
    Serial.print (distanceF);
    Serial.print ("--");
    Serial.print (distanceR);
    Serial.print (":");
    Serial.println ("Moving Backwards");
  }

   //Stopping
   else {
     digitalWrite(MotorLforward,LOW);
     digitalWrite(MotorLbackward,LOW);
     digitalWrite(MotorRforward,LOW);
     digitalWrite(MotorRbackward,LOW);
   }

}
//The End