3d-mouse-rpi-python/SpaceNavigatorTxLEDs.py at develop · jwick1234/3d-mouse-rpi-python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
# See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455
# Run python2 as root (sudo /usr/bin/python2.7 /home/pi/pythondev/SpaceNavigatorTxLEDs.py)

import usb.core
import usb.util
import sys
from time import gmtime, strftime
import time
import RPi.GPIO as GPIO

# Look for SpaceNavigator
dev = usb.core.find(idVendor=0x46d, idProduct=0xc626)
if dev is None:
    raise ValueError('SpaceNavigator not found');
else:
    print ('SpaceNavigator found')
    print dev

# Setup GPIO pins
TX_NEG_PIN = 11
TX_POS_PIN = 8
GPIO.setmode(GPIO.BCM) # logical numbering
GPIO.setup(TX_NEG_PIN,GPIO.OUT)
GPIO.setup(TX_POS_PIN,GPIO.OUT)

# Don't need all this but may want it for a full implementation

cfg = dev.get_active_configuration()
print 'cfg is ', cfg
intf = cfg[(0,0)]
print 'intf is ', intf
ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN)
print 'ep is ', ep

reattach = False
if dev.is_kernel_driver_active(0):
    reattach = True
    dev.detach_kernel_driver(0)

ep_in = dev[0][(0,0)][0]
ep_out = dev[0][(0,0)][1]

print ''
print 'Exit by pressing any button on the SpaceNavigator'
print ''

run = True
while run:
    try:
        data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0)
        # raw data
        # print data

        # print it correctly T: x,y,z R: x,y,z
        if data[0] == 1:
            # translation packet
            tx = data[1] + (data[2]*256)
            ty = data[3] + (data[4]*256)
            tz = data[5] + (data[6]*256)

            if data[2] > 127:
                tx -= 65536
            if data[4] > 127:
                ty -= 65536
            if data[6] > 127:
                tz -= 65536
            print "T: ", tx, ty, tz

        if data[0] == 2:
            # rotation packet
            rx = data[1] + (data[2]*256)
            ry = data[3] + (data[4]*256)
            rz = data[5] + (data[6]*256)

            if data[2] > 127:
                rx -= 65536
            if data[4] > 127:
                ry -= 65536
            if data[6] > 127:
                rz -= 65536
            print "R: ", rx, ry, rz

        if data[0] == 3 and data[1] == 0:
            # button packet - exit on the release
            run = False

        # light up either -tive or +tive LED
        if tx < 0:
            GPIO.output(TX_NEG_PIN, tx/2*2 == tx) # on/off for odd/even value; more interesting than steady on
        if tx > 0:
            GPIO.output(TX_POS_PIN, tx/2*2 == tx) # on/off for odd/even value; more interesting than steady on
        if tx == 0:
            # turn it off when the axis returns to zero
            GPIO.output(TX_NEG_PIN, GPIO.LOW)
            GPIO.output(TX_POS_PIN, GPIO.LOW)
            print ''
            print 'Exit by pressing any button on the SpaceNavigator'
            print ''

    except usb.core.USBError:
       print("USBError")
#    except:
#       print("read failed")
# end while

# Cleanup GPIO pins
GPIO.cleanup()

usb.util.dispose_resources(dev)

if reattach:
    dev.attach_kernel_driver(0)