program jawhead

#include <NewPing.h> //Library untuk Sensor Ultrasonic
#include <Servo.h> //Library untuk Servo
#define trigPin A0 //Set Trigger HCSR04 di Pin digital 12
#define echoPin A1 //Set Echo HCSR04 di Pin digital 13
#define MAX_DISTANCE 500 //Set jarak maksimal
NewPing sonar(trigPin, echoPin, MAX_DISTANCE);
int LEDhijau = A3, LEDmerah = A5;

Servo servoku13;
Servo servoku2;
Servo servoku3;
Servo servoku4;
Servo servoku5;
Servo servoku6;
Servo servoku7;
Servo servoku8;
Servo servoku9;
Servo servoku10;
Servo servoku11;
Servo servoku12;


void setup() {
 Serial.begin (505200); //Kecepatan komunikasi serial
 pinMode(trigPin, INPUT); //Set pin Trigger sebagai output
 pinMode(echoPin, INPUT); //Set pin Echo sebagai input
 pinMode(LEDmerah, OUTPUT); //Set LEDmerah sebagai output
 pinMode(LEDhijau, OUTPUT); //Set LEDhijau sebagai output

servoku13.attach(13); 
servoku2.attach(2); 
servoku3.attach(3);
servoku4.attach(4); 
servoku5.attach(5); 
servoku6.attach(6); 
servoku7.attach(7);
servoku8.attach(8); 
servoku9.attach(9); 
servoku10.attach(10);
servoku11.attach(11); 
servoku12.attach(12);
   
}

void loop() {
int duration, jarak,posisi=0,i;
 digitalWrite(trigPin, LOW);
 delayMicroseconds(2);
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);
 duration = pulseIn(echoPin, HIGH);
 jarak = (duration/2) / 29.1;
  Serial.print(jarak);
  Serial.println(" cm");

 if(jarak<=0) // belok kanan

 {
   digitalWrite(LEDhijau,HIGH ); //LEDhijau mati
   digitalWrite(LEDmerah, LOW); //LEDmerah hidup
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(20); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(100); //bawah
delay(150);
servoku5.write(80);
delay(90);
servoku4.write(160); //atas
delay(90);
servoku5.write(100);
delay(90);
servoku4.write(140); //tengah
delay(90);
servoku6.write(70);
servoku7.write(35); //bawah
delay(150);
servoku8.write(100);
delay(90);
servoku7.write(75); //atas
delay(90);
servoku8.write(80);
delay(90);
servoku7.write(55); //tengah
delay(90);
servoku9.write(80);
 }

if(jarak<=15) // belok kanan

 {
   digitalWrite(LEDhijau,LOW ); //LEDhijau mati
   digitalWrite(LEDmerah, HIGH); //LEDmerah hidup
////===mundur===////
servoku4.write(160); //encell tengah //
delay(150);
servoku5.write(70); //siku robot
delay(90);
servoku4.write(120); //encell tengah //
delay(90);
servoku5.write(90); //siku robot
delay(90);
servoku4.write(140); //encell tengah //
delay(90);
servoku6.write(75); //gerak jalan
servoku7.write(75); //geak jalan
delay(150);
servoku8.write(95); //encel tengah
delay(90);
servoku7.write(35); //geak jalan
delay(90);
servoku8.write(75); //encel tengah
delay(90);
servoku7.write(55); //geak jalan
delay(90);
servoku9.write(70); //siku robot
servoku13.write(10); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(50); //bawah
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(30); //atas
delay(90);
servoku3.write(155);
servoku10.write(30); //bawah
 delay (150);
servoku11.write(70);
 delay (90);
servoku10.write(80); //bawah
 delay (90);
servoku11.write(90);
 delay (90);
servoku10.write(60); //atas
 delay (90);
servoku12.write(50); //atas

 {
  delay(2000);
 }
 ///====belok kanan===//// 
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(0); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(160); //encell tengah //
delay(150);
servoku5.write(70); //siku robot
delay(90);
servoku4.write(120); //encell tengah //
delay(90);
servoku5.write(90); //siku robot
delay(90);
servoku4.write(140); //encell tengah //
delay(90);
servoku6.write(75); //gerak jalan
servoku7.write(75); //geak jalan
delay(150);
servoku8.write(95); //encel tengah
delay(90);
servoku7.write(35); //geak jalan
delay(90);
servoku8.write(75); //encel tengah
delay(90);
servoku7.write(55); //geak jalan
delay(90);
servoku9.write(70); //siku robot

{
  delay(200);
 }
 
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(0); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(160); //encell tengah //
delay(150);
servoku5.write(70); //siku robot
delay(90);
servoku4.write(120); //encell tengah //
delay(90);
servoku5.write(90); //siku robot
delay(90);
servoku4.write(140); //encell tengah //
delay(90);
servoku6.write(75); //gerak jalan
servoku7.write(75); //geak jalan
delay(150);
servoku8.write(95); //encel tengah
delay(90);
servoku7.write(35); //geak jalan
delay(90);
servoku8.write(75); //encel tengah
delay(90);
servoku7.write(55); //geak jalan
delay(90);
servoku9.write(70); //siku robot

{
  delay(200);
 }
 
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(0); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(160); //encell tengah //
delay(150);
servoku5.write(70); //siku robot
delay(90);
servoku4.write(120); //encell tengah //
delay(90);
servoku5.write(90); //siku robot
delay(90);
servoku4.write(140); //encell tengah //
delay(90);
servoku6.write(75); //gerak jalan
servoku7.write(75); //geak jalan
delay(150);
servoku8.write(95); //encel tengah
delay(90);
servoku7.write(35); //geak jalan
delay(90);
servoku8.write(75); //encel tengah
delay(90);
servoku7.write(55); //geak jalan
delay(90);
servoku9.write(70); //siku robot
{
  delay(200);
 }
 
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(0); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(160); //encell tengah //
delay(150);
servoku5.write(70); //siku robot
delay(90);
servoku4.write(120); //encell tengah //
delay(90);
servoku5.write(90); //siku robot
delay(90);
servoku4.write(140); //encell tengah //
delay(90);
servoku6.write(75); //gerak jalan
servoku7.write(75); //geak jalan
delay(150);
servoku8.write(95); //encel tengah
delay(90);
servoku7.write(35); //geak jalan
delay(90);
servoku8.write(75); //encel tengah
delay(90);
servoku7.write(55); //geak jalan
delay(90);
servoku9.write(70); //siku robot

{
  delay(200);
 }
 
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(0); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(160); //encell tengah //
delay(150);
servoku5.write(70); //siku robot
delay(90);
servoku4.write(120); //encell tengah //
delay(90);
servoku5.write(90); //siku robot
delay(90);
servoku4.write(140); //encell tengah //
delay(90);
servoku6.write(75); //gerak jalan
servoku7.write(75); //geak jalan
delay(150);
servoku8.write(95); //encel tengah
delay(90);
servoku7.write(35); //geak jalan
delay(90);
servoku8.write(75); //encel tengah
delay(90);
servoku7.write(55); //geak jalan
delay(90);
servoku9.write(70); //siku robot





}

 else{ //Jika jarak lebih dari yang ditentukan
    digitalWrite(LEDhijau,HIGH ); //LEDhijau mati
   digitalWrite(LEDmerah, LOW); //LEDmerah hidup
servoku13.write(80); //bawah
delay(150);
servoku2.write(110);
delay(90);
servoku13.write(20); //atas
delay(90);
servoku2.write(90);
delay(90);
servoku13.write(50); //tengah
delay(90);
servoku3.write(155);
servoku10.write(110); //bawah
 delay (150);
 servoku11.write(70);
 delay (90);
 servoku10.write(60); //atas
 delay (90);
  servoku11.write(90);
 delay (90);
 servoku10.write(80); //tengah
 delay (90);
 servoku12.write(45);
servoku4.write(100); //bawah
delay(150);
servoku5.write(80);
delay(90);
servoku4.write(160); //atas
delay(90);
servoku5.write(100);
delay(90);
servoku4.write(140); //tengah
delay(90);
servoku6.write(70);
servoku7.write(35); //bawah
delay(150);
servoku8.write(100);
delay(90);
servoku7.write(75); //atas
delay(90);
servoku8.write(80);
delay(90);
servoku7.write(55); //tengah
delay(90);
servoku9.write(80);

 }
delay(100); //Delay
}