from machine import Pin, PWM
import time

# ----------- Capteur Ultrasonic -----------
trig = Pin(12, Pin.OUT)
echo = Pin(14, Pin.IN)
# ----------- Servos -----------
servo = PWM(Pin(13), freq=50)

# ----------- Fonctions -----------

def set_angle(servo, angle):
    duty = int((angle / 180) * 102 + 26)
    servo.duty(duty)

def get_distance():
    # reset trig
    trig.off()
    time.sleep_us(2)

    # send pulse
    trig.on()
    time.sleep_us(10)
    trig.off()

    # attendre signal
    while echo.value() == 0:
        pass
    start = time.ticks_us()

    while echo.value() == 1:
        pass
    end = time.ticks_us()

    # calcul distance
    duration = time.ticks_diff(end, start)
    distance = duration / 58
    return distance

# ----------- Loop principale -----------

while True:
    distance = get_distance()
    print("Distance:", distance, "cm")

    if distance < 2:
        set_angle(servo, 0)
    else:        
        set_angle(servo, 90)