#include <ESP32Servo.h>

const int pinServoKanan = 13;
const int pinServoKiri  = 19;
const int pinTombol     = 14; // GPIO tombol sesuai wiring kamu

Servo servoKanan;
Servo servoKiri;

bool statusCapit = false;
bool tombolTerakhir = HIGH;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

void setup() {
  Serial.begin(115200);
  pinMode(pinTombol, INPUT_PULLUP);

  servoKanan.attach(pinServoKanan);
  servoKiri.attach(pinServoKiri);
  delay(100);  // Delay setelah attach

  servoKanan.write(90);
  servoKiri.write(90);

  Serial.println("Sistem siap. Tekan tombol untuk toggle capit.");
}

void loop() {
  bool tombolSekarang = digitalRead(pinTombol);

  if (tombolSekarang != tombolTerakhir) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (tombolTerakhir == HIGH && tombolSekarang == LOW) {
      statusCapit = !statusCapit;

      if (statusCapit) {
        servoKanan.write(180);
        servoKiri.write(0);
        Serial.println("➡️ Capit BUKA");
      } else {
        servoKanan.write(90);
        servoKiri.write(90);
        Serial.println("⬅️ Capit TUTUP");
      }
    }
  }

  tombolTerakhir = tombolSekarang;
  delay(100);
}