import machine
# 1. Declaración de constantes de sintonización de la Planta
Kp = 1.08
Ki = 52.42718447
Kd = 0.005562
Fs = 152.5878906
# Coeficientes pre-calculados de integración y derivación (Optimización de CPU)
Fi = Ki / (2.0 * Fs)
Fd = 2.0 * Fs * Kd
# 2. Configuración de Hardware
adc_sensor = machine.ADC(26)   # Canal de retroalimentación de la planta
adc_setpoint = machine.ADC(27) # Canal del potenciómetro de referencia
pwm_salida = machine.PWM(machine.Pin(15))
pwm_salida.freq(10000)         # Frecuencia base del PWM a 10 kHz
# 3. Variables Globales e Histórico (Memoria del PID)
e0 = 0.0; e1 = 0.0
yi0 = 0.0; yi1 = 0.0
yd0 = 0.0; yd1 = 0.0
ypid = 0.0
PUNTO_DE_CONTROL = 127.0
def isr_pid(timer):
    global e0, e1, yi0, yi1, yd0, yd1, ypid, PUNTO_DE_CONTROL
    # A) Adquisición de variables
    # El ADC del RP2040 es de 16 bits (0-65535). Lo desplazamos 8 bits (>> 8) 
    # para escalarlo al rango 0-255, emulando la matemática original de 8 bits.
    SENSOR = float(adc_sensor.read_u16() >> 8)
    PUNTO_DE_CONTROL = float(adc_setpoint.read_u16() >> 8)
    # B) Cálculo del nivel de error
    e0 = PUNTO_DE_CONTROL - SENSOR
    # C) Ecuación en diferencias (Control IIR)
    # Componente integral
    yi0 = Fi * (e0 + e1) + yi1
    # Componente derivativa
    yd0 = Fd * (e0 - e1) - yd1
    # D) Ecuación Total PID
    ypid = (Kp * e0) + yi0 + yd0
    # E) Ajuste y limitación de la salida (Anti-Windup base)
    # Delimitado a los límites clásicos de 0 a 255
    if ypid > 255.0: 
        ypid = 255.0
    elif ypid < 0.0: 
        ypid = 0.0
    # F) Actualización de la salida PWM de Hardware
    # Convertimos de 0-255 a 0-65535 para el duty_u16 del RP2040 multiplicando por 257
    pwm_salida.duty_u16(int(ypid * 257))
    print(UNTO_DE_CONTROL,SENSOR,ypid,ypid*257)
    # G) Actualización de muestras históricas (Shift)
    e1 = e0
    yi1 = yi0
    yd1 = yd0
def main():
    print("Iniciando Sistema de Control PID (Ziegler-Nichols)...")
    # Configuración de la interrupción por hardware
    timer_pid = machine.Timer()
    # Frecuencia de 152.58 Hz equivalente a ~6.55ms de periodo de muestreo
    timer_pid.init(freq=152.58, mode=machine.Timer.PERIODIC, callback=isr_pid)
    while True:
        # El microcontrolador queda libre para otras tareas (pantallas, comunicaciones)
        pass
main()