#include<ESP32Servo.h>
Servo myservo;
int angulo; //angulo el servomoyor en grados sexagesimales
const int trigger =5;
const int echo = 18;
int tiempo;
double distancia, nivel, error, kp=2, setpoint = 100.0;
void setup() {
  // put your setup code here, to run once:
  myservo.attach (4);
  pinMode (trigger, OUTPUT);
  pinMode (echo, INPUT);
  digitalWrite(trigger, LOW);
  Serial.begin(9600);
  myservo.write(0);
}

void loop() {
   digitalWrite(trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);
  tiempo=pulseIn(echo, HIGH);
  distancia=tiempo/58.30;
  nivel=200-distancia;
  //-----control PID------
  error=setpoint-nivel;
  angulo=kp*error;
  ///saturacion
  if(angulo>90){
    angulo=90;
  }
  if(angulo<0){
    angulo=0;
  }
  myservo.write(angulo);
  Serial.println(nivel);
  delay(200);
}