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

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

# Inicializa los pines del sensor ultrasónico
trig = Pin(16, Pin.OUT)
echo = Pin(17, Pin.IN)

# Inicializa el servomotor
servo_pin = Pin(18)
servo = PWM(servo_pin)
servo.freq(50)  # Configura la frecuencia del servo a 50 Hz

# Define los ángulos del servo para las posiciones elevada y bajada
ANGULO_SERVO_ELEVADO = 45
ANGULO_SERVO_BAJADO = 0

def medir_distancia():
    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()
    Duracion = utime.ticks_diff(Tmrfin, Tmrinicio)
    distancia_cm = Duracion * 0.0171
    return distancia_cm

def mover_servo(angulo):
    # Convierte el ángulo a ciclo de trabajo (0 a 65535)
    ciclo_de_trabajo = int((angulo / 180) * 65535)
    servo.duty_u16(ciclo_de_trabajo)

while True:
    distancia = medir_distancia()
    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr('Feliz viaje: ' + str(int(distancia)) + " cm")  # Muestra la distancia sin la parte decimal

    if distancia < 15:
        # El objeto está cerca, eleva el servo
        mover_servo(ANGULO_SERVO_ELEVADO)
    else:
        # El objeto está lejos, baja el servo
        mover_servo(ANGULO_SERVO_BAJADO)

    utime.sleep(2)