#include <ESP32Servo.h>
Servo myservo, myservo2;  //ประกาศตัวแปรแทน Servo
int val, val2;
int vibration = 25;
int smallbuzzer = 23;
int bigbuzzer = 14;
int ir1 = 26;
int ir2 = 27;
int sun = 0;
int r = 19;
int y = 5;
int g = 18;
long delayTime = 0;
unsigned long prevTime_T1 = millis();
unsigned long prevTime_T2 = millis();
int state = 0;
void setup() {
  pinMode(r, OUTPUT);
  pinMode(y, OUTPUT);
  pinMode(g, OUTPUT);

  digitalWrite(r, 0);
  digitalWrite(g, 0);
  digitalWrite(y, 0);
  Serial.begin(9600);
  pinMode(ir1, INPUT);
  pinMode(ir2, INPUT);
  pinMode(vibration, INPUT);
  pinMode(smallbuzzer, OUTPUT);
  pinMode(bigbuzzer, OUTPUT);
  myservo.attach(16);   // D22 (กำหนดขาควบคุม Servo)
  myservo2.attach(17);  // D22 (กำหนดขาควบคุม Servo)
}
void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - prevTime_T1 > 1500) {
    if (state == 0) {
      digitalWrite(y, 0);
      digitalWrite(r, 1);
      digitalWrite(g, 0);
      state = 1;
    } else if (state == 1) {
      digitalWrite(y, 0);
      digitalWrite(g, 1);
      digitalWrite(r, 0);
      state = 2;
    } else if (state == 2) {
      digitalWrite(g, 0);
      digitalWrite(y, 1);
      digitalWrite(r, 0);
      state = 0;
    }
    prevTime_T1 = currentMillis;
    // Update the timestamp
  }




  val = digitalRead(ir1);   //อ่านค่าสัญญาณ digital ขา8 ที่ต่อกับ เซ็นเซอร์ตรวจจับวัตถุ IR Infrared
  val2 = digitalRead(ir2);  //อ่
  sun = analogRead(vibration);
  if (val == 0) {
    myservo.write(90);   // สั่งให้ Servo หมุนไปองศาที่ 0
    delay(3000);         // หน่วงเวลา 1000m
    myservo.write(0);    // สั่งให้ Servo หมุนไปองศาที่ 180
    delay(1000);         // หน่วงเวลา 2000ms
    myservo2.write(90);  // สั่งให้ Servo หมุนไปองศาที่ 0
    delay(3000);         // หน่วงเวลา 1000m
    myservo2.write(0);   // สั่งให้ Servo หมุนไปองศาที่ 180
    //delay(5000); // หน่วงเวลา 2000ms
  }
  if (val2 == 0) {
    myservo2.write(90);  // สั่งให้ Servo หมุนไปองศาที่ 0
    delay(3000);         // หน่วงเวลา 1000m
    myservo2.write(0);   // สั่งให้ Servo หมุนไปองศาที่ 180
    delay(1000);         // หน่วงเวลา 2000ms
    myservo.write(90);   // สั่งให้ Servo หมุนไปองศาที่ 0
    delay(3000);         // หน่วงเวลา 1000m
    myservo.write(0);    // สั่งให้ Servo หมุนไปองศาที่ 180
    //delay(5000); // หน่วงเวลา 2000ms
  }

  if (sun == 4095) {
    digitalWrite(bigbuzzer, 1);
    delay(2000);
  } else {
    digitalWrite(bigbuzzer, 0);
  }
}