#include <ESP32Servo.h>
Servo myservo; //ประกาศตัวแปรแทน Servo

const int Trig = 13;
int Echo = 12;
long duration, distance;

int LEDGreen = 18;
int LEDYellow = 19;

void setup() 
{
  
  Serial.begin(9600);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);

  pinMode(LEDGreen, OUTPUT);
  pinMode(LEDYellow, OUTPUT);

  myservo.attach(13); // D22 (กำหนดขาควบคุม Servo)
  Serial.print("Start your Engine");
}
void loop()
{

  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(5);
  digitalWrite(Trig, LOW);

  duration = pulseIn(Echo, HIGH);
  distance = microsecondsToCentimeters(duration);
  Serial.print(distance);
  Serial.println(" cm");

  digitalWrite(LEDGreen, HIGH);
  digitalWrite(LEDYellow, LOW);

  delay(100);
  if (distance <= 20)
  {
    myservo.write(0); // สั่งให้ Servo หมุนไปองศาที่ 0
    digitalWrite(LEDGreen, LOW);
    digitalWrite(LEDYellow, HIGH);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, LOW);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, HIGH);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, LOW);
    delay(300); // หน่วงเวลา 1000ms
  }
  else if (distance > 20 && distance < 100)
  {
    myservo.write(90); // สั่งให้ Servo หมุนไปองศาที่ 90
    digitalWrite(LEDGreen, LOW);
    digitalWrite(LEDYellow, HIGH);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, LOW);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, HIGH);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, LOW);
    delay(300); // หน่วงเวลา 1000ms
  }
  else
  {
    myservo.write(180); // สั่งให้ Servo หมุนไปองศาที่ 180
    digitalWrite(LEDGreen, LOW);
    digitalWrite(LEDYellow, HIGH);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, LOW);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, HIGH);
    delay(300); // หน่วงเวลา 1000ms
    digitalWrite(LEDYellow, LOW);
    delay(300); // หน่วงเวลา 1000ms
  }
}
long microsecondsToCentimeters(long microseconds)
{
// ความเร็วเสียงในอากาศประมาณ 340 เมตร/วินาที หรือ 29 ไมโครวินาที/เซนติเมตร
// ระยะทางที่ส่งเสียงออกไปจนเสียงสะท้อนกลับมาสามารถใช้หาระยะทางของวัตถุได้
// เวลาที่ใช้คือ ระยะทางไปกลับ ดังนั้นระยะทางคือ ครึ่งหนึ่งของที่วัดได้
  return microseconds / 29 / 2;
}