Ultrasonic Distance Meter

/*
 This is a simple example of ultrasonic sensor measurement.
 Distance is used to control servo motor.
 */

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards
int pos = 0;    // variable to store the servo position

#define servoPin 9
#define trigPin 13
#define echoPin 12

#define MIN_DISTANCE 2
#define MAX_DISTANCE 20
#define MIN_ANGLE 0
#define MAX_ANGLE 180

void setup() {
  Serial.begin (9600);
  myservo.attach(servoPin);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  long duration, distance;
  // Make a Ping
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Get a pulse measurements in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculate the distance - half of the sound wave path.
  // distance = duration / 2 * speed_of_sound
  // 340.29 m/s = 1/29.411 cm/us
  distance = (duration/2) / 29.411;

  if (distance < MAX_DISTANCE && distance > MIN_DISTANCE)
  {
    Serial.println(distance);
    // MIN_DISTANCE cm -> MIN_ANGLE deegrees
    // MAX_DISTANCE cm -> MAX_ANGLE deegrees
    pos = map(distance, MIN_DISTANCE, MAX_DISTANCE, MIN_ANGLE, MAX_ANGLE);
  }
  else
  {
    // if we are getting measurement that dont make sense we will
    // put to servo to closer end position.
    if(pos < (MAX_ANGLE - MIN_ANGLE)/2)
      pos = MIN_ANGLE;
    else
      pos = MAX_ANGLE;
  }
  // Update servo position
  myservo.write(pos);
  delay(10);
}