# servo_step.py
#
# Raspberry Pi Pico - hobby servo motion demo
#
# Demonstrates stepping a hobby servo back and forth.
#
# This assumes a tiny 9G servo has been wired up to the Pico as follows:
#   Pico pin 40 (VBUS)  -> servo red   (+5V)
#   Pico pin 38 (GND)   -> servo brown (GND)
#   Pico pin 1  (GP0)   -> servo orange (SIG)

# links to CircuitPython module documentation:
# time    https://circuitpython.readthedocs.io/en/latest/shared-bindings/time/index.html
# math    https://circuitpython.readthedocs.io/en/latest/shared-bindings/math/index.html
# board   https://circuitpython.readthedocs.io/en/latest/shared-bindings/board/index.html
# pwmio   https://circuitpython.readthedocs.io/en/latest/shared-bindings/pwmio/index.html

################################################################################
# load standard Python modules
import math, time

# load the CircuitPython hardware definition module for pin definitions
import board

# load the CircuitPython pulse-width-modulation module for driving hardware
import pwmio

#--------------------------------------------------------------------------------
# Define a function to issue a servo command by updating the PWM signal output.
# This function maps an angle specified in degrees between 0 and 180 to a servo
# command pulse width between 1 and 2 milliseconds, and then to the
# corresponding duty cycle fraction, specified as a 16-bit fixed-point integer.

def servo_write(servo, angle, debug=False):
    # calculate the desired pulse width in units of seconds
    pulse_width  = 0.001 + angle * (0.001 / 180.0)

    # fetch the current pulse repetition rate from the hardware driver
    pulse_rate = servo.frequency
    
    # calculate the duration in seconds of a single pulse cycle
    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
    servo.duty_cycle = min(max(duty_fixed, 0), 65535)

    # print some diagnostics to the console
    if debug:
        print(f"Driving servo to angle {angle}")
        print(f" Pulse width {pulse_width} seconds")
        print(f" Duty cycle {duty_cycle}")
        print(f" Command value {servo.duty_cycle}\n")

#--------------------------------------------------------------------------------
# Create a PWMOut object on Pin GP0 to drive the servo. The frequency argument
# specifies the pulse repetition rate in Hz (pulses per second).

servo = pwmio.PWMOut(board.GP0, duty_cycle=0, frequency=50)

# Begin the main processing loop.
while True:
    servo_write(servo, 0.0, debug=True)
    time.sleep(2.0)

    servo_write(servo, 180.0, debug=True)
    time.sleep(2.0)