4 wheel 4 DC-motor Rover with 3 forward facing sonar sensors

#include <Wire.h>
#include <Adafruit_MotorShield.h>

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

Adafruit_DCMotor *MyMotor001 = AFMS.getMotor(1);
Adafruit_DCMotor *MyMotor002 = AFMS.getMotor(2);
Adafruit_DCMotor *MyMotor003 = AFMS.getMotor(3);
Adafruit_DCMotor *MyMotor004 = AFMS.getMotor(4);

// Define Sonar-Sensor Pinout numbers
const int trigPinBack = 3;
const int echoPinBack = 4;
const int trigPinOne = 5;
const int echoPinOne = 6;
const int trigPinTwo = 7;
const int echoPinTwo = 8;
const int trigPinThree = 11;
const int echoPinThree = 12;

// define variables
long duration;
int Distance;
String oneLine;
int Speed = 80;

void setup() {
  Serial.begin(9600);
  pinMode(trigPinBack, OUTPUT);
  pinMode(echoPinBack, INPUT);
  pinMode(trigPinOne, OUTPUT);
  pinMode(echoPinOne, INPUT);
  pinMode(trigPinTwo, OUTPUT);
  pinMode(echoPinTwo, INPUT);
  pinMode(trigPinThree, OUTPUT);
  pinMode(echoPinThree, INPUT);
  digitalWrite(trigPinOne, LOW);
  digitalWrite(trigPinTwo, LOW);
  digitalWrite(trigPinThree, LOW);

  AFMS.begin();

  // ---------------- Initialise and neutralise all motors ---------
  MyMotor001->setSpeed(Speed);
  MyMotor001->run(FORWARD);
  MyMotor001->run(RELEASE);
  MyMotor002->setSpeed(Speed);
  MyMotor002->run(FORWARD);
  MyMotor002->run(RELEASE);
  MyMotor003->setSpeed(Speed);
  MyMotor003->run(FORWARD);
  MyMotor003->run(RELEASE);
  MyMotor004->setSpeed(Speed);
  MyMotor004->run(FORWARD);
  MyMotor004->run(RELEASE);
}

// --------------Main Routine definitions------------------------------

void loop() {

  // ---- Start Sensor Polling
  SonarSensor(trigPinOne, echoPinOne); // Poll Left Sonar-Sensor
  int RightDistance = Distance;
  delay(10);
  SonarSensor(trigPinThree, echoPinThree); // Poll Right Sonar-Sensor
  int LeftDistance = Distance;
  delay(10);
  SonarSensor(trigPinTwo, echoPinTwo); // Poll Middle Sonar-Sensor
  int MiddleDistance = Distance;
  delay(10);
  SonarSensor(trigPinBack, echoPinBack); // Poll Backward Sonar-Sensor
  int BackDistance = Distance;
  delay(10);
  // ---- End Sensor Polling ----

  // ------------------ Direction choices --------------------------------

  int MinMiddle = 15;
  int MinLeftRight = 20;

  if (MiddleDistance > MinMiddle && LeftDistance > MinLeftRight && RightDistance > MinLeftRight) {
    Forward();
  }
  else if (MiddleDistance <= MinMiddle) {
    if (LeftDistance < RightDistance) {
      TurnRight();
    }
    else if (RightDistance < LeftDistance) {
      TurnLeft();
    }
  }

  // ---------------------------+++++++

  if (MiddleDistance > 30 && LeftDistance <= 20 || MiddleDistance > 30 && RightDistance <= 20) {
    if (LeftDistance <= 20) {
      TurnRight();
    }
    else if (RightDistance <= 20) {
      TurnLeft();
    }
  }

  // ---- Create serial reporting String oneLine :
  // "L:" + LeftDistance + " M:" + MiddleDistance + " R:" + RightDistance + " B:" + BackDistance

  /*oneLine += " L:";
    oneLine += LeftDistance;
    oneLine += " M:";
    oneLine += MiddleDistance;
    oneLine += " R:";
    oneLine += RightDistance;
    oneLine += " B:";
    oneLine += BackDistance;
    Serial.println(oneLine);
    oneLine = "";
  */
}

// --------------Sub-Routine definitions------------------------------

void Forward() {
  MyMotor001->run(BACKWARD);
  MyMotor002->run(BACKWARD);
  MyMotor003->run(BACKWARD);
  MyMotor004->run(BACKWARD);
  /*delay(500);
    MyMotor001->run(RELEASE);
    MyMotor002->run(RELEASE);
    MyMotor003->run(RELEASE);
    MyMotor004->run(RELEASE);
  */
}

void Backward() {
  MyMotor001->run(FORWARD);
  MyMotor002->run(FORWARD);
  MyMotor003->run(FORWARD);
  MyMotor004->run(FORWARD);
  delay(500);
  MyMotor001->run(BACKWARD);
  MyMotor002->run(BACKWARD);
  MyMotor003->run(FORWARD);
  MyMotor004->run(FORWARD);
  /*delay(500);
    MyMotor001->run(RELEASE);
    MyMotor002->run(RELEASE);
    MyMotor003->run(RELEASE);
    MyMotor004->run(RELEASE);
  */
}

void TurnRight() {
  MyMotor001->run(BACKWARD);
  MyMotor002->run(BACKWARD);
  MyMotor003->run(FORWARD);
  MyMotor004->run(FORWARD);
  /*delay(500);
    MyMotor001->run(RELEASE);
    MyMotor002->run(RELEASE);
    MyMotor003->run(RELEASE);
    MyMotor004->run(RELEASE);
  */
}

void TurnLeft() {
  MyMotor001->run(FORWARD);
  MyMotor002->run(FORWARD);
  MyMotor003->run(BACKWARD);
  MyMotor004->run(BACKWARD);
  /*delay(500);
    MyMotor001->run(RELEASE);
    MyMotor002->run(RELEASE);
    MyMotor003->run(RELEASE);
    MyMotor004->run(RELEASE);
  */
}

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;
  if (Distance >= 100) {
    Distance = 100;
  }
}