ATOAR - automated terrestrial object avoidance robot V1 code

#include <Servo.h> // servo library
const int ping_pin = 9; // signal pin for Ultrasonic Range Sensor
const int min_safe_distance = 20; // The distance the robot will stop at from a wall, 10cm
long duration, distance, right_distance, left_distance; // time it takes for ping to return, distance of ping scan, ping distance of right and left directions
Servo servo; // declaring servo that rotates ultrasoinc sensor

// --------------------------------------------------------------------------- Motors
int motor_left_1 = 2;
int motor_left_2 = 4;
int motor_right_1 = 7;
int motor_right_2 = 8;

int enable_right = 3;
int enable_left = 5;

// --------------------------------------------------------------------------- Setup
void setup() {
  servo.attach(10);
  servo.write(90);

  pinMode(motor_left_1, OUTPUT);
  pinMode(motor_left_2, OUTPUT);
  pinMode(motor_right_1, OUTPUT);
  pinMode(motor_right_2, OUTPUT);

  pinMode(enable_right, OUTPUT);
  pinMode(enable_left, OUTPUT);

  digitalWrite(enable_right, HIGH);
  digitalWrite(enable_left, HIGH);
}

// --------------------------------------------------------------------------- Loop
void loop() {
 int forward_distance = ping();
 if(forward_distance > min_safe_distance){
   Drive_forward();
 }
 else{
   Stop();
   servo.write(5); // look right
   delay(500);
   right_distance = ping();
   delay(1000);
   servo.write(175); // look left
   delay(500);
   left_distance = ping();
   delay(1000);
   servo.write(90);

   if(right_distance > left_distance){
     Turn_right();
     delay(325);
   }
   else{
     Turn_left();
     delay(325);
   }
 }
}

// --------------------------------------------------------------------------- Drive
void Stop(){ // Stop robot
  digitalWrite(motor_left_1, LOW);
  digitalWrite(motor_left_2, LOW);

  digitalWrite(motor_right_1, LOW);
  digitalWrite(motor_right_2, LOW);
  delay(25);
}

//---------------------------------
void Drive_forward(){ // go forward
  analogWrite(enable_left, 249);
  analogWrite(enable_right, 250);

  digitalWrite(motor_left_1, HIGH);
  digitalWrite(motor_left_2, LOW);

  digitalWrite(motor_right_1, HIGH);
  digitalWrite(motor_right_2, LOW);
}

//---------------------------------
void Drive_backward(){ // go backward
  analogWrite(enable_left, 249);
  analogWrite(enable_right, 250);

  digitalWrite(motor_left_1, LOW);
  digitalWrite(motor_left_2, HIGH);

  digitalWrite(motor_right_1, LOW);
  digitalWrite(motor_right_2, HIGH);
}

//---------------------------------
void Turn_left(){ // turn robot left
  analogWrite(enable_left, 249);
  analogWrite(enable_right, 250);

  digitalWrite(motor_left_1, LOW);
  digitalWrite(motor_left_2, HIGH);

  digitalWrite(motor_right_1, HIGH);
  digitalWrite(motor_right_2, LOW);
}

//---------------------------------
void Turn_right(){ // turn robot right
  analogWrite(enable_left, 249);
  analogWrite(enable_right, 250);

  digitalWrite(motor_left_1, HIGH);
  digitalWrite(motor_left_2, LOW);

  digitalWrite(motor_right_1, LOW);
  digitalWrite(motor_right_2, HIGH);
}

//---------------------------------
unsigned long ping(){ // find distance using ultrasonic sensor
  pinMode(ping_pin, OUTPUT);
  digitalWrite(ping_pin, LOW);
  delay(2);
  digitalWrite(ping_pin, HIGH);
  delay(5);
  digitalWrite(ping_pin, LOW);

  pinMode(ping_pin, INPUT);
  duration = pulseIn(ping_pin, HIGH);
  distance = duration/59;
  return distance;
}