#include<ESP32Servo.h>
Servo myservo;
double angulo = 0.00; //Grados sexagesimales
const int trigger = 5;
const int echo = 18;
int tiempo;
double distancia;
double nivel;
double SP = 100.00;
double error;
double Kp = 2.00;

void setup() {
  // put your setup code here, to run once:
myservo.attach(4);
Serial.begin(9600);
pinMode(trigger, OUTPUT);
pinMode(echo, INPUT);
digitalWrite(trigger, LOW);
//myservo.write(0);
}

void loop() {
  // put your main code here, to run repeatedly:
digitalWrite(trigger, HIGH);
delayMicroseconds(10);
digitalWrite(trigger, LOW);
tiempo = pulseIn(echo, HIGH);
distancia = tiempo/58.30;
nivel = 200.00 - distancia;

//----Control PID------
error = SP - nivel;
angulo = Kp * error; // Ctrl Proporcional

if (angulo>90){
angulo = 90;
}

if (angulo<0){
angulo = 0;
}

myservo.write(angulo);

// Imprimir
Serial.print("Nivel: ");
Serial.println(nivel);
delay(200);

}