#include <ESP32Servo.h>


#define TRIGGER_PIN 12
#define ECHO_PIN 11

Servo servo1;
Servo servo2;

int times = 0;

void setup() {
  servo1.attach(9);  // เชื่อมต่อ Servo1 กับพิน 9
  servo2.attach(10); // เชื่อมต่อ Servo2 กับพิน 10
  servo1.write(0);   // เซต servo1 เป็น 0 องศา
  servo2.write(0);   // เซต servo2 เป็น 0 องศา
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(9600);
}

long getDistance() {
  digitalWrite(TRIGGER_PIN, LOW);  
  delayMicroseconds(2);  
  digitalWrite(TRIGGER_PIN, HIGH); 
  delayMicroseconds(10);  
  digitalWrite(TRIGGER_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);  // อ่านเวลาที่เสียงสะท้อน
  long distance = (duration / 2) / 29.1;    // คำนวณระยะในหน่วยเซนติเมตร
  return distance;
}

void loop() {
  long distance = getDistance();  // อ่านระยะจาก ultrasonic

  if (distance > 0 && distance < 200) {
    // ถ้าระยะน้อยกว่า 3 cm
    servo2.write(180);  // สั่งให้ servo2 ไปที่ 180 องศา
    delay(5000);         // รอ 5 วินาที
    servo2.write(0);    // เซต servo2 กลับเป็น 0 องศา
  }

  if (times == 0) {
    servo1.write(0);    // เซต servo1 เป็น 0 องศา
  } else if (times == 1) {
    servo1.write(180);  // เซต servo1 เป็น 180 องศา
  }

  delay(100); // รอเวลาสักครู่ก่อนวนลูปใหม่
}