from machine import Pin, PWM
from hcsr04 import HCSR04
import time

# Setup sensore e servo
sensore = HCSR04(trigger_pin=4, echo_pin=5)
servo = PWM(Pin(9), freq=50)  # Servo sul pin 9, freq 50Hz
# Parametri
DIST_MIN = 10      # Distanza minima (cm) = 0 gradi
DIST_MAX = 200     # Distanza massima (cm) = 180 gradi
def set_angolo(angolo):
    # Il servo standard risponde a questi valori di duty
    min_duty = 1638    # 0.5ms/20ms * 65535
    max_duty = 8192    # 2.5ms/20ms * 65535
    duty = min_duty + (max_duty - min_duty) * (angolo / 180)
    servo.duty_u16(int(duty))
while True:
    distanza = sensore.measure_distance_cm()
    if distanza is not None:
        # Limita la distanza al range definito
        if distanza > DIST_MAX:
            distanza = DIST_MAX
        if distanza < DIST_MIN:
            distanza = DIST_MIN
        # Converte la distanza in angolo (0-180)
        angolo = 180 - ((distanza - DIST_MIN) * 180 / (DIST_MAX - DIST_MIN))
        
        # Muove il servo
        set_angolo(angolo)
    time.sleep(0.1)