source: trunk/Puzzlebox/Brainstorms/Wheelchair_Control.py

Last change on this file was 359, checked in by sc, 7 years ago

configuration:

  • WHEELCHAIR_CONTROL_EEG added to continue driving in a direction until stopped
  • Property svn:executable set to *
File size: 13.6 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Wheelchair Control
5#
6# Copyright Puzzlebox Productions, LLC (2010-2012)
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: 2012.08.20
13"""
14
15import sys
16import time
17import signal
18import serial
19if (sys.platform != 'win32'):
20        import tty, termios # console()
21        import select # console()
22
23import Configuration as configuration
24
25if configuration.ENABLE_PYSIDE:
26        try:
27                #import PySide as PyQt4
28                from PySide import QtCore
29        except Exception, e:
30                print "ERROR: Exception importing PySide:",
31                print e
32                configuration.ENABLE_PYSIDE = False
33        else:
34                print "INFO: [Brainstorms:Wheelchair_Control] Using PySide module"
35
36if not configuration.ENABLE_PYSIDE:
37        print "INFO: [Brainstorms:Wheelchair_Control] Using PyQt4 module"
38        from PyQt4 import QtCore
39
40
41#####################################################################
42# Globals
43#####################################################################
44
45DEBUG = 1
46
47DEFAULT_COMMAND = 'console'
48#DEFAULT_COMMAND = 'controller'
49DEFAULT_SERIAL_DEVICE = '/dev/arduino1'
50#DEFAULT_SERIAL_DEVICE = '/dev/ttyUSB0'
51#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0'
52#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM1'
53
54#DEFAULT_SERIAL_BAUD = 9600
55DEFAULT_SERIAL_BAUD = 57600
56
57#try:
58        ##PORT = serial.Serial("/dev/arduino1",baudrate=57600,bytesize=serial.EIGHTBITS)
59        ##PORT = serial.Serial("/dev/ttyUSB0",baudrate=57600,bytesize=serial.EIGHTBITS)
60        #PORT = serial.Serial(DEFAULT_SERIAL_DEVICE,baudrate=57600,bytesize=serial.EIGHTBITS) # controller()
61#except:
62        #pass
63
64DEFAULT_WHEELCHAIR_SPEED = 0 # tracks numeric value
65DEFAULT_WHEELCHAIR_COMMAND = 'stop'
66
67#ARDUINO_INITIALIZATION_TIME = 2
68ARDUINO_INITIALIZATION_TIME = 5 # increased 2012.08.17
69COMMAND_CHARACTER = 'x'
70GUI_SLEEP_TIMER = 1 * 100 # 100ms
71
72COMMAND_POWER_ON = 'Pz'
73COMMAND_POWER_OFF = 'Pa'
74COMMAND_STOP = 'SS'
75DEFAULT_DIRECTION = 'F'
76DEFAULT_SPEED = 6
77COMMAND_DEFAULT = DEFAULT_DIRECTION + chr(int(DEFAULT_SPEED)*255/9) + COMMAND_POWER_ON
78
79#WHEELCHAIR_COMMANDS = {
80        #1: { # speed
81                #'forward': '00110001',
82                #'reverse': '00111011',
83                #'left': '10110011',
84                #'right': '00010011',
85                #'stop': '00110011',
86        #},
87        #2: { # speed
88                #'forward': '00110010',
89                #'reverse': '00110111',
90##              'left': '01110011',  # Turning in Speed 2 is disproportionately fast
91##              'right': '00100011', # Turning in Speed 2 is disproportionately fast
92                #'left': '10110011',  # Turn Speed 1
93                #'right': '00010011', # Turn Speed 1
94                #'stop': '00110011',
95        #},
96        #3: { # speed
97                #'forward': '00110000',
98                #'reverse': '00111111',
99##              'left': '11110011',  # Turning in Speed 3 is too fast
100##              'right': '00000011', # Turning in Speed 3 is too fast
101                #'left': '01110011',  # Turn Speed 2
102                #'right': '00100011', # Turn Speed 2
103                #'stop': '00110011',
104        #},
105#}
106
107CONTROLS = {
108        'forward': 'F',
109        'reverse': 'B',
110        'fwd':     'F',
111        'rev':     'B',
112        'right':   'R',
113        'left':    'L',
114}
115
116MOVE_COMMANDS ={
117        ' ' : 'stop',
118        'i' : 'fwd',
119        'm' : 'rev',
120        'j' : 'left',
121        'k' : 'right',
122}
123
124SPEED_COMMANDS = {
125        's' : -1,
126        'd' : +1,
127        'f' : -25,
128        'g' : +25,
129}
130
131DIGITS = map(str,range(10))
132
133STOP_TIME = 0
134STOP_INTERVAL = 0.2
135ALARM_INTERVAL = 0.1
136
137#STEERING_SENSITIVITY = 0.5   # multiplier for left/right commands
138STEERING_SENSITIVITY = 1      # multiplier for left/right commands (updated resistor 2012.08.15)
139
140
141#####################################################################
142# Classes
143#####################################################################
144
145class puzzlebox_brainstorms_wheelchair_control(QtCore.QThread):
146       
147        def __init__(self, \
148                     device_address=DEFAULT_SERIAL_DEVICE, \
149                     command=DEFAULT_COMMAND, \
150                     DEBUG=DEBUG, \
151                     parent=None):
152               
153                QtCore.QThread.__init__(self, parent)
154               
155                self.log = None
156                self.DEBUG = DEBUG
157                self.parent = parent
158               
159                self.device_address = device_address
160                self.command = command
161               
162                self.wheelchair_speed = DEFAULT_WHEELCHAIR_SPEED
163                self.wheelchair_command = DEFAULT_WHEELCHAIR_COMMAND
164               
165                self.device = None
166                self.previous_serial_settings = None
167                self.initializeSerial()
168               
169                self.keep_running = True
170               
171                self.speed = None
172                self.setSpeed(0)
173       
174       
175        ##################################################################
176       
177        def initializeSerial(self):
178               
179                try:
180                        self.previous_serial_settings = termios.tcgetattr(sys.stdin)
181                except Exception, e:
182                        print "WARNING: failed to retrieve previous serial settings"
183               
184                baudrate = DEFAULT_SERIAL_BAUD
185                bytesize = 8
186                parity = 'NONE'
187                stopbits = 1
188                software_flow_control = 'f'
189                #rts_cts_flow_control = 't'
190                rts_cts_flow_control = 'f' # no hardward flow control for McHawking robot
191                #timeout = 15
192                timeout = 5
193               
194                # convert bytesize
195                if (bytesize == 5):
196                        init_byte_size = serial.FIVEBITS
197                elif (bytesize == 6):
198                        init_byte_size = serial.SIXBITS
199                elif (bytesize == 7):
200                        init_byte_size = serial.SEVENBITS
201                elif (bytesize == 8):
202                        init_byte_size = serial.EIGHTBITS
203                else:
204                        #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
205                        init_byte_size = serial.EIGHTBITS
206               
207                # convert parity
208                if (parity == 'NONE'):
209                        init_parity = serial.PARITY_NONE
210                elif (parity == 'EVEN'):
211                        init_parity = serial.PARITY_EVEN
212                elif (parity == 'ODD'):
213                        init_parity = serial.PARITY_ODD
214                else:
215                        #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
216                        init_parity = serial.PARITY_NONE
217               
218                # convert stopbits
219                if (stopbits == 1):
220                        init_stopbits = serial.STOPBITS_ONE
221                elif (stopbits == 2):
222                        init_stopbits = serial.STOPBITS_TWO
223                else:
224                        #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
225                        init_byte_size = serial.STOPBITS_ONE
226               
227                # convert software flow control
228                if (software_flow_control == 't'):
229                        init_software_flow_control = 1
230                else:
231                        init_software_flow_control = 0
232               
233                # convert rts cts flow control
234                if (rts_cts_flow_control == 't'):
235                        init_rts_cts_flow_control = 1
236                else:
237                        init_rts_cts_flow_control = 0
238               
239                self.device = serial.Serial(port = self.device_address, \
240                                            baudrate = baudrate, \
241                                            bytesize = init_byte_size, \
242                                            parity = init_parity, \
243                                            stopbits = init_stopbits, \
244                                            xonxoff = init_software_flow_control, \
245                                            rtscts = init_rts_cts_flow_control, \
246                                            timeout = timeout)
247               
248                self.writeStop()
249               
250                time.sleep(2) # pause for one second before sending anything further
251               
252                self.writeCommand() # send forward command once to activate
253               
254                #time.sleep(ARDUINO_INITIALIZATION_TIME)
255       
256       
257        ##################################################################
258       
259        def writeStop(self):
260                if self.DEBUG:
261                        print "Writing:",
262                        print COMMAND_STOP + COMMAND_POWER_ON
263                self.device.write(COMMAND_STOP + COMMAND_POWER_ON)
264                #debug.write(COMMAND_STOP + COMMAND_POWER_ON)
265
266        def writeCommand(self):
267                if self.DEBUG:
268                        print "Writing:",
269                        print COMMAND_DEFAULT
270                self.device.write(COMMAND_DEFAULT)
271                #debug.write(COMMAND_DEFAULT)
272
273        def writeKeepAlive(self):
274                if self.DEBUG > 2:
275                        print "Writing:",
276                        print COMMAND_POWER_ON
277                self.device.write(COMMAND_POWER_ON)
278                #debug.write(COMMAND_POWER_ON)
279       
280       
281        ##################################################################
282       
283        def sendCommand(self, speed, command):
284               
285               
286                if self.DEBUG:
287                        print "DEBUG [Brainstorms:Wheelchair_Control]: sendCommand( speed:",
288                        print speed,
289                        print " command:",
290                        print command,
291                        print ")"
292               
293               
294                if speed != self.wheelchair_speed:
295                        #setSpeed(speed)
296                        #self.handleCommand(speed)
297                        self.setSpeed(int(speed)*255/9)
298               
299                #output = '%s%s' % (COMMAND_CHARACTER, \
300                        #WHEELCHAIR_COMMANDS[self.speed][command])
301               
302               
303                #self.device.write(output)
304               
305               
306                if command in CONTROLS.keys():
307                        self.moveBot(command)
308               
309               
310                else:
311                        self.wheelchair_command = 'stop'
312               
313               
314                #if self.DEBUG:
315                        #print "--> Wheelchair Command: %s (Speed %i) [%s]" % \
316                           #(command, self.speed, output)
317               
318               
319                self.wheelchair_speed = speed
320                #self.wheelchair_command = command
321       
322       
323        ##################################################################
324       
325        def setOutput(self, data):
326               
327                if self.DEBUG:
328                        print "DEBUG [Brainstorms:Wheelchair_Control]: write: %sPz" % data
329               
330                #self.device.write(data)
331                #self.device.write("Pz")
332               
333                output = data + "Pz"
334               
335                self.wheelchair_command = output
336        #       print 'Output set to:', data[0], ord(data[1])
337        #       printHelp()
338       
339       
340        def setDrive(self, value):
341                if value == 'A':
342                        self.setOutput('DA')
343                if value == 'B':
344                        self.setOutput('DB')
345       
346       
347        def moveBot(self, dir):
348               
349                if (dir in ['left','right']):
350                        output = CONTROLS[dir] + chr(int(self.speed * STEERING_SENSITIVITY))
351                else:
352                        output = CONTROLS[dir] + chr(self.speed)
353               
354                self.setOutput(output)
355       
356       
357        def stopBot(self):
358                self.setOutput('SS')
359       
360       
361        def setSpeed(self, value):
362                new_speed = value
363                if new_speed > 255:
364                        new_speed = 255
365                if new_speed < 0:
366                        new_speed = 0
367               
368                self.speed = new_speed
369               
370                print 'speed:', self.speed, '/ 255'
371       
372       
373        def handleCommand(self, cmd):
374                if cmd in DIGITS: # 0-9
375                        self.setSpeed(int(cmd)*255/9)
376                if cmd in SPEED_COMMANDS.keys():
377                        self.setSpeed(self.speed + SPEED_COMMANDS[cmd])
378                elif cmd in MOVE_COMMANDS.keys():
379                        if MOVE_COMMANDS[cmd] == 'stop':
380                                self.stopBot()
381                        else:
382                                print MOVE_COMMANDS[cmd]
383                                self.moveBot(MOVE_COMMANDS[cmd])
384                #elif cmd == 'w':
385                        #rockets.up()
386                #elif cmd == 'e':
387                        #rockets.down()
388                #elif cmd == 'q':
389                        #rockets.left()
390                #elif cmd == 'r':
391                         #rockets.right()       
392                #elif cmd == 't':
393                        #rockets.laser()
394                #elif cmd == 'y':
395                        #rockets.fire()
396                elif cmd == 'a':
397                        self.setDrive('A')
398                elif cmd == 'b':
399                        self.setDrive('B')
400       
401         
402        def printHelp(self):
403                print 'commands: '
404                print 'i j k l to move, SPACE = stop, x = quit'
405                print 's d f g or 0-9 to adjust speed between 0 and 255.'
406       
407       
408        def isData(self):
409                return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])
410       
411       
412        #################################################################
413       
414        def consoleControl(self):
415               
416                print "DEBUG: consoleControl()"
417               
418                if (sys.platform == 'win32'):
419                        if self.DEBUG:
420                                print "---> Wheelchair Control: Console mode unavailable under Windows"
421                       
422                        self.exitThread()
423               
424               
425                MYGETCH = Getch()
426               
427                while True:
428                       
429                        self.writeKeepAlive()
430                       
431                        cmd = MYGETCH()
432                        #cmd = ""
433                        #cmd = sys.stdin.read(1)
434                       
435                        #if self.isData():
436                        if True:
437                                if cmd == 'x':
438                                        break
439                               
440                                if len(cmd) > 0:
441                                        self.handleCommand(cmd)
442                                        #if cmd == '?':
443                                        self.printHelp()
444                       
445                        if self.device.inWaiting() > 0:
446                                print self.device.readline().strip()
447                       
448                       
449                        #QtCore.QThread.msleep(GUI_SLEEP_TIMER)
450
451       
452       
453        ##################################################################
454       
455        def guiControl(self):
456               
457                while self.keep_running:
458                       
459                        self.writeKeepAlive()
460                       
461                        if self.wheelchair_command != 'stop':
462                                if self.DEBUG:
463                                        print "Wrote to device:",
464                                        print self.wheelchair_command
465                                self.device.write(self.wheelchair_command)
466                       
467                       
468                        if self.device.inWaiting() > 0:
469                                line = self.device.readline()
470                               
471                                line = line.strip()
472                               
473                                if self.DEBUG:
474                                        print "Read from device:",
475                                        print line
476                       
477                       
478                        QtCore.QThread.msleep(GUI_SLEEP_TIMER)
479       
480       
481        ##################################################################
482       
483        def processCommand(self):
484               
485                if (self.command == 'console'):
486                        self.consoleControl()
487               
488                elif (self.command == 'gui'):
489                        self.guiControl()
490               
491                else:
492                        self.guiControl()
493       
494       
495        ##################################################################
496       
497        def run(self):
498               
499                if self.DEBUG:
500                        print "<---- [%s] Main thread running" % "Wheelchair Control"
501               
502               
503                self.processCommand()
504               
505                self.exec_()
506       
507       
508        ##################################################################
509       
510        def stop(self):
511               
512                self.keep_running = False
513       
514       
515        ##################################################################
516       
517        def exitThread(self, callThreadQuit=True):
518               
519                print "DEBUG: exitThread()"
520               
521                try:
522                        self.stopBot()
523                except:
524                        pass
525               
526                if self.previous_serial_settings != None:
527                        try:
528                                termios.tcsetattr(sys.stdin, \
529                                                  termios.TCSADRAIN, \
530                                                  self.previous_serial_settings)
531                        except Exception, e:
532                                print "WARNING: failed to set previous serial settings"
533               
534                #self.wait()
535                if callThreadQuit:
536                        QtCore.QThread.quit(self)
537
538
539#####################################################################
540#####################################################################
541
542class Getch:
543       
544        def __init__(self):
545                import tty, sys
546       
547        def __call__(self):
548                import sys, tty, termios
549                fd = sys.stdin.fileno()
550                old_settings = termios.tcgetattr(fd)
551                try:
552                        tty.setraw(sys.stdin.fileno())
553                        ch = sys.stdin.read(1)
554                finally:
555                        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
556                return ch
557
558
559#####################################################################
560# Functions
561#####################################################################
562
563
564#####################################################################
565# Main
566#####################################################################
567
568if __name__ == '__main__':
569       
570        # Perform correct KeyboardInterrupt handling
571        signal.signal(signal.SIGINT, signal.SIG_DFL)
572       
573        # Collect default settings and command line parameters
574        device = DEFAULT_SERIAL_DEVICE
575        command = DEFAULT_COMMAND
576       
577        for each in sys.argv:
578               
579                if each.startswith("--device="):
580                        device = each[ len("--device="): ]
581                elif each.startswith("--command="):
582                        command = each[ len("--command="): ]
583       
584       
585        app = QtCore.QCoreApplication(sys.argv)
586       
587        wheelchair = puzzlebox_brainstorms_wheelchair_control( \
588                        device_address=device, \
589                        command=command, \
590                        DEBUG=DEBUG)
591       
592        wheelchair.start()
593       
594        sys.exit(app.exec_())
Note: See TracBrowser for help on using the repository browser.