#include <Servo.h>

int servo_pin[] = {10, 9, 6, 5};
Servo servo[4];

int btn_pin = 2;

int TrTh_Btn;
int DaHanhdong = false;

void setup() {
  pinMode(btn_pin, INPUT_PULLUP);
  
  for(int i = 0; i < 4; i ++){
    servo[i].attach(servo_pin[i]);
  }

  // Khi khởi động các servo quay về tọa độ góc
  for(int i = 0; i < 4; i ++){
    servo[i].write(0);
  }
  TrTh_Btn = digitalRead(btn_pin);
}

unsigned long ThG_Giu = 0;

void loop() {
  int TrTh_HT = digitalRead(btn_pin);

  if(TrTh_HT == LOW && TrTh_Btn == HIGH){
    ThG_Giu = millis();
    DaHanhdong = false;
  }

  if(TrTh_HT == HIGH && TrTh_Btn == LOW){
    if(millis() - ThG_Giu >= 3000){
      for (int i = 0; i < 4; i++) {
        servo[i].write(180);
      }
    }
    DaHanhdong = true;
  }
  TrTh_Btn = TrTh_HT;
}