from machine import Pin, time_pulse_us, PWM
import time

# Sensore ad ultrasuoni
trigger = Pin(15, Pin.OUT)
echo = Pin(14, Pin.IN)

# Servo
servo = PWM(Pin(16))
servo.freq(50)
MIN = 1638   # 0°
MAX = 4915   # 90°

# Bottone
button = Pin(17, Pin.IN, Pin.PULL_DOWN)

# Led
led_verde = Pin(18, Pin.OUT)
led_rosso = Pin(19, Pin.OUT)

servo.duty_u16(MIN)
led_rosso.value(1)
led_verde.value(0)

while True:
    trigger.high()
    time.sleep_us(10)
    trigger.low()
    durata = time_pulse_us(echo, 1, 30000)

    if durata > 0:
        distanza = (durata / 2) / 29.15
        print("La distanza è: ", distanza)

        if distanza <= 10:

            if button.value() == 1:
                
                led_rosso.value(0)
                led_verde.value(1)
                servo.duty_u16(MAX)

                while distanza <= 10:

                    trigger.high()
                    time.sleep_us(10)
                    trigger.low()
                    durata = time_pulse_us(echo, 1, 30000)

                    if durata > 0:
                        distanza = (durata / 2) / 29.15

                time.sleep(1)
                led_rosso.value(1)
                servo.duty_u16(MIN)
                led_verde.value(0)

        else:
            print("Non c'è la presenza di nessun veicolo.")

    time.sleep(0.1)