#  ___   ___  ___  _   _  ___   ___   ____ ___  ____  
# / _ \ /___)/ _ \| | | |/ _ \ / _ \ / ___) _ \|    \ 
#| |_| |___ | |_| | |_| | |_| | |_| ( (__| |_| | | | |
# \___/(___/ \___/ \__  |\___/ \___(_)____)___/|_|_|_|
#                  (____/ 
# Osoyoo Raspberry Pi V4 car Lesson 1 tutorial
# tutorial url: https://osoyoo.com/2023/03/09/raspberry-pi-robot-car-v4-0-lesson-1-basic-installation-and-movement/


import time
from gpiozero import OutputDevice,InputDevice
# Import the PCA9685 module.
from board import SCL, SDA
import busio
# Import the PCA9685 module.
from adafruit_pca9685 import PCA9685
from adafruit_servokit import ServoKit
kit = ServoKit(channels=16)
# Create the I2C bus interface.
i2c_bus = busio.I2C(SCL, SDA)
pwm =  PCA9685(i2c_bus)
pwm.frequency = 50

# Alternatively specify a different address and/or bus:
#pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
high_speed = 0x6FFF  # Max pulse length out of 4096
mid_speed = 0x5FFF  # Middle pulse length out of 4096
low_speed = 0x2FFF  # low pulse length out of 4096

#define L298N(Model-Pi motor drive board) GPIO pins
IN1 = OutputDevice(23)  # left motor direction GPIO 23
IN2 = OutputDevice(24)  # left motor direction GPIO 24
IN3 = OutputDevice(27)  #right motor direction GPIO 27
IN4 = OutputDevice(22)  #right motor direction GPIO 22
ENA = 0  #left motor speed PCA9685 port 0
ENB = 1  #right motor speed PCA9685 port 1

def changespeed(left_speed,right_speed):
    pwm.channels[ENA].duty_cycle = left_speed
    pwm.channels[ENB].duty_cycle = right_speed

def stopcar():
    IN1.off()
    IN2.off()
    IN3.off()
    IN4.off()
    changespeed(0,0)

def backward(speed_left,speed_right):
    IN1.on()
    IN2.off()
    IN3.on()
    IN4.off()
    changespeed(speed_left,speed_right)

def forward(speed_left,speed_right):
    IN1.off()
    IN2.on()
    IN3.off()
    IN4.on()
    changespeed(speed_left,speed_right)

def turnleft(speed_left,speed_right):
    IN1.on()
    IN2.off()
    IN3.off()
    IN4.on()
    changespeed(speed_left,speed_right)

def turnright(speed_left,speed_right):
    IN1.off()
    IN2.on()
    IN3.on()
    IN4.off()
    changespeed(speed_left,speed_right) 

print("forward")
forward(mid_speed,mid_speed)
time.sleep(1)  
stopcar()
time.sleep(0.1)

print("backward")
backward(mid_speed,mid_speed)
time.sleep(1)  
stopcar()
time.sleep(0.1) 

print("turn left")
turnleft(low_speed,mid_speed)
time.sleep(1)  
stopcar()
time.sleep(0.1) 

print("turn right")
turnright(mid_speed,low_speed)
time.sleep(1)  
stopcar()
time.sleep(0.1)