pico-servo-uds.py

gistfile1.txt

from machine import Pin, PWM
from math import floor
from utime import sleep, sleep_us, ticks_us

trig=Pin(17, Pin.OUT)
echo=Pin(16, Pin.IN)
servo = PWM(Pin(18))

servo.freq(50)
duty = 0
servo.duty_u16(duty)
sleep(0.1)

MIN_DUTY = 2500
MAX_DUTY = 8050
MIN_CM = 0
MAX_CM = 165

def cm_to_duty(cm):
    duty_step = ((MAX_DUTY - MIN_DUTY) / MAX_CM)

    if cm > MAX_CM:
        cm = MAX_CM
    elif cm < MIN_CM:
        cm = MIN_CM

    duty = floor((duty_step * cm) + MIN_DUTY)

    if duty > MAX_DUTY:
        duty = MAX_DUTY
    elif duty < MIN_DUTY:
        duty = MIN_DUTY

    return duty

while True: 
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()
    while echo.value() == 0:
        pass
    t1 = ticks_us()
    while echo.value() == 1:
        pass
    t2 = ticks_us()
    cm = (t2 - t1) / 58.0
    duty = cm_to_duty(cm)
    servo.duty_u16(duty)
    sleep(0.1)