# https://courses.ideate.cmu.edu/16-223/f2021/text/code/pico-servo.html

from machine import Pin, PWM;
import math, time;

# =============================================================================
# Map an angle specified in degrees between 0 and 180 
# to a servo command pulse width between 1 and 2 milliseconds, 
# and to the duty cycle fraction (as a 16-bit integer).

def angle_to_duty(angle):
    # Try Arduino Servo lib approach
    #MIN_PULSE_WIDTH = 544.0 / 1000000.0;  # the shortest pulse sent to a servo  
    #MAX_PULSE_WIDTH = 2400.0 / 1000000.0; # the longest pulse sent to a servo 

    # calculate the desired pulse width in units of seconds
    #pulse_width = 0.001 + angle * (0.002 / 180.0); # Max set to 0.002?
    pulse_width = 0.001 + angle * (0.001 / 180.0);
    #pulse_width = MIN_PULSE_WIDTH + angle * (MAX_PULSE_WIDTH / 180.0);
    #print("Angle: {}, Pulse width sent: {}".format(angle, pulse_width));

    # calculate the duration in seconds of a single pulse cycle
    # pulse_rate=50;
    pulse_rate = servo.freq();
    cycle_period = 1.0 / pulse_rate;

    # calculate the desired ratio of pulse ON time to cycle duration
    duty_cycle = pulse_width / cycle_period; 

    # convert the ratio into a 16-bit fixed point integer
    duty_fixed = int(2**16 * duty_cycle);

    # limit the ratio range and apply to the hardware driver
    return min(max(duty_fixed, 0), 65535);

# =============================================================================
# Init PWM object for the servo
servo_pin = Pin(28, Pin.OUT);
servo = PWM(Pin(28));
servo.freq(50);

# =============================================================================
if (__name__ == "__main__"):

    while True:
        servo.duty_u16( angle_to_duty(0.0) );
        print("Servo: freq = {}, duty = {}".format(servo.freq(), servo.duty_u16()));
        time.sleep(2.0);

        servo.duty_u16( angle_to_duty(180.0) );
        print("Servo: freq = {}, duty = {}".format(servo.freq(), servo.duty_u16()));
        time.sleep(2.0);

# Arduino Servo lib:
# https://github.com/arduino-libraries/Servo/blob/master/src/Servo.h
# https://github.com/arduino-libraries/Servo/blob/master/src/mbed/Servo.cpp