#include <Servo.

h>
#include <Ping.h>
Servo
Servo
Servo
Servo
Ping
Ping
Ping
Ping
Ping

servo1;
servo2;
servo3;
servo4;
ping1
ping2
ping3
ping4
ping5

=
=
=
=
=

Ping(37);
Ping(35);
Ping(33);
Ping(31);
Ping(29);

int p1, p2, p3, p4, p5;

void setup()
{
Serial.begin(9600);
servo1.attach(11);
servo2.attach(10);
servo3.attach(7);
servo4.attach(6);
}
void loop()
{
ping1.fire();
ping2.fire();
ping3.fire();
ping4.fire();
ping5.fire();
p1=ping1.centimeters();
p2=ping2.centimeters();
p3=ping3.centimeters();
p4=ping4.centimeters();
p5=ping5.centimeters();
jalan ();
}
void jalan()
{
/*kondisi jalan maju biasa*/
if (p3>3)
{
maju();
if ((p1<8) || (p2<7))
{
skanan();
delay(50);
}
if((p5<6) || (p4<7))

{
skiri();
delay(50);
}
}
/*kondisi dekat dinding*/
if (p3<3)
{
if((p1<40) && (p2<p4) || (p1<p5))
{
pkanan();
delay(600);
}
if((p5<40) && (p4<p2) || (p5<p1))
{
pkiri();
delay(600);
}
if((p1<7)&&(p5<7))
{
pkiri();
delay(2000);
}
}
/*kondisi simpang 4*/
if ((p1>60) && (p5>65) && (p3>25) &&(p2>35))
{
pkanan();
delay(1200);
if(p3>65)
{
maju();
delay(1000);
}
}
/*kondisi belok ruangan yang p3 jauh*/
if ((p1>60)&&(p3>65))
{
maju();
delay(500);
pkanan();
delay(1400);
if(p3>40)
{
maju();
delay(1600);
}
}
/*kondisi belok kanan dengan p3 dekat*/
if ((p1>70) && (p5<10) && (p3<5) &&(p2>10))
{
maju();
delay(500);
pkanan();
delay(1400);
if(p3>40)
{
maju();
delay(1600);
}

}
}
void maju()
{
Serial.println("maju");
servo1.write(180);
servo2.write(180);
servo3.write(0);
servo4.write(0);
}
void pkanan()
{
Serial.println("kanan");
servo1.write(180);
servo2.write(180);
servo3.write(180);
servo4.write(180);
}
void skanan()
{
Serial.println("skanan");
servo1.write(180);
servo2.write(180);
servo3.write(135);
servo4.write(135);
}
void pkiri()
{
Serial.println("kiri");
servo1.write(0);
servo2.write(0);
servo3.write(0);
servo4.write(0);
}
void skiri()
{
Serial.println("skiri");
servo1.write(45);
servo2.write(45);
servo3.write(0);
servo4.write(0);
}
void diam()
{
Serial.println("diam");
servo1.write(90);
servo2.write(90);
servo3.write(90);

servo4.write(90);
}
void mundur()
{
Serial.println("mundur");
servo1.write(0);
servo2.write(0);
servo3.write(180);
servo4.write(180);
}