# cy-mpi-pico-motor/quad.py
#
# Demonstrates operation of four high-power DC motor using:
#
#  Cytron Maker Pi Pico with Raspberry Pi Pico installed
#  four Cytron MD13S motor driver on Grove connectors 1, 2, 5, 6
#
# The MD13S connected via Grove 1 uses the following I/O:
#   GPIO0       -> PWM
#   GPIO1       -> DIR

# The MD13S connected via Grove 2 uses the following I/O:
#   GPIO2       -> PWM
#   GPIO3       -> DIR

# The MD13S connected via Grove 5 uses the following I/O:
#   GPIO8       -> PWM
#   GPIO9       -> DIR

# The MD13S connected via Grove 6 uses the following I/O:
#   GPIO26      -> PWM
#   GPIO27      -> DIR


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

# load standard Python modules
import math
import time

# load CircuitPython hardware modules
import board
import pwmio
import digitalio

# load the local module representing a single driver
import cytron_md13s

#--------------------------------------------------------------------------------
# Create objects to represent the motor drivers.
print("Creating driver objects.")
driver1 = cytron_md13s.Cytron_MD13S(PWM=board.GP0, DIR=board.GP1)
driver2 = cytron_md13s.Cytron_MD13S(PWM=board.GP2, DIR=board.GP3)
driver3 = cytron_md13s.Cytron_MD13S(PWM=board.GP8, DIR=board.GP9)
driver4 = cytron_md13s.Cytron_MD13S(PWM=board.GP26, DIR=board.GP27)

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

print("Starting main script.")

# initial pause
time.sleep(2.0)

while True:
    print("Starting motor test.")
    for driver in [driver1, driver2, driver3, driver4]:
        driver.write(1.0) # full forward
        time.sleep(2.0)
        driver.write(0.0) # off
        time.sleep(2.0)
        driver.write(-1.0) # full reverse
        time.sleep(2.0)
        driver.write(0.0) # off

    print("Ramp test.")
    for driver in [driver1, driver2, driver3, driver4]:
        for i in range(10):
            driver.write(i*0.1)
            time.sleep(0.5)
        driver.write(0.0)
        time.sleep(2.0)
