#include <Servo.h>
#define trigRight 11
#define echoRight 10
#define trigLeft 13
#define echoLeft 12

Servo esc;
Servo steering;

long i, j, duration, left_distance, right_distance, distance, r0, r1, r2, r3, r4, l0, l1, l2, l3, l4;

void setup() {
  esc.attach(9);
  steering.attach(8);
  Serial.begin(9600);
  pinMode(trigRight, OUTPUT);
  pinMode(echoRight, INPUT);
  pinMode(trigLeft, OUTPUT);
  pinMode(echoLeft, INPUT);
  esc.write(90);
  delay(100);
  esc.write(180);
  delay(100);
  esc.write(0);
  delay(100);
  esc.write(90);
}

void loop() {

  restart:

  esc.write(99.5);

  SonarSensor(trigRight, echoRight);
  right_distance = distance;
  SonarSensor(trigLeft, echoLeft);
  left_distance = distance;

  Serial.print(left_distance);
  Serial.print(" - ");
  Serial.println(right_distance);

   if (left_distance <= 3 || left_distance >= 2000){
    goto restart;
    }

   if (right_distance <= 3 || right_distance >= 2000){
    goto restart;
    }

   if (left_distance < right_distance){
     if ( left_distance <= 100 && left_distance > 3) {
       steering.write(180);
       delay(200);
       steering.write(90);
        delay(200);
     }

      else if ((left_distance > 100) && (left_distance < 150)){
        steering.write(180);
        delay(200);
        steering.write(90);
        delay(200);
        }

      else{
      steering.write(90);
      delay(200);
      }
   }

  else if (right_distance < left_distance){
    if (right_distance <= 100 && right_distance > 3){
      steering.write(0);
      delay(200);
      steering.write(90);
      delay(200);
    }
    else if( (right_distance > 100) && (right_distance < 150)){
      steering.write(0);
      delay(200);
      steering.write(90);
      delay(200);
    }
    else{
      steering.write(90);
      delay(200);
    }
  }
  else {
    esc.write(99.5);
    }
}

void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
}