/**********************************
Smart Object Detection System using ESP32 Microcontroller
SUBMITTED BY:SARCE,JUZELLE VANESSA 
SUBMITTED TO:PROF. MICAHEL T. SAMONTE *
******************************************/
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

#define PIN_TRIG 4
#define PIN_ECHO 2
#define SERVO_PIN 18

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.clear();

  lcd.setCursor(0, 0);
  lcd.print("Activity 6-7");

  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;

  lcd.setCursor(0, 1);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm");

  int angle;

  if (distance > 2 && distance < 25) {
    angle = map(distance, 2, 20, 0, 180);
  } else {
    angle = 90;
  }

  servo.write(angle);

  lcd.setCursor(0, 2);
  lcd.print("Angle: ");
  lcd.setCursor(13, 2);
  lcd.print(angle);
  lcd.print(" deg");

  lcd.setCursor(0, 3);
  lcd.print("Juzelle Sarce");

  delay(1000);
}