/* 28/08/2021

    RobotcarSingleSensorv1

    Sketch Goal

    To create a sketch that combines the L298N motor driver, a servo and a HC-SR04 Ultrasonic Distance sensor
    to create a basic obstacle avoiding car.

   This sketch will use elements learnt from:

   L298Nv2BasicControlFunctions.ino at http://www.digitaltown.co.uk/components2L298NMotorDriver.php
   HCSR04UltrasonicDistanceSensorv2Function.ino at http://www.digitaltown.co.uk/components3HCSR04UltrasonicSensor.php
   Servov1.ino at http://www.digitaltown.co.uk/components4servo.php

   Sketch will also use Arrays, the lessons on this can be found at http://www.digitaltown.co.uk/lesson11Arrays.php

   For wiring diagrams see http://www.digitaltown.co.uk/project2RobotCar.php#Building

   Arduino UNO

   pins used:

   6  Echo HC-SR04 Ultrasonic Distance Sensor
   7  Trigger HC-SR04 Ultrasonic Distance Sensor
   8  L298N Motor Driver Board pin IN1
   9  L298N Motor Driver Board pin IN2
   10 L298N Motor Driver Board pin IN3
   11 L298N Motor Driver Board pin IN4
   12 Servo control

   for this version the sensor will move 7 degrees either side of the centre position
   to take a distance reading as well as the centre position.

   If the car gets stuck or objects are too close the car will stop and take a set of
   readings at 10 degree intervals and work out the best route based on the return values.
*/

//Global variables
//Servo
const int myServoPin = 12;//constant integer that will not change

//L298N Pins
const int leftForwardPin = 10;
const int leftReversePin = 11;
const int rightForwardPin = 8;
const int rightReversePin = 9;

//HC-SR04 Ultrasonic Distance Sensor pins
const int triggerPin = 7;
const int echoPin = 6;

//Othe global variables
//To start with the car will stop if it gets closer than 400mm to an object
const int minimumDistance = 400;
//Array holding the 3 positions the servo will move to at 7 degree intervals while the car is moving.
int runningScanPositions[3] = {83, 90, 97};

//Array holding the 13 positions the servo will move to while the car is stopped at 10 degree intervals.
int stoppedScanPositions[13] = {30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150};
//Integer that will hold the current position of the servo
int scanPosition;

//The array that will hold the distances of the car when running
int scanRunDistances[3];
//The array that will hold the distances of the car when stopped
int scanStopDistances[13];

int okToDrive = 0;//0 = stop 1 = no obstacle detected

//include the servo library
#include "Servo.h"

//Create a servo object. In this example we will call the servo "myServo".
Servo myServo;  // create servo object to control a servo

//Motor functions from L298Nv2BasicControlFunctions.ino

//function to make the left motors run forwards
void forwardRight() {
  Serial.println("left motor forwards, turn right");
  digitalWrite(leftForwardPin, HIGH);
  //Making the pin LOW means we turn off the 5v and the pin returns to 0v
  digitalWrite(leftReversePin, LOW);
  //Right motor stopped, both pins low
  digitalWrite(rightForwardPin, LOW);
  digitalWrite(rightReversePin, LOW);
}
//function to make the left motors run in reverse
void reverseRight() {
  Serial.println("Reverse left motor, reverse right");
  digitalWrite(leftForwardPin, LOW);
  digitalWrite(leftReversePin, HIGH);
  //Right motor stopped, both pins low
  digitalWrite(rightForwardPin, LOW);
  digitalWrite(rightReversePin, LOW);
}
//function to make the right motors run forwards
void forwardLeft() {
  Serial.println("right motor forwards, forwards left");
  digitalWrite(rightForwardPin, HIGH);
  //Making the pin LOW means we turn off the 5v and the pin returns to 0v
  digitalWrite(rightReversePin, LOW);
  //left motor stopped, both pins low
  digitalWrite(leftForwardPin, LOW);
  digitalWrite(leftReversePin, LOW);
}
//function to make the right motors run in reverse
void reverseLeft() {
  Serial.println("Reverse right motor, reverse left");
  digitalWrite(rightForwardPin, LOW);
  digitalWrite(rightReversePin, HIGH);
  //left motor stopped, both pins low
  digitalWrite(leftForwardPin, LOW);
  digitalWrite(leftReversePin, LOW);
}

//this function calls both stop functions together
void stopMotors() {
  Serial.println("Stop motors");
  digitalWrite(rightForwardPin, LOW);
  digitalWrite(rightReversePin, LOW);
  digitalWrite(leftForwardPin, LOW);
  digitalWrite(leftReversePin, LOW);
}
//calls both forward functions
void driveForward() {
  Serial.println("Both motors forwards");
  //Both motors forwards pins HIGH
  digitalWrite(rightForwardPin, HIGH);
  digitalWrite(rightReversePin, LOW);
  digitalWrite(leftForwardPin, HIGH);
  digitalWrite(leftReversePin, LOW);

}
//calls both reverse functions
void driveReverse() {
  Serial.println("Both motors reverse");
  //Both motors reverse pins HIGH
  digitalWrite(rightForwardPin, LOW);
  digitalWrite(rightReversePin, HIGH);
  digitalWrite(leftForwardPin, LOW);
  digitalWrite(leftReversePin, HIGH);
}

//End of L298N functions

//Distance function from HCSR04UltrasonicDistanceSensorv2Function.ino
//It receives the pins of the sensor and send back the distance
int getDistanceHCSSR04(int funcTriggerPin, int funcEchoPin) {
  //An unsigned long is needed because the numbers produced can be much larger
  //than an int can hold (max value 32,767)
  //Local variables
  unsigned long duration;  //time taken for echo
  int myDistance;
  //make sure Trigger is low to start
  digitalWrite(funcTriggerPin, LOW);

  //A tiny delay 2 millionths of a second
  delayMicroseconds(2);

  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(funcTriggerPin, HIGH);

  //pin stays HIGH (5v) for 10 microseconds to trigger a pulse
  delayMicroseconds(10);

  //Pin taken LOW (0v) after pulse triggered ready for next pulse
  digitalWrite(funcTriggerPin, LOW);

  duration = pulseIn(funcEchoPin, HIGH);

  myDistance = duration * 0.34 / 2;
  return myDistance;
}

//This will find the best direction to move the car in when stopped
//Once found it will generate a short turn manoeuvre
void findStoppedBestDirection() {
  Serial.println("Doing Full Scan");
  //Local variables
  int steerDelay = 10;//alter this variable to turn more or less
  //stores the best direction found
  int bestDirection = 0;
  //Leeps track of the longest distance found in this scan routine
  int longestDistance = 0;
  //Move through the array of scan positions
  for (scanPosition = 0; scanPosition < 13; scanPosition++) {
    myServo.write(stoppedScanPositions[scanPosition]);
    //the delay must be long enough to allow the servo to move to it's next location before it takes a distance reading.
    //Otherwise it will try to take a reading while still moving.
    delay(250);
    //Put the Answer in the array
    scanStopDistances[scanPosition] = getDistanceHCSSR04(triggerPin, echoPin);
    //this "if" added due to odd results giving a value of 20000+, possibly because of the surfaces I had surrounding the car
    //Possible echo as it's when something is close
    if(scanStopDistances[scanPosition] > 5000){
      scanStopDistances[scanPosition] = 0; 
    }
    Serial.println(scanPosition);
    Serial.println(scanStopDistances[scanPosition]);
    //if the distance just scanned is greater than the best distance so far change the value to the new value
    if (scanStopDistances[scanPosition] > longestDistance) {
      //Update the longest distance
      longestDistance = scanStopDistances[scanPosition];
      //update the bestDirection
      bestDirection = scanPosition;
    }
  }

  //At this point the scanning process is over
  
  //If the longest distance found was less than 200mm (so no good route out found), try a short reverse
  //trying 200 as 100 caused crashes...couldn't brake in time
  if (longestDistance < 200) {
    //reverse for 0.3 seconds
    driveReverse();
    delay(300);
    stopMotors();
  } else { //otherwise decent escape route found turn to the direction
    //if a route has been found turn towards it.
    Serial.print("Best direction: ");
    Serial.println(bestDirection);
    //the switch statement does a turn with an appropriate delay based on the best direction angle
    switch (bestDirection) {
      case 0://60 degrees right
        forwardRight();
        delay(steerDelay * 60); //delay based on angle as will take longer to turn
        break;
      case 1: //50 degrees right
        forwardRight();
        delay(steerDelay * 50);
        break;
      case 2: //40 degrees right
        forwardRight();
        delay(steerDelay * 40);
        break;
      case 3: //30 degrees right
        forwardRight();
        delay(steerDelay * 30);
        break;
      case 4: //20 degrees right
        forwardRight();
        delay(steerDelay * 20);
        break;
      case 5: //10 degrees right
        forwardRight();
        delay(steerDelay * 10);
        break;
      case 6://centre
        driveForward();
        delay(20);
        break;
      case 7://10 degrees left
        forwardLeft();
        delay(steerDelay * 10);
        break;
      case 8://20 degrees left
        forwardLeft();
        delay(steerDelay * 20);
        break;
      case 9://30 degrees left
        forwardLeft();
        delay(steerDelay * 30);
        break;
      case 10://40 degrees left
        forwardLeft();
        delay(steerDelay * 40);
        break;
      case 11://50 degrees left
        forwardLeft();
        delay(steerDelay * 50);
        break;
      case 12://60 degrees left
        forwardLeft();
        delay(steerDelay * 60);
        break;
      default:
        stopMotors();
        Serial.println("steering error");
        break;
    }
  }
  //Once the function is complete the code returns to the main loop.
}

void setup() {
  // Serial.begin(9600) starts serial communication.
  Serial.begin(9600);
  //Send script name
  Serial.println("RobotCarSingleSensorv1...");
  Serial.println(" ");

  //L298N Pins
  Serial.println("L298N Pins set");
  pinMode(leftForwardPin, OUTPUT);
  pinMode(leftReversePin, OUTPUT);
  pinMode(rightForwardPin, OUTPUT);
  pinMode(rightReversePin, OUTPUT);

  //HC-SR04 Ultrasonic Distance Sensor pins
  Serial.println("HC-SR04 Ultrasonic Distance Sensor pins set");
  pinMode(triggerPin, OUTPUT);  //sends signal
  pinMode(echoPin, INPUT);      //receives signal

  //Servo
  //Servo object attached to a pin
  myServo.attach(myServoPin);
  Serial.println("Testing Servo");
  myServo.write(30);
  delay(500);
  myServo.write(150);
  delay(500);
  myServo.write(90);
  delay(500);
  Serial.println("Servo Ready");


  //now proceed to the main loop

}



void loop() {
  okToDrive = 1; //set to Drive
  //run through the three scanning positions that are used while the vehicle is in motion
  for (scanPosition = 0; scanPosition < 3; scanPosition++) {
    //Move to the next servo position
    myServo.write(runningScanPositions[scanPosition]);
    //wait 0.1 secons for the servo to move (only moving a short distance so short wait)
    delay(100);
    //take a disance reading
    scanRunDistances[scanPosition] = getDistanceHCSSR04(triggerPin, echoPin);
    Serial.println(scanRunDistances[scanPosition]);
    //check if the distance scanned has seen an obstacle nearby
    if (scanRunDistances[scanPosition] < minimumDistance) {
      //If something is blocking set okToDrive to 0... the car will stop.
      okToDrive = 0;
      Serial.println("Normal path blocked");
    }
  }

  //if obstacles detected do full scan.
  if (okToDrive < 1) {
    //Stop the motors
    stopMotors();
    //use the scan function to find the best position to go in
    findStoppedBestDirection();

  } else {//if no obstacle is found... okTodrive = 1
    //keep going until obstacle found
    driveForward();
  }

}