#include <ESP32Servo.h>
#define servoP 16

const int trigpin = 4;
const int echopin = 5;
int sm;
int distancia; 

Servo servoM;

void setup() {
  Serial.begin(115200);
  servoM.attach(servoP); 
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
}

void loop() {
  digitalWrite(trigpin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigpin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin, LOW);
  
  sm = pulseIn(echopin, HIGH);
  distancia = sm * 0.034 / 2;

  int angulo = map(distancia, 0, 400, 0, 180);
 // angulo = constrain(angulo, 0, 180);

  servoM.write(angulo);
  delay(50); 
}