#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

#define TRIG_PIN 5
#define ECHO_PIN 4
#define SERVO_PIN 18
#define LED_PIN 2

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo;

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);

  servo.attach(SERVO_PIN, 500, 2400); //0° e 180° - range PWM
  servo.write(0);  //posição inicial

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Sistema Pronto");
  delay(1000);
  lcd.clear();
}

void loop() {
  float distancia = medirDistancia(); //chama a função que calcula a distância e guarda na variavel

  lcd.setCursor(0, 0);
  lcd.print("Distancia:");
  lcd.setCursor(10, 0);
  lcd.print(distancia);
  lcd.print("cm");

  if (distancia < 30.0) {
    lcd.setCursor(0, 1);
    lcd.print("Objeto Detectado ");
    digitalWrite(LED_PIN, HIGH);
    servo.write(90);  //gira o servo em 90°
    delay(1000);
    servo.write(0);   //volta para posição inicial
    lcd.setCursor(0, 1);
    lcd.print("Aguardando...    ");
  } else {
    digitalWrite(LED_PIN, LOW);
    lcd.setCursor(0, 1);
    lcd.print("Sem Objeto       ");
  }

  delay(1000);
}
//Dispara um som;espera ele voltar;calcula a distância baseada no tempo;retorna o valor em centímetros
float medirDistancia() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duracao = pulseIn(ECHO_PIN, HIGH); //calcula o tempo que o som levou pra voltar
  float distancia = duracao * 0.034 / 2; //velocidade do som no ar
  return distancia;
}