/*
 HC-SR04 Ping distance sensor]
 VCC to arduino 5v GND to arduino GND
 Echo to Arduino pin 13 Trig to Arduino pin 12
 More info at: http://goo.gl/kJ8Gl
 */

#include <Servo.h>

#define trigPin 7
#define echoPin 8

  Servo servoA;  // create servo object to control a servo
  Servo servoB;  // a maximum of eight servo objects can be created


void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  servoA.attach(6); 
  servoB.attach(5);// attaches the servo on pin 5 to the servo 
}

void loop() {

  int duration, distance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  if (distance >= 200 || distance <= 0){
    Serial.println("Out of range");
  }
  else {
    Serial.print(distance);
    Serial.println(" cm");
  }
    if (distance >= 30){
      servoB.write(70);
      servoA.write(110);
  }
  else {
      servoB.write(90);
      servoA.write(90);
        delay(2000);
  }

  delay(500);
}