#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

#define merah1 1
#define kuning1 2
#define hijau1 3
#define merah2 4
#define kuning2 5
#define hijau2 6
#define merah3 7
#define kuning3 8
#define hijau3 9
#define merah4 10
#define kuning4 11
#define hijau4 12

void setup() {
  // Atur pin sebagai output
  pinMode(merah1, OUTPUT);
  pinMode(kuning1, OUTPUT);
  pinMode(hijau1, OUTPUT);
  pinMode(merah2, OUTPUT);
  pinMode(kuning2, OUTPUT);
  pinMode(hijau2, OUTPUT);
  pinMode(merah3, OUTPUT);
  pinMode(kuning3, OUTPUT);
  pinMode(hijau3, OUTPUT);
  pinMode(merah4, OUTPUT);
  pinMode(kuning4, OUTPUT);
  pinMode(hijau4, OUTPUT);

  // Attach servo ke pin
  servo1.attach(A0);
  servo2.attach(A1);
  servo3.attach(A2);
  servo4.attach(A3);
}

void loop() {
  // Set posisi awal servo
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);

  // Jalur 1: Hijau, Jalur 2-4: Merah
  digitalWrite(hijau1, HIGH);
  digitalWrite(merah2, HIGH);
  digitalWrite(merah3, HIGH);
  digitalWrite(merah4, HIGH);
  delay(5000);  // Tunggu 5 detik

  // Set posisi servo untuk mengarahkan jalur 1
  servo1.write(90);
  delay(2000);  // Tunggu 2 detik

  // Jalur 1: Kuning, Jalur 2-4: Merah
  digitalWrite(hijau1, LOW);
  digitalWrite(kuning1, HIGH);
  delay(2000);  // Tunggu 2 detik

  // Set posisi servo untuk mengarahkan jalur 2
  servo2.write(90);
  delay(2000);  // Tunggu 2 detik

  // Jalur 1-2: Merah, Jalur 3: Hijau, Jalur 4: Merah
  digitalWrite(merah1, HIGH);
  digitalWrite(kuning1, LOW);
  digitalWrite(merah2, HIGH);
  digitalWrite(hijau3, HIGH);
  digitalWrite(merah4, HIGH);
  delay(5000);  // Tunggu 5 detik

  // Set posisi servo untuk mengarahkan jalur 3
  servo3.write(90);
  delay(2000);  // Tunggu 2 detik

  // Jalur 1-3: Merah, Jalur 2: Kuning, Jalur 4: Merah
  digitalWrite(merah1, HIGH);
  digitalWrite(merah2, LOW);
  digitalWrite(kuning2, HIGH);
  digitalWrite(hijau4, HIGH);
  digitalWrite(merah3, HIGH);
  delay(5000);  // Tunggu 5 detik

  // Set posisi servo untuk mengarahkan jalur 4
  servo4.write(90);
  delay(2000);  // Tunggu 2 detik

  // Jalur 1-4: Merah, Jalur 2-3: Hijau
  digitalWrite(merah1, HIGH);
  digitalWrite(hijau2, HIGH);
  digitalWrite(hijau3, HIGH);
  digitalWrite(merah4, HIGH);
  delay(5000);  // Tunggu 5 detik

}