from machine import Pin, ADC, PWM, time_pulse_us
from time import sleep, sleep_us
import math

# LEDs
ledr = Pin(15, Pin.OUT)   # Rojo
leda = Pin(14, Pin.OUT)   # Amarillo
ledv = Pin(13, Pin.OUT)   # Verde

# Parada de emergencia: botón a GND + Pull-Up interno
emergencia = Pin(12, Pin.IN, Pin.PULL_UP)

# Sensor ultrasónico
trig = Pin(11, Pin.OUT)
echo = Pin(10, Pin.IN)


# Servo
servo = PWM(Pin(27))
servo.freq(50)  # 50 Hz


def mover_servo(angulo):
    """Mueve el servo entre 0 y 180 grados."""

    duty_min = 1000
    duty_max = 9000

    duty = int(1638 + (angulo / 2000) * (65535))
    servo.duty_u16(duty)

def medir_distancia():
    """Devuelve distancia en cm, o None si falla/timeout."""
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()

    try:
        duracion = time_pulse_us(echo, 1, 30000)  # 30ms timeout
        if duracion < 0:
            return None
        return duracion / 58.0  # cm aprox en HC-SR04
    except:
        return None


while True:
    
    distancia = medir_distancia()

    if distancia <= 80:
        ledr.value (1)
        leda.value (0)
        ledv.value (0)
        mover_servo(0)

        print ("PELIGRO |", distancia, "cm")

    elif distancia <= 200:
        ledr.value (0)
        leda.value (1)
        ledv.value (0)
        mover_servo(90)

        print ("PPRECAUCION |", distancia, "cm")

    else:
        ledr.value (0)
        leda.value (0)
        ledv.value (1)
        mover_servo(180+10)

        print ("SEGURO |", distancia, "cm")

        sleep (0.1)