├── HelloSpaceNavigator.py ├── README.md ├── SNDrive.py ├── SNLEDsPWM.py ├── SNLEDsPWMnl.py ├── SpaceNavigatorLEDs.py └── SpaceNavigatorTxLEDs.py /HelloSpaceNavigator.py: -------------------------------------------------------------------------------- 1 | # See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455 2 | # Run python2 as root (sudo /usr/bin/python2.7 /home/pi/pythondev/HelloSpaceNavigator.py) 3 | import usb.core 4 | import usb.util 5 | import sys 6 | from time import gmtime, strftime 7 | import time 8 | 9 | # Look for SpaceNavigator 10 | dev = usb.core.find(idVendor=0x46d, idProduct=0xc626) 11 | if dev is None: 12 | raise ValueError('SpaceNavigator not found'); 13 | else: 14 | print ('SpaceNavigator found') 15 | print dev 16 | 17 | 18 | # Don't need all this but may want it for a full implementation 19 | 20 | cfg = dev.get_active_configuration() 21 | print 'cfg is ', cfg 22 | intf = cfg[(0,0)] 23 | print 'intf is ', intf 24 | ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN) 25 | print 'ep is ', ep 26 | 27 | reattach = False 28 | if dev.is_kernel_driver_active(0): 29 | reattach = True 30 | dev.detach_kernel_driver(0) 31 | 32 | ep_in = dev[0][(0,0)][0] 33 | ep_out = dev[0][(0,0)][1] 34 | 35 | print '' 36 | print 'Exit by pressing any button on the SpaceNavigator' 37 | print '' 38 | 39 | run = True 40 | while run: 41 | try: 42 | data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) 43 | # raw data 44 | # print data 45 | 46 | # print it correctly T: x,y,z R: x,y,z 47 | if data[0] == 1: 48 | # translation packet 49 | tx = data[1] + (data[2]*256) 50 | ty = data[3] + (data[4]*256) 51 | tz = data[5] + (data[6]*256) 52 | 53 | if data[2] > 127: 54 | tx -= 65536 55 | if data[4] > 127: 56 | ty -= 65536 57 | if data[6] > 127: 58 | tz -= 65536 59 | print "T: ", tx, ty, tz 60 | 61 | if data[0] == 2: 62 | # rotation packet 63 | rx = data[1] + (data[2]*256) 64 | ry = data[3] + (data[4]*256) 65 | rz = data[5] + (data[6]*256) 66 | 67 | if data[2] > 127: 68 | rx -= 65536 69 | if data[4] > 127: 70 | ry -= 65536 71 | if data[6] > 127: 72 | rz -= 65536 73 | print "R: ", rx, ry, rz 74 | 75 | if data[0] == 3 and data[1] == 0: 76 | # button packet - exit on the release 77 | run = False 78 | 79 | except usb.core.USBError: 80 | print("USB error") 81 | except: 82 | print("read failed") 83 | # end while 84 | usb.util.dispose_resources(dev) 85 | 86 | if reattach: 87 | dev.attach_kernel_driver(0) 88 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 3d-mouse-rpi-python 2 | This project shows simple ways to access a 3D mouse on a Raspberry Pi. 3 | 4 | The samples are written in python. They rely on some standard imports. 5 | Unless otherwise mentioned, they have been developed on a RPi 3. 6 | 7 | HelloSpaceNavigator.py - opens a SpaceNavigator and prints the data to stdout 8 | 9 | SpaceNavigatorTxLEDs.py - reads a SN and lights up LEDs based on the data of the tx axis. One LED for positive; one for negative direction., 10 | 11 | SpaceNavigatorLEDs.py - reads a SN and lights up LEDs connected to every axis. 12 LEDs (and resistors) connected to 12 pins. One for each positive and one for each negative half-axis. Shows which half-axis is active. Doesn't show the relative value of that axis. Just on/off when abs(val) is != 0. 12 | 13 | SNLEDsPWM.py - reads the SN and lights up LEDs with an intensity proportional to the magnitude of the half-axis. It uses PWM (Pulse Width Modulation). That is, it turns the LED on/off really fast to achieve the dimming. This is a good start for any type of motor controller. The frequence can be set. By default it is 100Hz. This requries python3.4 because of the mechanism used to communicate with the PWM threads. 14 | 15 | SNLEDsPWMnl.py - same as SNLEDsPWM.py but uses a non-linear (squared) warping on the data to add more precision at the low end. 16 | The eye doesn't respond linearly to input. This makes the LEDs look more responsive. It is also probably more useful for motor control. 17 | 18 | SNDrive.py - SN drives a small tracked Pololu Zumo tank kit. Two micromotors on the front wheels. Ty goes forward/backwards. Rz turns it. Using the non-linear data it gives quite good user control. With encoders on the motors, and a feedback loop, I could have exceptional control. 19 | -------------------------------------------------------------------------------- /SNDrive.py: -------------------------------------------------------------------------------- 1 | # SNDrive.py - SpaceNavigator output for two motors to drive a car. 2 | # Also sends PRM nonlinear output to LEDs 3 | # 4 | # See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455 5 | # Run python3.4 as root (sudo -i ...) 6 | # (requires python3 for pypubsub) 7 | # python3 SNMotors.py 8 | 9 | # This sample reads the 3D mouse and uses Pulse Width Modulation to light 10 | # the LEDs relative to the magnitude of the half-axis value. 11 | # It also outputs the same PWM signals to two motors connected to an L298 12 | # H-bridge motor controller 13 | # 14 | # It is a bit more complex because threads need to be run for each axis to implement the PWM. 15 | # The main loop reads the USB device and sends the data to the threads using pypubsub. 16 | # 17 | 18 | import usb.core 19 | import usb.util 20 | import sys 21 | from time import gmtime, strftime 22 | import time 23 | import RPi.GPIO as GPIO 24 | import threading 25 | from pubsub import pub 26 | 27 | def NonLinear( val ): 28 | "Warp the incoming data to get more precision at the low end" 29 | val = val*(val*val)/(350*350) 30 | 31 | #comment this out if you want to implement a dead zone close to zero. 32 | #it helps to isolate user input 33 | if val > 0 and val < 1: 34 | val = 1 35 | 36 | return val 37 | 38 | ############################ Classes ####################################### 39 | class PWMThread(threading.Thread): 40 | # class variables 41 | updateRate = 100 # Hz 42 | period = 1 / updateRate 43 | axisRange = 350 44 | 45 | def __init__(self, pos, neg, event): 46 | self.posPin = pos 47 | self.negPin = neg 48 | self.axisVal = 0 49 | theThread = threading.Thread(target=self.PWMAxis) 50 | theThread.daemon = True 51 | theThread.start() 52 | 53 | pub.subscribe(self.setAxisValue, event) 54 | 55 | 56 | def setAxisValue(self, val): 57 | self.axisVal = val 58 | #print("The axis Val is now ", self.axisVal); 59 | return 60 | 61 | def PWMAxis(self): 62 | "PWM the LED on an axis" 63 | # Need fast cycle rates (>100Hz) to make the LEDs look like they are dimming 64 | while True: 65 | if self.axisVal < 0: 66 | pin = self.negPin 67 | val = -self.axisVal 68 | onTime = val/PWMThread.axisRange * PWMThread.period 69 | offTime = PWMThread.period - onTime 70 | GPIO.output(self.posPin, GPIO.LOW) 71 | elif self.axisVal > 0: 72 | pin = self.posPin 73 | val = self.axisVal 74 | onTime = val/PWMThread.axisRange * PWMThread.period 75 | offTime = PWMThread.period - onTime 76 | GPIO.output(self.negPin, GPIO.LOW) 77 | else: 78 | pin = 0 79 | GPIO.output(self.posPin, GPIO.LOW) 80 | GPIO.output(self.negPin, GPIO.LOW) 81 | time.sleep(.5) # wait longer to let other threads run 82 | 83 | if pin > 0: 84 | GPIO.output(pin, GPIO.HIGH) 85 | time.sleep(onTime) 86 | GPIO.output(pin, GPIO.LOW) 87 | time.sleep(offTime) 88 | 89 | return 90 | 91 | ######################## Main ############################################## 92 | 93 | 94 | # Look for SpaceNavigator 95 | dev = usb.core.find(idVendor=0x46d, idProduct=0xc626) 96 | if dev is None: 97 | raise ValueError('SpaceNavigator not found'); 98 | else: 99 | print ('SpaceNavigator found') 100 | print (dev) 101 | 102 | # Setup GPIO pins 103 | TX_NEG_PIN = 11 104 | TX_POS_PIN = 8 105 | TY_NEG_PIN = 5 106 | TY_POS_PIN = 7 107 | TZ_NEG_PIN = 6 108 | TZ_POS_PIN = 12 109 | RX_NEG_PIN = 13 110 | RX_POS_PIN = 16 111 | RY_NEG_PIN = 19 112 | RY_POS_PIN = 20 113 | RZ_NEG_PIN = 26 114 | RZ_POS_PIN = 21 115 | 116 | # pins to drive the motors left/right forward/backward 117 | MOT_RIGHT_FWD_PIN =17 118 | MOT_RIGHT_BCK_PIN = 27 119 | MOT_LEFT_FWD_PIN = 22 120 | MOT_LEFT_BCK_PIN = 18 121 | 122 | GPIO.setmode(GPIO.BCM) # logical numbering (and BCM Motorsports) 123 | GPIO.setwarnings(False); 124 | 125 | GPIO.setup(TX_NEG_PIN,GPIO.OUT) 126 | GPIO.setup(TX_POS_PIN,GPIO.OUT) 127 | GPIO.setup(TY_NEG_PIN,GPIO.OUT) 128 | GPIO.setup(TY_POS_PIN,GPIO.OUT) 129 | GPIO.setup(TZ_NEG_PIN,GPIO.OUT) 130 | GPIO.setup(TZ_POS_PIN,GPIO.OUT) 131 | GPIO.setup(RX_NEG_PIN,GPIO.OUT) 132 | GPIO.setup(RX_POS_PIN,GPIO.OUT) 133 | GPIO.setup(RY_NEG_PIN,GPIO.OUT) 134 | GPIO.setup(RY_POS_PIN,GPIO.OUT) 135 | GPIO.setup(RZ_NEG_PIN,GPIO.OUT) 136 | GPIO.setup(RZ_POS_PIN,GPIO.OUT) 137 | 138 | GPIO.setup(MOT_RIGHT_FWD_PIN,GPIO.OUT) 139 | GPIO.setup(MOT_RIGHT_BCK_PIN,GPIO.OUT) 140 | GPIO.setup(MOT_LEFT_FWD_PIN,GPIO.OUT) 141 | GPIO.setup(MOT_LEFT_BCK_PIN,GPIO.OUT) 142 | 143 | # Don't need all this but may want it for a full implementation 144 | 145 | cfg = dev.get_active_configuration() 146 | print ('cfg is ', cfg) 147 | intf = cfg[(0,0)] 148 | print ('intf is ', intf) 149 | ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN) 150 | print ('ep is ', ep) 151 | 152 | reattach = False 153 | if dev.is_kernel_driver_active(0): 154 | reattach = True 155 | dev.detach_kernel_driver(0) 156 | 157 | ep_in = dev[0][(0,0)][0] 158 | ep_out = dev[0][(0,0)][1] 159 | 160 | # Make threads for the 6 axes 161 | txThread = PWMThread(pos=TX_POS_PIN, neg=TX_NEG_PIN, event='tx') 162 | tyThread = PWMThread(pos=TY_POS_PIN, neg=TY_NEG_PIN, event='ty') 163 | tzThread = PWMThread(pos=TZ_POS_PIN, neg=TZ_NEG_PIN, event='tz') 164 | rxThread = PWMThread(pos=RX_POS_PIN, neg=RX_NEG_PIN, event='rx') 165 | ryThread = PWMThread(pos=RY_POS_PIN, neg=RY_NEG_PIN, event='ry') 166 | rzThread = PWMThread(pos=RZ_POS_PIN, neg=RZ_NEG_PIN, event='rz') 167 | 168 | motRightThread = PWMThread(pos=MOT_RIGHT_FWD_PIN, neg=MOT_RIGHT_BCK_PIN, event='motRight') 169 | motLeftThread = PWMThread(pos=MOT_LEFT_FWD_PIN, neg=MOT_LEFT_BCK_PIN, event='motLeft') 170 | 171 | print ('') 172 | print ('Exit by pressing any button on the SpaceNavigator') 173 | print ('') 174 | 175 | run = True 176 | tx = 0 177 | ty = 0 178 | tz = 0 179 | rx = 0 180 | ry = 0 181 | rz = 0 182 | while run: 183 | try: 184 | data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) 185 | # raw data 186 | # print data 187 | 188 | # print it correctly T: x,y,z R: x,y,z 189 | if data[0] == 1: 190 | # translation packet 191 | tx = data[1] + (data[2]*256) 192 | ty = data[3] + (data[4]*256) 193 | tz = data[5] + (data[6]*256) 194 | 195 | if data[2] > 127: 196 | tx -= 65536 197 | if data[4] > 127: 198 | ty -= 65536 199 | if data[6] > 127: 200 | tz -= 65536 201 | print ("T: ", tx, ty, tz) 202 | tx = NonLinear(tx); 203 | ty = NonLinear(ty); 204 | tz = NonLinear(tz); 205 | pub.sendMessage('tx', val=tx) 206 | pub.sendMessage('ty', val=ty) 207 | pub.sendMessage('tz', val=tz) 208 | 209 | if data[0] == 2: 210 | # rotation packet 211 | rx = data[1] + (data[2]*256) 212 | ry = data[3] + (data[4]*256) 213 | rz = data[5] + (data[6]*256) 214 | 215 | if data[2] > 127: 216 | rx -= 65536 217 | if data[4] > 127: 218 | ry -= 65536 219 | if data[6] > 127: 220 | rz -= 65536 221 | print ("R: ", rx, ry, rz) 222 | 223 | rx=NonLinear(rx) 224 | ry=NonLinear(ry) 225 | rz=NonLinear(rz) 226 | pub.sendMessage('rx', val=rx) 227 | pub.sendMessage('ry', val=ry) 228 | pub.sendMessage('rz', val=rz) 229 | 230 | if data[0] == 3 and data[1] == 0: 231 | # button packet - exit on the release 232 | run = False 233 | 234 | # -ty controls forward movement, +ty backwards 235 | # -rz turns right, rz turns left 236 | motRight = -ty - rz; 237 | motLeft = -ty + rz; 238 | if motRight > 350: 239 | motRight = 350; 240 | if motRight < -350: 241 | motRight = -350; 242 | if motLeft > 350: 243 | motLeft = 350; 244 | if motLeft < -350: 245 | motLeft = -350; 246 | pub.sendMessage('motRight', val=motRight); 247 | pub.sendMessage('motLeft', val=motLeft); 248 | 249 | except usb.core.USBError: 250 | print("USBError") 251 | # except: 252 | # print("read failed") 253 | # end while 254 | 255 | # Cleanup GPIO pins 256 | GPIO.cleanup() 257 | 258 | usb.util.dispose_resources(dev) 259 | 260 | if reattach: 261 | dev.attach_kernel_driver(0) 262 | 263 | -------------------------------------------------------------------------------- /SNLEDsPWM.py: -------------------------------------------------------------------------------- 1 | # See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455 2 | # Run python3.4 as root (sudo -i ...) 3 | # (requires python3.4 for pypubsub) 4 | 5 | # This sample reads the 3D mouse and uses Pulse Width Modulation to light the LEDs relative to the magnitude of the half-axis value 6 | # 7 | # It is a bit more complex because threads need to be run for each axis to implement the PWM. 8 | # The main loop reads the USB device and sends the data to the threads using pypubsub. 9 | # 10 | 11 | import usb.core 12 | import usb.util 13 | import sys 14 | from time import gmtime, strftime 15 | import time 16 | import RPi.GPIO as GPIO 17 | import threading 18 | from pubsub import pub 19 | 20 | 21 | 22 | ############################ Classes ####################################### 23 | class PWMThread(threading.Thread): 24 | # class variables 25 | updateRate = 100 # Hz 26 | period = 1 / updateRate 27 | axisRange = 350 28 | 29 | def __init__(self, pos, neg, event): 30 | self.posPin = pos 31 | self.negPin = neg 32 | self.axisVal = 0 33 | theThread = threading.Thread(target=self.PWMAxis) 34 | theThread.daemon = True 35 | theThread.start() 36 | 37 | pub.subscribe(self.setAxisValue, event) 38 | 39 | 40 | def setAxisValue(self, val): 41 | self.axisVal = val 42 | #print("The axis Val is now ", self.axisVal); 43 | return 44 | 45 | def PWMAxis(self): 46 | "PWM the LED on an axis" 47 | # Need fast cycle rates (>100Hz) to make the LEDs look like they are dimming 48 | while True: 49 | if self.axisVal < 0: 50 | pin = self.negPin 51 | val = -self.axisVal 52 | onTime = val/PWMThread.axisRange * PWMThread.period 53 | offTime = PWMThread.period - onTime 54 | GPIO.output(self.posPin, GPIO.LOW) 55 | elif self.axisVal > 0: 56 | pin = self.posPin 57 | val = self.axisVal 58 | onTime = val/PWMThread.axisRange * PWMThread.period 59 | offTime = PWMThread.period - onTime 60 | GPIO.output(self.negPin, GPIO.LOW) 61 | else: 62 | pin = 0 63 | GPIO.output(self.posPin, GPIO.LOW) 64 | GPIO.output(self.negPin, GPIO.LOW) 65 | time.sleep(.5) # wait longer to let other threads run 66 | 67 | if pin > 0: 68 | GPIO.output(pin, GPIO.HIGH) 69 | time.sleep(onTime) 70 | GPIO.output(pin, GPIO.LOW) 71 | time.sleep(offTime) 72 | 73 | return 74 | 75 | ######################## Main ############################################## 76 | 77 | 78 | # Look for SpaceNavigator 79 | dev = usb.core.find(idVendor=0x46d, idProduct=0xc626) 80 | if dev is None: 81 | raise ValueError('SpaceNavigator not found'); 82 | else: 83 | print ('SpaceNavigator found') 84 | print (dev) 85 | 86 | # Setup GPIO pins 87 | TX_NEG_PIN = 11 88 | TX_POS_PIN = 8 89 | TY_NEG_PIN = 5 90 | TY_POS_PIN = 7 91 | TZ_NEG_PIN = 6 92 | TZ_POS_PIN = 12 93 | RX_NEG_PIN = 13 94 | RX_POS_PIN = 16 95 | RY_NEG_PIN = 19 96 | RY_POS_PIN = 20 97 | RZ_NEG_PIN = 26 98 | RZ_POS_PIN = 21 99 | 100 | GPIO.setmode(GPIO.BCM) # logical numbering 101 | GPIO.setwarnings(False); 102 | 103 | GPIO.setup(TX_NEG_PIN,GPIO.OUT) 104 | GPIO.setup(TX_POS_PIN,GPIO.OUT) 105 | GPIO.setup(TY_NEG_PIN,GPIO.OUT) 106 | GPIO.setup(TY_POS_PIN,GPIO.OUT) 107 | GPIO.setup(TZ_NEG_PIN,GPIO.OUT) 108 | GPIO.setup(TZ_POS_PIN,GPIO.OUT) 109 | GPIO.setup(RX_NEG_PIN,GPIO.OUT) 110 | GPIO.setup(RX_POS_PIN,GPIO.OUT) 111 | GPIO.setup(RY_NEG_PIN,GPIO.OUT) 112 | GPIO.setup(RY_POS_PIN,GPIO.OUT) 113 | GPIO.setup(RZ_NEG_PIN,GPIO.OUT) 114 | GPIO.setup(RZ_POS_PIN,GPIO.OUT) 115 | 116 | # Don't need all this but may want it for a full implementation 117 | 118 | cfg = dev.get_active_configuration() 119 | print ('cfg is ', cfg) 120 | intf = cfg[(0,0)] 121 | print ('intf is ', intf) 122 | ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN) 123 | print ('ep is ', ep) 124 | 125 | reattach = False 126 | if dev.is_kernel_driver_active(0): 127 | reattach = True 128 | dev.detach_kernel_driver(0) 129 | 130 | ep_in = dev[0][(0,0)][0] 131 | ep_out = dev[0][(0,0)][1] 132 | 133 | # Make threads for the 6 axes 134 | txThread = PWMThread(pos=TX_POS_PIN, neg=TX_NEG_PIN, event='tx') 135 | tyThread = PWMThread(pos=TY_POS_PIN, neg=TY_NEG_PIN, event='ty') 136 | tzThread = PWMThread(pos=TZ_POS_PIN, neg=TZ_NEG_PIN, event='tz') 137 | rxThread = PWMThread(pos=RX_POS_PIN, neg=RX_NEG_PIN, event='rx') 138 | ryThread = PWMThread(pos=RY_POS_PIN, neg=RY_NEG_PIN, event='ry') 139 | rzThread = PWMThread(pos=RZ_POS_PIN, neg=RZ_NEG_PIN, event='rz') 140 | 141 | 142 | print ('') 143 | print ('Exit by pressing any button on the SpaceNavigator') 144 | print ('') 145 | 146 | run = True 147 | while run: 148 | try: 149 | data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) 150 | # raw data 151 | # print data 152 | 153 | # print it correctly T: x,y,z R: x,y,z 154 | if data[0] == 1: 155 | # translation packet 156 | tx = data[1] + (data[2]*256) 157 | ty = data[3] + (data[4]*256) 158 | tz = data[5] + (data[6]*256) 159 | 160 | if data[2] > 127: 161 | tx -= 65536 162 | if data[4] > 127: 163 | ty -= 65536 164 | if data[6] > 127: 165 | tz -= 65536 166 | print ("T: ", tx, ty, tz) 167 | pub.sendMessage('tx', val=tx) 168 | pub.sendMessage('ty', val=ty) 169 | pub.sendMessage('tz', val=tz) 170 | 171 | if data[0] == 2: 172 | # rotation packet 173 | rx = data[1] + (data[2]*256) 174 | ry = data[3] + (data[4]*256) 175 | rz = data[5] + (data[6]*256) 176 | 177 | if data[2] > 127: 178 | rx -= 65536 179 | if data[4] > 127: 180 | ry -= 65536 181 | if data[6] > 127: 182 | rz -= 65536 183 | print ("R: ", rx, ry, rz) 184 | 185 | pub.sendMessage('rx', val=rx) 186 | pub.sendMessage('ry', val=ry) 187 | pub.sendMessage('rz', val=rz) 188 | 189 | if data[0] == 3 and data[1] == 0: 190 | # button packet - exit on the release 191 | run = False 192 | 193 | 194 | except usb.core.USBError: 195 | print("USBError") 196 | # except: 197 | # print("read failed") 198 | # end while 199 | 200 | # Cleanup GPIO pins 201 | GPIO.cleanup() 202 | 203 | usb.util.dispose_resources(dev) 204 | 205 | if reattach: 206 | dev.attach_kernel_driver(0) 207 | 208 | -------------------------------------------------------------------------------- /SNLEDsPWMnl.py: -------------------------------------------------------------------------------- 1 | # SNLEDsPWMnl.py - SpaceNavigator output to LEDs with PWM and intensity non-linearly related to input (adds more precision at the low end) 2 | # 3 | # See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455 4 | # Run python3.4 as root (sudo -i ...) 5 | # (requires python3.4 for pypubsub) 6 | 7 | # This sample reads the 3D mouse and uses Pulse Width Modulation to light the LEDs relative to the magnitude of the half-axis value 8 | # 9 | # It is a bit more complex because threads need to be run for each axis to implement the PWM. 10 | # The main loop reads the USB device and sends the data to the threads using pypubsub. 11 | # 12 | 13 | import usb.core 14 | import usb.util 15 | import sys 16 | from time import gmtime, strftime 17 | import time 18 | import RPi.GPIO as GPIO 19 | import threading 20 | from pubsub import pub 21 | 22 | def NonLinear( val ): 23 | "Warp the incoming data to get more precision at the low end" 24 | val = val*(val*val)/(350*350) 25 | 26 | #comment this out if you want to implement a dead zone close to zero. 27 | #it helps to isolate user input 28 | if val > 0 and val < 1: 29 | val = 1 30 | 31 | return val 32 | 33 | ############################ Classes ####################################### 34 | class PWMThread(threading.Thread): 35 | # class variables 36 | updateRate = 100 # Hz 37 | period = 1 / updateRate 38 | axisRange = 350 39 | 40 | def __init__(self, pos, neg, event): 41 | self.posPin = pos 42 | self.negPin = neg 43 | self.axisVal = 0 44 | theThread = threading.Thread(target=self.PWMAxis) 45 | theThread.daemon = True 46 | theThread.start() 47 | 48 | pub.subscribe(self.setAxisValue, event) 49 | 50 | 51 | def setAxisValue(self, val): 52 | self.axisVal = val 53 | #print("The axis Val is now ", self.axisVal); 54 | return 55 | 56 | def PWMAxis(self): 57 | "PWM the LED on an axis" 58 | # Need fast cycle rates (>100Hz) to make the LEDs look like they are dimming 59 | while True: 60 | if self.axisVal < 0: 61 | pin = self.negPin 62 | val = -self.axisVal 63 | onTime = val/PWMThread.axisRange * PWMThread.period 64 | offTime = PWMThread.period - onTime 65 | GPIO.output(self.posPin, GPIO.LOW) 66 | elif self.axisVal > 0: 67 | pin = self.posPin 68 | val = self.axisVal 69 | onTime = val/PWMThread.axisRange * PWMThread.period 70 | offTime = PWMThread.period - onTime 71 | GPIO.output(self.negPin, GPIO.LOW) 72 | else: 73 | pin = 0 74 | GPIO.output(self.posPin, GPIO.LOW) 75 | GPIO.output(self.negPin, GPIO.LOW) 76 | time.sleep(.5) # wait longer to let other threads run 77 | 78 | if pin > 0: 79 | GPIO.output(pin, GPIO.HIGH) 80 | time.sleep(onTime) 81 | GPIO.output(pin, GPIO.LOW) 82 | time.sleep(offTime) 83 | 84 | return 85 | 86 | ######################## Main ############################################## 87 | 88 | 89 | # Look for SpaceNavigator 90 | dev = usb.core.find(idVendor=0x46d, idProduct=0xc626) 91 | if dev is None: 92 | raise ValueError('SpaceNavigator not found'); 93 | else: 94 | print ('SpaceNavigator found') 95 | print (dev) 96 | 97 | # Setup GPIO pins 98 | TX_NEG_PIN = 11 99 | TX_POS_PIN = 8 100 | TY_NEG_PIN = 5 101 | TY_POS_PIN = 7 102 | TZ_NEG_PIN = 6 103 | TZ_POS_PIN = 12 104 | RX_NEG_PIN = 13 105 | RX_POS_PIN = 16 106 | RY_NEG_PIN = 19 107 | RY_POS_PIN = 20 108 | RZ_NEG_PIN = 26 109 | RZ_POS_PIN = 21 110 | 111 | GPIO.setmode(GPIO.BCM) # logical numbering 112 | GPIO.setwarnings(False); 113 | 114 | GPIO.setup(TX_NEG_PIN,GPIO.OUT) 115 | GPIO.setup(TX_POS_PIN,GPIO.OUT) 116 | GPIO.setup(TY_NEG_PIN,GPIO.OUT) 117 | GPIO.setup(TY_POS_PIN,GPIO.OUT) 118 | GPIO.setup(TZ_NEG_PIN,GPIO.OUT) 119 | GPIO.setup(TZ_POS_PIN,GPIO.OUT) 120 | GPIO.setup(RX_NEG_PIN,GPIO.OUT) 121 | GPIO.setup(RX_POS_PIN,GPIO.OUT) 122 | GPIO.setup(RY_NEG_PIN,GPIO.OUT) 123 | GPIO.setup(RY_POS_PIN,GPIO.OUT) 124 | GPIO.setup(RZ_NEG_PIN,GPIO.OUT) 125 | GPIO.setup(RZ_POS_PIN,GPIO.OUT) 126 | 127 | # Don't need all this but may want it for a full implementation 128 | 129 | cfg = dev.get_active_configuration() 130 | print ('cfg is ', cfg) 131 | intf = cfg[(0,0)] 132 | print ('intf is ', intf) 133 | ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN) 134 | print ('ep is ', ep) 135 | 136 | reattach = False 137 | if dev.is_kernel_driver_active(0): 138 | reattach = True 139 | dev.detach_kernel_driver(0) 140 | 141 | ep_in = dev[0][(0,0)][0] 142 | ep_out = dev[0][(0,0)][1] 143 | 144 | # Make threads for the 6 axes 145 | txThread = PWMThread(pos=TX_POS_PIN, neg=TX_NEG_PIN, event='tx') 146 | tyThread = PWMThread(pos=TY_POS_PIN, neg=TY_NEG_PIN, event='ty') 147 | tzThread = PWMThread(pos=TZ_POS_PIN, neg=TZ_NEG_PIN, event='tz') 148 | rxThread = PWMThread(pos=RX_POS_PIN, neg=RX_NEG_PIN, event='rx') 149 | ryThread = PWMThread(pos=RY_POS_PIN, neg=RY_NEG_PIN, event='ry') 150 | rzThread = PWMThread(pos=RZ_POS_PIN, neg=RZ_NEG_PIN, event='rz') 151 | 152 | 153 | print ('') 154 | print ('Exit by pressing any button on the SpaceNavigator') 155 | print ('') 156 | 157 | run = True 158 | while run: 159 | try: 160 | data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) 161 | # raw data 162 | # print data 163 | 164 | # print it correctly T: x,y,z R: x,y,z 165 | if data[0] == 1: 166 | # translation packet 167 | tx = data[1] + (data[2]*256) 168 | ty = data[3] + (data[4]*256) 169 | tz = data[5] + (data[6]*256) 170 | 171 | if data[2] > 127: 172 | tx -= 65536 173 | if data[4] > 127: 174 | ty -= 65536 175 | if data[6] > 127: 176 | tz -= 65536 177 | print ("T: ", tx, ty, tz) 178 | pub.sendMessage('tx', val=NonLinear(tx)) 179 | pub.sendMessage('ty', val=NonLinear(ty)) 180 | pub.sendMessage('tz', val=NonLinear(tz)) 181 | 182 | if data[0] == 2: 183 | # rotation packet 184 | rx = data[1] + (data[2]*256) 185 | ry = data[3] + (data[4]*256) 186 | rz = data[5] + (data[6]*256) 187 | 188 | if data[2] > 127: 189 | rx -= 65536 190 | if data[4] > 127: 191 | ry -= 65536 192 | if data[6] > 127: 193 | rz -= 65536 194 | print ("R: ", rx, ry, rz) 195 | 196 | pub.sendMessage('rx', val=NonLinear(rx)) 197 | pub.sendMessage('ry', val=NonLinear(ry)) 198 | pub.sendMessage('rz', val=NonLinear(rz)) 199 | 200 | if data[0] == 3 and data[1] == 0: 201 | # button packet - exit on the release 202 | run = False 203 | 204 | 205 | except usb.core.USBError: 206 | print("USBError") 207 | # except: 208 | # print("read failed") 209 | # end while 210 | 211 | # Cleanup GPIO pins 212 | GPIO.cleanup() 213 | 214 | usb.util.dispose_resources(dev) 215 | 216 | if reattach: 217 | dev.attach_kernel_driver(0) 218 | 219 | -------------------------------------------------------------------------------- /SpaceNavigatorLEDs.py: -------------------------------------------------------------------------------- 1 | # See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455 2 | # Run python2 as root (sudo /usr/bin/python2.7 /home/pi/pythondev/SpaceNavigatorLEDs.py) 3 | 4 | import usb.core 5 | import usb.util 6 | import sys 7 | from time import gmtime, strftime 8 | import time 9 | import RPi.GPIO as GPIO 10 | 11 | def setAxisPins( negPin, posPin, val ): 12 | "Set the pins for an axis" 13 | # light up either -tive or +tive LED 14 | if val < 0: 15 | posVal = GPIO.LOW 16 | negVal = val/2*2 == val # flashing may be more interesting 17 | negVal = GPIO.HIGH # or more annoying 18 | elif val > 0: 19 | negVal = GPIO.LOW 20 | posVal = val/2*2 == val 21 | posVal = GPIO.HIGH 22 | else: 23 | posVal = GPIO.LOW 24 | negVal = GPIO.LOW 25 | 26 | GPIO.output(posPin, posVal) 27 | GPIO.output(negPin, negVal) 28 | return 29 | 30 | 31 | 32 | # Look for SpaceNavigator 33 | dev = usb.core.find(idVendor=0x46d, idProduct=0xc626) 34 | if dev is None: 35 | raise ValueError('SpaceNavigator not found'); 36 | else: 37 | print ('SpaceNavigator found') 38 | print dev 39 | 40 | # Setup GPIO pins 41 | TX_NEG_PIN = 11 42 | TX_POS_PIN = 8 43 | TY_NEG_PIN = 5 44 | TY_POS_PIN = 7 45 | TZ_NEG_PIN = 6 46 | TZ_POS_PIN = 12 47 | RX_NEG_PIN = 13 48 | RX_POS_PIN = 16 49 | RY_NEG_PIN = 19 50 | RY_POS_PIN = 20 51 | RZ_NEG_PIN = 26 52 | RZ_POS_PIN = 21 53 | 54 | GPIO.setmode(GPIO.BCM) # logical numbering 55 | 56 | GPIO.setup(TX_NEG_PIN,GPIO.OUT) 57 | GPIO.setup(TX_POS_PIN,GPIO.OUT) 58 | GPIO.setup(TY_NEG_PIN,GPIO.OUT) 59 | GPIO.setup(TY_POS_PIN,GPIO.OUT) 60 | GPIO.setup(TZ_NEG_PIN,GPIO.OUT) 61 | GPIO.setup(TZ_POS_PIN,GPIO.OUT) 62 | GPIO.setup(RX_NEG_PIN,GPIO.OUT) 63 | GPIO.setup(RX_POS_PIN,GPIO.OUT) 64 | GPIO.setup(RY_NEG_PIN,GPIO.OUT) 65 | GPIO.setup(RY_POS_PIN,GPIO.OUT) 66 | GPIO.setup(RZ_NEG_PIN,GPIO.OUT) 67 | GPIO.setup(RZ_POS_PIN,GPIO.OUT) 68 | 69 | # Don't need all this but may want it for a full implementation 70 | 71 | cfg = dev.get_active_configuration() 72 | print 'cfg is ', cfg 73 | intf = cfg[(0,0)] 74 | print 'intf is ', intf 75 | ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN) 76 | print 'ep is ', ep 77 | 78 | reattach = False 79 | if dev.is_kernel_driver_active(0): 80 | reattach = True 81 | dev.detach_kernel_driver(0) 82 | 83 | ep_in = dev[0][(0,0)][0] 84 | ep_out = dev[0][(0,0)][1] 85 | 86 | print '' 87 | print 'Exit by pressing any button on the SpaceNavigator' 88 | print '' 89 | 90 | run = True 91 | while run: 92 | try: 93 | data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) 94 | # raw data 95 | # print data 96 | 97 | # print it correctly T: x,y,z R: x,y,z 98 | if data[0] == 1: 99 | # translation packet 100 | tx = data[1] + (data[2]*256) 101 | ty = data[3] + (data[4]*256) 102 | tz = data[5] + (data[6]*256) 103 | 104 | if data[2] > 127: 105 | tx -= 65536 106 | if data[4] > 127: 107 | ty -= 65536 108 | if data[6] > 127: 109 | tz -= 65536 110 | print "T: ", tx, ty, tz 111 | 112 | setAxisPins(TX_NEG_PIN, TX_POS_PIN, tx) 113 | setAxisPins(TY_NEG_PIN, TY_POS_PIN, ty) 114 | setAxisPins(TZ_NEG_PIN, TZ_POS_PIN, tz) 115 | 116 | if data[0] == 2: 117 | # rotation packet 118 | rx = data[1] + (data[2]*256) 119 | ry = data[3] + (data[4]*256) 120 | rz = data[5] + (data[6]*256) 121 | 122 | if data[2] > 127: 123 | rx -= 65536 124 | if data[4] > 127: 125 | ry -= 65536 126 | if data[6] > 127: 127 | rz -= 65536 128 | print "R: ", rx, ry, rz 129 | 130 | setAxisPins(RX_NEG_PIN, RX_POS_PIN, rx) 131 | setAxisPins(RY_NEG_PIN, RY_POS_PIN, ry) 132 | setAxisPins(RZ_NEG_PIN, RZ_POS_PIN, rz) 133 | 134 | if data[0] == 3 and data[1] == 0: 135 | # button packet - exit on the release 136 | run = False 137 | 138 | 139 | except usb.core.USBError: 140 | print("USBError") 141 | # except: 142 | # print("read failed") 143 | # end while 144 | 145 | # Cleanup GPIO pins 146 | GPIO.cleanup() 147 | 148 | usb.util.dispose_resources(dev) 149 | 150 | if reattach: 151 | dev.attach_kernel_driver(0) 152 | 153 | -------------------------------------------------------------------------------- /SpaceNavigatorTxLEDs.py: -------------------------------------------------------------------------------- 1 | # See http://stackoverflow.com/questions/29345325/raspberry-pyusb-gets-resource-busy#29347455 2 | # Run python2 as root (sudo /usr/bin/python2.7 /home/pi/pythondev/SpaceNavigatorTxLEDs.py) 3 | 4 | import usb.core 5 | import usb.util 6 | import sys 7 | from time import gmtime, strftime 8 | import time 9 | import RPi.GPIO as GPIO 10 | 11 | # Look for SpaceNavigator 12 | dev = usb.core.find(idVendor=0x46d, idProduct=0xc626) 13 | if dev is None: 14 | raise ValueError('SpaceNavigator not found'); 15 | else: 16 | print ('SpaceNavigator found') 17 | print dev 18 | 19 | # Setup GPIO pins 20 | TX_NEG_PIN = 11 21 | TX_POS_PIN = 8 22 | GPIO.setmode(GPIO.BCM) # logical numbering 23 | GPIO.setup(TX_NEG_PIN,GPIO.OUT) 24 | GPIO.setup(TX_POS_PIN,GPIO.OUT) 25 | 26 | # Don't need all this but may want it for a full implementation 27 | 28 | cfg = dev.get_active_configuration() 29 | print 'cfg is ', cfg 30 | intf = cfg[(0,0)] 31 | print 'intf is ', intf 32 | ep = usb.util.find_descriptor(intf, custom_match = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN) 33 | print 'ep is ', ep 34 | 35 | reattach = False 36 | if dev.is_kernel_driver_active(0): 37 | reattach = True 38 | dev.detach_kernel_driver(0) 39 | 40 | ep_in = dev[0][(0,0)][0] 41 | ep_out = dev[0][(0,0)][1] 42 | 43 | print '' 44 | print 'Exit by pressing any button on the SpaceNavigator' 45 | print '' 46 | 47 | run = True 48 | while run: 49 | try: 50 | data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0) 51 | # raw data 52 | # print data 53 | 54 | # print it correctly T: x,y,z R: x,y,z 55 | if data[0] == 1: 56 | # translation packet 57 | tx = data[1] + (data[2]*256) 58 | ty = data[3] + (data[4]*256) 59 | tz = data[5] + (data[6]*256) 60 | 61 | if data[2] > 127: 62 | tx -= 65536 63 | if data[4] > 127: 64 | ty -= 65536 65 | if data[6] > 127: 66 | tz -= 65536 67 | print "T: ", tx, ty, tz 68 | 69 | if data[0] == 2: 70 | # rotation packet 71 | rx = data[1] + (data[2]*256) 72 | ry = data[3] + (data[4]*256) 73 | rz = data[5] + (data[6]*256) 74 | 75 | if data[2] > 127: 76 | rx -= 65536 77 | if data[4] > 127: 78 | ry -= 65536 79 | if data[6] > 127: 80 | rz -= 65536 81 | print "R: ", rx, ry, rz 82 | 83 | if data[0] == 3 and data[1] == 0: 84 | # button packet - exit on the release 85 | run = False 86 | 87 | # light up either -tive or +tive LED 88 | if tx < 0: 89 | GPIO.output(TX_NEG_PIN, tx/2*2 == tx) # on/off for odd/even value; more interesting than steady on 90 | if tx > 0: 91 | GPIO.output(TX_POS_PIN, tx/2*2 == tx) # on/off for odd/even value; more interesting than steady on 92 | if tx == 0: 93 | # turn it off when the axis returns to zero 94 | GPIO.output(TX_NEG_PIN, GPIO.LOW) 95 | GPIO.output(TX_POS_PIN, GPIO.LOW) 96 | print '' 97 | print 'Exit by pressing any button on the SpaceNavigator' 98 | print '' 99 | 100 | except usb.core.USBError: 101 | print("USBError") 102 | # except: 103 | # print("read failed") 104 | # end while 105 | 106 | # Cleanup GPIO pins 107 | GPIO.cleanup() 108 | 109 | usb.util.dispose_resources(dev) 110 | 111 | if reattach: 112 | dev.attach_kernel_driver(0) 113 | 114 | --------------------------------------------------------------------------------