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

Last change on this file since 240 was 240, checked in by sc, 11 years ago

puzzlebox_brainstorms_wheelchair_noisebridge.pde:

  • relocated to necessary location for ide

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

  • object model adopted

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

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