from machine import Pin, PWM, time_pulse_us, Timer
from time import sleep_us, sleep_ms, sleep

#Pines de los sensores de linea
P15=Pin(15, Pin.IN) #sensor Derecho
count=P15
P35=Pin(35, Pin.IN) #sensor atras
count1=P35


#Pines sensor ultrasonico
sonido=340
pulso=10
trig=Pin(18, Pin.OUT)
echo=Pin(22, Pin.IN)

#Pines de los motores
freq=5000
Ena=PWM(Pin(21),freq)
Enb=PWM(Pin(2),freq)
motor_der1=Pin(13, Pin.OUT)
motor_der2=Pin(12, Pin.OUT)
motor_izq1=Pin(14, Pin.OUT)
motor_izq2=Pin(25, Pin.OUT)


#Funciones para los motores
def avanzar():
    for duty_cycle in range(0,1023): #AVANZAR
      motor_der1.on()
      motor_der2.off()
      motor_izq1.on()
      motor_izq2.off()
      Ena.duty(duty_cycle)
      Enb.duty(duty_cycle)
      

def retroceder():
    for duty_cycle in range(0,1023): #RETROCEDER
      motor_der1.off()
      motor_der2.on()
      motor_izq1.off()
      motor_izq2.on()  
      Ena.duty(duty_cycle)
      Enb.duty(duty_cycle)
      

def derecha():
    for duty_cycle in range(0,1023): #DERECHA
      motor_der1.on()
      motor_der2.off()
      motor_izq1.off()
      motor_izq2.on()
      Ena.duty(duty_cycle)
      Enb.duty(int(duty_cycle/2))
      

def izquierda():
    for duty_cycle in range(0,1023): #IZQUIERDA
      motor_der1.off()
      motor_der2.on()
      motor_izq1.on()
      motor_izq2.off()
      Ena.duty(int(duty_cycle/2))
      Enb.duty(duty_cycle)
      

#Funcion sensor ultrasonico    
def sensorultrasonico():
    trig.value(0)
    sleep_us(5)
    trig.value(1)
    sleep_us(pulso)
    trig.value(0)
    duracion=time_pulse_us(echo, 1, 30000)
    distancia_cm=sonido*duracion/20000
    print("distancia: ", distancia_cm, "cm")
    sleep_ms(500)
    if  distancia_cm <= 50:
        avanzar()
    else:
        for i in range(10):
            avanzar()
            sleep_ms(100)
        for i in range(10):
            derecha()
            sleep_ms(100)
        for i in range(10):
            izquierda()
            sleep_ms(100)


#Funcion de los sensores linea
def setLed(pin): #sensor derecho
    if count()== 1:
        retroceder()
        sleep(1)
P15.irq(handler=setLed, trigger=Pin.IRQ_FALLING)    
        
def setLed1(pin): #sensor atras
    if count1()== 1:
        avanzar()
        sleep(1)  
P35.irq(handler=setLed1, trigger=Pin.IRQ_FALLING)                

#Funcion Principal
while True:
    sensorultrasonico()
    sleep(1)