from machine import Pin, time_pulse_us
from time import sleep, sleep_ms, sleep_us
from daner import *

ec = Pin(13, Pin.IN)
tr = Pin(11, Pin.OUT)
e = Pin(9, Pin.IN)
t = Pin(5, Pin.OUT)
trg = Pin(6, Pin.OUT)
ech = Pin(8, Pin.IN)
sensor_esq = Pin(14, Pin.IN, Pin.PULL_UP)
sensor_dir = Pin(15, Pin.IN, Pin.PULL_UP)


motor_esq = Pin(2, Pin.OUT)
motor_dir = Pin(3, Pin.OUT)

def frente():
    motor_esq.value(1)
    motor_dir.value(1)

def parar():
    motor_esq.value(0)
    motor_dir.value(0)

def virar_esquerda():
    motor_esq.value(0)
    motor_dir.value(1)

def virar_direita():
    motor_esq.value(1)
    motor_dir.value(0)

while True:

    d = mede_hc_sr04(trg, ech)
    f = mede_hc_sr04(t, e) 
    es = mede_hc_sr04(tr, ec)   

    if d <= 6.0 or f <= 6.0 or es <= 6.0:
        parar()
        print("Tem um objeto na sua frente, faça um desvio")
    else:
        print("Não há nada em sua frente, siga ligado")
    
    
    esq = sensor_esq.value()
    dir = sensor_dir.value()

    if esq == 1 and dir == 1:
        frente()
    elif esq == 0 and dir == 1:
        virar_esquerda()
    elif esq == 1 and dir == 0:
        virar_direita()
    elif esq == 0 and dir == 0:
        parar()

    if d <= 6.0:
        virar_esquerda()
    elif f <= 6.0:
        virar_direita() or virar_esquerda()
    elif es <= 6.0:
        virar_direita()
    else:
        frente()

    sleep(0.1)

#LEMBRAR QUE DEVO SINCRONIZAR A AÇÃO DO LED/SERVOMOTOR AO FOTORESISTOR (LDR)

#CORTE DO LDR É 47.5 LUX!!!!!!!