from machine import Pin
from machine import PWM
import utime

MAX_DUTY_CYCLE = 2**16 - 1 #largest unsigned int (65535)
SLEEP_PERIOD_IN_SECONDS = 2
MILLISECONDS_PER_SECOND = 1000
SERVO_PERIOD_IN_MILLISECONDS = 20
SERVO_FREQUENCY = int(MILLISECONDS_PER_SECOND / SERVO_PERIOD_IN_MILLISECONDS)
SERVO_RIGHT_TURN_DUTY = 0.5  #Set duty to 0.5 milliseconds
SERVO_GO_STRAIGHT_DUTY = 1.5 #Set duty to 1.5 milliseconds
SERVO_LEFT_TURN_DUTY = 2.5   #Set duty to 2.5 milliseconds
SERVO_NORTH_EAST_DUTY = 1.0  #Set duty to 1.0 milliseconds
SERVO_NORTH_WEST_DUTY = 2.0  #Set duty to 1.0 milliseconds

servo = PWM(Pin(28))

#Need a 20 millisecond period
#1000 Cycles/Second = 1 Millisecond/Cycle (1/1000)
#50 Cycles/Second = 20 Milliseconds/Cycle (1/50)
servo.freq(SERVO_FREQUENCY)
print("Servo PMW Frequency: " + str(SERVO_FREQUENCY))
print("Servo PWM Period (milliseconds): " + str(SERVO_PERIOD_IN_MILLISECONDS))
print("\n")

while True:
    
    print("Servo is going straight...")
    #percentage of the time the pin is high
    duty_cycle_percentage = (SERVO_GO_STRAIGHT_DUTY/SERVO_PERIOD_IN_MILLISECONDS) * 100 
    duty_cycle = MAX_DUTY_CYCLE * (duty_cycle_percentage/100)
    
    print("Duty (milliseconds): " + str(SERVO_GO_STRAIGHT_DUTY))
    print("Duty/Period percentage: " + str(duty_cycle_percentage))
    print("Duty value (1:65535): " + str(duty_cycle))
    print("\n")

    servo.duty_u16(int(duty_cycle))
    utime.sleep(SLEEP_PERIOD_IN_SECONDS)

    print("Servo is going right...")
    #percentage of the time the pin is high
    duty_cycle_percentage = (SERVO_RIGHT_TURN_DUTY/SERVO_PERIOD_IN_MILLISECONDS) * 100
    duty_cycle = MAX_DUTY_CYCLE * (duty_cycle_percentage/100)
    
    print("Duty (milliseconds): " + str(SERVO_RIGHT_TURN_DUTY))
    print("Duty/Period percentage: " + str(duty_cycle_percentage))
    print("Duty value (1:65535): " + str(duty_cycle))
    print("\n")
    
    servo.duty_u16(int(duty_cycle))
    utime.sleep(SLEEP_PERIOD_IN_SECONDS)
    
    print("Servo is going left...")
    #percentage of the time the pin is high
    duty_cycle_percentage = (SERVO_LEFT_TURN_DUTY/SERVO_PERIOD_IN_MILLISECONDS) * 100
    duty_cycle = MAX_DUTY_CYCLE * (duty_cycle_percentage/100)
    
    print("Duty (milliseconds): " + str(SERVO_LEFT_TURN_DUTY))
    print("Duty/Period percentage: " + str(duty_cycle_percentage))
    print("Duty value (1:65535): " + str(duty_cycle))
    print("\n")
    
    servo.duty_u16(int(duty_cycle))
    utime.sleep(SLEEP_PERIOD_IN_SECONDS)
    
    print("Servo is going NORTHEAST...")
    #percentage of the time the pin is high
    duty_cycle_percentage = (SERVO_NORTH_EAST_DUTY/SERVO_PERIOD_IN_MILLISECONDS) * 100
    duty_cycle = MAX_DUTY_CYCLE * (duty_cycle_percentage/100)
    
    print("Duty (milliseconds): " + str(SERVO_NORTH_EAST_DUTY))
    print("Duty/Period percentage: " + str(duty_cycle_percentage))
    print("Duty value (1:65535): " + str(duty_cycle))
    print("\n")
    
    servo.duty_u16(int(duty_cycle))
    utime.sleep(SLEEP_PERIOD_IN_SECONDS)
    
    print("Servo is going NORTHWEST...")
    #percentage of the time the pin is high
    duty_cycle_percentage = (SERVO_NORTH_WEST_DUTY/SERVO_PERIOD_IN_MILLISECONDS) * 100
    duty_cycle = MAX_DUTY_CYCLE * (duty_cycle_percentage/100)
    
    print("Duty (milliseconds): " + str(SERVO_NORTH_WEST_DUTY))
    print("Duty/Period percentage: " + str(duty_cycle_percentage))
    print("Duty value (1:65535): " + str(duty_cycle))
    print("\n")
    
    servo.duty_u16(int(duty_cycle))
    utime.sleep(SLEEP_PERIOD_IN_SECONDS)