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

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

trunk/brainstorms/Puzzlebox/Brainstorms/Wheelchair_Control.py:

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