source: trunk/Puzzlebox/Brainstorms/Wheelchair_Control.py

Last change on this file was 359, checked in by sc, 8 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
RevLine 
[227]1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Wheelchair Control
5#
[349]6# Copyright Puzzlebox Productions, LLC (2010-2012)
[227]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__ = """
[359]12Last Update: 2012.08.20
[227]13"""
14
[240]15import sys
[227]16import time
17import signal
[232]18import serial
[359]19if (sys.platform != 'win32'):
20        import tty, termios # console()
21        import select # console()
[227]22
[322]23import Configuration as configuration
24
25if configuration.ENABLE_PYSIDE:
26        try:
[357]27                #import PySide as PyQt4
[322]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"
[239]38        from PyQt4 import QtCore
39
[322]40
[227]41#####################################################################
42# Globals
43#####################################################################
44
[239]45DEBUG = 1
46
[357]47DEFAULT_COMMAND = 'console'
48#DEFAULT_COMMAND = 'controller'
49DEFAULT_SERIAL_DEVICE = '/dev/arduino1'
50#DEFAULT_SERIAL_DEVICE = '/dev/ttyUSB0'
51#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0'
[352]52#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM1'
53
[349]54#DEFAULT_SERIAL_BAUD = 9600
55DEFAULT_SERIAL_BAUD = 57600
[241]56
[357]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
[352]63
[355]64DEFAULT_WHEELCHAIR_SPEED = 0 # tracks numeric value
[241]65DEFAULT_WHEELCHAIR_COMMAND = 'stop'
66
[356]67#ARDUINO_INITIALIZATION_TIME = 2
68ARDUINO_INITIALIZATION_TIME = 5 # increased 2012.08.17
[239]69COMMAND_CHARACTER = 'x'
[241]70GUI_SLEEP_TIMER = 1 * 100 # 100ms
[239]71
[356]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
[352]78
[357]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#}
[239]106
[353]107CONTROLS = {
[355]108        'forward': 'F',
109        'reverse': 'B',
110        'fwd':     'F',
111        'rev':     'B',
112        'right':   'R',
113        'left':    'L',
[349]114}
115
[350]116MOVE_COMMANDS ={
[353]117        ' ' : 'stop',
118        'i' : 'fwd',
119        'm' : 'rev',
120        'j' : 'left',
121        'k' : 'right',
122}
[227]123
[353]124SPEED_COMMANDS = {
125        's' : -1,
126        'd' : +1,
127        'f' : -25,
128        'g' : +25,
[349]129}
130
[350]131DIGITS = map(str,range(10))
[349]132
[227]133STOP_TIME = 0
134STOP_INTERVAL = 0.2
135ALARM_INTERVAL = 0.1
136
[356]137#STEERING_SENSITIVITY = 0.5   # multiplier for left/right commands
138STEERING_SENSITIVITY = 1      # multiplier for left/right commands (updated resistor 2012.08.15)
[349]139
140
[227]141#####################################################################
142# Classes
143#####################################################################
144
[239]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               
[241]162                self.wheelchair_speed = DEFAULT_WHEELCHAIR_SPEED
163                self.wheelchair_command = DEFAULT_WHEELCHAIR_COMMAND
164               
165                self.device = None
[349]166                self.previous_serial_settings = None
[241]167                self.initializeSerial()
168               
169                self.keep_running = True
[349]170               
[350]171                self.speed = None
172                self.setSpeed(0)
[239]173       
174       
175        ##################################################################
176       
[240]177        def initializeSerial(self):
[239]178               
[349]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
[239]185                bytesize = 8
186                parity = 'NONE'
187                stopbits = 1
188                software_flow_control = 'f'
[356]189                #rts_cts_flow_control = 't'
190                rts_cts_flow_control = 'f' # no hardward flow control for McHawking robot
[239]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               
[241]239                self.device = serial.Serial(port = self.device_address, \
[356]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)
[239]247               
[356]248                self.writeStop()
[239]249               
[356]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)
[240]255       
256       
257        ##################################################################
258       
[356]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       
[242]283        def sendCommand(self, speed, command):
[275]284               
[354]285               
[355]286                if self.DEBUG:
287                        print "DEBUG [Brainstorms:Wheelchair_Control]: sendCommand( speed:",
288                        print speed,
289                        print " command:",
290                        print command,
291                        print ")"
[349]292               
[355]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               
[352]303                #self.device.write(output)
[240]304               
305               
[355]306                if command in CONTROLS.keys():
307                        self.moveBot(command)
308               
309               
[356]310                else:
311                        self.wheelchair_command = 'stop'
312               
313               
[355]314                #if self.DEBUG:
315                        #print "--> Wheelchair Command: %s (Speed %i) [%s]" % \
316                           #(command, self.speed, output)
317               
318               
319                self.wheelchair_speed = speed
[356]320                #self.wheelchair_command = command
[240]321       
322       
323        ##################################################################
324       
[349]325        def setOutput(self, data):
[355]326               
327                if self.DEBUG:
328                        print "DEBUG [Brainstorms:Wheelchair_Control]: write: %sPz" % data
329               
[356]330                #self.device.write(data)
331                #self.device.write("Pz")
332               
333                output = data + "Pz"
334               
335                self.wheelchair_command = output
[349]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       
[350]347        def moveBot(self, dir):
[356]348               
[349]349                if (dir in ['left','right']):
[350]350                        output = CONTROLS[dir] + chr(int(self.speed * STEERING_SENSITIVITY))
[349]351                else:
[350]352                        output = CONTROLS[dir] + chr(self.speed)
[357]353               
[240]354                self.setOutput(output)
[349]355       
356       
[240]357        def stopBot(self):
[349]358                self.setOutput('SS')
359       
360       
361        def setSpeed(self, value):
[355]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               
[349]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]
[350]383                                self.moveBot(MOVE_COMMANDS[cmd])
[349]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       
[358]408        def isData(self):
409                return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], [])
[349]410       
411       
412        #################################################################
413       
[242]414        def consoleControl(self):
[349]415               
[350]416                print "DEBUG: consoleControl()"
417               
[241]418                if (sys.platform == 'win32'):
419                        if self.DEBUG:
420                                print "---> Wheelchair Control: Console mode unavailable under Windows"
421                       
422                        self.exitThread()
423               
424               
[350]425                MYGETCH = Getch()
[241]426               
[240]427                while True:
[350]428                       
[357]429                        self.writeKeepAlive()
430                       
[350]431                        cmd = MYGETCH()
[358]432                        #cmd = ""
433                        #cmd = sys.stdin.read(1)
[350]434                       
[358]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()
[350]444                       
[349]445                        if self.device.inWaiting() > 0:
[357]446                                print self.device.readline().strip()
[349]447                       
448                       
[358]449                        #QtCore.QThread.msleep(GUI_SLEEP_TIMER)
[357]450
[240]451       
[354]452       
[240]453        ##################################################################
454       
[352]455        def guiControl(self):
[241]456               
[352]457                while self.keep_running:
[241]458                       
[356]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                       
[352]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)
[241]479       
480       
481        ##################################################################
482       
483        def processCommand(self):
484               
485                if (self.command == 'console'):
[242]486                        self.consoleControl()
[241]487               
[354]488                elif (self.command == 'gui'):
489                        self.guiControl()
[352]490               
491                else:
492                        self.guiControl()
[241]493       
494       
495        ##################################################################
496       
[240]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       
[354]510        def stop(self):
511               
512                self.keep_running = False
513       
514       
515        ##################################################################
516       
[240]517        def exitThread(self, callThreadQuit=True):
518               
[354]519                print "DEBUG: exitThread()"
520               
[240]521                try:
[357]522                        self.stopBot()
[240]523                except:
524                        pass
525               
[349]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               
[240]534                #self.wait()
535                if callThreadQuit:
536                        QtCore.QThread.quit(self)
537
538
[239]539#####################################################################
540#####################################################################
541
[227]542class Getch:
[240]543       
[227]544        def __init__(self):
545                import tty, sys
[240]546       
[227]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
[352]563
[240]564#####################################################################
565# Main
566#####################################################################
[227]567
[240]568if __name__ == '__main__':
[232]569       
[240]570        # Perform correct KeyboardInterrupt handling
571        signal.signal(signal.SIGINT, signal.SIG_DFL)
[232]572       
[240]573        # Collect default settings and command line parameters
574        device = DEFAULT_SERIAL_DEVICE
575        command = DEFAULT_COMMAND
[239]576       
[240]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="): ]
[239]583       
[240]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.