# **********************************************************************;
# * Project           : RPILib
# *
# * Program name      : TB6612FNG.py
# *
# * Author            : Viacheslav Karpizin
# *
# * Date created      : 20200313
# *
# * Purpose           : Control both channels of TB6612 motor driver chip
# *
# * Revision History  :
# *
# * Date        Author      Ref    Revision (Date in YYYYMMDD format)
# * 202003    Viacheslav Karpizin      1       original version
# *
#
# Copyright (c) 2020, Viacheslav Karpizin

#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
#
# https://www.sparkfun.com/datasheets/Robotics/TB6612FNG.pdf
# **********************************************************************
import RPi.GPIO as GPIO
import time
import smbus_m
import sys
sys.path.append('./../')


GPIO.setmode(GPIO.BOARD)
# GPIO.setmode(GPIO.BCM)


class Motor:
    ain1 = ""
    ain2 = ""
    apwm = ""
    bin1 = ""
    bin2 = ""
    bpwm = ""
    standbyPin = ""

    # Defaults
    freq = 200
    reverse = False  # Reverse flips the direction of the motor

    # Constructor
    def __init__(self, ain1, ain2, apwm, bin1=None, bin2=None, bpwm=None, standbyPin=None, reverse=False):
        self.ain1 = ain1
        self.ain2 = ain2
        self.apwm = apwm
        self.bin1 = bin1
        self.bin2 = bin2
        self.bpwm = bpwm

        self.standbyPin = standbyPin
        self.reverse = reverse

        GPIO.setup(ain1, GPIO.OUT)
        GPIO.setup(ain2, GPIO.OUT)
        GPIO.setup(apwm, GPIO.OUT)
        if (bin1 != None):
            GPIO.setup(bin1, GPIO.OUT)
        if bin2 != None:
            GPIO.setup(bin2, GPIO.OUT)
        if bpwm != None:
            GPIO.setup(bpwm, GPIO.OUT)
            self.pb = GPIO.PWM(bpwm, self.freq)
            self.pb.start(0)

        if standbyPin != None:
            GPIO.setup(standbyPin, GPIO.OUT)
            GPIO.output(standbyPin, GPIO.HIGH)

        self.pa = GPIO.PWM(apwm, self.freq)
        self.pa.start(0)

    # Speed from -100 to 100
    def drive(self,  speed, motor=1):
        # Negative speed for reverse, positive for forward
        # If necessary use reverse parameter in constructor
        dutyCycle = speed
        if(speed < 0):
            dutyCycle = dutyCycle * -1

        if(self.reverse):
            speed = speed * -1
        if motor == 1:
            if(speed > 0):
                print("speed + for motor1")
                GPIO.output(self.ain1, GPIO.HIGH)
                GPIO.output(self.ain2, GPIO.LOW)
            else:
                print("speed - for motor1")
                GPIO.output(self.ain1, GPIO.LOW)
                GPIO.output(self.ain2, GPIO.HIGH)
            self.pa.ChangeDutyCycle(dutyCycle)
        elif motor == 2 and self.bin1 != None:
            if(speed > 0):
                GPIO.output(self.bin1, GPIO.HIGH)
                GPIO.output(self.bin2, GPIO.LOW)
            else:
                GPIO.output(self.bin1, GPIO.LOW)
                GPIO.output(self.bin2, GPIO.HIGH)
            self.pb.ChangeDutyCycle(dutyCycle)
        else:
            print("incorrect motor id")

    def brake(self, motor=1):
        if motor == 1:
            self.pa.ChangeDutyCycle(0)
            GPIO.output(self.ain1, GPIO.HIGH)
            GPIO.output(self.ain2, GPIO.HIGH)
        else:
            self.pb.ChangeDutyCycle(0)
            GPIO.output(self.bin1, GPIO.HIGH)
            GPIO.output(self.bin2, GPIO.HIGH)

    def standby(self, value):
        self.pa.ChangeDutyCycle(0)
        if self.bin1 != None:
            self.pb.ChangeDutyCycle(0)
        GPIO.output(self.standbyPin, value)

    def __del__(self):
        GPIO.cleanup()


def main():
    print "=========================================================="
    print "TB6612FNG for RPI Series "
    print "=========================================================="
    time.sleep(0.5)

    #test = Motor (a1, a2, apwm, b1, b2, bpwm, standby, reverse)
    test = Motor(15, 16, 13, None, None, None, 11, False)
    # blue - GND
    # violet, grey - motor1
    # violet, white - 5V(VM), 3v3 (VCC)
    # orange - pwm a (13, GPIO27)
    # yellow - a2 (16 GPIO23)
    # green  - a1 (15 GPIO22)
    # black   - standby (11, GPIO17)

    try:
        while True:

            print("motor1, forward, 100%")
            test.drive(100)  # Forward 100% dutycycle
            time.sleep(3)

            print("motor1, forward, 20%")
            test.drive(20)  # Forward 100% dutycycle
            time.sleep(3)

            print("motor1, reverse, 100%")
            test.drive(-100)  # Backwards 100% dutycycle
            time.sleep(3)

            print("motor1, reverse, 10%")
            test.drive(-10)  # Backwards 100% dutycycle
            time.sleep(3)

            print("motor1, brake")
            test.brake()  # Short brake
            time.sleep(1)

            print("motor1, standby")

            test.standby(True)  # Enable standby
            test.standby(False)  # Disable standby
            time.sleep(2)
    except KeyboardInterrupt:
        # Ctrl-C to exit
        sys.exit(0)


if __name__ == "__main__":
    main()
