#define trigPinL 13
#define echoPinL 12
#define trigPinR 7
#define echoPinR 5

#define ledL1 8
#define ledL2 9
#define ledR1 2
#define ledR2 3


void setup() {
  // put your setup code here, to run once:
  Serial.begin (9600);
  pinMode(trigPinL, OUTPUT);
  pinMode(echoPinL, INPUT);
  pinMode(trigPinR, OUTPUT);
  pinMode(echoPinR, INPUT);
  pinMode(ledL1, OUTPUT);
  pinMode(ledL2, OUTPUT);
  pinMode(ledR1, OUTPUT);
  pinMode(ledR2, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly;

  long durationL, distanceL;
  digitalWrite(trigPinL, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinL, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinL, LOW);
  durationL = pulseIn(echoPinL, HIGH);
  distanceL = (durationL/2) /29.1;
  
  long durationR, distanceR;
  digitalWrite(trigPinR, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinR, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinR, LOW);
  durationR = pulseIn(echoPinR, HIGH);
  distanceR = (durationR/2) /29.1;

  Serial.print("aventage distance  ");
  Serial.print((distanceR+distanceL)/2);
  Serial.print("  cm");
  Serial.print("  ,Left distance  ");
  Serial.print(distanceL);
  Serial.print("  cm");
  Serial.print("  ,Right distance  ");
  Serial.print(distanceR);
  Serial.print("  cm");



  if (distanceL==distanceR) { 
    digitalWrite(ledL1,HIGH);
    digitalWrite(ledL2,HIGH);
    digitalWrite(ledR1,HIGH);
    digitalWrite(ledR2,HIGH);
    Serial.println("  Go forward");
    }
  if (distanceL<distanceR) {
    digitalWrite(ledL1,LOW);
    digitalWrite(ledL2,LOW);
    digitalWrite(ledR1,HIGH);
    digitalWrite(ledR2,HIGH);
    Serial.println("  TURN left");
    }

  if (distanceR<distanceL) {
    digitalWrite(ledL1,HIGH);
    digitalWrite(ledL2,HIGH);
    digitalWrite(ledR1,LOW);
    digitalWrite(ledR2,LOW);
    Serial.println("  TURN right");
    }

  if (((distanceR+distanceL)/2) > 402) {
    digitalWrite(ledL1,LOW);
    digitalWrite(ledL2,LOW);
    digitalWrite(ledR1,LOW);
    digitalWrite(ledR2,LOW);
    Serial.print("Out of reach");
    Serial.println("   Not moving");
    }
    

delay(500);

}