#include <AccelStepper.h>
#include <Ultrasonic.h>

// Definisikan pin-penjakit untuk motor stepper
#define A1_PIN1 34
#define A1_PIN2 35
#define B1_PIN1 32
#define B1_PIN2 33
#define A2_PIN1 25
#define A2_PIN2 26
#define B2_PIN1 27
#define B2_PIN2 14

// Definisikan pin untuk sensor ultrasonik
#define TRIGGER_PIN 2
#define ECHO_PIN 4

// Inisialisasi motor stepper
AccelStepper motor1(AccelStepper::FULL4WIRE, A1_PIN1, A1_PIN2, B1_PIN1, B1_PIN2);
AccelStepper motor2(AccelStepper::FULL4WIRE, A2_PIN1, A2_PIN2, B2_PIN1, B2_PIN2);

// Inisialisasi sensor ultrasonik
Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);

void setup() {
  // Set kecepatan awal motor stepper
  motor1.setMaxSpeed(1000);
  motor2.setMaxSpeed(1000);
}

void loop() {
  // Baca jarak dari sensor ultrasonik (dalam sentimeter)
  int distance = ultrasonic.read();

  // Mengubah jarak menjadi kecepatan motor
  int motorSpeed = map(distance, 0, 200, 0, 1000);

  // Batasi kecepatan maksimum motor
  motorSpeed = constrain(motorSpeed, 0, 1000);

  // Set kecepatan motor stepper
  motor1.setSpeed(motorSpeed);
  motor2.setSpeed(motorSpeed);

  // Langkahkan motor
  motor1.runSpeed();
  motor2.runSpeed();

  delay(100); // Perubahan kecepatan motor setiap 100ms
}