## Positional Servo Sweep
import machine
import math,time

class Servo:
    def __init__(self,pin_id,min_us=544.0,max_us=2400.0,min_deg=0.0,max_deg=180.0,freq=50):
        self.pwm = machine.PWM(machine.Pin(pin_id))
        self.pwm.freq(freq)
        self.current_us = 0.0
        self._slope = (min_us-max_us)/(math.radians(min_deg)-math.radians(max_deg))
        self._offset = min_us
        
    def write(self,deg):
        self.write_rad(math.radians(deg))

    def read(self):
        return math.degrees(self.read_rad())
        
    def write_rad(self,rad):
        self.write_us(rad*self._slope+self._offset)
    
    def read_rad(self):
        return (self.current_us-self._offset)/self._slope
        
    def write_us(self,us):
        self.current_us=us
        self.pwm.duty_ns(int(self.current_us*1000.0))
    
    def read_us(self):
        return self.current_us

    def off(self):
        self.pwm.duty_ns(0)



my_servo = Servo(pin_id=16)

my_servo_2 = Servo(pin_id=15)

delay_ms = 5  # Amount of milliseconds to wait between servo movements
#delay_ms = 100 - very slow

while True:
    for position in range(0, 180):  # Step the position forward from 0deg to 180deg
        print(position)  # Show the current position in the Shell/Plotter
        my_servo.write(position)  # Set the Servo to the current position
        my_servo_2.write(position+25)
        time.sleep_ms(delay_ms)  # Wait for the servo to make the movement
    
    for position in reversed(range(0, 280)):  # Step the position reverse from 180deg to 0deg
        print(position)  # Show the current position in the Shell/Plotter
        my_servo.write(position)  # Set the Servo to the current position
        time.sleep_ms(delay_ms)  # Wait for the servo to make the movement
        my_servo_2.write(position+25)