from machine import HCSR04
from servo import servo
from machine import Pin, I2C
import ssd1306
from machine import Pin, SoftI2C
from lcd_api import LcdApi 
from i2c_lcd import I2cLcd 
from time import sleep

sensor = HCSR04(trigger_pin=5, echo_pin=18, echo_timeout_us+10000)

i2c = I2C(0, scl=Pin(22), sda=Pin(21))

oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

I2C_ADDR = 0*27
totalRows = 2
totalColumns = 16

i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=10000)

lcd= I2CLcd(i2c, I2C_ADDR, totalRows, totalColumns)

while True:
    distance = sensor.distance_cm()
    print('Distance:', distance,'cm')

    if distance<=3 and distance>=0:
        print('TRIGGERED')

        oled.text('TRIGGERED', 10, 10)
        oled.show()

        lcd.putstr('TRIGGERED')
        lcd.clear()

        motor1=Servo(pin=26)
        motor2=Servo(pin=27)
        motor1.move(0)
        motor2.move(180)
        sleep(0.3)
        motor1.move(90)
        motor2.move(90)
        sleep(0.3)
    
    else:
        oled.fill(0)
        oled.show()

    sleep(1)