# cpb_sine_servo.py

# Demonstrate oscillating movement of a hobby servo using a non-blocking event
# loop structure.  This program style supports operating multiple devices for
# input and output.

# Note that only a tiny micro-servo (e.g. "SG90 9g") is safe to drive directly
# off the board; in general, hobby servos require a separate battery supply.
# The wiring for this is as follows:
#
#  1. The servo control line (typ. orange) connects to SDA A5  
#  2. The servo ground line  (typ. brown)  connects to GND     
#  3. The servo power line   (typ. red)    connects to 6V battery

# Related documentation:
# https://circuitpython.readthedocs.io/en/latest/shared-bindings/pwmio/index.html#module-pwmio
# https://circuitpython.readthedocs.io/projects/motor/en/latest/api.html#module-adafruit_motor.servo

# ----------------------------------------------------------------
# Import standard Python modules.
import time, math

# Import the low-level hardware libraries.
import board
import pwmio

# Import the Adafruit helper library.
from adafruit_motor import servo

# ----------------------------------------------------------------
# Initialize hardware.

# Create a PWMOut object on pad SDA A5 to generate control signals.
pwm = pwmio.PWMOut(board.A5, duty_cycle=0, frequency=50)

# Create a Servo object which controls a hobby servo using the PWMOut.
actuator = servo.Servo(pwm, min_pulse=1000, max_pulse=2000)

# ----------------------------------------------------------------
# Initialize global variables for the main loop.

phase_angle = 0.0
phase_rate  = 2*math.pi / 6.0  # one cycle per six seconds, in radians/second
next_servo_update = time.monotonic_ns()
next_status_update = time.monotonic_ns()

# ----------------------------------------------------------------
# Enter the main event loop.
while True:
    # Read the current integer clock.
    now = time.monotonic_ns()

    # If the time has arrived to update the servo command signal:
    if now >= next_servo_update:
        next_servo_update += 20000000  # 20 msec in nanoseconds (50 Hz update)
        actuator.angle = 90 + 90 * math.sin(phase_angle)
        phase_angle += phase_rate * 0.020

    # If the time has arrived to display the status:
    if now >= next_status_update:
        next_status_update += 500000000  # 0.5 sec in nanoseconds (2 Hz update)
        print(actuator.angle)