THE CODE

// Reference materials:
// https://randomnerdtutorials.com/complete-guide-for-ultrasonic-sensor-hc-sr04/
// https://www.intorobotics.com/tutorial-how-to-control-the-tower-pro-sg90-servo-with-arduino-uno/

#include <Servo.h>

const int servoPin = 6; // servo motor pin
const int rightTrig = 7; //Communicates with Right Ultrasonic Distance Sensor's Trig Pin.
const int rightEcho = 8; //Communicates with Right Ultrasonic Distance Sensor's Echo Pin.
const int leftTrig = 9; //Communicates with Left Ultrasonic Distance Sensor's Trig Pin.
const int leftEcho = 10; //Communicates with Left Ultrasonic Distance Sensor's Echo Pin.
unsigned long duration, durationL, durationR, cm, inches; /*initializes some variables for future reference:
durationL is how long it takes the left ultrasonic distance sensor signal to return to the echo pin, durationR is the same for the right sensor
duration is whichever one is shorter (durationL or durationR) as that's the side where the obstacle is closer
cm is the distance value converted to centimeters, and inches is the distance value converted to inches
*/
boolean error, turnDir; /*initializes variables for future reference:
leftError checks to see if the left usds read an error value (value > 800), rightError is the same for the right usds
turnDir stores which direction the turn will be in, left is true and right is false
*/
unsigned long delta = 0; // the variable that stores the next time void loop will run
const int interval = 100; // the variable that says how much time will pass between void loop iterations
const int range = 100; // the variabe that decides at what distance the ultrasonic distance sensor will react
const int right = 180;
const int left = 90;
const int centered = 150;

Servo servo;

int servoAngle = 0;

void setup()
{
  Serial.begin(9600);

  pinMode(leftTrig, OUTPUT); //Outputs a sound signal
  pinMode(leftEcho, INPUT); //Recieves the sound signal
  pinMode(rightTrig, OUTPUT); //Outputs a sound signal
  pinMode(rightEcho, INPUT); //Recieves the sound signal

  servo.attach(servoPin);

  //This line is to set the servo motor to it's middle position, since it isn't able to rotate in the negative direction.
  servoWrite(centered);
  cm = 0;
}

void loop() {
  if (millis() > delta) // delta timing not needed in final code, but while the code is still being worked on not having debug info be fired out at hundreds of lines a second is nice
  {
    calcDistance(); // calculates distance from nearest obstacle in centimeters and inches
    if (obstacle()) // checks for obstacles, and if an obstacle if found, goes through the turning procedure
    {
      turnCheck();
    }
    //readOut(); // debug info about the ultrasonic distance sensor readouts. serves no other purpose
    delta += interval; //more delta timing
  }
}

void calcDistance() // calculates the distance the nearest obstacle in centimeters and inches
{
  digitalWrite(leftTrig, LOW);
  delayMicroseconds(5);
  digitalWrite(leftTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(leftTrig, LOW);

  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  //pinMode(leftEcho, INPUT);
  durationL = pulseIn(leftEcho, HIGH);

  digitalWrite(rightTrig, LOW);
  delayMicroseconds(5);
  digitalWrite(rightTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(rightTrig, LOW);

  //pinMode(rightEcho, INPUT);
  durationR = pulseIn(rightEcho, HIGH);

  if (durationL < durationR) //Sets the duration variable as the shortest duration time.
  {
    duration = durationL;
  }
  else
  {
    duration = durationR;
  }
  // Convert the time into a distance
  cm = (duration / 2) / 29.1;   // Divide by 29.1 or multiply by 0.0343
  inches = (duration / 2) / 74; // Divide by 74 or multiply by 0.0135
}

boolean obstacle() //checks for obstacles. Returns true if an obstacle is within range of the vehicle.
                   //false otherwise.
{
  if (cm < range)
  {
    Serial.println("There is an obstacle");
    return true;
  }
  else
  {
    Serial.println("There is no obstacle");
    return false;
  }
}

int pickASide() // decides which direction the drone will turn
{
  Serial.println(cm);
  if (durationL < durationR)
  {
    turnDir = true;  //left
    return durationL;
  }
  else
  {
    turnDir = false;  //right
    return durationR;
  }
}

void turnCheck() //Outputs errors in the selection of turn direction and actually calls the turn direction, could probably be combined with pickASide
{
  int readVal = pickASide();
  if (turnDir)
  {
    if (readVal > 800)
    {
      error = true;
    }
    else
    {
      error = false;
      turnLeft();
      return;
    }
  }
  else
  {
    if (readVal > 800)
    {
      error = true;
    }
    else
    {
      error = false;
      turnRight();
      return;
    }
  }
  if (true)
  {
    if (error == true)
    {
      Serial.println("An error has occured with the turn direction choice mechanism. No turn will be made.");
    }
    else
    {
      Serial.println("This message shouldn't ever appear, so if it did, you done fricked up. No turn will be made.");
    }
  }
}

void turnLeft() // tells the servo to turn left
{
  servoWrite(left);
  Serial.println("Turning left!");
  turning();
}

void turnRight() // tells the servo to turn right
{
  servoWrite(right);
  Serial.println("Turning right!");
  turning();
}

void turning() // tells the servo motor to return to its initial position once obstacle() is false
{
  delay(3000);
  while (obstacle())
  {
    calcDistance();
    /* maybe put a u-turn thingy here at some point
     * 
     * 
     */
    //Serial.println(durationL);
    //Serial.println(durationR);
  }
  servoWrite(centered);
}

void readOut() // outputs info from the ultrasonic distance sensors
{
  Serial.print("Distance in inches: ");
  Serial.println(inches);
  Serial.print("Distance in centimeters: ");
  Serial.println(cm);
}

void servoWrite(int angle) // because if there's a function that tracks what angle the servo motor is at, I don't know what it is
{
  servo.write(angle);
  servoAngle = angle;
  Serial.print("Rotating to ");
  Serial.print(angle);
  Serial.println(" degrees.");
}