#include <ESP32Servo.h>

const int servoPinX = 18;   // Servo untuk pergerakan X
const int servoPinY1 = 19;  // Servo pertama untuk pergerakan Y
const int servoPinY2 = 5;   // Servo kedua untuk pergerakan Y
const int servoPinGrip = 17;// Servo untuk pengendali capitan

Servo servoX;
Servo servoY1;
Servo servoY2;
Servo servoGrip;

void setup() {
  servoX.attach(servoPinX, 500, 2400);
  servoY1.attach(servoPinY1, 500, 2400);
  servoY2.attach(servoPinY2, 500, 2400);
  servoGrip.attach(servoPinGrip, 500, 2400);
}

int posX = 0;
int posY1 = 0;
int posY2 = 0;
int posGrip = 0;

void loop() {
  // Gerakan maju
  for (posX = 0; posX <= 180; posX += 1) {
    servoX.write(posX);
    delay(15);
  }
  
  // Gerakan naik dan turun
  for (posY1 = 0, posY2 = 180; posY1 <= 180; posY1 += 1, posY2 -= 1) {
    servoY1.write(posY1);
    servoY2.write(posY2);
    delay(15);
  }
  
  // Membuka capitan
  for (posGrip = 0; posGrip <= 180; posGrip += 1) {
    servoGrip.write(posGrip);
    delay(15);
  }
  
  // Gerakan mundur
  for (posX = 180; posX >= 0; posX -= 1) {
    servoX.write(posX);
    delay(15);
  }
  
  // Gerakan naik dan turun
  for (posY1 = 180, posY2 = 0; posY1 >= 0; posY1 -= 1, posY2 += 1) {
    servoY1.write(posY1);
    servoY2.write(posY2);
    delay(15);
  }
  
  // Menutup capitan
  for (posGrip = 180; posGrip >= 0; posGrip -= 1) {
    servoGrip.write(posGrip);
    delay(15);
  }
}