// =============================================
//  Project: Kalibrasi Servo AC Lift
//  Tujuan : Mengukur jumlah step antar lantai (servo driver type STEP-DIR)
//  Author : Pak Eko
//  Versi  : v1.1.0
// =============================================

#define STEP_PIN     21   // Output STEP ke driver servo AC
#define DIR_PIN      19   // Output DIR ke driver servo AC
#define LIMIT_PIN    32   // Input dari limit switch (aktif LOW)

#define TOMBOL_NAIK   12  // Tombol manual naik
#define TOMBOL_TURUN  14  // Tombol manual turun

long totalStep = 0;
bool lastLimit = HIGH;
bool startHit = false;

unsigned long lastStepTime = 0;
unsigned long stepDelayMicros = 300; // Servo cepat, tapi tetap beri delay kecil

void setup() {
  Serial.begin(115200);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(LIMIT_PIN, INPUT_PULLUP);
  pinMode(TOMBOL_NAIK, INPUT_PULLUP);
  pinMode(TOMBOL_TURUN, INPUT_PULLUP);

  Serial.println("=== KALIBRASI LIFT - SERVO AC MODE ===");
  Serial.println("Tekan NAik atau TURUN, baca jumlah step saat limit tersentuh.");
  delay(2000);
}

void loop() {
  bool naik = digitalRead(TOMBOL_NAIK) == LOW;
  bool turun = digitalRead(TOMBOL_TURUN) == LOW;

  if (naik || turun) {
    digitalWrite(DIR_PIN, naik ? HIGH : LOW);

    // Step minimal delay untuk driver servo menerima pulsa
    if (micros() - lastStepTime >= stepDelayMicros * 2) {
      satuLangkah(stepDelayMicros);
      totalStep++;
      lastStepTime = micros();
    }
  }

  // Cek limit switch
  bool limitNow = digitalRead(LIMIT_PIN);

  // Jika baru menyentuh limit (transisi HIGH -> LOW)
  if (lastLimit == HIGH && limitNow == LOW) {
    if (!startHit) {
      Serial.println(">> START posisi terdeteksi. Hitung dimulai...");
      totalStep = 0;
      startHit = true;
    } else {
      Serial.print(">> STOP posisi terdeteksi. Total step antar lantai: ");
      Serial.println(totalStep);
      startHit = false;
    }
    delay(800); // Debounce dan beri jeda sebelum lanjut
  }

  lastLimit = limitNow;
}

void satuLangkah(int delay_us) {
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(delay_us);
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(delay_us);
}