from machine import Pin, ADC, PWM, I2C
import time
from ssd1306 import SSD1306_I2C

ledA= Pin(1, Pin.OUT)
ledR= Pin(0, Pin.OUT)
ledV= Pin(2, Pin.OUT)

trig= Pin(5, Pin.OUT)
echo= Pin(6, Pin.IN)
CEstado= Pin(3, Pin.IN, Pin.PULL_UP)
CEstado2= Pin(4, Pin.IN, Pin.PULL_UP)
Grado1= PWM(Pin(19))
Grado2= PWM(Pin(20))
Grado3= PWM(Pin(21))
Grado4= PWM(Pin(22))
Grado5= PWM(Pin(15))
Grado6= PWM(Pin(14))

J1= ADC(Pin(28))
J2= ADC(Pin(26))

WIDTH =128 
HEIGHT= 64
i2c=I2C(0,scl=Pin(17),sda=Pin(16),freq=200000)
oled = SSD1306_I2C(WIDTH,HEIGHT,i2c)
oled.text("Iniciando...", 0, 20)
oled.show()
time.sleep(1)

while True:
    oled.fill(0)

    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    duracion = machine.time_pulse_us(echo, 1, 30000)
    distancia= (duracion*0.0343)/2
    print(distancia)

    CEstado.value()
    CEstado2.value()
    print(CEstado.value())
    print(CEstado2.value())