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

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

Brainstorms/Wheelchair_Control.py:

  • started conversion to proper Python module
  • Property svn:executable set to *
File size: 8.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.11.30
13
14"""
15
16import time
17import signal
18#import parallel
19import serial
20
21try:
22        import PySide as PyQt4
23        from PySide import QtCore
24except:
25        print "Using PyQt4 module"
26        from PyQt4 import QtCore
27else:
28        print "Using PySide module"
29
30#####################################################################
31# Globals
32#####################################################################
33
34DEBUG = 1
35
36DEFAULT_COMMAND = 'stop'
37DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0'
38
39COMMAND_CHARACTER = 'x'
40
41WHEELCHAIR_COMMANDS = {
42        'stop': '00110011',
43}
44
45# 'dir' : (low_pin, high_pin)
46CONTROLLS = {
47                'fwd'   : (1<<1, 1<<0),
48                'rev'   : (1<<3, 1<<2),
49                'right' : (1<<5, 1<<4),
50                'left'  : (1<<7, 1<<6),
51                }
52
53MOVE_COMANDS ={
54                ' ' : 'stop',
55                'i' : 'fwd',
56                'm' : 'rev',
57                'j' : 'left',
58                'k' : 'right',
59                }
60
61SPEED_COMMANDS = {
62                '1' : 'speed-1',
63                '2' : 'speed-2',
64                '3' : 'speed-3',
65                }
66
67STOP_TIME = 0
68STOP_INTERVAL = 0.2
69ALARM_INTERVAL = 0.1
70#PORT = parallel.Parallel()
71
72#####################################################################
73# Classes
74#####################################################################
75
76class puzzlebox_brainstorms_wheelchair_control(QtCore.QThread):
77       
78        def __init__(self, \
79                     device_address=DEFAULT_SERIAL_DEVICE, \
80                     command=DEFAULT_COMMAND, \
81                     DEBUG=DEBUG, \
82                     parent=None):
83               
84                QtCore.QThread.__init__(self, parent)
85               
86                self.log = None
87                self.DEBUG = DEBUG
88                self.parent = parent
89               
90                self.device_address = device_address
91                self.command = command
92               
93                self.device = self.initializeSerial()
94       
95       
96        ##################################################################
97       
98        def run(self):
99               
100                pass
101       
102       
103        ##################################################################
104       
105        def exitThread(self, callThreadQuit=True):
106               
107                try:
108                        self.device.stop()
109                except:
110                        pass
111               
112                #self.wait()
113                if callThreadQuit:
114                        QtCore.QThread.quit(self)
115       
116       
117        ##################################################################
118       
119        def initializeSerial():
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, DEFAULT_COMMAND)
185                if self.DEBUG:
186                        print "--> Wheelchair Command:",
187                        print DEFAULT_COMMAND
188               
189                time.sleep(2)
190               
191                return(device)
192
193
194#####################################################################
195#####################################################################
196
197class Getch:
198        def __init__(self):
199                import tty, sys
200
201        def __call__(self):
202                import sys, tty, termios
203                fd = sys.stdin.fileno()
204                old_settings = termios.tcgetattr(fd)
205                try:
206                        tty.setraw(sys.stdin.fileno())
207                        ch = sys.stdin.read(1)
208                finally:
209                        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
210                return ch
211
212
213#####################################################################
214# Functions
215#####################################################################
216
217def alarmHandler(arg1, arg2):
218        print 'alarm!'
219        if STOP_TIME < time.time():
220                stopBot()
221
222def initAlarm():
223        signal.alarm(ALARM_INTERVAL)
224        signal.signal(signal.SIGALRM, alarmHandler)
225
226def setOutput(data):
227        output = data ^ int('00110011', 2)
228        #PORT.setData(output)
229       
230        output = int2bin(output)
231        device.write('x%s' % output)
232       
233        #print 'Output set to: ', int2bin(output)
234        print 'Output set to: ', output
235        print 'commands: i j k m to move, SPACE = stop, x = quit'
236
237def moveBot(dir, speed):
238        output = 0
239        pins = CONTROLLS[dir]
240        if speed == 1:
241                output = pins[0]
242        elif speed == 2:
243                output = pins[1]
244        elif speed == 3:
245                output = pins[0] | pins[1]
246        setOutput(output)
247        time.sleep(STOP_INTERVAL)
248        stopBot()
249
250def stopBot():
251        setOutput(0)
252
253def int2bin(n, count=8):
254        return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)])
255
256#####################################################################
257
258def initializeSerial():
259        baudrate = 9600
260        bytesize = 8
261        parity = 'NONE'
262        stopbits = 1
263        software_flow_control = 'f'
264        rts_cts_flow_control = 't'
265        #timeout = 15
266        timeout = 5
267
268        # convert bytesize
269        if (bytesize == 5):
270                init_byte_size = serial.FIVEBITS
271        elif (bytesize == 6):
272                init_byte_size = serial.SIXBITS
273        elif (bytesize == 7):
274                init_byte_size = serial.SEVENBITS
275        elif (bytesize == 8):
276                init_byte_size = serial.EIGHTBITS
277        else:
278                #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
279                init_byte_size = serial.EIGHTBITS
280
281        # convert parity
282        if (parity == 'NONE'):
283                init_parity = serial.PARITY_NONE
284        elif (parity == 'EVEN'):
285                init_parity = serial.PARITY_EVEN
286        elif (parity == 'ODD'):
287                init_parity = serial.PARITY_ODD
288        else:
289                #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
290                init_parity = serial.PARITY_NONE
291
292        # convert stopbits
293        if (stopbits == 1):
294                init_stopbits = serial.STOPBITS_ONE
295        elif (stopbits == 2):
296                init_stopbits = serial.STOPBITS_TWO
297        else:
298                #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
299                init_byte_size = serial.STOPBITS_ONE
300
301        # convert software flow control
302        if (software_flow_control == 't'):
303                init_software_flow_control = 1
304        else:
305                init_software_flow_control = 0
306
307        # convert rts cts flow control
308        if (rts_cts_flow_control == 't'):
309                init_rts_cts_flow_control = 1
310        else:
311                init_rts_cts_flow_control = 0
312       
313        device = serial.Serial(port = '/dev/ttyACM0', \
314                                                                         baudrate = baudrate, \
315                                                                         bytesize = init_byte_size, \
316                                                                         parity = init_parity, \
317                                                                         stopbits = init_stopbits, \
318                                                                         xonxoff = init_software_flow_control, \
319                                                                         rtscts = init_rts_cts_flow_control, \
320                                                                         timeout = timeout)
321       
322        device.write('x00110011')
323        print "sent stop"
324        time.sleep(2)
325        print "slept 2"
326
327        return(device)
328
329
330#####################################################################
331# Main
332#####################################################################
333
334if __name__ == '__main__':
335
336        device = initializeSerial()
337
338        MYGETCH = Getch()
339
340        #initAlarm()
341
342        stopBot()
343
344        speed = 1
345        while True:
346                cmd = MYGETCH()
347                if cmd == 'x':
348                        exit()
349                if cmd in SPEED_COMMANDS.keys():
350                        speed = int(cmd)
351                        print SPEED_COMMANDS[cmd]
352                elif cmd in MOVE_COMANDS.keys():
353                        if MOVE_COMANDS[cmd] == 'stop':
354                                stopBot()
355                        else:
356                                print MOVE_COMANDS[cmd]
357                                moveBot(MOVE_COMANDS[cmd], speed)
358                                STOP_TIME = time.time() + STOP_INTERVAL
Note: See TracBrowser for help on using the repository browser.