#include <Servo.h>
#define echopin 44  //ultrasonik 1
#define trigpin 45

#define echopin2 24 //ultrasonik 2
#define trigpin2 25 

#define echopin3 6 //ultrasonik 3
#define trigpin3 7 

#define echopin4 32 //ultrasonik 4
#define trigpin4 33 

Servo s1;
Servo s2;
Servo s3;
Servo s4;
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9;
Servo s10;
Servo s11;
Servo s12;
Servo s13;
Servo s14;
Servo s15;
Servo s16;
Servo s17;
Servo s18;
void setup()
{
  Serial.begin(9600);
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
  pinMode(echopin2, INPUT);
  pinMode(trigpin2, OUTPUT);
  pinMode(echopin3, INPUT);
  pinMode(trigpin3, OUTPUT);
  pinMode(echopin4, INPUT);
  pinMode(trigpin4, OUTPUT);

s1.attach(17);//kaki 1
s2.attach(18);
s3.attach(19);

s4.attach(41);//kaki 2
s5.attach(42);
s6.attach(43);

s7.attach(14);//kaki 3
s8.attach(15);
s9.attach(16);

s10.attach(38);//kaki 4
s11.attach(39);
s12.attach(40);

s13.attach(2);//kaki 5
s14.attach(3);
s15.attach(4);

s16.attach(36 );//kaki 6
s17.attach(35);
s18.attach(34);

  s1.write(90);
  s2.write(90);
  s3.write(90);
  delay(20);
  s4.write(90);
  s5.write(90);
  s6.write(90);
   delay(20);
  s7.write(90);
  s8.write(90);
  s9.write(90);
   delay(20);
  s10.write(90);
  s11.write(90);
  s12.write(90);
   delay(20);
  s13.write(90);
  s14.write(90);
  s15.write(90);
   delay(20);
  s16.write(90);
  s17.write(90);
  s18.write(90);
  delay(2000);
} 
void loop(void)
{
  digitalWrite(trigpin, LOW);
  digitalWrite(trigpin2, LOW);     
  digitalWrite(trigpin3, LOW);
  digitalWrite(trigpin4, LOW);             
  delayMicroseconds(2);
  digitalWrite(trigpin, HIGH);
  digitalWrite(trigpin2,HIGH);
  digitalWrite(trigpin3,HIGH);
  digitalWrite(trigpin4,HIGH);                 
  delayMicroseconds(10);
  digitalWrite(trigpin, LOW);
  digitalWrite(trigpin2, LOW);
  digitalWrite(trigpin3, LOW);
  digitalWrite(trigpin4, LOW);  
  long duration = pulseIn(echopin, HIGH);        
  int distance = duration*0.0343/2;       
  Serial.print("jarak sensor depan = ");
  Serial.print(distance);
  Serial.println(" cm");
  
  long durasi = pulseIn (echopin2, HIGH);
  int distance2 = durasi*0.343/2;
  Serial.print("jarak sensor belakang = ");
  Serial.print(distance2);
  Serial.println(" cm");
  
  long duration3 = pulseIn(echopin3, HIGH);        
  int distance3 = duration3*0.0343/2;       
  Serial.print("jarak sensor kiri = ");
  Serial.print(distance3);
  Serial.println(" cm");
  
  long duration4 = pulseIn(echopin4, HIGH);        
  int distance4 = duration4*0.0343/2;       
  Serial.print("jarak sensor kanan = ");
  Serial.print(distance4);
  Serial.println(" cm");
  delay (100);

  if(distance <20 && distance2 >20){ //MUNDUR
    //bagian 1    
    s2.write(45);
    s11.write(45);
    s14.write(45);
    
    delay(100);
    s1.write(120);
    s10.write(60);
    s13.write(120);
      
    delay(50);
    s2.write(90);
    s11.write(90);
    s14.write(90);
    
    delay(130);
    s1.write(90);
    s10.write(90);
    s13.write(90);
    
    //bagian 2
    s5.write(45);
    s8.write(45);
    s17.write(45);
    
    delay(100);
    s4.write(60);
    s7.write(120);
    s16.write(60);
    
    delay(50);
    s5.write(90);
    s8.write(90);
    s17.write(90);
    
    delay(130);
    s4.write(90);
    s7.write(90);
    s16.write(90);
  }
  else if(distance2 <20 && distance >20){ //MAJU
     //bagian 1    
    s2.write(45);
    s11.write(45);
    s14.write(45);
    
    delay(100);
    s1.write(60);
    s10.write(120);
    s13.write(60);
      
    delay(50);
    s2.write(90);
    s11.write(90);
    s14.write(90);
    
    delay(130);
    s1.write(90);
    s10.write(90);
    s13.write(90);
    
    //bagian 2
    s5.write(45);
    s8.write(45);
    s17.write(45);
    
    delay(100);
    s4.write(120);
    s7.write(60);
    s16.write(120 );
    
    delay(50);
    s5.write(90);
    s8.write(90);
    s17.write(90);
    
    delay(130);
    s4.write(90);
    s7.write(90);
    s16.write(90);
    }
    else if(distance <20 && distance2 <20 && distance3 <20){ //kanan
     //bagian 1
    s1.write(120);
    s10.write(90);
    s13.write(70);
    s4.write(70);
    s7.write(90);
    s16.write(120);
    
    delay(100);
    s2.write(45);
    s11.write(45);
    s14.write(45);

    s3.write(45);
    s12.write(135);
    s15.write(45);
    
    delay(100);
    s2.write(90);
    s11.write(90);
    s14.write(90);
    
    delay(100);
    s3.write(90);
    s12.write(90);
    s15.write(90);

     //bagian 2
    s1.write(130);
    s10.write(90);
    s13.write(70);
    s4.write(60);
    s7.write(90);
    s16.write(130);
    
    delay(100);
    s5.write(45);
    s8.write(45);
    s17.write(45);

    s6.write(135);
    s9.write(45);
    s18.write(135);
    
    delay(100);
    s5.write(90);
    s8.write(90);
    s17.write(90);
    
    delay(100);
    s6.write(90);
    s9.write(90);
    s18.write(90);
    }
    else if(distance <20 && distance2 <20 && distance4 <20){ //kiri
     //bagian 1
    s1.write(120); 
    s10.write(90);
    s13.write(70);
    s4.write(70);
    s7.write(90);
    s16.write(120);
    
    delay(100);
    s2.write(45);
    s11.write(45);
    s14.write(45);

    s3.write(135);
    s12.write(45);
    s15.write(135);
    
    delay(100);
    s2.write(90);
    s11.write(90);
    s14.write(90);
    
    delay(100);
    s3.write(90);
    s12.write(90);
    s15.write(90);

     //bagian 2
    s1.write(130);
    s10.write(90);
    s13.write(70);
    s4.write(60);
    s7.write(90);
    s16.write(130);
    
    delay(100);
    s5.write(45);
    s8.write(45);
    s17.write(45);

    s6.write(45);
    s9.write(135);
    s18.write(45);
    
    delay(100);
    s5.write(90);
    s8.write(90);
    s17.write(90);
    
    delay(100);
    s6.write(90);
    s9.write(90);
    s18.write(90);
}}