#
# hello.servo.RP2350.serial.py
#    RP2350 servo hello-world, serial port version
#
# 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,sys

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:
    angles = input('angle1 angle2?').split() #float(input(''))
    if (len(angles) == 2):
        angle1 = float(angles[0])
        angle2 = float(angles[1])
        set_angle(angle1,servo1)
        set_angle(angle2,servo2)