#include <AFMotor.h>
#include <NewPing.h>
#include <Servo.h>

#define TRIG_PIN A0
#define ECHO_PIN A1
#define MAX_DISTANCE 200
#define MAX_SPEED 180 // sets speed of DC  motors
#define MAX_SPEED_OFFSET 20


NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);
Servo myservo;


boolean goesForward = false;
int distance = 100;
int speedSet = 0;

long jarak (int TRIG_PIN, int ECHO_PIN ){
pinMode(TRIG_PIN, OUTPUT); //set pin trigger as output
digitalWrite(TRIG_PIN, LOW); // Pin standby Low
digitalWrite(TRIG_PIN, HIGH); // Mulai generate Gelombang set HIGH
delayMicroseconds(10); // diam 10 microseconds
digitalWrite(TRIG_PIN, LOW); // lanjutan gelombang set LOW
pinMode(ECHO_PIN, INPUT);
return pulseIn(ECHO_PIN, HIGH); 
}
void setup() {

myservo.attach(A3);
myservo.attach(A4);

Serial.begin(9600);

  myservo.write(115);
  delay(500);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  //Serial.begin(9600);
}

void loop() {

  int cm = 0; //reset cm ke 0
cm = (0.03446 * jarak (A0, A1))/2;
Serial.print(cm);
Serial.println("cm");
delay(10);

  int distanceR = 0;
  int distanceL =  0;
  //Serial.println(distance);
  delay(40);

  if (distance <= 5)
  {
     
    moveStop();
    delay(50);
    moveBackward();
    delay(150);
    moveStop();
    delay(100);
    turnRight();
    moveStop();
    //Serial.println("belok kanan");s
  } else if (distance <= 50){ 
    myservo.attach(A2);
    myservo.write(0);
    delay(1000); 
    myservo.write(290); 
    delay(1000); 
    myservo.write(490);
    delay(1000); 
    myservo.write(0);
    delay(1000); 


    myservo.attach(A3);
    myservo.write(0);
    delay(50); 
    myservo.write(300);  
    delay(50);
    myservo.write(450);  
    delay(50);
    myservo.write(0);
    delay(50); 
    


    myservo.attach(A4);
    myservo.write(0);
    delay(50); 
    myservo.write(150);  
    delay(50);
    myservo.write(190);  
    delay(50);
    myservo.write(0);
    delay(50); 


    myservo.attach(A5);
    myservo.write(0);
    delay(50); 
    myservo.write(290);  
    delay(50);
    myservo.write(490);  
    delay(50);
    myservo.write(0);
    delay(50); 
   


   myservo.attach(A2);
   myservo.write(0);
    delay(50); 
    myservo.write(490); 
    delay(50); 
    myservo.write(290);
    delay(50); 
    myservo.write(0);
    delay(50); 


    myservo.attach(A3);
    myservo.write(0);
    delay(50); 
    myservo.write(450);  
    delay(50);
    myservo.write(300);  
    delay(50);
    myservo.write(0);
    delay(50); 


    myservo.attach(A4);
    myservo.write(190);  
    delay(50);
    myservo.write(320);  
    delay(50);
    myservo.write(0);
    delay(50); 


    myservo.attach(A5);
    myservo.write(0);
    delay(50); 
    myservo.write(340);  
    delay(50);
    myservo.write(150);  
    delay(50);
    myservo.write(0);
    delay(50); 

    myservo.attach(A4);
    myservo.write(0);
    delay(50); 
    myservo.write(300);  
    delay(50);
    myservo.write(410);  
    delay(50);
    myservo.write(0);
    delay(50); 


    myservo.attach(A5);
    myservo.write(0);
    delay(50); 
    myservo.write(290);  
    delay(50);
    myservo.write(490);  
    delay(50);
    myservo.write(0);
    delay(50); 


     myservo.attach(A5);
     myservo.write(0);
    delay(50); 
    myservo.write(490);  
    delay(50);
    myservo.write(290);  
    delay(50);
    myservo.write(0);
    delay(50); 
   

   myservo.attach(A4);
   myservo.write(0);
    delay(50); 
    myservo.write(410);  
    delay(50);
    myservo.write(300);  
    delay(50);
    myservo.write(0);
    delay(50); 


    myservo.attach(A3);
    myservo.write(0);
    delay(50); 
    myservo.write(320);  
    delay(50);
    myservo.write(150);  
    delay(50);
    myservo.write(0);
    delay(50); 

    myservo.attach(A2);
    myservo.write(0);
    delay(50); 
    myservo.write(150); 
    delay(50); 
    myservo.write(340);
    delay(50);
    myservo.write(0);
    delay(50); 

  }else 
  {
    moveForward();
    //Serial.println("maju");
  }
  distance = readPing();
}


int readPing() {
  delay(70);
  int cm = sonar.ping_cm();
  if (cm == 0)
  {
    cm = 250;
  }
  return cm;
}

void moveStop() {
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  motor3.run(RELEASE);
  motor4.run(RELEASE);
}

void moveForward() {

  if (!goesForward)
  {
    goesForward = true;
    motor1.run(FORWARD);
    motor2.run(FORWARD);
    motor3.run(FORWARD);
    motor4.run(FORWARD);
    for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
    {
      motor1.setSpeed(speedSet);
      motor2.setSpeed(speedSet);
      motor3.setSpeed(speedSet);
      motor4.setSpeed(speedSet);
      delay(5);
    }
  }
}

void moveBackward() {
  goesForward = false;
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
    motor3.setSpeed(speedSet);
    motor4.setSpeed(speedSet);
    delay(5);
  }
}

void turnRight() {
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(BACKWARD);
  motor4.run(BACKWARD);
  delay(200);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}

void turnLeft() {
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
  delay(200);
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  motor3.run(FORWARD);
  motor4.run(FORWARD);
}