#include <Servo.h>

Servo myservo;

const int potPin = A1; // Pin del potenciómetro
int potVal; // Valor del potenciómetro
int potMin = 0; // Valor mínimo del potenciómetro
int potMax = 1023; // Valor máximo del potenciómetro

double setpoint; // Posición deseada del servomotor
double input; // Posición actual del servomotor
double output; // Valor de control calculado por el PID

// Parámetros del control PID
double Kp = 0.2;
double Ki = 9;
double Kd = 0.001;

double integral, derivative;
double prevInput = 0;
double dt = 0.1; // Intervalo de muestreo en segundos

void setup() {
  myservo.attach(9); // Pin de control del servomotor
  Serial.begin(9600);
  // Inicializar el PID
  integral = 0;
  prevInput = 0;
  myservo.write(0);
}

void loop() {
  // Leer el valor del potenciómetro
  potVal = analogRead(potPin);
  setpoint = map(potVal, potMin, potMax, 0, 180); // Mapear el valor del potenciómetro al rango de 0 a 180 grados

  // Leer la posición actual del servomotor
  input = myservo.read();

  // Calcular el error
  double error = setpoint - input;

  // Calcular la parte integral del PID
  integral += (error * dt);

  // Calcular la parte derivativa del PID
  derivative = (error - prevInput) / dt;

  // Calcular la salida del PID
  output = (Kp * error) + (Ki * integral) + (Kd * derivative);

  // Limitar la salida entre 0 y 180 grados
  output = constrain(output, 0, 180);

  // Aplicar la señal de control al servomotor
  myservo.write(output);

  // Guardar la posición actual para el cálculo de la parte derivativa en el siguiente ciclo
  prevInput = error;

  // Esperar el intervalo de muestreo
  delay(dt * 1000);

  // Mostrar información de PID en el puerto serie para debug
  Serial.print(output);     //Variable de control CV
  Serial.print(",");
  Serial.print(setpoint);         //Set Point           SP
  Serial.print(",");
  Serial.println(input); //Variable de proceso PV

}