'Bastón'
from machine import Pin, PWM, time_pulse_us
from time import sleep, sleep_us

# Componentes
motor = PWM(Pin(12), freq=1000)
servo = PWM(Pin(25), freq=50)
led = Pin(13, Pin.OUT)
trig = Pin(32, Pin.OUT)
echo = Pin(33, Pin.IN)
boton = Pin(14, Pin.IN, Pin.PULL_DOWN)

sistemaActivo = 0
ultimo_angulo = 0

def angle_servo(angle):
    maximum = 128
    minimum = 26
    dutyC = int(minimum + (angle / 180) * (maximum - minimum))
    servo.duty(dutyC)

def medirDistancia():
    trig.off()
    sleep_us(2)
    trig.on()
    sleep_us(10)
    trig.off()
    duracion = time_pulse_us(echo, 1, 30000)
    distancia = duracion * 0.0343 / 2
    return distancia

while True:
    if boton.value() == 1 and sistemaActivo == 0:
        while boton.value() == 1:
            pass 
        sistemaActivo = 1
        print("Sistema Activo")

    if sistemaActivo == 1 and boton.value() == 0:
        angle_servo(180)
        ultimo_angulo = 180
        motor.duty(1023)
        distancia = medirDistancia()
        print("Distancia:", int(distancia), "cm")
        if distancia <= 10 and ultimo_angulo != 0:
            angle_servo(0)
            ultimo_angulo = 0
            led.on()
            sleep(2)
        elif distancia > 10 and ultimo_angulo != 180:
            angle_servo(180)
            ultimo_angulo = 180
            led.off()

    elif sistemaActivo == 1 and boton.value() == 1:
        while boton.value() == 1:
            pass 
        sistemaActivo = 0
        print("Sistema Inactivo")
        led.off()
        angle_servo(0)
        ultimo_angulo = 0
        motor.duty(0)

    sleep(0.5)