// MrLunk's Autonomous Vehikel 2017  - v0.04

#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;

int LedLeft = 6;
int LedMiddle = 9;
int LedRight = 13;

const int ProxSensorLeft=11;
const int ProxSensorRight=12;

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

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

  Serial.begin(9600);

  pinMode(trigPin1, OUTPUT);  // Left sonar sensor
  pinMode(echoPin1, INPUT);

  pinMode(trigPin2, OUTPUT);  // Middle sonar sensor
  pinMode(echoPin2, INPUT);

  pinMode(trigPin3, OUTPUT);  // Right sonar sensor
  pinMode(echoPin3, INPUT);

  pinMode(LedLeft, OUTPUT);    // Left Detection indicator
  pinMode(LedMiddle, OUTPUT);  // Middle ...
  pinMode(LedRight, OUTPUT);   // Right ...

  pinMode(13, OUTPUT);
  pinMode(6, OUTPUT);

  pinMode(ProxSensorLeft,INPUT);
  pinMode(ProxSensorRight,INPUT);
}

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

int Loop1 = 0;
int maxspeed = 65;

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

void loop() {

  SonarSensor(trigPin2, echoPin2);
  if (distance <= 25){
    digitalWrite(LedMiddle, HIGH);

    SteeringServo.write(135);   // steer right
    DCMotor->run(BACKWARD);
    DCMotor->setSpeed(maxspeed);

    // Whilst driving backwards check for collisions with ir sensors
    while (Loop1 < 1){
      if(digitalRead(ProxSensorLeft)==LOW)
      {
        Loop1 = 2;
      }
      if(digitalRead(ProxSensorRight)==LOW)
      {
        Loop1 = 2;
      }
    }
    // --- End of backward ir collision check loop) ----

    DCMotor->run(RELEASE);
    SteeringServo.write(90);
    digitalWrite(LedMiddle, LOW);
  }
  Loop1 = 0;


  SonarSensor(trigPin1, echoPin1);
  if (distance <= 32){
    digitalWrite(LedRight, HIGH);
    SteeringServo.write(135);     //steer right
    Forwards();
    delay(5);
    digitalWrite(LedRight, LOW);
  }

  SonarSensor(trigPin3, echoPin3);
  if (distance <= 32){
    digitalWrite(LedLeft, HIGH);
    SteeringServo.write(45);   // steer left
    Forwards();
    delay(5);
    digitalWrite(LedLeft, LOW);
  }

  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;

}