# dual_spin.py
#
# Raspberry Pi Pico - DC motor motion demo
#
# Demonstrates operating two DC motors driven by a DRV8833.
#
# This assumes a Pololu DRV8833 dual motor driver has been wired up to the Pico as follows:
#   Pico pin 24, GPIO18   -> AIN1
#   Pico pin 25, GPIO19   -> AIN2
#   Pico pin 26, GPIO20   -> BIN2
#   Pico pin 27, GPIO21   -> BIN1
#   any Pico GND          -> GND

# DRV8833 carrier board: https://www.pololu.com/product/2130

################################################################
# 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

################################################################################
# print a banner as reminder of what code is loaded
print("Starting dual_spin script.")

# 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

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

class DRV8833():
    def __init__(self, AIN1=board.GP18, AIN2=board.GP19, BIN2=board.GP20, BIN1=board.GP21, pwm_rate=20000):
        # Create a pair of PWMOut objects for each motor channel.
        self.ain1 = pwmio.PWMOut(AIN1, duty_cycle=0, frequency=pwm_rate)
        self.ain2 = pwmio.PWMOut(AIN2, duty_cycle=0, frequency=pwm_rate)

        self.bin1 = pwmio.PWMOut(BIN1, duty_cycle=0, frequency=pwm_rate)
        self.bin2 = pwmio.PWMOut(BIN2, duty_cycle=0, frequency=pwm_rate)

    def write(self, channel, rate):
        """Set the speed and direction on a single motor channel.

        :param channel:  0 for motor A, 1 for motor B
        :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)

        if channel == 0:
            if rate < 0:
                self.ain1.duty_cycle = pwm
                self.ain2.duty_cycle = 0
            else:
                self.ain1.duty_cycle = 0
                self.ain2.duty_cycle = pwm
        else:
            if rate < 0:
                self.bin1.duty_cycle = pwm
                self.bin2.duty_cycle = 0
            else:
                self.bin1.duty_cycle = 0
                self.bin2.duty_cycle = pwm


#--------------------------------------------------------------------------------
# Create an object to represent a dual motor driver.
print("Creating driver object.")
driver = DRV8833()

#--------------------------------------------------------------------------------
# Begin the main processing loop.  This is structured as a looping script, since
# each movement primitive 'blocks', i.e. doesn't return until the action is
# finished.

print("Starting main script.")
while True:
    # initial pause
    time.sleep(2.0)

    print("Testing motor A.")
    driver.write(0, 1.0)
    time.sleep(2.0)

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

    driver.write(0, -1.0)
    time.sleep(2.0)
    
    driver.write(0, 0.0)
    time.sleep(2.0)

    print("Testing motor B.")    
    driver.write(1, 1.0)
    time.sleep(2.0)

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

    driver.write(1, -1.0)
    time.sleep(2.0)
    
    driver.write(1, 0.0)
    time.sleep(2.0)

    print("Ramp test.")

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

    driver.write(0, 0.0)
    driver.write(1, 0.0)
    time.sleep(2.0)
        
    for i in range(10):
        driver.write(0, -i*0.1)
        driver.write(1, -i*0.1)
        time.sleep(0.5)
        
    driver.write(0, 0.0)
    driver.write(1, 0.0)
    time.sleep(2.0)