#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
Servo myservo; // Buat objek dari kelas Servo
const int trigPin = 2; // Pin trigger HC-SR04 terhubung ke pin D2 pada ESP32
const int echoPin = 4; // Pin echo HC-SR04 terhubung ke pin D4 pada ESP32
// Inisialisasi objek LCD
LiquidCrystal_I2C lcd(0x27, 20, 4); // Alamat I2C bisa berbeda tergantung koneksi

void setup() {
  myservo.attach(15); // Sambungkan servo ke pin D15
  Serial.begin(9600);  // Inisialisasi komunikasi serial

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Inisialisasi komunikasi I2C
  Wire.begin();

  // Inisialisasi LCD dengan dimensi 20x4
  lcd.init();
  lcd.backlight();
  // Tampilkan pesan awal
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  lcd.setCursor(0, 1);
  lcd.print("Servo Angle: ");
}

void loop() {
  long duration, distance;

  // Send a pulse to trigger the HC-SR04
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the pulse from the HC-SR04
  duration = pulseIn(echoPin, HIGH);

  // Convert the duration into distance (in centimeters)
  distance = (duration / 2) / 29.1; // Speed of sound is approximately 29.1 microseconds per centimeter

  // Limit the servo movement between 0 and 180 degrees
  int servoAngle = map(distance, 0, 100, 0, 180);

  // Move the servo to the calculated angle
  myservo.write(servoAngle);
  
  // Print the distance and servo angle to the serial monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.print(" cm\t");
  Serial.print("Servo Angle: ");
  Serial.println(servoAngle);

  // Update the LCD display
  lcd.setCursor(9, 0); // Set the cursor position for distance
  lcd.print("      "); // Clear previous distance value
  lcd.setCursor(9, 0);
  lcd.print(distance);
  
  lcd.setCursor(12, 1); // Set the cursor position for servo angle
  lcd.print("      "); // Clear previous servo angle value
  lcd.setCursor(12, 1);
  lcd.print(servoAngle);

  delay(500); // Delay for a moment before taking another reading
}
esp:VIN
esp:GND.2
esp:D13
esp:D12
esp:D14
esp:D27
esp:D26
esp:D25
esp:D33
esp:D32
esp:D35
esp:D34
esp:VN
esp:VP
esp:EN
esp:3V3
esp:GND.1
esp:D15
esp:D2
esp:D4
esp:RX2
esp:TX2
esp:D5
esp:D18
esp:D19
esp:D21
esp:RX0
esp:TX0
esp:D22
esp:D23
ultrasonic1:VCC
ultrasonic1:TRIG
ultrasonic1:ECHO
ultrasonic1:GND
servo1:GND
servo1:V+
servo1:PWM
lcd1:GND
lcd1:VCC
lcd1:SDA
lcd1:SCL