from machine import Pin, PWM, SoftI2C
from ultrasonic import HCSR04
from time import sleep
import oled

ultra = HCSR04(19, 18)
led = Pin(17, Pin.OUT)
# buzz = PWM(Pin(16, Pin.OUT), 50)

i2c_1 = SoftI2C(scl=Pin(26), sda=Pin(27))
dis_oled = oled.I2C(128, 64, i2c_1, addr=0x3C)

warn_oled = oled.I2C(128, 64, i2c_1, addr=0x3D)

print("Program started.")
while True:
    dis_oled.clear()
    dis_oled.text("Dis: " + str(ultra.get_distance_cm()) + " cm.", 10, 3)
    dis_oled.show()

    if ultra.get_distance_cm() < 70:
        led.on()
        # buzz.duty(555)
        print("Bzz Bzz")
        warn_oled.clear()
        warn_oled.text("Warning!", 10, 3)
        warn_oled.show()
    else:
        led.off()
        # buzz.duty(0)
        warn_oled.clear()
        warn_oled.text("Path is clear.", 10, 3)
        warn_oled.show()

    sleep(0.5)