#include <Servo.h>
Servo servo1;
#define echo 2
#define trig 3
#define servo 5

unsigned long t_a_micros;
unsigned long t_b_micros;
int periodomicros = 5 ;
int pos_servo = 0;
int limitecm = 20;

void setup() {
  Serial.begin(9600);
  //pinMode(LED_BUILTIN, OUTPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  t_a_micros = micros();
  t_b_micros = micros();
  servo1.attach(servo);
  servo1.write(pos_servo);
}

void loop() {
  float dist = distanciacm();
  Serial.println(dist);
   if (dist <= limitecm) {
     servo1.write(180);
   }
   else{
     servo1.write(0);
   }
}

float distanciacm() {
  float t_ultrasonico;
  float distancia;
  digitalWrite(trig, LOW);
  if (micros() - t_a_micros >= periodomicros)
  {
    t_a_micros = micros();
    digitalWrite(trig, HIGH);
  }
  if (micros() - t_b_micros >= 2 * periodomicros)
  {
    t_b_micros = micros();
    digitalWrite(trig, LOW);
  }
  t_ultrasonico = pulseIn(echo, HIGH);
  distancia = t_ultrasonico / 58.3;
  /*if (distancia >= 10)
    sprintf(men, "DISTANCIA:%.1fcm", distancia);
    else
    sprintf(men, "DISTANCIA:%.2fcm", distancia);*/
  return distancia;
}