#include <Servo.h>

// Inisialisasi objek servo
Servo servo1;
Servo servo2;

// Pin yang terhubung ke toggle switch dan servo
const int toggleSwitchPin = 2;
const int servoPin1 = 9;
const int servoPin2 = 10;

// Posisi servo saat idle dan aktif
const int servo1IdlePos = 90;
const int servo1ActivePos1 = 0;
const int servo1ActivePos2 = 45;
const int servo1ActivePos3 = 60;
const int servo1ActivePos4 = 30;

const int servo2IdlePos = 90;
const int servo2ActivePos1 = 180;
const int servo2ActivePos2 = 135;
const int servo2ActivePos3 = 120;
const int servo2ActivePos4 = 150;

int moveCount = 0; // Menghitung gerakan aktif

void setup() {
  // Menghubungkan servo ke pin yang sesuai
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  
  // Menyetel posisi awal servo ke posisi idle
  servo1.write(servo1IdlePos);
  servo2.write(servo2IdlePos);
  
  // Mengatur pin toggle switch sebagai input
  pinMode(toggleSwitchPin, INPUT_PULLUP);
}

void loop() {
  // Membaca status toggle switch
  int toggleState = digitalRead(toggleSwitchPin);

  // Jika toggle switch aktif
  if (toggleState == LOW) {
    moveCount++; // Menambah hitungan gerakan setiap kali switch ditekan

    if (moveCount == 1) {
      // Gerakan 1
      servo1.write(servo1ActivePos1);
      delay(500);
      servo2.write(servo2ActivePos1);
      delay(500);
    } 
    else if (moveCount == 2) {
      // Gerakan 2
      servo1.write(servo1ActivePos2);
      delay(500);
      servo2.write(servo2ActivePos2);
      delay(500);
    } 
    else if (moveCount == 3) {
      // Gerakan 3
      servo1.write(servo1ActivePos3);
      delay(500);
      servo2.write(servo2ActivePos3);
      delay(500);
    } 
    else if (moveCount == 4) {
      // Gerakan 4 (baru ditambahkan)
      servo1.write(servo1ActivePos4);
      delay(500);
      servo2.write(servo2ActivePos4);
      delay(500);
      moveCount = 0; // Reset gerakan setelah mencapai 4 gerakan
    }

    // Kembali ke posisi idle setelah setiap gerakan
    servo1.write(servo1IdlePos);
    delay(500); 
    servo2.write(servo2IdlePos);
  }

  // Tambahkan sedikit jeda agar program tidak terlalu cepat
  delay(100);
}