from machine import Pin
import utime
from pico_i2c_lcd import I2cLcd
from machine import I2C
from machine import PWM

# Mapeo de ángulos a ciclo de trabajo para el servo
angle_duty_map = {
    0: 1638,
    60: 3100,
}

# Inicialización del servo motor
servo1 = PWM(Pin(15))
servo1.freq(50)

# Inicialización de los botones
boton0 = Pin(19, Pin.IN, Pin.PULL_DOWN)
boton60 = Pin(18, Pin.IN, Pin.PULL_DOWN)

# Inicialización de la pantalla LCD
i2c = I2C(id=1, scl=Pin(27), sda=Pin(26), freq=100000)
lcd = I2cLcd(i2c, 0x27, 2, 16)

# Función para mover el servo lentamente al ángulo deseado
def move_servo_slowly(target_angle, duration=1):
    current_angle = servo1.duty_u16()
    target_duty = angle_duty_map.get(target_angle, current_angle)
    step = (target_duty - current_angle) / (duration * 200)  # 200 pasos por segundo

    for _ in range(int(duration * 200)):
        current_angle += step
        servo1.duty_u16(int(current_angle))
        utime.sleep(0.02)  # Esperar 20 ms (50 Hz)

while True:
    if boton0.value() == 1:
        move_servo_slowly(0)
        lcd.clear()
        lcd.move_to(0, 1)
        lcd.putstr('PASE USTED')

    if boton60.value() == 1:
        move_servo_slowly(60)
        lcd.clear()
        lcd.move_to(0, 1)
        lcd.putstr('NO PASAR')

    # Código del sensor ultrasónico
    trig = Pin(16, Pin.OUT)
    echo = Pin(17, Pin.IN)

    trig.value(0)
    utime.sleep_us(5)
    trig.value(1)
    utime.sleep_us(10)
    trig.value(0)
    while echo.value() == 0:
        pass
    Tmrinicio = utime.ticks_us()
    while echo.value() == 1:
        pass
    Tmrfin = utime.ticks_us()
    Duration = utime.ticks_diff(Tmrfin, Tmrinicio)
    distancecm = Duration * 0.0171

    # Mostrar mensaje en la pantalla LCD
    lcd.clear()
    lcd.move_to(0, 1)
    if distancecm <= 30:
        lcd.putstr('NO PASAR')
    else:
        lcd.putstr('PASE USTED')

    utime.sleep(2)