from time import sleep
from machine import SoftI2C, Pin, PWM
from i2c_lcd import I2cLcd
from hcsr04 import HCSR04

# Define pins for I2C
i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=400000)

# Initialize the LCD
lcd = I2cLcd(i2c, 0x27, 2, 16)

# Initialize the HC-SR04 ultrasonic sensor
ultrasonic = HCSR04(trigger_pin=Pin(19), echo_pin=Pin(18))

# Initialize the servo
servo = PWM(Pin(23), freq=50)

def move_servo(angle):
    # Map the angle to PWM duty cycle (50 - 125)
    duty_cycle = int(50 + angle / 180 * 75)
    servo.duty(duty_cycle)

try:
    while True:
        # Read distance
        distance = ultrasonic.distance_cm()

        # Display distance on LCD
        lcd.move_to(0, 0)
        lcd.putstr('Distance: ')
        lcd.move_to(0, 1)
        lcd.putstr('%.2f cm' % distance)

        # Control servo based on distance
        if distance < 100:  # If object is very close
            move_servo(0)  # Open the servo
        else:
            move_servo(90)   # Close the servo

        sleep(1)

except KeyboardInterrupt:
    pass