from machine import Pin, I2C, PWM
import time
from hcsr04 import HCSR04
from lcd_i2c import I2cLcd

# Configure LCD
def init_lcd():
    i2c = I2C(scl=Pin(22), sda=Pin(21))
    lcd = I2cLcd(i2c, 0x27, 2, 16)
    return lcd

 # Get distance
def get_distance(sensor):
    return sensor.distance_cm()

# Display distance in LCD
def display_distance(lcd, distance):
    lcd.clear()
    lcd.putstr("Distance:\n{} cm".format(distance))
    print('Distance:', distance, 'cm')

# Main function
def main():

    lcd = init_lcd()

    # Configure ultrasonic sensor
    TRIGGER_PIN = 13
    ECHO_PIN = 14
    sensor = HCSR04(trigger_pin=TRIGGER_PIN, echo_pin=ECHO_PIN)

    # Configure servo
    SERVO_PIN = 12
    servo = PWM(Pin(SERVO_PIN), freq=50)

    while True:
        distance = get_distance(sensor)

        display_distance(lcd, distance)

        if distance is not None:
            if distance <= 100:
                servo.duty(115)
            else:
                servo.duty(70)
        else:
            servo.duty(0)

        time.sleep(5)

if __name__ == "__main__":
    main()