import sys,tty,termios
import time
from SunFounder_TB6612 import TB6612
import RPi.GPIO as GPIO
import PCA9685


#motor#
#Connect MA to BCM20
#Connect MB to BCM16
#Connect PWMA to BCM12
#Connect PWMB to BCM26

#def main():
#
#    GPIO.setmode(GPIO.BCM)
#    GPIO.setup((12, 26), GPIO.OUT)
#    a = GPIO.PWM(12, 60)
#    b = GPIO.PWM(26, 60)
#    a.start(0)
#    b.start(0)
#
#    def a_speed(value):
#        a.ChangeDutyCycle(value)
#
#    def b_speed(value):
#        b.ChangeDutyCycle(value)
#
#    motorA = TB6612.Motor(20)
#    motorB = TB6612.Motor(16)
#    motorA.debug = True
#    motorB.debug = True
#    motorA.pwm = a_speed
#    motorB.pwm = b_speed
#
#def destroy():
#    motorA.stop()
#    motorB.stop()

    #if __name__ == '__main__':
    #	try:
    #	main()
    #except KeyboardInterrupt:
#	destroy()



#Servo#

##class Servo(object):
#    '''Servo driver class'''
#    _MIN_PULSE_WIDTH = 600
#    _MAX_PULSE_WIDTH = 2400
#    _DEFAULT_PULSE_WIDTH = 1500
#    _FREQUENCY = 60
#
#    _DEBUG = False
#    _DEBUG_INFO = 'DEBUG "Servo.py":'
#
#    def __init__(self, channel, offset=0, lock=True, bus_number=None, address=0x40):
#        ''' Init a servo on specific channel, this offset '''
#        if channel<0 or channel > 16:
#            raise ValueError("Servo channel \"{0}\" is not in (0, 15).".format(channel))
#        if self._DEBUG:
#            print self._DEBUG_INFO, "Debug on"
#        self.channel = channel
#        self.offset = offset
#        self.lock = lock
#
#        self.pwm = PCA9685.PWM(bus_number=bus_number, address=address)
#        self.frequency = self._FREQUENCY
#        self.write(90)
#
#    def setup(self):
#        self.pwm.setup()
#
#    def _angle_to_analog(self, angle):
#        ''' Calculate 12-bit analog value from giving angle '''
#        pulse_wide   = self.pwm.map(angle, 0, 180, self._MIN_PULSE_WIDTH, self._MAX_PULSE_WIDTH)
#        analog_value = int(float(pulse_wide) / 1000000 * self.frequency * 4096)
#        if self._DEBUG:
#            print self._DEBUG_INFO, 'Angle %d equals Analog_value %d' % (angle, analog_value)
#        return analog_value
#
#    @property
#    def frequency(self):
#        return self._frequency
#
#    @frequency.setter
#    def frequency(self, value):
#        self._frequency = value
#        self.pwm.frequency = value
#
#    @property
#    def offset(self):
#        return self._offset
#
#    @offset.setter
#    def offset(self, value):
#        ''' Set offset for much user-friendly '''
#        self._offset = value
#        if self._DEBUG:
#            print self._DEBUG_INFO, 'Set offset to %d' % self.offset
#
#def write(self, angle):
#    ''' Turn the servo with giving angle. '''
#        if self.lock:
#            if angle > 180:
#                angle = 180
#            if angle < 0:
#                angle = 0
#        else:
#        if angle<0 or angle>180:
#            raise ValueError("Servo \"{0}\" turn angle \"{1}\" is not in (0, 180).".format(self.channel, angle))
#        val = self._angle_to_analog(angle)
#        val += self.offset
#        self.pwm.write(self.channel, 0, val)
#        if self._DEBUG:
#            print self._DEBUG_INFO, 'Turn angle = %d' % angle
#
#    @property
#    def debug(self):
#        return self._DEBUG
#
#    @debug.setter
#    def debug(self, debug):
#        ''' Set if debug information shows '''
#        if debug in (True, False):
#            self._DEBUG = debug
#        else:
#            raise ValueError('debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'.format(debug))
#
#        if self._DEBUG:
#            print self._DEBUG_INFO, "Set debug on"
#        else:
#            print self._DEBUG_INFO, "Set debug off"
#
##def test():
##    '''Servo driver test on channel 1'''
##    a = Servo(1)
##    a.setup()
#
#        #for i in range(0, 180, 5):
#        #print i
#        #a.write(i)
#        #time.sleep(0.1)
#        #for i in range(180, 0, -5):
#        #print i
#        #a.write(i)
#        #time.sleep(0.1)
#        #for i in range(0, 91, 2):
#        #a.write(i)
#        #time.sleep(0.05)
##print i
#
#def install():
#    all_servo = [0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0]
#    for i in range(16):
#        all_servo[i] = Servo(i)
#    for servo in all_servo:
#        servo.setup()
#        servo.write(90)
#
#control#

class _Getch:
    def __call__(self):
        fd = sys.stdin.fileno()
        old_settings = termios.tcgetattr(fd)
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(3)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

            #def get():
            #    inkey = _Getch()
            #    while(1):
            #        k=inkey()
            #        if k!='':break
            #    if k=='\x1b[A':
            #        print("up")
            #    elif k=='\x1b[B':
            #        print("down")
            #    elif k=='\x1b[C':
            #        print("right")
            #    elif k=='\x1b[D':
            #        print("left")
            #    else:
            #        print("not an arrow key!")

#def main():
#for i in range(0,20):
#    get()

#if __name__=='__main__':
#    main()


def test():
#    s1 = Servo(1)
#    s2 = Servo(2)
#    s1.setup()
#    s2.setup()
    import time
    GPIO.setwarnings(False)
    
    GPIO.setmode(GPIO.BCM)
    GPIO.setup((12, 26), GPIO.OUT)
    a = GPIO.PWM(12, 60)
    b = GPIO.PWM(26, 60)
    a.start(0)
    b.start(0)
    delay = 0.2
    delay1 = 0.1
    
    def a_speed(value):
        a.ChangeDutyCycle(value)
    
    def b_speed(value):
        b.ChangeDutyCycle(value)
    
    motorA = TB6612.Motor(20)
    motorB = TB6612.Motor(16)
    #    motorA.debug = True
    #    motorB.debug = True
    motorA.pwm = a_speed
    motorB.pwm = b_speed

    inkey = _Getch()
    
    if inkey=='\x1b[A':
        print("forward")
        motorA.forward()
        motorA.speed = 100
        motorB.forward()
        motorB.speed = 100
        time.sleep(delay)

    elif inkey=='\x1b[B':
        print("backward")
        motorA.backward()
        motorA.speed = 100
        motorB.backward()
        motorB.speed = 100
        time.sleep(delay)

    elif inkey=='\x1b[C':
        print("right")
        motorA.forward()
        motorA.speed = 100
        motorB.backward()
        motorB.speed = 100
        time.sleep(delay1)

    elif inkey=='\x1b[D':
        print("left")
        motorA.backward()
        motorA.speed = 100
        motorB.forward()
        motorB.speed = 100
        time.sleep(delay1)
        
#--------------------#

if __name__ == '__main__':
    test()
    import sys
#    if len(sys.argv) == 2:
#        if sys.argv[1] == "install":
#            install()
#    else:
#    except KeyboardInterrupt()

