#  ___   ___  ___  _   _  ___   ___   ____ ___  ____  
# / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \|    \ 
#| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
# \___/(___/ \___/ \__  |\___/ \___(_)____)___/|_|_|_|
#                  (____/ 
# Osoyoo Model-Pi L298N DC motor driver programming guide
# tutorial url: https://osoyoo.com/2020/03/01/python-programming-tutorial-model-pi-l298n-motor-driver-for-raspberry-pi/

from __future__ import division
import time
# Import the PCA9685 module.
# If you haven't install PCA9685 driver, run sudo pip3 install adafruit-pca9685
import Adafruit_PCA9685
import RPi.GPIO as GPIO
# Initialise the PCA9685 using the default address (0x40).
# Alternatively specify a different address and/or bus:
#pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
pwm = Adafruit_PCA9685.PCA9685()

#define motor speed
move_speed = 2000  # Max pulse length out of 4096 

# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)
GPIO.setmode(GPIO.BCM) # GPIO number  in BCM mode
GPIO.setwarnings(False)

#define L298N(Model-Pi motor drive board) GPIO pins
#IN1 IN2 control the motor direction on K1/K2 port in L293 Motor board, please connect left motor to K1 or K2
#When IN1 is set to HIGH voltage  1, IN2 is LOW voltage 0, then left motor(K1 or K2) rotate backward
#When IN2 is set to HIGH voltage  1, IN1 is LOW voltage 0, then left motor(K1 or K2) rotate forward
IN1 = 23  #IN1 connect to GPIO#23(physical location# 16)
IN2 = 24  #IN2 connect to GPIO#24(physical location# 18)

#IN3 IN4 control the motor direction on K3/K4 port in L293 Motor board, please connect right motor to K3 or K4
#When IN3 is set to HIGH voltage  1, IN4 is LOW voltage 0, then right motor(K3 or K4) rotate backward
#When IN4 is set to HIGH voltage  1, IN3 is LOW voltage 0, then right motor(K3 or K4) rotate forward
IN3 = 27  #IN3 connect to GPIO#27(physical location# 13)
IN4 = 22  #IN4 connect to GPIO#22(physical location# 15)

#The motor Speed is controlled by PCA9685 PWM signal generator
#K1/K2 motor speed is controlled by PWM signal value of ENA 
#K3/K4 motor speed is controlled by PWM signal value of ENB 
ENA = 0  #Left motor(K1/K2) speed PCA9685 port 0
ENB = 1  #Right motor(K3/K4) speed PCA9685 port 1

# Define motor control  pins as output
GPIO.setup(IN1, GPIO.OUT)   
GPIO.setup(IN2, GPIO.OUT) 
GPIO.setup(IN3, GPIO.OUT)   
GPIO.setup(IN4, GPIO.OUT) 

def changespeed(speed):
    pwm.set_pwm(ENA, 0, speed) #set ENA pwm signal value(variable speed), speed value from 0 to 4096
    pwm.set_pwm(ENB, 0, speed) #set ENA pwm signal value(variable speed), speed value from 0 to 4096

def stopcar():
    #set all direction pin to 0
    GPIO.output(IN1, GPIO.LOW) 
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    changespeed(0)  #reduce speed to 0


def backward():
    #make all motors moving backward at the speed of variable move_speed
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    changespeed(move_speed)
 
    #following two lines can be removed if you want car make continuous movement without pause
    #time.sleep(0.25)  
    #stopcar()
    
def forward():
    #make all motors moving forward at the speed of variable move_speed
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    changespeed(move_speed)
    #following two lines can be removed if you want car make continuous movement without pause
    #time.sleep(0.25)  
    #stopcar()
    
def turnRight():
    #make left motors moving forward, right motor moving backward
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    changespeed(move_speed)
    #following two lines can be removed if you want car make continuous movement without pause
    #time.sleep(0.25)  
    #stopcar()
    
def turnLeft():
    #make right motors moving forward, left motor moving backward
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    changespeed(move_speed)	
    #following two lines can be removed if you want car make continuous movement without pause
    #time.sleep(0.25)  
    #stopcar()
    
forward()
time.sleep(1)  
stopcar()
time.sleep(0.25)

backward()
time.sleep(1)  
stopcar()
time.sleep(0.25) 

turnLeft()
time.sleep(1)  
stopcar()
time.sleep(0.25)
    
turnRight()
time.sleep(1)  
stopcar()
time.sleep(0.25)

print('Moving servo on channel 0, press Ctrl-C to quit...')
#while True:
# Move servo on channel O between extremes.

time.sleep(2)
pwm.set_pwm(15, 0, 0)