#include <AFMotor.h>                  // include the Adafruit Motor Lib. in our code.
AF_DCMotor RMotor(3, MOTOR12_64KHZ);   // Creats a Motor-Object ( port# 2, the Controlling-Signal-Speed).
AF_DCMotor LMotor(4, MOTOR12_64KHZ);   // Creats a Motor-Object ( port# 2, the Controlling-Signal-Speed).

int distance[6];

void setup() {
  for (int i=15; i<=17; i++) {
    pinMode(i, INPUT);        // Set Pin2 as Input Pin  (connected to ECHO Pin)
  }
  pinMode(A0, OUTPUT);       // Set Pin3 as Output Pin (connected to TRIG Pin)
  
  LMotor.run(FORWARD);      // FORWARD, BACKWARD, RELEASE.
  RMotor.run(FORWARD);      // FORWARD, BACKWARD, RELEASE.
  Serial.begin(115200);
}

void loop() {
  // Start a new measurement:
  for (int i=15; i<=17; i++) {
    digitalWrite(A0, HIGH);          // Start sending the UltraSound Sound
    delayMicroseconds(10);          // Sending Duration 10uSec.
    digitalWrite(A0, LOW);           // End sending the UltraSound Sound

    // Read the result:
    distance[i] = pulseIn(i, HIGH) / 58; 
    //Serial.println(i);     // Convert Duration to Distance (cm)
    //Serial.println(distance[i]);
    delay(50);     // Set a Delay 0.1sec. to un-overload the processor.
  }  

  if (distance[16] > 10) {  //direction = "F";
      LMotor.setSpeed(255);
      RMotor.setSpeed(255);
      Serial.println("F");
  }  else 
  if (distance[15] > distance[17]) { //direction = "L";
      LMotor.setSpeed(10);
      RMotor.setSpeed(255);
      Serial.println("L");
  }   else 
   if (distance[15] < distance[17]) { //direction = "R";
      LMotor.setSpeed(255);
      RMotor.setSpeed(10);
      Serial.println("R");
  } 

//Serial.println("=");
  delay(50);    

}