#include <AccelStepper.h>

// Definisi pin untuk driver motor A4988
#define MOTOR1_STEP_PIN 2
#define MOTOR1_DIR_PIN 3
#define MOTOR2_STEP_PIN 4
#define MOTOR2_DIR_PIN 5
#define trigPin1 8 
#define echoPin1 9 
#define trigPin2 10 
#define echoPin2 11 
#define trigPin3 12 
#define echoPin3 13 



// Inisialisasi motor stepper
AccelStepper stepper1(AccelStepper::DRIVER, MOTOR1_STEP_PIN, MOTOR1_DIR_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, MOTOR2_STEP_PIN, MOTOR2_DIR_PIN);

void setup() {
  // Set kecepatan dan percepatan motor (sesuaikan dengan karakteristik motor dan kecepatan yang diinginkan)
  pinMode(echoPin1, INPUT); 
  pinMode(trigPin1, OUTPUT); 
  pinMode(echoPin2, INPUT); 
  pinMode(trigPin2, OUTPUT); 
  pinMode(echoPin3, INPUT); 
  pinMode(trigPin3, OUTPUT); 
  Serial.begin(9600); //inisialisasi serial monitor untuk start
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(500);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(500);
 

 
}

void loop() {

  digitalWrite(trigPin1, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin1, LOW); 
  int durasiPantulan1 = pulseIn(echoPin1, HIGH); 
  int jarak1 = durasiPantulan1 * 0.034 / 2; 
  digitalWrite(trigPin2, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH); 
  delayMicroseconds(10); 
  digitalWrite(trigPin2, LOW);
  int durasiPantulan2 = pulseIn(echoPin2, HIGH); 
  int jarak2 = durasiPantulan2 * 0.034 / 2; 
  digitalWrite(trigPin3, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin3, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin3, LOW); 
  int durasiPantulan3 = pulseIn(echoPin3, HIGH); 
  int jarak3 = durasiPantulan3 * 0.034 / 2;
  Serial.print("Jarak1 : "); 
  Serial.print(jarak1);
  Serial.println("cm ");
  Serial.print("Jarak2 : "); 
  Serial.print(jarak2);
  Serial.println("cm ");
  Serial.print("Jarak3 : "); 
  Serial.print(jarak3);
  Serial.println("cm ");

  delay(1000);
  // Maju
  if (jarak1==100&&jarak2>=100&&jarak3==100)
  {moveForward();
  }
else if ( jarak1<=100&&jarak2<=100&&jarak3<=100)
  // Mundur
  {moveBackward();
  }
else if (jarak1>=100&&jarak2<=100&&jarak3<=100)
  // Ke Kiri
  {turnLeft();
  }

else if(jarak1>=100&&jarak2>=100&&jarak3<=100)
  // Ke Kanan
 {turnRight();
 }
  delay(1000);

  
}



// Fungsi untuk bergerak maju
void moveForward() {
  Serial.println("Maju ");
  stepper1.move(-200);
  stepper2.move(-200);
 
  runSteppers();
}

// Fungsi untuk bergerak mundur
void moveBackward() {
  Serial.println("Mundur");
  stepper1.move(200);
  stepper2.move(200);
  
  runSteppers();
}

// Fungsi untuk bergerak ke kiri
void turnLeft() {
  Serial.println("kiri ");
  stepper1.move(-100);
  stepper2.move(100);
  
  runSteppers();
}

// Fungsi untuk bergerak ke kanan
void turnRight() {
  Serial.println("Kanan ");
  stepper1.move(100);
  stepper2.move(-100);
  
  runSteppers();
}


// Fungsi untuk menjalankan motor stepper
void runSteppers() {
  while (stepper1.run() && stepper2.run() ) {
    // Tunggu hingga semua motor stepper mencapai tujuan
  }
}