// Servo Sweep example for the ESP32
// https://wokwi.com/arduino/projects/323706614646309460


#include<ESP32Servo.h>

Servo servo1;
Servo servoLengan;
Servo servoYRobotAtas;
Servo servoYRobotBawah;

const int servoPin1 = 2;
//int pinServoLengan = 0;
int pinServoYRobotAtas = 18;
int pinServoYRobotBawah = 19;

void setup() {
  //servoCapit.attach(pinServoCapit);
 // servoLengan.attach(pinServoLengan);
  servo1.attach(servoPin1);
  servoYRobotAtas.attach(pinServoYRobotAtas);
  servoYRobotBawah.attach(pinServoYRobotBawah);
}

void loop() {
  // Contoh pergerakan servo-servo sesuai dengan input atau logika tertentu
  //servoCapit.write(90); // Contoh: menggerakkan capit ke posisi tengah
  //delay(1000);
  //servoLengan.write(0); // Contoh: menggerakkan lengan ke posisi kiri
  //delay(1000);
  // Menggerakkan kedua servo Y robot bersamaan

  // Open the gripper
  openGripper();
  delay(2000); // Wait for 2 seconds

  // Close the gripper
  closeGripper();
  delay(2000); // Wait for 2 seconds

  servoYRobotAtas.write(0); // Contoh: menggerakkan Y robot ke posisi atas
  servoYRobotBawah.write(0);
  delay(1000);
  servoYRobotAtas.write(105); // Contoh: menggerakkan Y robot ke posisi bawah
  servoYRobotBawah.write(105);
  delay(1000);
}

void openGripper() {
  // Set servo angles for opening the gripper
  servo1.write(90); // Adjust according to your servo's range
  //servo2.write(90); // Adjust according to your servo's range
}

void closeGripper() {
  // Set servo angles for closing the gripper
  servo1.write(45); // Adjust according to your servo's range
  //servo2.write(135); // Adjust according to your servo's range
}