import time
from machine import Pin, ADC, PWM, I2C, SoftI2C
from pico_i2c_lcd import I2cLcd


i2c = SoftI2C(sda=Pin(14), scl=Pin(13), freq=400000)
lcd_address = i2c.scan()[0]
lcd = I2cLcd(i2c, lcd_address, 2, 16) 




verticle = ADC(26)
select = Pin(22, Pin.IN)


motor = PWM(Pin(19))
motor.freq(50)

position = 4500

trig = Pin(1, Pin.OUT)
echo = Pin(0, Pin.IN)


def action():
    trig.low()
    time.sleep_us(2)
    trig.high()
    time.sleep_us(10)
    trig.low()
    time_off = 0
    while echo.value()==0:
        time_off = time.ticks_us()
    while echo.value()==1:
        time_on = time.ticks_us()
    return ((time_on-time_off)*0.0343)/2

while True:
    distance = action()
    lcd.clear()
    lcd.move_to(0,0)
    lcd.putstr("Distance:-")
    lcd.move_to(0,1)
    lcd.putstr(f"{distance} cm")

    if distance <= 100:
        motor.duty_u16(2000)
        print("car came")
        time.sleep(2)
        print("car out")
        motor.duty_u16(4750)
    # if select.value():
    #     position=5000
    #     motor.duty_u16(position)
    #     print("check")
    while verticle.read_u16()<32759 and position<4750:
        position+=1
        motor.duty_u16(position)
    while verticle.read_u16()>32759 and position>1001:
        position-=1
        motor.duty_u16(position)
    print(verticle.read_u16(),position)
    time.sleep(1)