#include <Servo.h>
// Definir pines y variables para el servo y el PID
Servo servo;            // Crear objeto servo
int servoPin = 9;       // Pin al que está conectado el servo
int angleSetPoint = 0;  // El valor deseado del ángulo
int currentAngle = 0;   // Valor actual del ángulo
float previousError = 0;
float integral = 0;

// Coeficientes del PID
float kp = 2.0;   // Ganancia proporcional
float ki = 0.05;  // Ganancia integral
float kd = 1.0;   // Ganancia derivativa

unsigned long previousTime = 0;
int motorSpeed = 0;

void setup() {
  // Inicializar el servo
  servo.attach(servoPin);
  servo.write(90);  // Empezar en la posición de 90 grados

  // Inicializar comunicación serial
  Serial.begin(9600);
  Serial.println("Ingrese el angulo deseado (0 a 180):");
}

void loop() {
  // Leer el valor del ángulo desde el puerto serial
  if (Serial.available() > 0) {
    int input = Serial.parseInt();
    if (input >= 0 && input <= 180) {
      angleSetPoint = input;
      Serial.print("Nuevo set point: ");
      Serial.println(angleSetPoint);
    }
  }

  // Leer la posición actual del servo
  currentAngle = readServoPosition();

  // Controlador PID
  unsigned long currentTime = millis();
  float elapsedTime = (currentTime - previousTime) / 1000.0; // Convertir a segundos
  
  float error = angleSetPoint - currentAngle;  // Error entre el setpoint y el valor actual
  integral += error * elapsedTime;             // Calcular la parte integral
  float derivative = (error - previousError) / elapsedTime; // Calcular la derivada
  
  motorSpeed = kp * error + ki * integral + kd * derivative;  // Fórmula del PID
  
  // Restringir el valor de motorSpeed para que esté entre 0 y 180 (rango válido del servo)
  motorSpeed = constrain(motorSpeed, 0, 180);

  // Mover el servo según el valor de motorSpeed
  servo.write(motorSpeed);

  // Actualizar variables para la próxima iteración
  previousError = error;
  previousTime = currentTime;
  
  delay(50);  // Esperar un poco antes de la próxima iteración
}

// Función simulada para leer la posición actual del servo
// Esta función solo devuelve el último valor movido por el servo
int readServoPosition() {
  // En una aplicación real, necesitarías un encoder u otro sensor para obtener la posición actual del servo.
  return motorSpeed;
}