# El objetivo del programa es controlar un servomotor con dos modos,
# uno manual con un potenciometro, y otro automatico con barridos
# 
#
#
#--------------------------MICROPYTHON----------------------------------
#
#
#
#
# Hecho por Rodrigo González Quidiello

from machine import PWM,ADC,Pin,time_pulse_us
import utime
import _thread


servo=PWM(Pin(0))
servo.freq(50)
adc=ADC(2)

trigger = Pin(3,Pin.OUT)
echo = Pin(5,Pin.IN)


led_rojo = Pin(16, Pin.OUT)
led_azul = Pin(17, Pin.OUT)

manual = Pin(14, Pin.IN, Pin.PULL_DOWN)
auto = Pin(13, Pin.IN, Pin.PULL_DOWN)

contador = 0

def Mover_servo():
    pot=int(adc.read_u16()*180/65535)
    ton=(pot+45)*100000/9
    servo.duty_ns(int(ton))
    utime.sleep_ms(50)
    
def Hacer_barridos():
    num_barridos = int(input('Elija el numero de barridos que desee hacer: '))
    for i in range(num_barridos):
        for i in range(500000,2500000,1000):
            servo.duty_ns(i)
            utime.sleep_ms(1)
            if i == 2499000:
                for i in range (2500000, 500000, -1000):
                    servo.duty_ns(i)
                    utime.sleep_ms(1)
                    
def Hilo():
    trigger.high()
    utime.sleep(0.0001)
    trigger.low()
    duracion = time_pulse_us(echo, Pin.high)    
    distancia = (duracion*0.0343)/2
    print('Distancia',distancia,'cm')
    if distancia < 20:
        print('Posible enemigo!!!')
        utime.slee_ms(500)


                
def Modo_manual(pin):
    global contador
    contador = 1
    
    
def Modo_automatico(pins):
    global contador
    contador = 2

manual.irq(Modo_manual, Pin.IRQ_FALLING,2)
auto.irq(Modo_automatico, Pin.IRQ_FALLING,1)    

led_rojo.off()
led_azul.off()

_thread.start_new_thread(Hilo,())

while True:
    while contador == 1:
        led_rojo.on()
        led_azul.off()
        Mover_servo()
    while contador == 2:
        led_azul.on()
        led_rojo.off()
        Hacer_barridos()