# cy-mpi-pico-motor/code.py
#
# Demonstrates operation of one high-power DC motor using:
#
#  Cytron Maker Pi Pico with Raspberry Pi Pico installed
#  Cytron MD13S motor driver on Grove 1 connector
#
# The MD13S connected via Grove 1 uses the following I/O:
#   GPIO0       -> PWM
#   GPIO1       -> DIR
#
# If the motor includes a quadrature encoder, it can be connected to Grove 2:
#   GPIO2       <- A
#   GPIO3       <- B

################################################################################
# print a banner as reminder of what code is loaded
print("Starting cy-mpi-pico-motor script.")

# load standard Python modules
import math
import time

# load CircuitPython hardware modules
import board
import pwmio
import digitalio
import rotaryio

#--------------------------------------------------------------------------------
# Class to represent a single H-bridge driver.

class Cytron_MD13S():
    def __init__(self, PWM=board.GP0, DIR=board.GP1, pwm_rate=20000):

        self.pwm_pin = pwmio.PWMOut(PWM, duty_cycle=0, frequency=pwm_rate)
        self.dir_pin = digitalio.DigitalInOut(DIR)
        self.dir_pin.direction = digitalio.Direction.OUTPUT
        return

    def write(self, rate, verbose=True):
        """Set the speed and direction of the motor drive.

        :param rate: modulation value between -1.0 and 1.0, full reverse to full forward."""

        # convert the rate into a 16-bit fixed point integer
        pwm = min(max(int(2**16 * abs(rate)), 0), 65535)

        self.pwm_pin.duty_cycle = pwm

        # this defines 'forward' as a CCW rotation as viewed from the shaft end
        # (i.e., shaft right-hand rule), if the red motor lead is attached to MA
        # and the black motor lead to MB.
        self.dir_pin.value = (rate < 0)

        if verbose:
            print("Setting motor driver:", rate)
            print("Current position:", encoder.position)

        return

#--------------------------------------------------------------------------------
# Create an object to represent the motor driver.
print("Creating driver objects.")
driver = Cytron_MD13S()

# Create an object to represent a quadrature encoder.  The A/B signals are
# swapped for this particular motor so the positive direction is CCW, same as
# the driver code.
encoder = rotaryio.IncrementalEncoder(board.GP3, board.GP2, divisor=1)

#--------------------------------------------------------------------------------
# Begin the main processing loop.

print("Starting main script.")

# initial pause
time.sleep(2.0)

while True:
    print("Starting motor test.")
    driver.write(1.0)
    time.sleep(2.0)

    driver.write(0.0)
    time.sleep(2.0)

    driver.write(-1.0)
    time.sleep(2.0)

    driver.write(0.0)
    time.sleep(2.0)

    driver.write(0.5)
    time.sleep(2.0)

    driver.write(-0.5)
    time.sleep(2.0)

    print("Ramp test.")

    for i in range(10):
        driver.write(i*0.1)
        time.sleep(0.5)

    driver.write(0.0)
    time.sleep(2.0)

    for i in range(10):
        driver.write(-i*0.1)
        time.sleep(0.5)

    driver.write(0.0)
    time.sleep(2.0)

    if True:
        print("Closed-loop test.")
        target_position = 10000
        while True:
            position_error = target_position - encoder.position
            if abs(position_error) < 10:
                break
            # the gain is chosen for a modest linear region before saturation in either direction
            motor_command = 0.004 * position_error
            driver.write(motor_command)

    driver.write(0.0)
    time.sleep(2.0)