source: trunk/brainstorms/Puzzlebox/Brainstorms/Wheelchair_Control.py @ 247

Last change on this file since 247 was 247, checked in by sc, 12 years ago

Interface.py:

  • Fixed bugs with device port connections

Wheelchair_Control.py:

  • Turning speeds reduced

setup.py:

  • whitespace cleanup
  • Property svn:executable set to *
File size: 9.1 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Wheelchair Control
5#
6# Copyright Puzzlebox Productions, LLC (2010)
7#
8# This code is released under the GNU Pulic License (GPL) version 2
9# For more information please refer to http://www.gnu.org/copyleft/gpl.html
10
11__changelog__ = """
12Last Update: 2010.12.04
13
14"""
15
16import sys
17import time
18import signal
19#import parallel
20import serial
21
22try:
23        import PySide as PyQt4
24        from PySide import QtCore
25except:
26        print "Using PyQt4 module"
27        from PyQt4 import QtCore
28else:
29        print "Using PySide module"
30
31#####################################################################
32# Globals
33#####################################################################
34
35DEBUG = 1
36
37DEFAULT_COMMAND = 'console'
38DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0'
39
40DEFAULT_WHEELCHAIR_SPEED = 1
41DEFAULT_WHEELCHAIR_COMMAND = 'stop'
42
43ARDUINO_INITIALIZATION_TIME = 2
44COMMAND_CHARACTER = 'x'
45GUI_SLEEP_TIMER = 1 * 100 # 100ms
46
47WHEELCHAIR_COMMANDS = {
48        1: { # speed
49                'forward': '00110001',
50                'reverse': '00111011',
51                'left': '10110011',
52                'right': '00010011',
53                'stop': '00110011',
54        },
55        2: { # speed
56                'forward': '00110010',
57                'reverse': '00110111',
58#               'left': '01110011',  # Turning in Speed 2 is disproportionately fast
59#               'right': '00100011', # Turning in Speed 2 is disproportionately fast
60                'left': '10110011',  # Turn Speed 1
61                'right': '00010011', # Turn Speed 1
62                'stop': '00110011',
63        },
64        3: { # speed
65                'forward': '00110000',
66                'reverse': '00111111',
67#               'left': '11110011',  # Turning in Speed 3 is too fast
68#               'right': '00000011', # Turning in Speed 3 is too fast
69                'left': '01110011',  # Turn Speed 2
70                'right': '00100011', # Turn Speed 2
71                'stop': '00110011',
72        },
73}
74
75# 'dir' : (low_pin, high_pin)
76CONTROLLS = {
77                'fwd'   : (1<<1, 1<<0),
78                'rev'   : (1<<3, 1<<2),
79                'right' : (1<<5, 1<<4),
80                'left'  : (1<<7, 1<<6),
81                }
82
83MOVE_COMANDS ={
84                ' ' : 'stop',
85                'i' : 'fwd',
86                'm' : 'rev',
87                'j' : 'left',
88                'k' : 'right',
89                }
90
91SPEED_COMMANDS = {
92                '1' : 'speed-1',
93                '2' : 'speed-2',
94                '3' : 'speed-3',
95                }
96
97STOP_TIME = 0
98STOP_INTERVAL = 0.2
99ALARM_INTERVAL = 0.1
100
101#####################################################################
102# Classes
103#####################################################################
104
105class puzzlebox_brainstorms_wheelchair_control(QtCore.QThread):
106       
107        def __init__(self, \
108                     device_address=DEFAULT_SERIAL_DEVICE, \
109                     command=DEFAULT_COMMAND, \
110                     DEBUG=DEBUG, \
111                     parent=None):
112               
113                QtCore.QThread.__init__(self, parent)
114               
115                self.log = None
116                self.DEBUG = DEBUG
117                self.parent = parent
118               
119                self.device_address = device_address
120                self.command = command
121               
122                self.wheelchair_speed = DEFAULT_WHEELCHAIR_SPEED
123                self.wheelchair_command = DEFAULT_WHEELCHAIR_COMMAND
124               
125                self.device = None
126                self.initializeSerial()
127               
128                self.keep_running = True
129       
130       
131        ##################################################################
132       
133        def initializeSerial(self):
134               
135                baudrate = 9600
136                bytesize = 8
137                parity = 'NONE'
138                stopbits = 1
139                software_flow_control = 'f'
140                rts_cts_flow_control = 't'
141                #timeout = 15
142                timeout = 5
143               
144                # convert bytesize
145                if (bytesize == 5):
146                        init_byte_size = serial.FIVEBITS
147                elif (bytesize == 6):
148                        init_byte_size = serial.SIXBITS
149                elif (bytesize == 7):
150                        init_byte_size = serial.SEVENBITS
151                elif (bytesize == 8):
152                        init_byte_size = serial.EIGHTBITS
153                else:
154                        #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
155                        init_byte_size = serial.EIGHTBITS
156               
157                # convert parity
158                if (parity == 'NONE'):
159                        init_parity = serial.PARITY_NONE
160                elif (parity == 'EVEN'):
161                        init_parity = serial.PARITY_EVEN
162                elif (parity == 'ODD'):
163                        init_parity = serial.PARITY_ODD
164                else:
165                        #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
166                        init_parity = serial.PARITY_NONE
167               
168                # convert stopbits
169                if (stopbits == 1):
170                        init_stopbits = serial.STOPBITS_ONE
171                elif (stopbits == 2):
172                        init_stopbits = serial.STOPBITS_TWO
173                else:
174                        #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
175                        init_byte_size = serial.STOPBITS_ONE
176               
177                # convert software flow control
178                if (software_flow_control == 't'):
179                        init_software_flow_control = 1
180                else:
181                        init_software_flow_control = 0
182               
183                # convert rts cts flow control
184                if (rts_cts_flow_control == 't'):
185                        init_rts_cts_flow_control = 1
186                else:
187                        init_rts_cts_flow_control = 0
188               
189                self.device = serial.Serial(port = self.device_address, \
190                                                                                baudrate = baudrate, \
191                                                                                bytesize = init_byte_size, \
192                                                                                parity = init_parity, \
193                                                                                stopbits = init_stopbits, \
194                                                                                xonxoff = init_software_flow_control, \
195                                                                                rtscts = init_rts_cts_flow_control, \
196                                                                                timeout = timeout)
197               
198               
199                self.sendCommand(self.wheelchair_speed, self.wheelchair_command)
200               
201                time.sleep(ARDUINO_INITIALIZATION_TIME)
202       
203       
204        ##################################################################
205       
206        def sendCommand(self, speed, command):
207
208                output = '%s%s' % (COMMAND_CHARACTER, \
209                        WHEELCHAIR_COMMANDS[speed][command])
210       
211                self.device.write(output)
212               
213                if self.DEBUG:
214                        print "--> Wheelchair Command: %s (Speed %i) [%s]" % \
215                           (command, speed, output)
216               
217                self.wheelchair_speed = speed
218                self.wheelchair_command = command
219       
220       
221        ##################################################################
222       
223        def alarmHandler(self, arg1, arg2):
224                print 'alarm!'
225                if STOP_TIME < time.time():
226                        self.stopBot()
227
228
229        def initAlarm(self):
230                signal.alarm(ALARM_INTERVAL)
231                signal.signal(signal.SIGALRM, alarmHandler)
232
233
234        def setOutput(self, data):
235                output = data ^ int('00110011', 2)
236               
237                output = self.int2bin(output)
238                self.device.write('x%s' % output)
239               
240                #print 'Output set to: ', int2bin(output)
241                print 'Output set to: ', output
242                print 'commands: i j k m to move, SPACE = stop, x = quit'
243
244
245        def moveBot(self, dir, speed):
246                output = 0
247                pins = CONTROLLS[dir]
248                if speed == 1:
249                        output = pins[0]
250                elif speed == 2:
251                        output = pins[1]
252                elif speed == 3:
253                        output = pins[0] | pins[1]
254                self.setOutput(output)
255                time.sleep(STOP_INTERVAL)
256                self.stopBot()
257
258
259        def stopBot(self):
260                self.setOutput(0)
261
262
263        def int2bin(self, n, count=8):
264                return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)])
265
266
267        ##################################################################
268
269        def consoleControl(self):
270
271                if (sys.platform == 'win32'):
272                        if self.DEBUG:
273                                print "---> Wheelchair Control: Console mode unavailable under Windows"
274                       
275                        self.exitThread()
276               
277               
278                MYGETCH = Getch()
279               
280                #initAlarm()
281               
282                self.stopBot()
283               
284                speed = DEFAULT_WHEELCHAIR_SPEED
285                while True:
286                        cmd = MYGETCH()
287                        if cmd == 'x':
288                                exit()
289                        if cmd in SPEED_COMMANDS.keys():
290                                speed = int(cmd)
291                                print SPEED_COMMANDS[cmd]
292                        elif cmd in MOVE_COMANDS.keys():
293                                if MOVE_COMANDS[cmd] == 'stop':
294                                        self.stopBot()
295                                else:
296                                        print MOVE_COMANDS[cmd]
297                                        self.moveBot(MOVE_COMANDS[cmd], speed)
298                                        STOP_TIME = time.time() + STOP_INTERVAL
299       
300       
301        ##################################################################
302       
303        #def guiControl(self):
304               
305                #while self.keep_running:
306                       
307                        #QtCore.QThread.msleep(GUI_SLEEP_TIMER)
308       
309       
310        ##################################################################
311       
312        def processCommand(self):
313               
314                if (self.command == 'console'):
315                        self.consoleControl()
316               
317                #elif (self.command == 'gui'):
318                        #self.gui_control()
319       
320       
321        ##################################################################
322       
323        def run(self):
324               
325                if self.DEBUG:
326                        print "<---- [%s] Main thread running" % "Wheelchair Control"
327               
328               
329                self.processCommand()
330               
331                self.exec_()
332       
333       
334        ##################################################################
335       
336        def exitThread(self, callThreadQuit=True):
337               
338                try:
339                        self.device.stop()
340                except:
341                        pass
342               
343                #self.wait()
344                if callThreadQuit:
345                        QtCore.QThread.quit(self)
346
347
348#####################################################################
349#####################################################################
350
351class Getch:
352       
353        def __init__(self):
354                import tty, sys
355       
356        def __call__(self):
357                import sys, tty, termios
358                fd = sys.stdin.fileno()
359                old_settings = termios.tcgetattr(fd)
360                try:
361                        tty.setraw(sys.stdin.fileno())
362                        ch = sys.stdin.read(1)
363                finally:
364                        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
365                return ch
366
367
368#####################################################################
369# Functions
370#####################################################################
371
372#####################################################################
373# Main
374#####################################################################
375
376if __name__ == '__main__':
377       
378        # Perform correct KeyboardInterrupt handling
379        signal.signal(signal.SIGINT, signal.SIG_DFL)
380       
381        # Collect default settings and command line parameters
382        device = DEFAULT_SERIAL_DEVICE
383        command = DEFAULT_COMMAND
384       
385        for each in sys.argv:
386               
387                if each.startswith("--device="):
388                        device = each[ len("--device="): ]
389                elif each.startswith("--command="):
390                        command = each[ len("--command="): ]
391       
392       
393        app = QtCore.QCoreApplication(sys.argv)
394       
395        wheelchair = puzzlebox_brainstorms_wheelchair_control( \
396                        device_address=device, \
397                        command=command, \
398                        DEBUG=DEBUG)
399       
400        wheelchair.start()
401       
402        sys.exit(app.exec_())
Note: See TracBrowser for help on using the repository browser.