#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()
{
skanan();
if (p3<15)
{
if((p1<25) && (p2<p4) || (p1<p5))
{
pkanan();
}
if((p5<25) && (p4<p2) || (p5<p1))
{
pkiri();
}
}

if (p3>15)
{
maju();
if ((p1<7) || (p2<12))
{
pkanan();
}
if((p5<7) || (p4<12))
{
pkiri();
}
if (p5>9)
{
skanan();
if ((p5<7) && (p4<12))
{
skiri();
}
if ((p5>9) && (p4>16))
{
skanan();
}
else if ((9<p5<7) && (16<p4<12))
{
maju();
}
}
}
}
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(90);
servo4.write(90);

}
void pkiri()
{
Serial.println("kiri");
servo1.write(0);
servo2.write(0);
servo3.write(0);
servo4.write(0);
}
void skiri()
{
Serial.println("skiri");
servo1.write(90);
servo2.write(90);
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);
}