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

#define red_led_pin 42
#define green_led_pin 41
#define servo_pin 39
#define trig_pin 38
#define echo_pin 37

Servo myservo;
LiquidCrystal_I2C mylcd(0x27, 16, 2);

long duration, distance;

void setup() {
  pinMode(red_led_pin, OUTPUT);
  pinMode(green_led_pin, OUTPUT);
  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);
  myservo.attach(servo_pin);
  Wire.begin(8, 9);
  mylcd.init();
  mylcd.backlight();
}

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

  duration = pulseIn(echo_pin, HIGH);
  distance = duration * 0.034 / 2;

  mylcd.setCursor(0, 0);
  mylcd.print("UPRAK 2025");
  mylcd.setCursor(0, 1);
  mylcd.print("Servo: ");

  if (distance <= 100) {
    digitalWrite(red_led_pin, LOW);
    digitalWrite(green_led_pin, HIGH);
    myservo.write(90);
    mylcd.print("Terbuka ");
  } else {
    digitalWrite(red_led_pin, HIGH);
    digitalWrite(green_led_pin, LOW);
    myservo.write(0);
    mylcd.print("Tertutup");
  }

  delay(1000);
}