#include <AccelStepper.h>

// Definisi pin
#define DIR  12   // dir modul motor 1 dan 2
#define STEP 13   // Step motor 1 dan 2
#define TRIG 23   // trigger ultrasonik 1
#define ECHO 22   // Echo ultrasonik 1 

// Konfigurasi stepper motor (Tipe: DRIVER)
AccelStepper stepper(AccelStepper::DRIVER, STEP, DIR);

// Hitung jarak dengan sampah
float readDistance() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  
  int duration = pulseIn(ECHO, HIGH);
  return duration * 0.034 / 2;
}

// Variabel untuk delay berhenti
unsigned long waktuTerakhirDekat = 0;
const unsigned long delayBerhenti = 2000; // 2 detik

void setup() {
  Serial.begin(115200);

  // Konfigurasi Ultrasonik
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  //konfigurasi stepper motor
  stepper.setMaxSpeed(5000.0); // Kecepatan maksimum
  stepper.setAcceleration(500.0); // Akselerasi
}

void loop() {
  float distance = readDistance();
  Serial.println(distance);

  unsigned long sekarang = millis();

  if (distance <= 200) {
    // Jika jarak dekat, perbarui waktu terakhir dekat dan jalan terus
    waktuTerakhirDekat = sekarang;
    stepper.setSpeed(5000);
    stepper.runSpeed();
  } else {
    // Jika sudah lewat waktu delay sejak terakhir dekat, baru berhenti
    if (sekarang - waktuTerakhirDekat < delayBerhenti) {
      stepper.setSpeed(5000);
      stepper.runSpeed();
    } else {
      stepper.stop(); // Set kecepatan ke 0
    }
  }
}

// test