//==========================================================================================
//  Program ini di buat oleh Misbah najh (FB)
//  Program dikendalikan menguanakan sensor jarak Ultrasonic 
//  mengunakan motor DC, sensor hc-sr04, flame sensor 5 channel, fan L9110, arduino uno, dan chassis
//  Chassis+motor modif didapatkan https://www.tokopedia.com/microbot/frame-chassis-robot-pemadam-api-servo-roda
//  Tutorial dan video klik  https://www.sinauduino.blogspot.co.id
//  full tutorial video https://www.youtube.com/watch?v=BL8ZB5-ql9Q
//==========================================================================================

// Inisialisasi pin motor untuk L298N
#define IN1 2
#define IN2 3
#define IN3 4
#define IN4 5
#define ENA 9
#define ENB 10

// KALIBRASI KECEPATAN DAN SETPOIN SENSOR
int kecepatan = 150; // Kecepatan motor (0-255)
int setpoin    = 5;   // NILAI TOLERANSI JARAK HINDAR (7-15);
int lintasan   = 35; // NILAI TOLERANSI WARNA LINTASAN (0-1024)
int api        = 800; // NILAI TOLERANSI CAHAYA API (0-1024)
 
// Inisialisasi sensor 1 sebelah kiri
#include <NewPing.h>
#define echo1    10
#define trigger1 9
NewPing sonar1(trigger1, echo1, 100);

// Inisialisasi sensor 2 sebelah depan
#define echo2    12
#define trigger2 11
NewPing sonar2(trigger2, echo2, 100);

// Inisialisasi sensor 3 sebelah kanan
#define echo3    8
#define trigger3 7
NewPing sonar3(trigger3, echo3, 100);

// Definisi pin kipas dan pin buzzer
#define kipas 6
#define buzzer 3
bool keadaan;

void setup() {
  Serial.begin(9600); // serial monitor "menampilkan data sensor"
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(kipas, INPUT);
  pinMode(buzzer, OUTPUT);
  keadaan = 0;
}

void loop() {
  // Sensor mengukur dan membaca
  int jarak1 = sonar1.ping_cm(); // sensor kiri
  int jarak2 = sonar2.ping_cm(); // sensor depan
  int jarak3 = sonar3.ping_cm(); // sensor kanan
  int base    = analogRead(A0); // pin sensor tcr5000
  int api1    = analogRead(A1); // pin sensor Api A1-A5
  int api2    = analogRead(A2);
  int api3    = analogRead(A3);
  int api4    = analogRead(A4);
  int api5    = analogRead(A5);

  // Sensor menampilkan data yang dibaca
  Serial.print(jarak1);
  Serial.print("   ||   ");
  Serial.print(jarak2);
  Serial.print("   ||   ");
  Serial.print(jarak3);
  Serial.print("   ||   ");
  Serial.print(api3);
  Serial.print("   ||   ");
  Serial.println(base);

  // Premis 1 jika api terdeteksi maka eksekusi
  if ((api2 > api || api3 > api || api4 > api) && (jarak2 > 1 && jarak2 < 15)) {
    digitalWrite(buzzer, HIGH);  // buzzer bunyi
    pinMode(kipas, OUTPUT); digitalWrite(kipas, LOW); // kipas bergerak
    diam(2000); // panggil diam
    tiup(kecepatan);  // panggil tiup
    diam(1000); // panggil diam
    keadaan = 1; 
  } else {
    // Premis 2 jika api tidak terdeteksi maka eksekusi
    digitalWrite(buzzer, LOW); // buzzer mati
    pinMode(kipas, INPUT); digitalWrite(kipas, HIGH); // kipas mati

    if (jarak2 > 1 && jarak2 < setpoin) {
      balik_kiri();
      delay(kecepatan);
    } else if (jarak3 > setpoin + 3 || jarak3 < 1) { 
      kanan();
      delay(50);
    } else if (jarak3 > 1 && jarak3 < setpoin) {
      kiri();
      delay(50);
    } else if (base > lintasan && keadaan > 0) {
      maju();
      delay(kecepatan);
      diam(100000); // berhenti total
    } else {
      maju();
      delay(50);
    }
  }
}

// Fungsi kendali motor dengan L298N
void maju() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, kecepatan);
  analogWrite(ENB, kecepatan);
}

void mundur() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENA, kecepatan);
  analogWrite(ENB, kecepatan);
}

void balik_kiri() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, kecepatan);
  analogWrite(ENB, kecepatan);
}

void balik_kanan() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENA, kecepatan);
  analogWrite(ENB, kecepatan);
}

void kanan() {
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, kecepatan);
  analogWrite(ENB, kecepatan / 2);
}

void kiri() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, kecepatan / 2);
  analogWrite(ENB, kecepatan);
}

void diam(int x) {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  delay(x);
}

void tiup(int x) {
  balik_kanan();
  delay(x / 4);
  diam(x);
  balik_kiri();
  delay(x / 2);
  diam(x);
  balik_kanan();
  delay(x / 2);
  diam(x);
  balik_kiri();
  delay(x / 4);
  diam(x);
}