#
# hello.servo.RP2350.py
#    RP2350 servo hello-world
#
# Neil Gershenfeld 8/9/26
#
# This work may be reproduced, modified, distributed,
# performed, and displayed for any purpose, but must
# acknowledge this project. Copyright is retained and
# must be preserved. The work is provided as is; no
# warranty is provided, and users accept all liability.
#
# install MicroPython
#    https://micropython.org/download/RPI_PICO/
#

import machine,time

delay_time = 1e-2 # delay time between steps (s)
pause_time = 1 # pause time betewen steps (s)
servo_pin1 = machine.Pin(0) # GPIO0
servo_pin2 = machine.Pin(1) # GPIO1
max_angle = 180 # maximum angle
min_ns = 0.5e6 # min PWM duty cycle (ns)
max_ns = 2.5e6 # max PWM duty cycle (ns)

servo1 = machine.PWM(servo_pin1,freq=50) # 50 Hz PWM
servo2 = machine.PWM(servo_pin2,freq=50) # 50 Hz PWM

def set_angle(angle,servo):
    if angle < 0: angle = 0
    if angle > max_angle: angle = max_angle    
    ns = int(min_ns+(angle/max_angle)*(max_ns-min_ns))
    servo.duty_ns(ns)

while True:
    #
    # step servo 1 slowly
    #
    for angle in range(0,max_angle):
        set_angle(angle,servo1)
        time.sleep(delay_time)
    #
    # step servo 2 slowly
    #
    for angle in range(0,max_angle):
        set_angle(angle,servo2)
        time.sleep(delay_time)
    #
    # step both quickly
    #
    set_angle(0,servo1)
    set_angle(0,servo2)
    time.sleep(pause_time)
    set_angle(180,servo1)
    set_angle(180,servo2)
    time.sleep(pause_time)
    set_angle(0,servo1)
    set_angle(0,servo2)
    time.sleep(pause_time)