Program robot pemadam api dan skema

#include <Servo.h>
#define RODA1 4
#define RODA2 5
Servo Roda1; Servo Roda2;
//definisi pin kipas dan pin buzzer
#define pompa 13
#define buzzer A7
bool keadaan;
//KALIBRASI KECEPATAN DAN SETPOIN SENSOR
int kecepataan =600; // NILAI GERAK BELOK 90* (500-1000);
int rodakanan  =93;  // NILAI BERHENTI RODA KANAN (+-90);
int rodakiri   =95;  // NILAI BERHENTI RODA KIRI (+-90);
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    9
#define trigger1 10
NewPing sonar1(trigger1,echo1,100);

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

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


void setup() {
 Serial.begin(9600); // serial monitor "menampilkan data sensor"
 diam(3000);
 Roda1.attach(RODA1);  //pin motor servo
 Roda2.attach(RODA2); 
 pinMode(pompa,OUTPUT);
 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);

//PEMIS 1 jika api terdeteksi maka exsekusi
if ((api2>api||api3>api||api4>api) && (jarak2>1&&jarak2<15)){
digitalWrite(buzzer,HIGH);  //buzzer bunyi
 digitalWrite(pompa,LOW);
diam(2000); // panggil diam "lihat inisialisasi progam pangillan void diam(x);"
tiup(kecepatan);  //panggil tiup
diam(1000); //panggil diam
keadaan = 1;

}

//PREMIS 2 Jika api tidak terdeteksi maka exsekusi
else{
digitalWrite(buzzer,LOW); // buzzer mati
pinMode(pompa, INPUT); digitalWrite(pompa,HIGH); // kipas berhenti
if (base>lintasan && keadaan > 0){
  maju();
  delay(kecepataan);
  diam (100000);//berhenti total
}
//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(kecepataan);
  }
  //jika sensor kanan > sensor kanan maka ditentukan "balik kiri"
  else if (jarak1>jarak3){
  balik_kiri();
  delay(kecepataan);
}
}
//JIKA NIALI SET POIN SEMUA SENSOR TIDAK TERLAMPAUI MAKA "ROBOT MAJU"
else{
  maju();
  delay(100);
}
}
}

// inisialisasi progam pangillan
void maju(){          //SEMUA RODA MAJU
  Roda1.write(380);
  Roda2.write(-380);
}
void mundur(){        //SEMUA RODA MUNDUR
  Roda1.write(-380);
  Roda2.write(380);
}
void balik_kiri(){
  Roda1.write(-380);   //RODA1 MUNDUR
  Roda2.write(-380);   //RODA2 MAJU
}
void balik_kanan(){
  Roda1.write(380);    //RODA1 MAJU
  Roda2.write(380);    //RODA2 MUNDUR
}
void kanan(){
 Roda1.write(380);       //RODA1 MAJU
 Roda2.write(rodakanan); //RODA2 BERHENTI
}
void kiri(){
 Roda1.write(rodakiri); //RODA1 BERHENTI
 Roda2.write(-380);     //RODA2 MAJU
}
void diam(int x){
  Roda1.write(93);  //RODA KANAN BERHENTI
  Roda2.write(95);  //RODA KIRI BERHENTI
  delay(x);         //SEMUA RODA BERHENTI
  }
void tiup(int x){   // ROBOT BERHENTI DAN MENIUP KEKANAN DAN KRKIRI
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);
}