from machine import Pin, time_pulse_us
import math
import time

led_verde = Pin(12, Pin.OUT)
led_amarelo = Pin(27, Pin.OUT)
led_vermelho = Pin(25, Pin.OUT)
motor = Pin(13, Pin.OUT)

trig = Pin(18, Pin.OUT)
echo = Pin(19, Pin.IN)

def ler_distancia():
    trig.value(0)
    time.sleep_us(5)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    duracao = time_pulse_us(echo, 1, 30000)

    if (duracao < 0):
        return -1
    
    distancia = (duracao / 2) / 29.1
    distancia = math.floor(distancia)
    return distancia

def funcionar(distancia):
    led_verde.off()
    led_amarelo.off()
    led_vermelho.off()
    motor.off()

    if distancia < 70:
        led_verde.on()

    if distancia < 40:
        led_amarelo.on()

    if distancia < 20:
        led_vermelho.on()
        motor.on()

while True:
    distancia = ler_distancia()
    print(f"Distância: {distancia}")
    funcionar(distancia)
    time.sleep_ms(200)