#include <Ticker.h>
#include <absencoder.h> // Librería que implementa encoderRead()

// Definición de entradas y salidas digitales
#define ENCODER 27
#define DIR_PIN 14
#define PWM_PIN 13

// Configuración PWM
const int PWMChannel = 1;
const int frequency = 1000;
const int resolution = 8; // 0-255

// Variables del controlador PID
const float Kp = 5.0;  // Proporcional
const float Ki = 0.1;  // Integral
const float Kd = 2.0;  // Derivativo
const int snMax = 255; // Límite para el acumulador integral

// Variables para el control
volatile int sn = 0;     // Acumulador para el término integral
volatile int en = 0;     // Error actual
volatile int enOld = 0;  // Error previo
volatile int mn = 0;     // Variable de actuación
volatile int ref = 0;    // Referencia en valores del encoder

Ticker blinker;  // Timer para la ISR

// Función que implementa el algoritmo PID
void PIDControl() {
    // Actualización del acumulador integral con límites
    sn += en; //suma del error tomado desde el instante 0 hasta nTs (Ts) = 1ms --> acumulador de el termino integral
    if (sn > snMax) sn = snMax;
    if (sn < -snMax) sn = -snMax;

    // Cálculo del PID
    mn = (Kp * en) + (Ki * sn) + (Kd * (en - enOld));

    // Actualizar el error previo
    enOld = en;
}

// Función que genera la salida PWM y sentido
void actuador() {
    // Limitar la variable de actuación
    if (mn > 255) mn = 255;
    if (mn < -255) mn = -255;

    // Aplicar PWM y dirección
    if (mn >= 0) {
        digitalWrite(DIR_PIN, LOW);
        ledcWrite(PWMChannel, (unsigned int)mn);
    } else {
        digitalWrite(DIR_PIN, HIGH);
        ledcWrite(PWMChannel, (unsigned int)(-mn));
    }
}

// Manejador de interrupción que se ejecuta cada 1 ms
void onTimerISR() {
    en = ref - encoderRead(); // Calcular el error
    PIDControl();             // Ejecutar el control PID
    actuador();               // Actualizar el actuador
}

// Función setup que se ejecuta una vez al iniciar
void setup() {
    // Inicialización del puerto serie
    Serial.begin(9600);

    // Configuración del Timer
    blinker.attach_ms(1, onTimerISR);

    // Configuración de las entradas y salidas digitales
    pinMode(ENCODER_A, INPUT);
    pinMode(ENCODER_B, INPUT);
    pinMode(DIR_PIN, OUTPUT);
    ledcSetup(PWMChannel, frequency, resolution);
    ledcAttachPin(PWM_PIN, PWMChannel);

    // Inicialización de variables
    sn = 0;
    en = 0;
    enOld = 0;
    mn = 0;
    ref = 0;
}

// Bucle principal
void loop() {
    // Leer referencia desde el puerto serie en grados
    if (Serial.available() > 0) {
        int readRef = Serial.parseInt();
        if (readRef >= 0 && readRef <= 360) {
            ref = (readRef * 255) / 360; // Convertir grados a valor del encoder
        }
    }

    // Mostrar datos en el puerto serie
    Serial.print("Referencia (grados): ");
    Serial.print((ref * 360) / 255);
    Serial.print(" Error: ");
    Serial.print(en);
    Serial.print(" Actuación: ");
    Serial.println(mn);
}