#include <Servo.h>

Servo servos[2];
int buttons[4] = {2, 3, 4, 5};
int pos[2] = {0, 0};  // ตำแหน่งเริ่มต้นของเซอร์โวเป็น 0 องศา
bool buttonState[4] = {HIGH, HIGH, HIGH, HIGH};  // สถานะปัจจุบันของปุ่ม
bool lastButtonState[4] = {HIGH, HIGH, HIGH, HIGH}; // สถานะก่อนหน้าของปุ่ม

void setup() {
  for (int i = 0; i < 2; i++) servos[i].attach(i + 9);  // เซอร์โวทั้ง 2 ตัวเชื่อมต่อที่ขา 9 และ 10
  for (int i = 0; i < 4; i++) pinMode(buttons[i], INPUT_PULLUP);  // กำหนดให้ปุ่มทำงานเป็น INPUT_PULLUP
  servos[0].write(pos[0]);  // เซอร์โวตัวแรกเริ่มที่ 0 องศา
  servos[1].write(pos[1]);  // เซอร์โวตัวที่สองเริ่มที่ 0 องศา
}

void loop() {
  for (int i = 0; i < 4; i++) {
    buttonState[i] = digitalRead(buttons[i]);  // อ่านสถานะปุ่ม

    // เช็คว่าเป็นการกดปุ่ม (จาก HIGH เป็น LOW) และยังไม่ได้กดซ้ำ
    if (buttonState[i] == LOW && lastButtonState[i] == HIGH) {
      int servoIndex = i / 2;  // กำหนดว่า button1, button2 ใช้ servo1, button3, button4 ใช้ servo2
      int direction = (i % 2 == 0) ? 30 : -30;  // button1, button3 หมุนตามเข็มนาฬิกา, button2, button4 หมุนทวนเข็มนาฬิกา
      rotateServo(&servos[servoIndex], &pos[servoIndex], direction);
    }
    
    lastButtonState[i] = buttonState[i];  // เก็บสถานะปัจจุบันของปุ่ม
  }
  delay(50);
}

void rotateServo(Servo* servo, int* pos, int step) {
  *pos += step;
  if (*pos > 180) *pos = 180;  // ตรวจสอบไม่ให้เกิน 180 องศา
  if (*pos < 0) *pos = 0;      // ตรวจสอบไม่ให้ต่ำกว่า 0 องศา
  servo->write(*pos);  // ส่งคำสั่งให้เซอร์โวไปยังตำแหน่งใหม่
}