source: trunk/Puzzlebox/Brainstorms/Wheelchair_Control.py @ 358

Last change on this file since 358 was 358, checked in by sc, 8 years ago

Wheelchair_Control:

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