//file Header
#include <NewPing.h>
#define trig_pin A3
#define echo_pin A2
#define maximum_distance 200

const int kananmaju = 5;
const int kananmundur = 4;
const int kirimaju = 3;
const int kirimundur = 2;

//inisialisasi ultrasonic
boolean goesForward = false;
int distance = 100;
NewPing sonar(trig_pin, echo_pin, maximum_distance);

void setup() {
  pinMode(kananmaju, OUTPUT);
  pinMode(kananmundur, OUTPUT);
  pinMode(kirimaju, OUTPUT);
  pinMode(kirimundur, OUTPUT);

  // Inisialisasi jarak
  for (int i = 0; i < 5; i++) {
    distance = readPing();
    delay(100);
  }
}

void loop() {
  int jarakkanan = 0;
  int jarakkiri = 0;
  delay(50);

  if (jarak <= 20){
    berenti();
    delay(300);
    mundur();
    delay(400);
    berenti();
    delay(300);
    jarakkanan = lihatkanan();
    delay(300);
    jarakkiri = lihatkiri();
    delay(300);

    if (distance >= jarakkiri) {
      kanan();
      berenti();
    } else {
      kiri();
      berenti();
    }
  } else {
    maju();
  }
  distance = readPing();
}

int lihatkanan() {
  kanan();
  int distance = readPing();
  delay(100);
  berenti();
  return distance;
}

int lihatkiri() {
  kiri();
  int distance = readPing();
  delay(100);
  berenti();
  return distance;
}

// int nama_fungsi(){}
int readPing() {
  delay(70);
  int cm = sonar.ping_cm();
  if (cm == 0) {
    cm = 250;
  }
  return cm;
}

void kanan() {
  digitalWrite(kirimaju, HIGH);
  digitalWrite(kananmundur, HIGH);
  digitalWrite(kirimundur, LOW);
  digitalWrite(kananmaju, LOW);
  delay(500);
  berenti();
}

void kiri() {
  digitalWrite(kirimundur, HIGH);
  digitalWrite(kananmaju, HIGH);
  digitalWrite(kirimaju, LOW);
  digitalWrite(kananmundur, LOW);
  delay(500);
  berenti();
}

void maju() {
  digitalWrite(kananmaju, HIGH);
  digitalWrite(kananmundur, LOW);
  digitalWrite(kirimaju, HIGH);
  digitalWrite(kirimundur, LOW);
}

void mundur() {
  digitalWrite(kananmaju, LOW);
  digitalWrite(kananmundur, HIGH);
  digitalWrite(kirimaju, LOW);
  digitalWrite(kirimundur, HIGH);
}

void berenti() {
  digitalWrite(kananmaju, LOW);
  digitalWrite(kananmundur, LOW);
  digitalWrite(kirimaju, LOW);
  digitalWrite(kirimundur, LOW);
}