fireRobotArduino

#include <Servo.h>
#define m1  4
#define m2  5
#define m3  6
#define m4  7
int setpoin    =7;   // NILAI TOLERANSI JARAK HINDAR (7-15);
int lintasan   =200; // NILAI TOLERANSI WARNA LINTASAN (0-1024)
int api        =500; // NILAI TOLERANSI CAHAYA API (0-1024)
 
//inisialisasi sensor 1 sebelah kiri
#include <NewPing.h>
#define echo1    8
#define trigger1 9
NewPing sonar1(trigger1,echo1,100);

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

//inisialisasi sensor 2 sebelah kanan
#define echo3    10
#define trigger3 11
NewPing sonar3(trigger3,echo3,100);

//definisi pin kipas dan pin buzzer
#define kipas 2
#define buzzer 3
bool keadaan;
void setup() {
 Serial.begin(9600); 
 pinMode(m1, OUTPUT);
 pinMode(m2, OUTPUT);
 pinMode(m3, OUTPUT);
 pinMode(m4, OUTPUT);
 diam(3000);
 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);

//case utama jika api terdeteksi maka exsekusi
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 "lihat inisialisasi progam pangillan"
tiup(500);  //panggil tiup
diam(1000); //panggil diam
keadaan = 1; 
}

//case terakhir jika api tidak terdeteksi maka exsekusi
else{
digitalWrite(buzzer,LOW); // buzzer mati
pinMode(kipas, INPUT); digitalWrite(kipas,HIGH); // kipas berhenti
if (base>lintasan && keadaan > 0){
  maju();
  delay(500);
  diam(100000);
}
//jika sensor kiri terdeteksi maka belok kanan
else if (jarak1<setpoin&&jarak1>1){ 
  kanan(); //panggil kanan
  delay(50);
  }
//jika sensor kanan terdeteksi maka belok kiri
else if (jarak3<setpoin&&jarak3>1){
  kiri();
  delay(50);
  }
//jika sensor depan terdeteksi maka balik (balik belum ditentukan)
else if (jarak2<setpoin&&jarak2>1){
  //jika sensor kiri < sensor kanan maka ditentukan "balik kanan"
  if (jarak1<jarak3){
  balik_kanan();
  delay(500);
  }
  //jika sensor kanan > sensor kanan maka ditentukan "balik kiri"
  else if (jarak1>jarak3){
  balik_kiri();
  delay(500);
}
}
//jika semua sensor (sensor api dan sensor ultrasonik tidak terdeteksi) maka maju
else{
  maju();
  delay(100);
}
}
}

// inisialisasi progam pangillan
void maju(){
  digitalWrite(m1,HIGH);
  digitalWrite(m2,LOW);
  digitalWrite(m3,LOW);
  digitalWrite(m4,HIGH);
}
void mundur(){
  digitalWrite(m1,LOW);
  digitalWrite(m2,HIGH);
  digitalWrite(m3,HIGH);
  digitalWrite(m4,LOW);
}
void balik_kiri(){
  digitalWrite(m1,HIGH);
  digitalWrite(m2,LOW);
  digitalWrite(m3,HIGH);
  digitalWrite(m4,LOW);
}
void balik_kanan(){
  digitalWrite(m1,LOW);
  digitalWrite(m2,HIGH);
  digitalWrite(m3,LOW);
  digitalWrite(m4,HIGH);
}
void kanan(){
  digitalWrite(m1,LOW);
  digitalWrite(m2,HIGH);
  digitalWrite(m3,LOW);
  digitalWrite(m4,LOW);
}
void kiri(){
 digitalWrite(m1,LOW);
 digitalWrite(m2,LOW);
 digitalWrite(m3,HIGH);
 digitalWrite(m4,LOW);
}
void diam(int x){
 pinMode(m1, INPUT);
 pinMode(m2, INPUT);
 pinMode(m3, INPUT);
 pinMode(m4, INPUT);
 digitalWrite(m1,LOW);
 digitalWrite(m2,LOW);
 digitalWrite(m3,LOW);
 digitalWrite(m4,LOW);
 delay(x);
 pinMode(m1, OUTPUT);
 pinMode(m2, OUTPUT);
 pinMode(m3, OUTPUT);
 pinMode(m4, OUTPUT);
  }
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);
}