from machine import Pin, I2C, PWM, time_pulse_us
from pico_i2c_lcd import I2cLcd
import time

servo = PWM(Pin(15))
servo.freq(50)

def set_angle(angle):
    duty = int(1638 + (angle/180)*819)
    servo.duty_u16(duty)

# LCD
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
addr = i2c.scan()[0]
lcd = I2cLcd(i2c, addr, 4, 20)

# Ultrasonic
trig = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)





def get_distance():
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()
    duration = time_pulse_us(echo, 1)
    distance = duration * 0.0343 / 2
    return distance
while True:
    dist = get_distance()
    lcd.clear()
    lcd.move_to(0,0)
    lcd.putstr("Distance:")
    lcd.move_to(10,0)
    lcd.putstr(str(round(dist,1))+" cm")
    lcd.move_to(0,1)
    if dist <= 10:
        lcd.putstr("Place: OCCUPIED")
        set_angle(180)
    else:
        lcd.putstr("Place: EMPTY")
        set_angle(90)
    time.sleep(1)