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

// กำหนดขาที่ใช้เชื่อมต่อกับ Trig และ Echo ของ Ultrasonic Sensor
#define Trig 2
#define Echo 3

Servo myservo;  // สร้าง Object ของ Servo Motor
long duration;  // เก็บค่าเวลาที่ Ultrasonic Sensor รับสัญญาณกลับ
int distance;   // เก็บค่าระยะทางที่วัดได้จาก Ultrasonic Sensor

LiquidCrystal_I2C lcd(0x27, 16, 2);  // สร้าง Object ของ LCD ที่เชื่อมต่อผ่าน I2C

void setup() {
  myservo.attach(4);  // กำหนดขาที่ Servo Motor ต่อมา
  pinMode(Trig, OUTPUT);  // กำหนดขา Trig เป็น Output
  pinMode(Echo, INPUT);   // กำหนดขา Echo เป็น Input

  lcd.init();  // กำหนดค่าเริ่มต้นของ LCD
}

void loop() {
  digitalWrite(Trig, LOW);  // ส่งสัญญาณ Trig ให้ Ultrasonic Sensor เริ่มทำงาน
  delayMicroseconds(5);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);

  duration = pulseIn(Echo, HIGH);  // วัดเวลาที่ Ultrasonic Sensor ใช้ในการรับสัญญาณ
  distance = duration * 0.034 / 2;  // คำนวณระยะทางจากเวลาที่ได้

  lcd.clear();  // ลบหน้าจอ LCD

  // แสดงข้อมูลระยะทางบน LCD
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm");

  // ตรวจสอบระยะทางและควบคุม Servo Motor
  if (distance <= 15) {
    myservo.write(170);  // หมุน Servo Motor ที่มุม 170 องศา
    lcd.setCursor(0, 1);
    lcd.print("Angle: 170");
    delay(5000);
  } else {
    myservo.write(80);  // หมุน Servo Motor ที่มุม 80 องศา
    lcd.setCursor(0, 1);
    lcd.print("Angle: 80");
  }

  delay(500);  // รอสักครู่
}