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

// กำหนดขา
#define trigPin 7
#define echoPin 6

Servo myServo;

// กำหนดที่อยู่ของจอ LCD I2C (0x27 เป็นค่าเริ่มต้นสำหรับจอ I2C ส่วนมาก)
LiquidCrystal_I2C lcd(0x27, 20, 4); // ขนาดจอ 20x4

long duration;
int distance;

void setup() {
  // เริ่มต้นการสื่อสาร I2C กับจอ LCD
  lcd.init();
  lcd.backlight();
  
  // กำหนดการทำงานของขาเซ็นเซอร์
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // ติดตั้งเซอร์โวที่ขา 9
  myServo.attach(9);

  // ตั้งค่าเริ่มต้นของจอ LCD
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  
  // ตั้งค่าเริ่มต้นของเซอร์โว
  myServo.write(0); // หมุนเซอร์โวไปที่ตำแหน่ง 0 องศา
}

void loop() {
  // ส่งสัญญาณ Ultrasonic
  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(10, 0); // ตั้งตำแหน่งที่จะแสดงข้อความบนจอ
  lcd.print("    ");    // ลบข้อมูลเก่าบนจอ
  lcd.setCursor(10, 0);
  lcd.print(distance);
  lcd.print(" cm");

  // การควบคุมเซอร์โวตามระยะทางที่วัดได้
  if (distance < 100) {
    myServo.write(180); // หมุนเซอร์โวไปที่ตำแหน่ง 180 องศา
  } else {
    myServo.write(0); // หมุนเซอร์โวไปที่ตำแหน่ง 0 องศา
  }

  // หน่วงเวลาเล็กน้อยก่อนวัดใหม่
  delay(500);
}