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

#define TRIGGER_PIN  2  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     4  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

float d;
int pos = 0;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
Servo myservo;

bool forward = true;

void setup() {
  Serial.begin(9600);// Open serial monitor at 115200 baud to see ping results.

  myservo.attach(9);  // attaches the servo on pin 9 to the Servo object
  myservo.write(0);
  delay(15);
}

void loop() {
 delay(50);   

  d = sonar.ping_cm();
  
  // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  Serial.print("D");
  Serial.print(d); // Send ping, get distance in cm and print result (0 = outside set distance range)
  Serial.print("A");
  Serial.println(pos);

  myservo.write(pos);              // tell servo to go to position in variable 'pos'

  if (forward == true)
    pos = pos + 1;

  if (forward == false)
    pos = pos - 1;

  if (pos == 180)
    forward= false ;

  if (pos==0)
  forward= true;
}