#!/usr/bin/env python2.7  

#------------------------------Libraries
import RPi.GPIO as GPIO # always needed with RPi.GPIO  
from time import sleep  # pull in the sleep function from time module 

import termios, sys, os

#------------------------------Functions
TERMIOS = termios
def getkey():
    fd = sys.stdin.fileno()
    old = termios.tcgetattr(fd)
    new = termios.tcgetattr(fd)
    new[3] = new[3] & ~TERMIOS.ICANON & ~TERMIOS.ECHO
    new[6][TERMIOS.VMIN] = 1
    new[6][TERMIOS.VTIME] = 0
    termios.tcsetattr(fd, TERMIOS.TCSANOW, new)
    c = None
    try:
        c = os.read(fd, 3)
    finally:
        termios.tcsetattr(fd, TERMIOS.TCSAFLUSH, old)
    return c

#------------------------------

GPIO.setmode(GPIO.BOARD)  # choose BCM or BOARD numbering schemes.

GPIO.setup(12, GPIO.OUT)
GPIO.setup(16, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)
GPIO.setup(18, GPIO.OUT)

motor1f = GPIO.PWM(16, 100)    # create object white for PWM on port 16 at 100 Hertz  
motor1b = GPIO.PWM(12, 100)    # create object red for PWM on port 12 at 100 Hertz  

motor2f = GPIO.PWM(18, 100)    # create object white for PWM on port 18 at 100 Hertz  
motor2b = GPIO.PWM(22, 100)    # create object red for PWM on port 22 at 100 Hertz  

motor1f.start(0)
motor1b.start(0)
motor2f.start(0)
motor2b.start(0)
motor1f.ChangeDutyCycle(0)
motor1b.ChangeDutyCycle(0)
motor2f.ChangeDutyCycle(0)
motor2b.ChangeDutyCycle(0)


pause_time = 0.2           # you can change this to slow down/speed up
motor1_speed = 0
motor2_speed = 0

max_speed=80

# End initialisation
print ("Start")

try:  
	while True:  
		key = getkey()
		if ord(key[0]) == 27 and len(key)>1 and ord(key[1]) == 91: #Special keys 
			if ord(key[2]) == 65: #Up arrow
				if motor1_speed < 1 and motor2_speed < 1:
					motor1_speed+=.1 #increase speed
					motor2_speed+=.1 #increase speed
			elif ord(key[2]) == 66: #Down arrow
				if motor1_speed > -1 and motor2_speed > -1:
					motor1_speed-=.1 #decrease speed
					motor2_speed-=.1 #decrease speed
			elif ord(key[2]) == 67: #Right arrow
				if motor1_speed > -1 and motor2_speed < 1:
					motor1_speed-=.05 #decrease speed
					motor2_speed+=.05 #increase speed
			elif ord(key[2]) == 68: #Left arrow
				if motor1_speed < 1 and motor2_speed > -1:
					motor1_speed+=.05 #increase speed
					motor2_speed-=.05 #decrease speed
			print ord(key[0])
			print ord(key[1])
			print ord(key[2])
			print ("---------")
		elif ord(key[0]) == 32:
					motor1_speed=0 #0 speed
					motor2_speed=0 #0 speed
		
		
		print ("speed")
		print (motor1_speed)
		print (motor2_speed)
		if motor1_speed >=0:
			motor1f.ChangeDutyCycle(motor1_speed*max_speed)
			motor1b.ChangeDutyCycle(0)
		elif motor1_speed <0:
			motor1f.ChangeDutyCycle(0)
			motor1b.ChangeDutyCycle(-1*motor1_speed*max_speed)
		
		if motor2_speed >=0:
			motor2f.ChangeDutyCycle(motor2_speed*max_speed)
			motor2b.ChangeDutyCycle(0)
		elif motor2_speed <0:
			motor2f.ChangeDutyCycle(0)
			motor2b.ChangeDutyCycle(-1*motor2_speed*max_speed)

except KeyboardInterrupt:  
	motor1f.ChangeDutyCycle(0)
	motor1b.ChangeDutyCycle(0)
	motor2f.ChangeDutyCycle(0)
	motor2b.ChangeDutyCycle(0)
	motor1f.stop()            # stop the white PWM output  
	motor1b.stop()            # stop the white PWM output  
	motor2f.stop()            # stop the white PWM output  
	motor2b.stop()            # stop the white PWM output  
	GPIO.cleanup()          # clean up GPIO on CTRL+C exit 

except Exception:
	motor1f.ChangeDutyCycle(0)
	motor1b.ChangeDutyCycle(0)
	motor2f.ChangeDutyCycle(0)
	motor2b.ChangeDutyCycle(0)
	motor1f.stop()            # stop the white PWM output  
	motor1b.stop()            # stop the white PWM output  
	motor2f.stop()            # stop the white PWM output  
	motor2b.stop()            # stop the white PWM output  
	GPIO.cleanup()          # clean up GPIO on CTRL+C exit