# tb6612.py
#
# Raspberry Pi Pico - TB6612 dual H-bridge motor driver support
#
# This module provides a class for controlling a TB6612 dual H-bridge DC motor driver.
# This device can drive two low-power DC motors bidirectionally with variable speed.
#
# A typical usage requires six digital outputs.  The defaults assumes a Pololu
# TB6612 dual motor driver has been wired up to the Pico as follows:

#   Pico pin 11, GPIO8    -> PWMA
#   Pico pin 12, GPIO9    -> AIN2
#   Pico pin 14, GPIO10   -> AIN1
#
#   Pico pin 15, GPIO11   -> BIN1
#   Pico pin 16, GPIO12   -> BIN2
#   Pico pin 17, GPIO13   -> PWMB
#
#   any Pico GND          -> GND

# TB6612 carrier board: https://www.pololu.com/product/713

################################################################
# CircuitPython module documentation:
# time      https://docs.circuitpython.org/en/latest/shared-bindings/time/index.html
# math      https://docs.circuitpython.org/en/latest/shared-bindings/math/index.html
# board     https://docs.circuitpython.org/en/latest/shared-bindings/board/index.html
# pwmio     https://docs.circuitpython.org/en/latest/shared-bindings/pwmio/index.html
# digitalio https://docs.circuitpython.org/en/latest/shared-bindings/digitalio/index.html
#
# Driver lifecycle documentation:
# https://docs.circuitpython.org/en/latest/docs/design_guide.html#lifetime-and-contextmanagers
#
################################################################################
# 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

#  load the CircuitPython basic digital pin support for driving hardware
import digitalio

#--------------------------------------------------------------------------------
class TB6612:
    def __init__(self,
                 PWMA=board.GP8,  AIN1=board.GP10, AIN2=board.GP9,   # control pins for motor A
                 PWMB=board.GP13, BIN1=board.GP11, BIN2=board.GP12,  # control pins for motor B
                 pwm_rate=20000):
        """This class represents a single dual H-bridge driver.  It configures
        two pins for PWM output and four pins for digital output and can be used
        to control two DC motors bidirectionally at variable speed.

        N.B. this does not implement any other timing process, it simply sets
        motor PWM levels but does not apply feedback, duration, or trajectory.

        """
        # The PWM pins are used to control driver output power.
        self.pwma = pwmio.PWMOut(PWMA, duty_cycle=0, frequency=pwm_rate)
        self.pwmb = pwmio.PWMOut(PWMB, duty_cycle=0, frequency=pwm_rate)

        # The IN pins are used to set direction.
        self.ain1 = digitalio.DigitalInOut(AIN1)
        self.ain2 = digitalio.DigitalInOut(AIN2)
        self.bin1 = digitalio.DigitalInOut(BIN1)
        self.bin2 = digitalio.DigitalInOut(BIN2)

        self.ain1.direction = digitalio.Direction.OUTPUT
        self.ain2.direction = digitalio.Direction.OUTPUT
        self.bin1.direction = digitalio.Direction.OUTPUT
        self.bin2.direction = digitalio.Direction.OUTPUT

        self.ain1.value = False
        self.ain2.value = False
        self.bin1.value = False
        self.bin2.value = False

        return

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

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

        # the direction control pins are always driven with opposite polarity
        if channel == 0 or channel == 'A' or channel == 'a':
            self.pwma.duty_cycle = pwm
            if rate < 0:
                self.ain1.value = False
                self.ain2.value = True
            else:
                self.ain1.value = True
                self.ain2.value = False
        else:
            self.pwmb.duty_cycle = pwm
            if rate < 0:
                self.bin1.value = False
                self.bin2.value = True
            else:
                self.bin1.value = True
                self.bin2.value = False

    def deinit(self):
        """Manage resource release as part of object lifecycle."""
        self.pwma.deinit()
        self.ain1.deinit()
        self.ain2.deinit()

        self.pwmb.deinit()
        self.bin1.deinit()
        self.bin2.deinit()

        self.pwma = None
        self.ain1 = None
        self.ain2 = None

        self.pwmb = None
        self.bin1 = None
        self.bin2 = None

    def __enter__(self):
        return self

    def __exit__(self):
        # Automatically deinitializes the hardware when exiting a context.
        self.deinit()