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

#define TRIGGER_PIN 2
#define ECHO_PIN 3
#define MAX_DISTANCE 200
#define SERVO1_PIN 9
#define SERVO2_PIN 10

Servo servo1;
Servo servo2;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
}

void loop() {
  // Bergerak dalam looping
  for (int angle = 0; angle <= 180; angle++) {
    servo1.write(angle);
    servo2.write(angle);
    delay(15);
  }

  for (int angle = 180; angle >= 0; angle--) {
    servo1.write(angle);
    servo2.write(angle);
    delay(15);
  }

  // Mengecek jarak dengan sensor ultrasonik
  delay(50);
  int distance = sonar.ping_cm();

  if (distance < 20) {
    servo1.write(LOW);
    servo2.write(HIGH);
  }
}