import machine
import time

# --- CONFIGURACIÓN DE PINES ---

# Motor A (Izquierdo)
pin_in1 = machine.Pin(3, machine.Pin.OUT)
pin_in2 = machine.Pin(4, machine.Pin.OUT)
ena_pwm = machine.PWM(machine.Pin(2))
ena_pwm.freq(1000)

# Motor B (Derecho)
pin_in3 = machine.Pin(5, machine.Pin.OUT)
pin_in4 = machine.Pin(6, machine.Pin.OUT)
enb_pwm = machine.PWM(machine.Pin(7))
enb_pwm.freq(1000)

# --- FUNCIONES DE MOVIMIENTO ---

def fijar_velocidad(motor, porcentaje):
    valor_pwm = int((max(0, min(100, porcentaje)) / 100.0) * 65535)
    if motor == "A":
        ena_pwm.duty_u16(valor_pwm)
    elif motor == "B":
        enb_pwm.duty_u16(valor_pwm)

def avanzar(velocidad=80):
    # Motor A
    pin_in1.on()
    pin_in2.off()
    # Motor B
    pin_in3.on()
    pin_in4.off()
    fijar_velocidad("A", velocidad)
    fijar_velocidad("B", velocidad)
    print(f"Avanzando al {velocidad}%")

def retroceder(velocidad=80):
    pin_in1.off()
    pin_in2.on()
    pin_in3.off()
    pin_in4.on()
    fijar_velocidad("A", velocidad)
    fijar_velocidad("B", velocidad)
    print("Retrocediendo")

def girar_derecha(velocidad=70):
    # Motor A hacia adelante, Motor B hacia atrás
    pin_in1.on()
    pin_in2.off()
    pin_in3.off()
    pin_in4.on()
    fijar_velocidad("A", velocidad)
    fijar_velocidad("B", velocidad)
    print("Girando a la derecha")

def detener():
    pin_in1.off()
    pin_in2.off()
    pin_in3.off()
    pin_in4.off()
    ena_pwm.duty_u16(0)
    enb_pwm.duty_u16(0)
    print("Motores detenidos")

# --- PRUEBA DE FUNCIONAMIENTO ---

try:
    while True:
        avanzar(100)
        time.sleep(2)
        
        detener()
        time.sleep(1)
        
        girar_derecha(80)
        time.sleep(1.5)
        
        retroceder(60)
        time.sleep(2)
        
        detener()
        break # Ejecuta una vez y para

except KeyboardInterrupt:
    detener()