/******************************************
|     Smart Object Detection System       |
|      using ESP32 Microcontroller        |
|     SUBMITTED BY: MAGAT, ANGELO G.      |
|  SUBMITTED TO: PROF. MICHAEL T. SAMONTE |
|*****************************************/

#include<ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#define TRIG_PIN 18
#define ECHO_PIN 19
#define SERVO 2

Servo gelo_servo;
LiquidCrystal_I2C lcd(0x27, 16, 4);

long Duration;
int Distance_Cm;

void setup() {

gelo_servo.attach(SERVO);  
lcd.init(); // initialize the LCD
lcd.backlight(); // turn on backlight

pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);

}

void loop() {
  
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);

Duration = pulseIn(ECHO_PIN, HIGH);
Distance_Cm= (Duration*0.0341)/2;
  
int angle = map(Distance_Cm, 2, 400, 0, 180); 
gelo_servo.write(angle);

  if (Distance_Cm < 10) { // Example threshold value
    // If an object is very close, point the servo to the right
    gelo_servo.write(45);
    lcd.setCursor(0,0); 
    lcd.print("|Object Detected: R|"); 
  } else if (Distance_Cm < 20) {
    // If an object is within a medium range, point the servo to the center
    gelo_servo.write(90);
    lcd.setCursor(0,0); 
    lcd.print("|Object Detected: C|"); 
  } else if (Distance_Cm < 30) {
    // If an object is within a medium range, point the servo to the left
    gelo_servo.write(135);
    lcd.setCursor(0,0); 
    lcd.print("|Object Detected: L|"); 
  } else {
    // If an object is far away, point the servo to the center
    gelo_servo.write(90);
    lcd.setCursor(0,0); 
    lcd.print("|No Object Detected|"); 
  }

lcd.setCursor(0,1);
lcd.print("| Distance: "); 
lcd.print(Distance_Cm); 
lcd.print(" cm   |");
lcd.setCursor(0, 2);
lcd.print("|    Created by:   |");
lcd.setCursor(0, 3);
lcd.print("| -Angelo G. Magat-|");

}