collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Joseph RyanMerge pull request #8 from ut-ras/motor22f4007

master
9.9 KiB260 linesraw
1""" Xbox 360 controller support for Python
211/9/13 - Steven Jacobs
3
4This class module supports reading a connected xbox controller.
5It requires that xboxdrv be installed first:
6
7    sudo apt-get install xboxdrv
8
9See http://pingus.seul.org/~grumbel/xboxdrv/ for details on xboxdrv
10
11Example usage:
12
13    import xbox
14    joy = xbox.Joystick()         #Initialize joystick
15
16    if joy.A():                   #Test state of the A button (1=pressed, 0=not pressed)
17        print 'A button pressed'
18    x_axis   = joy.leftX()        #X-axis of the left stick (values -1.0 to 1.0)
19    (x,y)    = joy.leftStick()    #Returns tuple containing left X and Y axes (values -1.0 to 1.0)
20    trigger  = joy.rightTrigger() #Right trigger position (values 0 to 1.0)
21
22    joy.close()                   #Cleanup before exit
23"""
24
25import subprocess
26import os
27import select
28import time
29
30
31class Joystick:
32    """Initializes the joystick/wireless receiver, launching 'xboxdrv' as a subprocess
33    and checking that the wired joystick or wireless receiver is attached.
34    The refreshRate determines the maximnum rate at which events are polled from xboxdrv.
35    Calling any of the Joystick methods will cause a refresh to occur, if refreshTime has elapsed.
36    Routinely call a Joystick method, at least once per second, to avoid overfilling the event buffer.
37
38    Usage:
39        joy = xbox.Joystick()
40    """
41
42    def __init__(self, refreshRate=30):
43        self.proc = subprocess.Popen(
44            ['xboxdrv', '--no-uinput', '--detach-kernel-driver'],
45            stdout=subprocess.PIPE)
46        self.pipe = self.proc.stdout
47        #
48        self.connectStatus = False  #will be set to True once controller is detected and stays on
49        self.reading = '0' * 140  #initialize stick readings to all zeros
50        #
51        self.refreshTime = 0  #absolute time when next refresh (read results from xboxdrv stdout pipe) is to occur
52        self.refreshDelay = 1.0 / refreshRate  #joystick refresh is to be performed 30 times per sec by default
53        #
54        # Read responses from 'xboxdrv' for upto 2 seconds, looking for controller/receiver to respond
55        found = False
56        waitTime = time.time() + 2
57        while waitTime > time.time() and not found:
58            readable, writeable, exception = select.select([self.pipe], [], [],
59                                                           0)
60            if readable:
61                response = self.pipe.readline()
62                # Hard fail if we see this, so force an error
63                if response[0:7] == 'No Xbox':
64                    raise IOError('No Xbox controller/receiver found')
65                # Success if we see the following
66                if response[0:12] == 'Press Ctrl-c':
67                    found = True
68                # If we see 140 char line, we are seeing valid input
69                if len(response) == 140:
70                    found = True
71                    self.connectStatus = True
72                    self.reading = response
73        # if the controller wasn't found, then halt
74        if not found:
75            self.close()
76            raise IOError(
77                'Unable to detect Xbox controller/receiver - Run python as sudo'
78            )
79
80    """Used by all Joystick methods to read the most recent events from xboxdrv.
81    The refreshRate determines the maximum frequency with which events are checked.
82    If a valid event response is found, then the controller is flagged as 'connected'.
83    """
84
85    def refresh(self):
86        # Refresh the joystick readings based on regular defined freq
87        if self.refreshTime < time.time():
88            self.refreshTime = time.time(
89            ) + self.refreshDelay  #set next refresh time
90            # If there is text available to read from xboxdrv, then read it.
91            readable, writeable, exception = select.select([self.pipe], [], [],
92                                                           0)
93            if readable:
94                # Read every line that is availabe.  We only need to decode the last one.
95                while readable:
96                    response = self.pipe.readline()
97                    # A zero length response means controller has been unplugged.
98                    if len(response) == 0:
99                        raise IOError('Xbox controller disconnected from USB')
100                    readable, writeable, exception = select.select([self.pipe],
101                                                                   [], [], 0)
102                # Valid controller response will be 140 chars.
103                if len(response) == 140:
104                    self.connectStatus = True
105                    self.reading = response
106                else:  #Any other response means we have lost wireless or controller battery
107                    self.connectStatus = False
108
109    """Return a status of True, when the controller is actively connected.
110    Either loss of wireless signal or controller powering off will break connection.  The
111    controller inputs will stop updating, so the last readings will remain in effect.  It is
112    good practice to only act upon inputs if the controller is connected.  For instance, for
113    a robot, stop all motors if "not connected()".
114
115    An inital controller input, stick movement or button press, may be required before the connection
116    status goes True.  If a connection is lost, the connection will resume automatically when the
117    fault is corrected.
118    """
119
120    def connected(self):
121        self.refresh()
122        return self.connectStatus
123
124    # Left stick X axis value scaled between -1.0 (left) and 1.0 (right) with deadzone tolerance correction
125    def leftX(self, deadzone=4000):
126        self.refresh()
127        raw = int(self.reading[3:9])
128        return self.axisScale(raw, deadzone)
129
130    # Left stick Y axis value scaled between -1.0 (down) and 1.0 (up)
131    def leftY(self, deadzone=4000):
132        self.refresh()
133        raw = int(self.reading[13:19])
134        return self.axisScale(raw, deadzone)
135
136    # Right stick X axis value scaled between -1.0 (left) and 1.0 (right)
137    def rightX(self, deadzone=4000):
138        self.refresh()
139        raw = int(self.reading[24:30])
140        return self.axisScale(raw, deadzone)
141
142    # Right stick Y axis value scaled between -1.0 (down) and 1.0 (up)
143    def rightY(self, deadzone=4000):
144        self.refresh()
145        raw = int(self.reading[34:40])
146        return self.axisScale(raw, deadzone)
147
148    # Scale raw (-32768 to +32767) axis with deadzone correcion
149    # Deadzone is +/- range of values to consider to be center stick (ie. 0.0)
150    def axisScale(self, raw, deadzone):
151        if abs(raw) < deadzone:
152            return 0.0
153        else:
154            if raw < 0:
155                return (raw + deadzone) / (32768.0 - deadzone)
156            else:
157                return (raw - deadzone) / (32767.0 - deadzone)
158
159    # Dpad Up status - returns 1 (pressed) or 0 (not pressed)
160    def dpadUp(self):
161        self.refresh()
162        return int(self.reading[45:46])
163
164    # Dpad Down status - returns 1 (pressed) or 0 (not pressed)
165    def dpadDown(self):
166        self.refresh()
167        return int(self.reading[50:51])
168
169    # Dpad Left status - returns 1 (pressed) or 0 (not pressed)
170    def dpadLeft(self):
171        self.refresh()
172        return int(self.reading[55:56])
173
174    # Dpad Right status - returns 1 (pressed) or 0 (not pressed)
175    def dpadRight(self):
176        self.refresh()
177        return int(self.reading[60:61])
178
179    # Back button status - returns 1 (pressed) or 0 (not pressed)
180    def Back(self):
181        self.refresh()
182        return int(self.reading[68:69])
183
184    # Guide button status - returns 1 (pressed) or 0 (not pressed)
185    def Guide(self):
186        self.refresh()
187        return int(self.reading[76:77])
188
189    # Start button status - returns 1 (pressed) or 0 (not pressed)
190    def Start(self):
191        self.refresh()
192        return int(self.reading[84:85])
193
194    # Left Thumbstick button status - returns 1 (pressed) or 0 (not pressed)
195    def leftThumbstick(self):
196        self.refresh()
197        return int(self.reading[90:91])
198
199    # Right Thumbstick button status - returns 1 (pressed) or 0 (not pressed)
200    def rightThumbstick(self):
201        self.refresh()
202        return int(self.reading[95:96])
203
204    # A button status - returns 1 (pressed) or 0 (not pressed)
205    def A(self):
206        self.refresh()
207        return int(self.reading[100:101])
208
209    # B button status - returns 1 (pressed) or 0 (not pressed)
210    def B(self):
211        self.refresh()
212        return int(self.reading[104:105])
213
214    # X button status - returns 1 (pressed) or 0 (not pressed)
215    def X(self):
216        self.refresh()
217        return int(self.reading[108:109])
218
219    # Y button status - returns 1 (pressed) or 0 (not pressed)
220    def Y(self):
221        self.refresh()
222        return int(self.reading[112:113])
223
224    # Left Bumper button status - returns 1 (pressed) or 0 (not pressed)
225    def leftBumper(self):
226        self.refresh()
227        return int(self.reading[118:119])
228
229    # Right Bumper button status - returns 1 (pressed) or 0 (not pressed)
230    def rightBumper(self):
231        self.refresh()
232        return int(self.reading[123:124])
233
234    # Left Trigger value scaled between 0.0 to 1.0
235    def leftTrigger(self):
236        self.refresh()
237        return int(self.reading[129:132]) / 255.0
238
239    # Right trigger value scaled between 0.0 to 1.0
240    def rightTrigger(self):
241        self.refresh()
242        return int(self.reading[136:139]) / 255.0
243
244    # Returns tuple containing X and Y axis values for Left stick scaled between -1.0 to 1.0
245    # Usage:
246    #     x,y = joy.leftStick()
247    def leftStick(self, deadzone=4000):
248        self.refresh()
249        return (self.leftX(deadzone), self.leftY(deadzone))
250
251    # Returns tuple containing X and Y axis values for Right stick scaled between -1.0 to 1.0
252    # Usage:
253    #     x,y = joy.rightStick()
254    def rightStick(self, deadzone=4000):
255        self.refresh()
256        return (self.rightX(deadzone), self.rightY(deadzone))
257
258    # Cleanup by ending the xboxdrv subprocess
259    def close(self):
260        os.system('pkill xboxdrv')