MustCleanUp

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Arduino.h>
#define OLED_RESET 4

int IN1           = 4;  // MOTOR FOR GOING FORWARD
int IN2           = 5;  // MOTOR FOR GOING BACKWARD
int ENA           = 11;
int SPEED         = 150;
int trigpin       = 2;
int trigpin2      = 6;
int trigpin3      = 10;
int echopin       = 3;
int echopin2      = 7;
int echopin3      = 11;

long duration     = 0;
long distance     = 0;
long UltraSensor1 = 0;
long UltraSensor2 = 0;
long UltraSensor3 = 0;

char data         = 0;
String SerialData = "";

// trigpin,echopin = starting sensor
// trigpin2, echopin2 = Open Door Sensor
// trigpin3, echopin3 = Stop Door, move motor back to initial position sensor

Adafruit_SSD1306 display(OLED_RESET);

void SonarSensor(int trigPinSensor, int echoPinSensor){

  digitalWrite(trigPinSensor, LOW);   // put trigpin LOW
  delayMicroseconds(2);               // wait 2 microseconds
  digitalWrite(trigPinSensor, HIGH);  // switch trigpin HIGH
  delayMicroseconds(10);              // wait 10 microseconds
  digitalWrite(trigPinSensor, LOW);   // turn it LOW again

  duration = pulseIn(echoPinSensor, HIGH);
  distance = (duration / 2) / 29.1;
}


void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  delay(1);
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
  pinMode(IN1, OUTPUT);
  pinMode(trigpin2, OUTPUT);
  pinMode(echopin2, INPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENA, OUTPUT);
  analogWrite(ENA, SPEED);

  Serial.begin(9600);
}

void loop(){

  SonarSensor(trigpin, echopin);
  UltraSensor1 = distance;
  SonarSensor(trigpin2, echopin2);
  UltraSensor2 = distance;
  SonarSensor(trigpin3, echopin3);
  UltraSensor3 = distance;

  if (UltraSensor1 <= 5 && UltraSensor3 <= 10 && UltraSensor2 < 100) {
    delay(500);
    display.clearDisplay();
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    display.setTextSize(1.8);
    display.setCursor(0, 0);
    display.setTextColor(WHITE);
    display.print("CLOSED");
    display.display();
  }  else if (IN1 == LOW && IN2 == LOW && UltraSensor3 >= 11 && UltraSensor1 > 6 && UltraSensor2 < 100) {
    delay(500);
    display.clearDisplay();
    digitalWrite(IN2, LOW);
    digitalWrite(IN1, HIGH);
    display.setTextSize(1.8);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("OPENING");
    display.display();
  }  else if (IN1 == HIGH && IN2 == LOW && UltraSensor2 >= 100 && UltraSensor1 > 6 && UltraSensor3 >= 11) {
    delay(500);
    display.clearDisplay();
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    display.setTextSize(1.8);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("RETURNING");
    display.display();
  } else {
    delay(500);
    display.clearDisplay();
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    display.setTextSize(1.8);
    display.setCursor(0, 0);
    display.setTextColor(WHITE);
    display.print("CLOSED");
    display.display();
  }
}