######################################################################
# 20171618 Dong_Seola 's code
# this swing_point_turn.py file is
# to control raspberry car's truning
######################################################################

# =======================================================================
# import GPIO library and time module
# =======================================================================

import RPi.GPIO as GPIO 
import time


# =======================================================================
# Set the motor's true / false value to go forward.
# Set the motor's true / false value to go opposite(backward).
# =======================================================================

forward0 = True
forward1 = False
backward0 = not forward0
backward1 = not forward1

# =======================================================================
# declare the pins of 12, 11, 35 in the Rapberry Pi
# as the left motor control pins in order to control left motor
# left motor needs three pins to be controlled
# =======================================================================

MotorLeft_A = 12 
MotorLeft_B = 11
MotorLeft_PWM = 35

# =======================================================================
# declare the pins of 15, 13, 37 in the Rapberry Pi
# as the right motor control pins in order to control right motor
# right motor needs three pins to be controlled
# =======================================================================

MotorRight_A = 15 
MotorRight_B = 13
MotorRight_PWM = 37

# =======================================================================
# set Both wheel to move by GPIO
# =======================================================================

GPIO.setup(MotorLeft_A,GPIO.OUT)
GPIO.setup(MotorLeft_B,GPIO.OUT)
GPIO.setup(MotorLeft_PWM,GPIO.OUT)

GPIO.setup(MotorRight_A,GPIO.OUT)
GPIO.setup(MotorRight_B,GPIO.OUT)
GPIO.setup(MotorRight_PWM,GPIO.OUT)

# =======================================================================
# create left and right pwm object to control the speed of left motor
# The speed is between 0 and 100
# =======================================================================

LeftPwm=GPIO.PWM(MotorLeft_PWM,100)
RightPwm=GPIO.PWM(MotorRight_PWM,100)

# ===========================================================================
# Control the DC motor to make it rotate clockwise,
# so the car will move forward.
# ===========================================================================

def left_motor(x):
    if x == True:
        GPIO.output(MotorLeft_A, GPIO.HIGH)
        GPIO.output(MotorLeft_B, GPIO.LOW)
    elif x == False:
        GPIO.output(MotorLeft_A, GPIO.LOW)
        GPIO.output(MotorLeft_B, GPIO.HIGH)
    else:
        print ('Config Error')

def right_motor(x):
    if x == True:
        GPIO.output(MotorRight_A, GPIO.LOW)
        GPIO.output(MotorRight_B, GPIO.HIGH)
    elif x == False:
        GPIO.output(MotorRight_A, GPIO.HIGH)
        GPIO.output(MotorRight_B, GPIO.LOW)
    else :
        print('Config Error')

# =======================================================================
# perform right swing turn of 90 degree  
# =======================================================================

def rightSwingTurn(speed, running_time):

    # set the left motor to go fowrard
    left_motor(forward0)
    # set the left motor pwm to be ready to go forward
    GPIO.output(MotorLeft_PWM,GPIO.HIGH)

    # set right motor pwm to stop = turn off
    GPIO.output(MotorRight_PWM,GPIO.LOW)

    # set the speed of the left motor to go fowrard
    LeftPwm.ChangeDutyCycle(speed)
    # set the speed of the right motor to stop
    RightPwm.ChangeDutyCycle(0)
    # set the running time of the left motor to go fowrard
    time.sleep(running_time)

# =======================================================================
# perform left swing turn of 90 degree  
# =======================================================================

def leftSwingTurn(speed, running_time):

    # set left motor pwm to stop = turn off
    GPIO.output(MotorLeft_PWM,GPIO.LOW)  

    # set the right motor to go fowrard
    right_motor(forward0)
    # set the right motor pwm to be ready to go forward   
    GPIO.output(MotorRight_PWM,GPIO.HIGH)

    # set the speed of the left motor to stop
    LeftPwm.ChangeDutyCycle(0)
    # set the speed of the right motor to go fowrard
    RightPwm.ChangeDutyCycle(speed)

    # set the running time of the right motor to go fowrard
    time.sleep(running_time)

# =======================================================================
# perform right point turn of 90 degree  # student assignment (1)
# ======================================================================

def rightPointTurn(speed, running_time):

    # set the left motor to go forward
    left_motor(forward0)
    # set the left motor pwm to be ready to go forward
    GPIO.output(MotorLeft_PWM,GPIO.HIGH)

    # set the right motor to go backward
    right_motor(backward0)
    # set the right motor pwm to be ready to go backward
    GPIO.output(MotorRight_PWM,GPIO.LOW)

    # set the speed of the left motor to go fowrard
    LeftPwm.ChangeDutyCycle(speed)
    # set the speed of the right motor to backward
    RightPwm.ChangeDutyCycle(speed)

    # set the running time of the left and right motor to go
    time.sleep(running_time)


#=======================================================================
# perform left point turn of 90 degree   # student assignment (2)
# ======================================================================

def leftPointTurn(speed, running_time):

    # set the left motor to go backward
    left_motor(backward0)
    # set the left motor pwm to be ready to go backward
    GPIO.output(MotorLeft_PWM,GPIO.LOW)

    # set the right motor to go fowrard
    right_motor(forward0)
    # set the right motor pwm to be ready to go forward
    GPIO.output(MotorRight_PWM,GPIO.HIGH)

    # set the speed of the left motor to stop
    LeftPwm.ChangeDutyCycle(speed)
    # set the speed of the right motor to go fowrard
    RightPwm.ChangeDutyCycle(speed)

    # set the running time of the left and right motor to go
    time.sleep(running_time)
