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

const int trigPin = 33;          // ขา Trig ของ Ultrasonic
const int echoPin = 27;          // ขา Echo ของ Ultrasonic
const int relayPin = 25;         // ขา Relay
const int servoPin = 18;         // ขา Servo
//const int buzzerPin = 17;        // ขา Buzzer

LiquidCrystal_I2C lcd(0x27, 16, 2); // กำหนดที่อยู่ของ LCD

Servo myServo;                   // สร้างวัตถุเซอร์โว

void setup() {
  Serial.begin(115200);
  
  // เริ่มต้น LCD
  lcd.init();
  lcd.backlight();
  lcd.print("Ultrasonic");

  // กำหนดโหมดของขา
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(relayPin, OUTPUT);
  //pinMode(buzzerPin, OUTPUT);
  
  myServo.attach(servoPin);     // เชื่อมต่อเซอร์โว
  myServo.write(0);             // ตั้งค่าเซอร์โวให้เริ่มที่ 0 องศา
}

void loop() {
  long duration, distance;

  // ส่งเสียงไปที่เซ็นเซอร์อัลตราโซนิก
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // อ่านค่าจากเซ็นเซอร์
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; // แปลงระยะทางเป็นเซนติเมตร

  // แสดงค่าระยะทางบน LCD
  lcd.setCursor(0, 1);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm   ");

  // ควบคุม Relay และ Servo ตามระยะทาง
  if (distance > 200) { // ถ้าระยะทางน้อยกว่า 20 ซม.
    digitalWrite(relayPin, HIGH);  // เปิด Relay
    myServo.write(180);              // เปลี่ยนตำแหน่งเซอร์โว
    //tone(buzzerPin, 1000);          // เปิด Buzzer
    //delay(200);                     // รอ 200 มิลลิวินาที
    //noTone(buzzerPin);              // ปิด Buzzer
    //delay(200);                     // รอ 200 มิลลิวินาที
  } else {
    digitalWrite(relayPin, LOW);   // ปิด Relay
    myServo.write(0);               // กลับไปที่ 0 องศา
    //noTone(buzzerPin);              // ปิด Buzzer
  }

  delay(100); // รอ 100 มิลลิวินาที
}