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

#define echoPinDepan 3  // Sensor depan
#define trigPinDepan 2
#define echoPinKanan 4  // Sensor kanan
#define trigPinKanan 5
#define echoPinKiri 6   // Sensor kiri
#define trigPinKiri 7

#define motorPin1 8     // Pin untuk motor driver
#define motorPin2 9
#define motorPin3 10
#define motorPin4 11

Servo myServo;
Stepper myStepper(200, motorPin1, motorPin2, motorPin3, motorPin4);  // Stepper dengan 200 langkah per revolusi

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

  pinMode(echoPinDepan, INPUT);
  pinMode(trigPinDepan, OUTPUT);
  pinMode(echoPinKanan, INPUT);
  pinMode(trigPinKanan, OUTPUT);
  pinMode(echoPinKiri, INPUT);
  pinMode(trigPinKiri, OUTPUT);

  myServo.attach(12);  // Servo terpasang pada pin 12
  myServo.write(45);   // Set servo ke posisi tengah

  myStepper.setSpeed(60); // Kecepatan stepper
}

float getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  int duration = pulseIn(echoPin, HIGH);
  float distance = duration * 0.034 / 2;
  return distance;
}

void loop() {
  float jarakDepan = getDistance(trigPinDepan, echoPinDepan);
  float jarakKanan = getDistance(trigPinKanan, echoPinKanan);
  float jarakKiri = getDistance(trigPinKiri, echoPinKiri);

  Serial.print("Jarak Depan: ");
  Serial.print(jarakDepan);
  Serial.print(" cm, Kanan: ");
  Serial.print(jarakKanan);
  Serial.print(" cm, Kiri: ");
  Serial.println(jarakKiri);

  if (jarakDepan < 10) {
    myStepper.step(0);       // Berhenti sejenak jika ada halangan di depan
    delay(1000);             // Delay untuk berhenti
  } 
  else if (jarakKanan < 10) {
    myServo.write(0);        // Belok kiri
    myStepper.step(200);     // Maju sedikit
  } 
  else if (jarakKiri < 10) {
    myServo.write(90);       // Belok kanan
    myStepper.step(200);     // Maju sedikit
  } 
  else {
    myServo.write(45);       // Lurus
    myStepper.step(400);     // Maju lurus
  }

  delay(1000);               // Delay untuk stabilitas
}