Robô Arduino Autônomo de Fuga Que Verifica os Lados

#include <Servo.h>
#include <NewPing.h>

const int servoPin = 3;
const int RightMotorForward = 6;
const int RightMotorBackward = 7;
const int LeftMotorForward = 5;
const int LeftMotorBackward = 4;

#define trig_pin A0
#define echo_pin A1
#define maximum_distance 200
int distance = 100;

NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name

void setup(){
  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);

  servo_motor.attach(servoPin);
  servo_motor.write(95);

  delay(200);
  distance = readPing();
  delay(20);
  distance = readPing();
  Serial.begin(9600);
}

void loop(){
  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);

  if (distance <= 20){
    moveStop();
    delay(300);
    moveBackward();
    delay(200);
    moveStop();
    delay(300);
    distanceRight = lookRight();
    delay(200);
    distanceLeft = lookLeft();
    delay(200);

    if (distanceRight >= distanceLeft){
      turnRight();
      delay(200);
      moveStop();
    }
    else{
      turnLeft();
      delay(200);
      moveStop();
    }
  }
   delay(200);
   moveForward();
   distance = readPing();
  Serial.print("Leitura à frente: ");
  Serial.println(distance);
}

int lookRight(){
  servo_motor.write(50);
  delay(500);
  int distance = readPing();
  for (int i=0; i<6; i++){
    int d = readPing();
    if (d<distance){
      distance = d;
    }
  }
  servo_motor.write(95);
  Serial.print("Leitura à direita: ");
  Serial.println(distance);
  return distance;
}

int lookLeft(){
  servo_motor.write(150);
  delay(500);
  int distance = readPing();
  for (int i=0; i<6; i++){
    int d = readPing();
    if (d<distance){
      distance = d;
    }
  }
  servo_motor.write(95);
  Serial.print("Leitura à esquerda: ");
  Serial.println(distance);
  return distance;
}

int readPing(){
  delay(70);
  int cm = sonar.ping_cm();
  if (cm==0){
    cm=250;
  }
  return cm;
}

void moveForward(){
  for (int i=0; i<10; i++){
    motorLForward();
    motorRForward();
    delay(10);
    moveStop();
    delay(10);
  }
}

void moveBackward(){
  motorLBackward();
  motorRBackward();
}

void turnRight(){
  Serial.println("Vira à direita");
  motorLForward();
  motorRBackward();
}

void turnLeft(){
  Serial.println("Vira à esquerda");
  motorRForward();
  motorLBackward();
}

void moveStop(){
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
}

void motorRForward(){
  digitalWrite(RightMotorForward, HIGH);
  digitalWrite(RightMotorBackward, LOW);
}

void motorRBackward(){
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(RightMotorBackward, HIGH);
}

void motorLForward(){
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(LeftMotorBackward, LOW);
}

void motorLBackward(){
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(LeftMotorBackward, HIGH);
}