#include <ESP32Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define servoPin 18

int triggerPin = 17;
int echoPin = 16;
float speedofSound = 0.034;
long durn;
float distanceCM;
Servo s; // constructor
Adafruit_SSD1306 display(128,64,&Wire,-1); // oled Object Class


void setup() {
  Serial.begin(9600);
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
  s.attach(servoPin);
  if(!display.begin(SSD1306_SWITCHCAPVCC,0x3c)){ // Error Handling
    Serial.println("Can't Read Details!");
  }
}

void loop() {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  durn = pulseIn(echoPin, HIGH);
  distanceCM = durn * speedofSound / 2;
  Serial.println("Distance: " + String(distanceCM));

  display.clearDisplay(); // reset screen for each fetching of sensor reading
  display.setTextSize(1); //
  display.setCursor(0,10);
  display.setTextColor(WHITE);

  if(distanceCM <= 100){
    display.println("Distance :" + String(distanceCM));
    // Open the gate if the distance is increasing
  
    for(int r = 90; r > 0; r--) {
          s.write(r);
          delay(15);
        }
    delay(15);  
    
    for(int r = 0; r <= 90; r++) {
          s.write(r);
          delay(15);
        }
    delay(500);   


  } else {
    Serial.println("Gate Close!");
    display.println("GATE CLOSED!");
  }
  display.display();
}