#include <Servo.h>
#include <Stepper.h>

// Konfigurasi motor stepper
const int stepsPerRevolution = 200;  // Jumlah langkah per putaran
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11); // Inisialisasi motor stepper dengan pin 8, 9, 10, 11

// Konfigurasi sensor ultrasonik
const int trigPin1 = 2; // Pin Trigger sensor ultrasonik pertama
const int echoPin1 = 3; // Pin Echo sensor ultrasonik pertama
const int trigPin2 = 4; // Pin Trigger sensor ultrasonik kedua
const int echoPin2 = 5; // Pin Echo sensor ultrasonik kedua

// Konfigurasi servo
Servo servoLeft;  // Servo untuk roda kiri
Servo servoRight; // Servo untuk roda kanan
int servoPosLeft = 90;  // Posisi awal servo kiri
int servoPosRight = 90; // Posisi awal servo kanan

void setup() {
  // Inisialisasi Serial Monitor
  Serial.begin(9600);

  // Inisialisasi servo
  servoLeft.attach(6);  // Hubungkan servo kiri ke pin 6
  servoRight.attach(7); // Hubungkan servo kanan ke pin 7

  // Konfigurasi pin sensor ultrasonik pertama
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);

  // Konfigurasi pin sensor ultrasonik kedua
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
}

void loop() {
  // Mengukur jarak dengan sensor ultrasonik pertama
  long duration1, distance1;
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = (duration1 / 2) / 29.1; // Menghitung jarak dalam cm

  // Mengukur jarak dengan sensor ultrasonik kedua
  long duration2, distance2;
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = (duration2 / 2) / 29.1; // Menghitung jarak dalam cm

  // Menggerakkan servo berdasarkan jarak
  if (distance1 < 20) {
    servoPosLeft = 20; // Putar servo kiri ke kiri jika ada rintangan di ultrasonik 1
    servoPosRight = 20; // Putar servo kanan ke kiri jika ada rintangan di ultrasonik 1
  } else if (distance2 < 20) {
    servoPosLeft = 160; // Putar servo kiri ke kanan jika ada rintangan di ultrasonik 2
    servoPosRight = 160; // Putar servo kanan ke kanan jika ada rintangan di ultrasonik 2
  } else {
    servoPosLeft = 90; // Putar servo kiri ke tengah jika tidak ada rintangan di kedua ultrasonik
    servoPosRight = 90; // Putar servo kanan ke tengah jika tidak ada rintangan di kedua ultrasonik
  }
  servoLeft.write(servoPosLeft);
  servoRight.write(servoPosRight);

  // Mengatur kecepatan motor stepper berdasarkan jarak
  int stepperSpeed = 100; // Kecepatan motor stepper default
  if (distance1 < 20 && distance2 < 20) {
    stepperSpeed = 50; // Kurangi kecepatan motor jika keduanya mendeteksi rintangan
  }
  myStepper.setSpeed(stepperSpeed); // Set kecepatan motor stepper
  myStepper.step(100); // Melakukan 100 langkah maju

  delay(100); // Delay sebelum mengulangi
}