#include <ESP32Servo.h>

Servo servo_18;
unsigned long task_time_ms=0;

double fnc_ultrasonic_distance(int _t, int _e){
	unsigned long dur=0;
	digitalWrite(_t, LOW);
	delayMicroseconds(5);
	digitalWrite(_t, HIGH);
	delayMicroseconds(10);
	digitalWrite(_t, LOW);
	dur = pulseIn(_e, HIGH, 18000);
	if(dur==0)return 999.0;
	return (dur/57);
}

void setup()
{
  	pinMode(21, OUTPUT);
	pinMode(19, INPUT);
	servo_18.attach(18);

	Serial.begin(115200);
	Serial.flush();
	while(Serial.available()>0)Serial.read();

}


void loop()
{
	yield();

  	if((millis()-task_time_ms)>=150){
  		task_time_ms=millis();
  		if ((fnc_ultrasonic_distance(21,19) < 15)) {
  			servo_18.write(90);
  		}
  		else {
  			servo_18.write(45);
  		}

  		Serial.print(String("Distancia: "));
  		Serial.print(fnc_ultrasonic_distance(21,19));
  		Serial.println(String(" cm"));
  	}

}