from machine import Pin, I2C
import ssd1306
from HCSR04 import HCSR04
from time import sleep

# ESP32 Pin assignment 
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
sensor = HCSR04(trigger_pin= 25, echo_pin=26)
ledr= Pin(32, Pin.OUT)
leda= Pin(33, Pin.OUT)

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

while True:
    #oled.text("hello world",10, 10)
    #oled.show()
    distancia = sensor.distance_cm()
    print(distancia)
    #oled.text("distancia",10, 10)
    #oled.show()
    if distancia <200:
        oled.fill(0)
        leda.on()
        oled.text("azul",10, 10)
        sleep(0.5)
        oled.show()
        leda.off()
    elif distancia>200:
        oled.fill(0)
        ledr.on()
        oled.text("rojo",10, 10)
        sleep(0.5)
        oled.show()
        ledr.off()