/************************************************** 
* Smart Object Detection System using ESP32 Microcontroller * 
* SUBMITTED BY: ESQUILONA, JAYDEL E. * 
* SUBMITTED TO: PROF. MICHAEL T. SAMONTE * 
* *************************************************/
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#define PIN_TRIG 18
#define PIN_ECHO 19
#define SERVO_PIN 5

Servo servo;
LiquidCrystal_I2C lcd(0x27, 20, 4);

void setup() {
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  lcd.init(); 
  lcd.backlight(); 

  servo.attach(SERVO_PIN);
}

void loop() {
  long duration, distance;


  lcd.setCursor(0, 0);
  lcd.print("Object Detector");
  lcd.setCursor(0, 1);
  lcd.print("Distance: ");
  lcd.setCursor(0,2);
  lcd.print("Esquilona, Jaydel E.");
  lcd.setCursor(0, 3);
  lcd.print("Direction: ");
  
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  
  duration = pulseIn(PIN_ECHO, HIGH);
  distance = 0.017 * duration;
/* * 0.0343 */
/* /29.1 */
/* distance = 0.017 * duration; */
/* distance = (duration / 2) /29.1; */
  lcd.setCursor(10, 1);
  lcd.print("     "); 
  lcd.setCursor(10, 1);
  lcd.print(distance);
  
  int angle = map(distance, 2, 15, 0, 180); 
  servo.write(angle);
   
  if(distance < 2 || distance >15){
    servo.write(90);
  }

  lcd.setCursor(11, 3);
  lcd.print("     "); 
  lcd.setCursor(11, 3);
  lcd.print(angle);

  delay(20000000000000000);
}