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

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

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

  • function name capitalization consistency fix

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

  • Wheelchair control now working from GUI buttons
  • 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.sendCommand(self.wheelchair_speed, self.wheelchair_command)
196               
197                time.sleep(ARDUINO_INITIALIZATION_TIME)
198       
199       
200        ##################################################################
201       
202        def sendCommand(self, speed, command):
203       
204                self.device.write('%s%s' % (COMMAND_CHARACTER, \
205                        WHEELCHAIR_COMMANDS[speed][command]))
206               
207                if self.DEBUG:
208                        print "--> Wheelchair Command: %s [Speed %i]" % \
209                           (command, speed)
210               
211                self.wheelchair_speed = speed
212                self.wheelchair_command = command
213       
214       
215        ##################################################################
216       
217        def alarmHandler(self, arg1, arg2):
218                print 'alarm!'
219                if STOP_TIME < time.time():
220                        self.stopBot()
221
222
223        def initAlarm(self):
224                signal.alarm(ALARM_INTERVAL)
225                signal.signal(signal.SIGALRM, alarmHandler)
226
227
228        def setOutput(self, data):
229                output = data ^ int('00110011', 2)
230               
231                output = self.int2bin(output)
232                self.device.write('x%s' % output)
233               
234                #print 'Output set to: ', int2bin(output)
235                print 'Output set to: ', output
236                print 'commands: i j k m to move, SPACE = stop, x = quit'
237
238
239        def moveBot(self, dir, speed):
240                output = 0
241                pins = CONTROLLS[dir]
242                if speed == 1:
243                        output = pins[0]
244                elif speed == 2:
245                        output = pins[1]
246                elif speed == 3:
247                        output = pins[0] | pins[1]
248                self.setOutput(output)
249                time.sleep(STOP_INTERVAL)
250                self.stopBot()
251
252
253        def stopBot(self):
254                self.setOutput(0)
255
256
257        def int2bin(self, n, count=8):
258                return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)])
259
260
261        ##################################################################
262
263        def consoleControl(self):
264
265                if (sys.platform == 'win32'):
266                        if self.DEBUG:
267                                print "---> Wheelchair Control: Console mode unavailable under Windows"
268                       
269                        self.exitThread()
270               
271               
272                MYGETCH = Getch()
273               
274                #initAlarm()
275               
276                self.stopBot()
277               
278                speed = DEFAULT_WHEELCHAIR_SPEED
279                while True:
280                        cmd = MYGETCH()
281                        if cmd == 'x':
282                                exit()
283                        if cmd in SPEED_COMMANDS.keys():
284                                speed = int(cmd)
285                                print SPEED_COMMANDS[cmd]
286                        elif cmd in MOVE_COMANDS.keys():
287                                if MOVE_COMANDS[cmd] == 'stop':
288                                        self.stopBot()
289                                else:
290                                        print MOVE_COMANDS[cmd]
291                                        self.moveBot(MOVE_COMANDS[cmd], speed)
292                                        STOP_TIME = time.time() + STOP_INTERVAL
293       
294       
295        ##################################################################
296       
297        #def guiControl(self):
298               
299                #while self.keep_running:
300                       
301                        #QtCore.QThread.msleep(GUI_SLEEP_TIMER)
302       
303       
304        ##################################################################
305       
306        def processCommand(self):
307               
308                if (self.command == 'console'):
309                        self.consoleControl()
310               
311                #elif (self.command == 'gui'):
312                        #self.gui_control()
313       
314       
315        ##################################################################
316       
317        def run(self):
318               
319                if self.DEBUG:
320                        print "<---- [%s] Main thread running" % "Wheelchair Control"
321               
322               
323                self.processCommand()
324               
325                self.exec_()
326       
327       
328        ##################################################################
329       
330        def exitThread(self, callThreadQuit=True):
331               
332                try:
333                        self.device.stop()
334                except:
335                        pass
336               
337                #self.wait()
338                if callThreadQuit:
339                        QtCore.QThread.quit(self)
340
341
342#####################################################################
343#####################################################################
344
345class Getch:
346       
347        def __init__(self):
348                import tty, sys
349       
350        def __call__(self):
351                import sys, tty, termios
352                fd = sys.stdin.fileno()
353                old_settings = termios.tcgetattr(fd)
354                try:
355                        tty.setraw(sys.stdin.fileno())
356                        ch = sys.stdin.read(1)
357                finally:
358                        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
359                return ch
360
361
362#####################################################################
363# Functions
364#####################################################################
365
366#####################################################################
367# Main
368#####################################################################
369
370if __name__ == '__main__':
371       
372        # Perform correct KeyboardInterrupt handling
373        signal.signal(signal.SIGINT, signal.SIG_DFL)
374       
375        # Collect default settings and command line parameters
376        device = DEFAULT_SERIAL_DEVICE
377        command = DEFAULT_COMMAND
378       
379        for each in sys.argv:
380               
381                if each.startswith("--device="):
382                        device = each[ len("--device="): ]
383                elif each.startswith("--command="):
384                        command = each[ len("--command="): ]
385       
386       
387        app = QtCore.QCoreApplication(sys.argv)
388       
389        wheelchair = puzzlebox_brainstorms_wheelchair_control( \
390                        device_address=device, \
391                        command=command, \
392                        DEBUG=DEBUG)
393       
394        wheelchair.start()
395       
396        sys.exit(app.exec_())
Note: See TracBrowser for help on using the repository browser.