# Thanks to USBSnoop for making this simple

# Thanks to my lack of a social life for giving me both a friday AND saturday night free to do this.

# pyUSB d2xx module 
# Available at http://bleyer.org/pyusb/


import d2xx
import time


# Taken from http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/510399

# HexByteConversion

# Convert a byte string to it's hex representation for output or visa versa.

# ByteToHex converts byte string "\xFF\xFE\x00\x01" to the string "FF FE 00 01"
# HexToByte converts string "FF FE 00 01" to the byte string "\xFF\xFE\x00\x01"


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

def ByteToHex( byteStr ):
    
    # Convert a byte string to it's hex string representation e.g. for output.
    
    
    # Uses list comprehension which is a fractionally faster implementation than
    # the alternative, more readable, implementation below
    #   
    #    hex = []
    #    for aChar in byteStr:
    #        hex.append( "%02X " % ord( aChar ) )
    #
    #    return ' '.join( hex )

    return ' '.join( [ "%02X " % ord( x ) for x in byteStr ] )

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

def HexToByte( hexStr ):
    
    # Convert a string hex byte values into a byte string. The Hex Byte values may
    # or may not be space separated.
    
    # The list comprehension implementation is fractionally slower in this case    
    #
    #    hexStr = ''.join( hexStr.split(" ") )
    #    return ''.join( ["%c" % chr( int ( hexStr[i:i+2],16 ) ) \
    #                                   for i in range(0, len( hexStr ), 2) ] )
 
    bytes = []

    hexStr = ''.join( hexStr.split(" ") )

    for i in range(0, len(hexStr), 2):
        bytes.append( chr( int (hexStr[i:i+2], 16 ) ) )

    return ''.join( bytes )

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


# 16 bytes sent to the Falcon gets you 16 bytes back. It's expected that you're constantly polling it to set 
# motor positions, as it is assumed that the PID loops for the control exist in the software, not the hardware.
# These control loops will most likely be available in the upcoming SDK.

# =============================================

# Motor and Encoder Translation:

# To Retrieve Axis Position
# - Subtract 1 from all bytes
# - Chop top nibble off all bytes (x & 0x0f)
# - Little endian representation of motor position now available
# Note: GUIs return reverse of what you're expecting. Negatives in driver are translated are positive, and vice versa.
# This doesn't really matter, you can interpret things however you damn well please as long as it gets you where you wanna go.

# Example:
# GUI gives us 1447 for first encoder
# Driver gives us 0x3c 0x4A 0x46 0x4B 0x50 ...
# Isolate 4 Bytes for axis:
# 0x4A 0x46 0x4B 0x50
# Subtract 1 from all bytes:
# 0x49 0x45 0x4A 0x4F
# Chop top nibble (x & 0x0f):
# 0x9 0x5 0xA 0xF
# Reverse into Big Endian (this is for readability here, but if you're writing drivers for a big endian system...):
# 0xF 0xA 0x5 0x9
# Shift into 16 bit number:
# 0xFA59
# 2's compliment conversion (~x + 1):
# 0x05A7
# Decimal conversion (16 bit signed):
# 1447

# GUI gives us -359 for first encoder
# Driver gives us 0x3c 0x48 0x47 0x42 0x41 ...
# Isolate 4 Bytes for axis:
# 0x48 0x47 0x42 0x41
# Subtract 1 from all bytes:
# 0x47 0x46 0x41 0x40
# Chop top nibble (x & 0x0f):
# 0x7 0x6 0x1 0x0
# Reverse into Big Endian (this is for readability here, but if you're writing drivers for a big endian system...):
# 0x0 0x1 0x6 0x7
# Shift into 16 bit number:
# 0x0167
# 2's compliment conversion (~x + 1):
# 0xFE99
# Decimal conversion (16 bit signed):
# -359

# =============================================

# NOP:

# 0x3c 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x41 0x3e

# =============================================

# To Controller - 16 bytes:

# 0x3C 0x(g0) 0x(g1) 0x(g2) 0x(g3) 0x(h0) 0x(h1) 0x(h2) 0x(h3) 0x(i0) 0x(i1) 0x(i2) 0x(i3) 0x(j0) 0x(k0) 03E

# 0x3C - Start Byte

# g thru i - Motor Torque for the 3 feedback axes
# - Read "Motor and Encoder Translation" Section for more info

# j - LED Control
# - 0x3 - Green
# - 0x5 - Blue
# - 0x7 - Blue + Green
# - 0x9 - Red
# - 0xb - Red + Green
# - 0xd - Red + Blue
# - 0xf - Red + Blue + Green

# k - (??? Possibly Homing Control ???)

# 0x3E - End Byte

# =============================================

# From Controller - 16 bytes:

# 0x3C 0x(m0) 0x(m1) 0x(m2) 0x(m3) 0x(n0) 0x(n1) 0x(n2) 0x(n3) 0x(o0) 0x(o1) 0x(o2) 0x(o3) 0x4(p0) 0x(q0) 0x3E

# 0x3C - Start Byte

# m thru o - Axis position report
# - Read "Motor and Encoder Translation" Section for more info

# p - Button Layout for Controller Handle
# - 0x2 - Far Right Button
# - 0x3 - Center (Forward) Button
# - 0x5 - Center (Circle) Button
# - 0x9 - Far Left Button

# q - (??? Possibly Homing Status ???)

# 0x3E - End Byte


# The Falcon is the only FTDI device that I have open right now, so I can just choose 0
h = d2xx.open(0)

# Create output that just slams the motor to one end or the other. Very, very stupid and probably bad for the motors.
in_str = HexToByte("3c 4f 4f 4f 41 4f 4f 4f 41 4f 4f 4f 41 41 41 3e")
out_str = HexToByte("3c 41 41 41 50 41 41 41 50 41 41 41 50 41 41 3e")

print h
try:
    while 1:
        # The falcon is built so that a command string only triggers the motor for a small portion of time
        # This allows us to implement PID control to exercise the minimum amount of power to the motors
        # to keep the position we want
        # However, it's 3am and I've got brunch at 11am tomorrow
        # So, we move to the outer limits and hold there for a small period of time
        # We write the packets as fast as possible, which keeps the motor at that force.
        for i in range(500):
            h.write(in_str)
        for i in range(500):
            h.write(out_str)
except:
    h.close()