from time import sleep
from machine import Pin
from machine import PWM

pwm = PWM(Pin(7))
pwm.freq(50)


pwm2 = PWM(Pin(11))
pwm2.freq(50)

'''
PWM - duty set a 0 - 1023 (512 is 50% duty cycle)
Hobby servo motors require a frequency of 50Hz and then a duty cycle that
    relates the servo angle to the pulse width.
    1 ms = 0 degrees
    2 ms = 180 degress
'''
MIN0 =  500000  # Needed for MG995
MIN  = 1000000
MID  = 1500000
MAX  = 2000000
MAX2 = 2500000  # Needed for MG995

degree_interval = int(1000000/180/2)

while True:
    for pos in range(MIN, MAX, degree_interval):
        pwm.duty_ns(pos)
        sleep(0.02)
    for pos in range(MAX, MIN, -1*degree_interval):
        pwm.duty_ns(pos)
        sleep(0.02)