#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()
{
if (p3>15)
{
maju();
if ((p1<7) && (p2<20))
{
pkanan();
delay(600);

}
if((p5<7) && (p4<20))
{
pkiri();
delay(600);
}
if ((p2<5) || (p4<5))
{
if (p1<p5)
{
pkanan();
delay(600);
}
if (p5<p1)
{
pkiri();
delay(600);
}
}
}
if (p3<15)
{
if (p1<p5)
{
pkanan();
delay(600);
}
if (p5<p1)
{
pkiri();
delay(600);
}
}
}
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 pkiri()
{
Serial.println("kiri");

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