#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// Definición de pines
#define SETPOINT_PIN A0
#define POSITION_SENSOR_PIN A1
#define SERVO_PIN 9

// Inicialización del LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Inicialización del servo
Servo myServo;

// Variables del PID
double Kp = 2.0, Ki = 0.05, Kd = 1.0;
double setpoint = 90;
double input = 0;
double output = 0;

double lastError = 0;
double integral = 0;
unsigned long lastTime = 0;

void setup() {
  pinMode(SETPOINT_PIN, INPUT);
  pinMode(POSITION_SENSOR_PIN, INPUT);
  myServo.attach(SERVO_PIN);

  Serial.begin(9600); // Opcional
  lcd.init();
  lcd.backlight();
}

void loop() {
  unsigned long now = millis();
  double timeChange = (now - lastTime) / 1000.0;

  if (timeChange >= 0.1) { // Cada 0.1 segundos
    // Leer entradas
    int rawSetpoint = analogRead(SETPOINT_PIN);
    setpoint = map(rawSetpoint, 0, 1023, 0, 180); // Escala 0-180°

    int rawPosition = analogRead(POSITION_SENSOR_PIN);
    input = map(rawPosition, 0, 1023, 0, 180); // Escala 0-180°

    // Cálculo PID
    double error = setpoint - input;
    integral += error * timeChange;
    double derivative = (error - lastError) / timeChange;

    output = Kp * error + Ki * integral + Kd * derivative;
    output = constrain(output, -180, 180); // Limitar salida

    // Aplicar salida al servomotor
    int angle = constrain(input + output, 0, 180);
    myServo.write(angle);

    // Actualizar variables
    lastError = error;
    lastTime = now;

    // Mostrar en pantalla
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Ang.Actual:");
    lcd.print(input, 0);
    lcd.print(" ");

    lcd.setCursor(0, 1);
    lcd.print("Setpoint:");
    lcd.print(setpoint, 0);
    lcd.print(" ");
  }
}