#include <ESP32Servo.h> //memasukkan library servo
Servo rodaKanan; //servo kanan sebagai roda kanan
Servo rodaKiri; //servo kiri sebagai roda kiri
const int sensorKanan = 12; //sensor infrared kanan di pin 12
const int sensorKiri = 14; //sensor infrared kiri di pin 14
int hasilSensorKanan; //variabel untuk menyimpan hasil pembacaan sensor infrared kanan
int hasilSensorKiri; //variabel untuk menyimpan hasil pembacaan sensor infrared kiri

void setup() {
  Serial.begin(9600); 
  pinMode (sensorKanan, INPUT); //sensor infrared kanan sebagai input
  pinMode (sensorKiri, INPUT); //sensor infrared kiri sebagai input
  rodaKanan.attach(27); //servo kanan di pin 27
  rodaKiri.attach(26); //servo kiri di pin 26
}

void loop() {
  hasilSensorKanan = digitalRead(sensorKanan); //membaca hasil sensor infrared kanan
  hasilSensorKiri = digitalRead(sensorKiri); //membaca hasil sensor infrared kiri
  
  if (hasilSensorKanan == LOW && hasilSensorKiri == LOW) {
    maju (); //jika tidak ada garis di sebelah kiri dan kanan robot, maka robot tetap berjalan lurus
  }
  if (hasilSensorKanan == HIGH && hasilSensorKiri == LOW) {
    belok_kanan (); //jjika terdapat garis di sebelah kanan robot, maka robot berbelok ke kanan
  }
  if (hasilSensorKanan == LOW && hasilSensorKiri == HIGH) {
    belok_kiri (); //jika terdapat garis di sebelah kiri robot, maka robot berbelok ke kiri
  }
  else if (hasilSensorKanan == HIGH && hasilSensorKiri == HIGH) {
    maju(); //jika terdapat garis di sebelah kanan dan kiri robot, maka robot berhenti
  }
  Serial.print("hasil sensor kanan: "); 
  Serial.println(hasilSensorKanan); //menampilkan hasil sensor infrared kanan
  Serial.print("hasil sensor kiri: ");
  Serial.println(hasilSensorKiri); //menampilkan hasil sensor infrared kiri
  Serial.println(""); 
}

void maju () { //fungsi maju yang dipanggil di loop ketika syarat-syaratnya terpenuhi
  rodaKanan.write(180); //roda kanan tetap menyala
  rodaKiri.write(180); //roda kiri tetap menyala
  delay(500);
  rodaKanan.write(0);
  rodaKiri.write(0);
  delay(500);
}

void belok_kanan () { //fungsi berbelok kanan yang dipanggil di loop ketika syarat-syaratnya terpenuhi
  rodaKiri.write(180); //roda kiri tetap menyala
  rodaKanan.write(90); //roda kanan berhenti
  delay(500);
  rodaKiri.write(0);
  delay(500);
}

void belok_kiri () { //fungsi berbelok ke kiri yang dipanggil di loop ketika syarat-syaratnya terpenuhi
  rodaKanan.write(180); //roda kanan tetap menyala
  rodaKiri.write(90); //roda kiri berhenti
  delay(500);
  rodaKanan.write(0);
  delay(500);
}

void berhenti () { //fungsi berhenti yang dipanggil di loop ketika syarat-syaratnya terpenuhi
  rodaKanan.write(90); //roda kanan berhenti
  rodaKiri.write(90); //roda kiri berhenti
}