#include <Servo.h>
#define trigPin 7
#define echoPin 6
Servo myservo;

int val;

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

void loop()
{
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  if (distance > 40 or distance == 0){
    Serial.println("te ver");
    delay(400); }
  else{
    myservo.write(distance * 5);
    delay(400);
    Serial.println(distance);
  }
}