sonar.ino

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

#define SERIAL_BAUD       115200
#define TRIGGER_PIN       11
#define ECHO_PIN          12
#define MAX_DISTANCE      200

#define SERVO_PIN         9
#define VERTICAL_ANGLE    113
#define HORIZONTAL_ANGLE  25

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
Servo servo;

void setup() {
  Serial.begin(SERIAL_BAUD);
  servo.attach(SERVO_PIN);
}

void loop() {
  delay(50);
  Serial.print("Ping: ");
  Serial.print(sonar.ping_cm());
  Serial.println(" cm");

  while(true) {
      servoPointVertical();
      delay(500);
      servoPointHorizontal();
      delay(500);
  }
}

void servoPointVertical() {
  servo.write(VERTICAL_ANGLE);
}

void servoPointHorizontal() {
  servo.write(HORIZONTAL_ANGLE);
}