#include <Servo.h>

// Objetos y pines
Servo myServo;
const int potPin = A0;   // Potenciómetro
const int servoPin = 9;  // Servo

// Variables del PID
double setpoint;     // Valor deseado (posición del potenciómetro)
double input;        // Valor actual (posición del servo simulada)
double output;       // Señal PID al servo

// Constantes PID
double kP = 1.0;     // Reduce la agresividad
double kI = 0.02;    // Integral pequeña para evitar acumulación excesiva
double kD = 0.1;     // Derivativo más alto para amortiguar las oscilaciones


// Variables PID auxiliares
double previousError = 0;
double integral = 0;

// Variables de posición del servo
int servoPos = 0;   // Posición actual del servo

void setup() {
  myServo.attach(servoPin);
  Serial.begin(9600);
}

void loop() {
  // Leer el potenciómetro (0-1023) y escalarlo a 0-180 grados
  int potValue = analogRead(potPin);
  setpoint = map(potValue, 0, 1023, 0, 180);

  // Tomamos la posición actual del servo como entrada (input)
  input = servoPos;

  // Calcular error
  double error = setpoint - input;

  // Calcular parte integral
  integral += error;
  integral = constrain(integral, -100, 100);  // Anti-windup

  // Calcular parte derivativa
  double derivative = error - previousError;

  // Calcular salida PID
  output = (kP * error) + (kI * integral) + (kD * derivative);

  // Actualizar posición del servo
  servoPos += output;   // Mover según el resultado PID
  servoPos = constrain(servoPos, 0, 180);
  myServo.write(servoPos);

  // Guardar error anterior
  previousError = error;

  // Mostrar datos por Serial
  Serial.print("Setpoint: ");
  Serial.print(setpoint);
  Serial.print("  Input: ");
  Serial.print(input);
  Serial.print("  Output: ");
  Serial.print(output);
  Serial.print("  ServoPos: ");
  Serial.println(servoPos);

  delay(50); // Pequeño retardo para estabilidad
}