from machine import ADC, Pin, PWM
import time

# ---------------------------------------------------------
# 1. CONFIGURACIÓN DE ENTRADAS (ADC de 16 bits)
# ---------------------------------------------------------
adc_setpoint = ADC(Pin(26))  # ADC0: Referencia deseada
adc_sensor = ADC(Pin(27))    # ADC1: Potenciómetro de posición real

# ---------------------------------------------------------
# 2. CONFIGURACIÓN DE SALIDAS (L293D)
# ---------------------------------------------------------
# Configuración del pin Enable con PWM (Frecuencia: 1kHz a 10kHz típica para DC)
motor_pwm = PWM(Pin(15))
motor_pwm.freq(1000)

# Pines lógicos de dirección
in1 = Pin(14, Pin.OUT)
in2 = Pin(13, Pin.OUT)

# ---------------------------------------------------------
# 3. PARÁMETROS DEL CONTROLADOR PID
# ---------------------------------------------------------
# Estas ganancias transforman el error (en grados) a resolución PWM (0-65535)
Kp = 300.0   
Ki = 10.0    
Kd = 50.0    

integral = 0.0
error_previo = 0.0
tiempo_previo = time.ticks_ms()

# ---------------------------------------------------------
# 4. FUNCIÓN DE CONVERSIÓN
# ---------------------------------------------------------
def a_grados(valor_16bits):
    return (valor_16bits / 65535.0) * 360.0

# ---------------------------------------------------------
# 5. BUCLE DE CONTROL CÍCLICO
# ---------------------------------------------------------
while True:
    # --- A. Cálculo del Delta de Tiempo (dt) ---
    tiempo_actual = time.ticks_ms()
    dt = time.ticks_diff(tiempo_actual, tiempo_previo) / 1000.0
    
    # Evitar divisiones por cero en ejecuciones ultra-rápidas
    if dt <= 0.001:
        time.sleep_ms(1)
        continue
        
    tiempo_previo = tiempo_actual

    # --- B. Lectura y Escalamiento ---
    sp_grados = a_grados(adc_setpoint.read_u16())
    pv_grados = a_grados(adc_sensor.read_u16())
    
    # --- C. Ecuación PID ---
    error = sp_grados - pv_grados
    
    proporcional = Kp * error
    
    integral += Ki * error * dt
    # Anti-windup adaptado a los límites del PWM
    if integral > 65535.0: integral = 65535.0
    elif integral < -65535.0: integral = -65535.0
    
    derivativo = Kd * ((error - error_previo) / dt)
    error_previo = error
    
    salida_pid = proporcional + integral + derivativo
    
    # --- D. Decodificación de Acción de Control ---
    pwm_salida = 0
    
    if salida_pid > 0:
        # Error positivo: Mover hacia adelante
        in1.value(1)
        in2.value(0)
        senthor=1
        pwm_salida = int(salida_pid)
    elif salida_pid < 0:
        # Error negativo: Mover en reversa
        in1.value(0)
        in2.value(1)
        senthor=2
        pwm_salida = int(-salida_pid) # Valor absoluto para el PWM
    else:
        # En posición exacta
        in1.value(0)
        in2.value(0)
        senthor=0
        pwm_salida = 0

    # --- E. Saturación del Actuador ---
    if pwm_salida > 65535:
        pwm_salida = 65535
        
    # --- F. Ejecución de Salida Físico ---
    motor_pwm.duty_u16(pwm_salida)
    
    # Monitorización (Descomentar para ver en consola, afecta el rendimiento)
    print(f"SP: {sp_grados:.1f}° | PV: {pv_grados:.1f}° | Err: {error:.1f}° | PWM: {pwm_salida}| Sent: {senthor}")
    
    # Estabilización del bucle (aprox 100Hz)
    time.sleep_ms(10)