MrLunk's Autonomous Arduino r3 / Raspberry Pi 3b - Vehikel 2

// MrLunk's Autonomous Arduino r3 / Raspberry Pi 3b - Vehikel 2017

#define trigPin1 2
#define echoPin1 3
#define trigPin2 4
#define echoPin2 5
#define trigPin3 7
#define echoPin3 8
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

long duration, distance, RightSensor,BackSensor,FrontSensor,LeftSensor;
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *DCMotor = AFMS.getMotor(1);
Servo SteeringServo;
int usbRead =0;

// --------------------------------------------------------------

void setup() {
  AFMS.begin();
  DCMotor->setSpeed(0);
  DCMotor->run(RELEASE);
  SteeringServo.attach(10);

  Serial.begin(9600);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
}

// --------------------------------------------------------------

int maxspeed = 65;

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

void loop() {

  SonarSensor(trigPin2, echoPin2);
  if (distance <= 20){
    SteeringServo.write(135);   // steer right
    DCMotor->run(BACKWARD);
    DCMotor->setSpeed(maxspeed);
    delay(1100);
    DCMotor->run(RELEASE);
    SteeringServo.write(90);
  }

  SonarSensor(trigPin1, echoPin1);
  if (distance <= 30){
  SteeringServo.write(135);     //steer right
    Forwards();
  }

  SonarSensor(trigPin3, echoPin3);
  if (distance <= 30){
  SteeringServo.write(45);   // steer left
    Forwards();
  }

  //SonarSensor(trigPin2, echoPin2);
  //if (distance > 30){
    SteeringServo.write(90);
    Forwards();
  //}
}

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

void Forwards(){
  DCMotor->run(FORWARD);
  DCMotor->setSpeed(maxspeed);
  delay(250);
  DCMotor->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;

}