# servo.py
#
# CircuitPython - hobby servo driver
#
# This module provides a class for generating hobby servo PWM control signals.
#
# links to CircuitPython module documentation:
# pwmio   https://circuitpython.readthedocs.io/en/latest/shared-bindings/pwmio/index.html

################################################################################

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

#--------------------------------------------------------------------------------
# Class to represent a single hardware hobby servo.  This wraps up all the
# configuration information and current state into a single object.  The
# creation of the object also initializes the physical pin hardware.

class Servo():
    def __init__(self, pin, pulse_rate=50):
        # Create a PWMOut object on the desired pin to drive the servo.
        # Note that the initial duty cycle of zero generates no pulses, which
        # for many servos will present as a quiescent low-power state.
        self.pwm = pwmio.PWMOut(pin, duty_cycle=0, frequency=pulse_rate)

        # Save the initialization arguments within the object for later reference.
        self.pin = pin
        self.pulse_rate = pulse_rate

        # Initialize the other state variables.
        self.target = None    # target angle; None indicates the "OFF" state
        self.debug = False    # flag to control print output

    def write(self, angle):
        # Object method 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.  As a special case, an angle value of None
        # will turn off the output; many servos will then become backdrivable.

        # calculate the desired pulse width in units of seconds
        if angle is None:
            pulse_width = 0.0
        else:
            pulse_width  = 0.001 + angle * (0.001 / 180.0)

        # calculate the duration in seconds of a single pulse cycle
        cycle_period = 1.0 / self.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
        self.pwm.duty_cycle = min(max(duty_fixed, 0), 65535)

        # save the target value in the object attribute (i.e. variable)
        self.target = angle

        # if requested, print some diagnostics to the console
        if self.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 {self.pwm.duty_cycle}\n")