Ultrasonic Sensor Based Obstacle Avoiding Robot with Arduino

#include "NewPing.h"

int LeftMotorPlus = 9;
int LeftMotorMinus = 8;
int LeftMotorEnable = 12;

int RightMotorPlus = 11;
int RightMotorMinus = 10;
int RightMotorEnable = 13;

#define TRIGGER_PIN_RIGHT  6  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN_RIGHT     7  // Arduino pin tied to echo pin on the ultrasonic sensor.
int distance_left;

#define TRIGGER_PIN_LEFT  2  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN_LEFT     3
int distance_right;

#define TRIGGER_PIN_FRONT  4  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN_FRONT     5  // Arduino pin tied to echo pin on the ultrasonic sensor.
int distance_front;

#define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
int min_distance;
NewPing sonar_right(TRIGGER_PIN_RIGHT, ECHO_PIN_RIGHT, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonar_left(TRIGGER_PIN_LEFT, ECHO_PIN_LEFT, MAX_DISTANCE);
NewPing sonar_front(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT, MAX_DISTANCE);

void setup()
{
  pinMode(LeftMotorPlus, OUTPUT);
  pinMode(LeftMotorMinus, OUTPUT);
  pinMode(LeftMotorEnable, OUTPUT);

  pinMode(RightMotorEnable, OUTPUT);
  pinMode(RightMotorMinus , OUTPUT);
  pinMode(RightMotorPlus , OUTPUT);
}

void move_forward() {
  digitalWrite(LeftMotorEnable, HIGH);
  digitalWrite(RightMotorEnable, HIGH);

  digitalWrite(LeftMotorPlus, HIGH);
  digitalWrite(LeftMotorMinus, LOW);

  digitalWrite(RightMotorPlus, LOW);
  digitalWrite(RightMotorMinus, HIGH);
}

void move_backward() {
  digitalWrite(LeftMotorEnable, HIGH);
  digitalWrite(RightMotorEnable, HIGH);

  digitalWrite(LeftMotorPlus, LOW);
  digitalWrite(LeftMotorMinus, HIGH);

  digitalWrite(RightMotorPlus, HIGH);
  digitalWrite(RightMotorMinus, LOW);
}

void turn_left() {
  digitalWrite(LeftMotorEnable, HIGH);
  digitalWrite(RightMotorEnable, HIGH);

  digitalWrite(LeftMotorPlus, HIGH);
  digitalWrite(LeftMotorMinus, LOW);

  digitalWrite(RightMotorPlus, HIGH);
  digitalWrite(RightMotorMinus, LOW);
}

void turn_right() {
  digitalWrite(LeftMotorEnable, HIGH);
  digitalWrite(RightMotorEnable, HIGH);

  digitalWrite(LeftMotorPlus, LOW);
  digitalWrite(LeftMotorMinus, HIGH);

  digitalWrite(RightMotorPlus, LOW);
  digitalWrite(RightMotorMinus, HIGH);
}

void loop() {
  //move_forward();
  // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int uS_left = sonar_left.ping(); // Send ping, get ping time in microseconds (uS).
  //Serial.print("Left Sensor Ping: ");
  distance_left = uS_left / US_ROUNDTRIP_CM;
  //Serial.print(distance_left); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  //Serial.println("cm");
  unsigned int uS_right = sonar_right.ping(); // Send ping, get ping time in microseconds (uS).
  //Serial.print("Right Sensor Ping: ");
  distance_right = uS_right / US_ROUNDTRIP_CM;
  //Serial.print(distance_right); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  //Serial.println("cm");
  //Serial.print("The Minimum distance of obstacle is");
  unsigned int uS_front = sonar_front.ping(); // Send ping, get ping time in microseconds (uS).
  //Serial.print("Right Sensor Ping: ");
  distance_front = uS_front / US_ROUNDTRIP_CM;
  if (distance_front > 30)
  {
    move_forward();
  }
  if (distance_front < 30)
  {
    if (distance_left > 30)
    {
      turn_left();
      delay(500);
      move_forward();
    }
    else if (distance_left < 30)
    {
      if (distance_right > 30)
      {
        turn_right();
        delay(500);
        move_forward();
      }
      else
      {
        move_backward();
        delay(2000);
        turn_left();
        delay(500);
        move_forward();
      }
    }
  }
  delay(50);
}