from machine import Pin, PWM
import utime

class Servo():
    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
    # For some reason 2.5 milliseconds was not moving the servo at all on Wokwi --
    # Setting it to 2.48 did the trick though
    SERVO_LEFT_TURN_DUTY = 2.48  #Set duty to 2.48 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
    
    def __init__(self, pin, frequency=50):
        self.pwm_pin = PWM(pin)
        self.pwm_pin.freq(frequency)
        self.duty_cycle = 0
        
    def set_position_by_duty_cycle(self, duty_cycle_period):
        self.duty_cycle_period = duty_cycle_period
        
        self.duty_cycle_percentage = (duty_cycle_period/Servo.SERVO_PERIOD_IN_MILLISECONDS) * 100 
        self.duty_cycle = Servo.MAX_DUTY_CYCLE * (self.duty_cycle_percentage/100)
    
        self.pwm_pin.duty_u16(int(self.duty_cycle))
    
    def set_position_by_angle(self, angle):
        # Divide angle by 90 and add 0.48 to get the equivalent duty cycle period
        # (i.e. (90 / 90) + 0.48 = 1.48 {practically equivalent to SERVO_GO_STRAIGHT_DUTY}
        #  and (180 / 90) + 0.48 = 2.48 {same as SERVO_LEFT_TURN_DUTY})
        self.duty_cycle_period = (angle / 90) + 0.48
        
        self.duty_cycle_percentage = (self.duty_cycle_period/Servo.SERVO_PERIOD_IN_MILLISECONDS) * 100 
        self.duty_cycle = Servo.MAX_DUTY_CYCLE * (self.duty_cycle_percentage/100)
    
        self.pwm_pin.duty_u16(int(self.duty_cycle))
    
    def print_settings(self):
        print("Servo PMW Frequency: " + str(Servo.SERVO_FREQUENCY))
        print("Servo PWM Period (milliseconds): " + str(Servo.SERVO_PERIOD_IN_MILLISECONDS))
        print("Duty (milliseconds): " + str(self.duty_cycle_period))
        print("Duty/Period percentage: " + str(self.duty_cycle_percentage))
        print("Duty value (1:65535): " + str(self.duty_cycle))
        print("\n")
        utime.sleep(Servo.SLEEP_PERIOD_IN_SECONDS)

#END_CLASS     

#Need a 20 millisecond period
#1000 Cycles/Second = 1 Millisecond/Cycle (1/1000)
#50 Cycles/Second = 20 Milliseconds/Cycle (1/50)
servo = Servo(Pin(16), Servo.SERVO_FREQUENCY)

while True:
    # Default behavior
    print("Servo is going straight...")   
    servo.set_position_by_duty_cycle(Servo.SERVO_GO_STRAIGHT_DUTY)
    servo.print_settings()

    print("Servo is going right...")   
    servo.set_position_by_duty_cycle(Servo.SERVO_RIGHT_TURN_DUTY)
    servo.print_settings()

    print("Servo is going left...")   
    servo.set_position_by_duty_cycle(Servo.SERVO_LEFT_TURN_DUTY)
    servo.print_settings()

    # Test set_position_by_angle

    # This should set the servo to the same position as SERVO_RIGHT_TURN_DUTY did
    print("Servo is going 0 degrees...")   
    servo.set_position_by_angle(0)
    servo.print_settings()

    # Test angle not represented in the preset duties
    print("Servo is going 60 degrees...")   
    servo.set_position_by_angle(60)
    servo.print_settings()

    # This should set the servo to the same position as SERVO_GO_STRAIGHT_DUTY did
    print("Servo is going 90 degrees...")   
    servo.set_position_by_angle(90)
    servo.print_settings()

    # Test another angle not represented in the preset duties
    print("Servo is going 128 degrees...")   
    servo.set_position_by_angle(128)
    servo.print_settings()

    # This should set the servo to the same position as SERVO_LEFT_TURN_DUTY did
    print("Servo is going 180 degrees...")   
    servo.set_position_by_angle(180)
    servo.print_settings()