Changeset 4996638 in robotics


Ignore:
Timestamp:
08/12/13 07:17:16 (8 years ago)
Author:
Steve Castellotti <sc@…>
Branches:
master
Children:
f88941a
Parents:
de50de9
Message:
  • Drive forward loop working
File:
1 edited

Legend:

Unmodified
Added
Removed
  • wheelchair/python/Wheelchair_Control.py

    r98791c8 r4996638  
    88# This code is released under the GNU Pulic License (GPL) version 2 
    99# For more information please refer to http://www.gnu.org/copyleft/gpl.html 
    10  
     10# 
     11# Note: Some scripts available at 
     12#       https://mchawking.googlecode.com/svn/trunk 
     13# 
    1114__changelog__ = """ 
    1215Last Update: 2013.08.11 
     
    1417""" 
    1518 
     19import select 
     20import serial 
     21import signal 
     22import termios 
    1623import sys 
    1724import time 
    18 import signal 
    19 import serial 
    20 import select 
    21 import termios, select, sys, tty 
     25import tty 
    2226 
    2327#import Configuration as configuration 
     
    4751DEBUG = 1 
    4852 
    49 DEFAULT_COMMAND = 'console' 
     53#DEFAULT_COMMAND = 'console' 
     54#DEFAULT_COMMAND = 'manual' 
     55DEFAULT_COMMAND = 'test' 
    5056#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0' 
    5157DEFAULT_SERIAL_DEVICE = '/dev/arduino1' 
     
    5359DEFAULT_SERIAL_BAUD = 57600 
    5460 
    55 DEFAULT_WHEELCHAIR_SPEED = 1 
     61DEFAULT_SPEED = 0 
     62DRIVE_SPEED = 5 
     63 
     64#DEFAULT_WHEELCHAIR_SPEED = 5 
     65DEFAULT_WHEELCHAIR_SPEED = 7 
    5666DEFAULT_WHEELCHAIR_COMMAND = 'stop' 
    5767 
    5868ARDUINO_INITIALIZATION_TIME = 2 
     69ROBOT_CONTROLLER_INITIALIZATION_TIME = 4 
     70 
    5971COMMAND_CHARACTER = 'x' 
     72 
     73 
     74#const int commandexpires = 500;  //milliseconds before direction command expires 
     75#const int wheelchairpowerexpires = 5000;  //milliseconds before wheelchair power-on command expires 
     76 
     77 
     78 
    6079GUI_SLEEP_TIMER = 1 * 100 # 100ms 
    61  
    62 DEVICE_KEEPALIVE_TIMER = 1500 # 1.5s 
     80#DEVICE_KEEPALIVE_TIMER = 1.5 * 1000 # 1.5s 
     81DEVICE_KEEPALIVE_TIMER = 4 * 1000 # 4s 
     82COMMAND_KEEPALIVE_TIMER = 4 * 100 # 400 ms 
     83 
    6384 
    6485DEVICE_KEEPALIVE_CHECKSUM = 'Pz' 
    65  
    66 WHEELCHAIR_COMMANDS = { 
    67         1: { # speed 
    68                 'forward': '00110001', 
    69                 'reverse': '00111011', 
    70                 'left': '10110011', 
    71                 'right': '00010011', 
    72                 'stop': '00110011',  
    73         }, 
    74         2: { # speed 
    75                 'forward': '00110010', 
    76                 'reverse': '00110111', 
    77 #               'left': '01110011',  # Turning in Speed 2 is disproportionately fast 
    78 #               'right': '00100011', # Turning in Speed 2 is disproportionately fast 
    79                 'left': '10110011',  # Turn Speed 1 
    80                 'right': '00010011', # Turn Speed 1 
    81                 'stop': '00110011',  
    82         }, 
    83         3: { # speed 
    84                 'forward': '00110000', 
    85                 'reverse': '00111111', 
    86 #               'left': '11110011',  # Turning in Speed 3 is too fast 
    87 #               'right': '00000011', # Turning in Speed 3 is too fast 
    88                 'left': '01110011',  # Turn Speed 2 
    89                 'right': '00100011', # Turn Speed 2 
    90                 'stop': '00110011',  
    91         }, 
    92 } 
    93  
    94 ## 'dir' : (low_pin, high_pin) 
    95 #CONTROLLS = { 
    96                 #'fwd'  : (1<<1, 1<<0), 
    97                 #'rev'  : (1<<3, 1<<2), 
    98                 #'right'        : (1<<5, 1<<4), 
    99                 #'left' : (1<<7, 1<<6), 
    100                 #} 
    10186 
    10287CONTROLS = { 'fwd': 'F', 
     
    124109} 
    125110 
    126  
    127111#SPEED_COMMANDS = { 
    128112                #'1' : 'speed-1', 
     
    181165                 
    182166                self.speed = None 
    183                 self.setSpeed(0) 
    184                  
    185                  
    186                 timer = QtCore.QTimer(self) 
    187                 QtCore.QObject.connect(timer, QtCore.SIGNAL("timeout()"), self.deviceKeepalive) 
    188                 #timer.start(256) 
    189                 timer.start(DEVICE_KEEPALIVE_TIMER) 
     167                 
     168                #self.setSpeed(0) 
     169                #self.setSpeed(DEFAULT_WHEELCHAIR_SPEED) 
     170                 
     171                 
     172                #self.stopBot() 
     173                 
     174                #self.speed = (DEFAULT_WHEELCHAIR_SPEED) * 255/9 
     175                 
     176                #self.setSpeed(self.speed) 
     177                 
     178                 
     179                self.initializeWheelchair() 
     180                 
     181                self.timer = QtCore.QTimer(self) 
     182                QtCore.QObject.connect(self.timer, QtCore.SIGNAL("timeout()"), self.deviceKeepalive) 
     183                self.timer.start(COMMAND_KEEPALIVE_TIMER) 
     184         
     185         
     186        ################################################################## 
     187         
     188        def initializeWheelchair(self): 
     189                 
     190                self.setSpeed(int(DEFAULT_SPEED)*255/9) 
     191                 
     192                self.device.write("Pz") 
     193                 
     194                time.sleep(ARDUINO_INITIALIZATION_TIME) 
     195                 
     196                print "[INFO] Intializing robot" 
     197                 
     198                self.moveBot('fwd') 
     199                 
     200                time.sleep(ROBOT_CONTROLLER_INITIALIZATION_TIME) 
     201                 
     202                self.setSpeed(int(DRIVE_SPEED)*255/9) 
    190203         
    191204         
     
    197210                        print "deviceKeepalive()" 
    198211                         
    199                 if self.wheelchair_command == 'stop': 
    200                         #self.moveBot('stop') 
    201                         self.setOutput(DEVICE_KEEPALIVE_CHECKSUM) 
     212                #if self.wheelchair_command == 'stop': 
     213                        #self.setOutput(DEVICE_KEEPALIVE_CHECKSUM) 
     214                 
     215                #if self.wheelchair_command == 'stop': 
     216                #self.setOutput(DEVICE_KEEPALIVE_CHECKSUM) 
     217                #self.setOutput('') 
     218                 
     219                 
     220                self.device.write(DEVICE_KEEPALIVE_CHECKSUM) 
     221                 
     222                self.moveBot('fwd') 
    202223         
    203224         
     
    265286                        init_rts_cts_flow_control = 0 
    266287                 
    267                 try: 
    268                         self.device = serial.Serial(port = self.device_address, \ 
    269                                                                                 baudrate = baudrate, \ 
    270                                                                                 bytesize = init_byte_size, \ 
    271                                                                                 parity = init_parity, \ 
    272                                                                                 stopbits = init_stopbits, \ 
    273                                                                                 xonxoff = init_software_flow_control, \ 
    274                                                                                 rtscts = init_rts_cts_flow_control, \ 
    275                                                                                 timeout = timeout) 
    276                 except: 
    277                         if self.DEBUG: 
    278                                 print "ERROR: FATAL: Serial device not initialized:", 
    279                                 print self.device_address 
     288                #try: 
     289                        #self.device = serial.Serial(port = self.device_address, \ 
     290                                                                                #baudrate = baudrate, \ 
     291                                                                                #bytesize = init_byte_size, \ 
     292                                                                                #parity = init_parity, \ 
     293                                                                                #stopbits = init_stopbits, \ 
     294                                                                                #xonxoff = init_software_flow_control, \ 
     295                                                                                #rtscts = init_rts_cts_flow_control, \ 
     296                                                                                #timeout = timeout) 
     297                #except: 
     298                        #if self.DEBUG: 
     299                                #print "ERROR: FATAL: Serial device not initialized:", 
     300                                #print self.device_address 
     301 
     302 
     303                self.device = serial.Serial(DEFAULT_SERIAL_DEVICE,baudrate=DEFAULT_SERIAL_BAUD,bytesize=serial.EIGHTBITS) 
     304 
     305 
    280306                 
    281307                # NOTE disabled 2012-08-04 due to console not responding 
     
    317343        ################################################################## 
    318344         
    319         #def writeOutput(self, output): 
    320                  
    321                 #self.device.write(output) 
    322          
    323          
    324         ################################################################## 
    325          
    326         #def alarmHandler(self, arg1, arg2): 
    327                 #print 'alarm!' 
    328                 #if STOP_TIME < time.time(): 
    329                         #self.stopBot() 
    330          
    331          
    332         #def initAlarm(self): 
    333                 #signal.alarm(ALARM_INTERVAL) 
    334                 #signal.signal(signal.SIGALRM, alarmHandler) 
    335          
    336          
    337         #def setOutput(self, data): 
    338                 #output = data ^ int('00110011', 2) 
    339                  
    340                 #output = self.int2bin(output) 
    341                 #self.device.write('x%s' % output) 
    342                  
    343                 ##print 'Output set to: ', int2bin(output) 
    344                 #print 'Output set to: ', output 
    345                 #print 'commands: i j k m to move, SPACE = stop, x = quit' 
    346          
    347          
    348         #def moveBot(self, dir, speed): 
    349                 #output = 0 
    350                 #pins = CONTROLLS[dir] 
    351                 #if speed == 1: 
    352                         #output = pins[0] 
    353                 #elif speed == 2: 
    354                         #output = pins[1] 
    355                 #elif speed == 3: 
    356                         #output = pins[0] | pins[1] 
    357                 #self.setOutput(output) 
    358                 #time.sleep(STOP_INTERVAL) 
    359                 #self.stopBot() 
    360          
    361          
    362         #def stopBot(self): 
    363                 #self.setOutput(0) 
    364          
    365          
    366         #def int2bin(self, n, count=8): 
    367                 #return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)]) 
    368          
    369          
    370         ################################################################## 
    371          
    372345        def setOutput(self, data): 
     346                 
     347                if self.DEBUG: 
     348                        print "INFO: setOutput(%s)" % data 
    373349                 
    374350                try: 
    375351                        self.device.write(data) 
    376                         self.device.write("Pz") 
     352                        #self.device.write("Pz") 
     353                        self.device.write(DEVICE_KEEPALIVE_CHECKSUM) 
    377354                 
    378355                except: 
     
    412389                 
    413390                if self.DEBUG: 
    414                         print "moveBot: Output:", 
     391                        print "INFO: moveBot(%s): Output:" % dir, 
    415392                        print output 
    416393                 
     
    419396         
    420397        def stopBot(self): 
     398                 
     399                if self.DEBUG: 
     400                        print "INFO: stopBot()" 
     401                 
    421402                self.setOutput('SS') 
    422403         
    423404         
    424405        def setSpeed(self, value): 
     406                 
     407                if self.DEBUG: 
     408                        print "INFO: setSpeed(%i)" % value 
     409                 
    425410                #global speed 
    426411                self.speed = value 
     
    473458         
    474459         
    475         def main(): 
    476                 while True: 
    477                         PORT.write("Pz") 
    478                         cmd = "" #cmd = MYGETCH() 
    479                         if isData(): 
    480                                 cmd = sys.stdin.read(1) 
    481                                 if cmd == 'x': 
    482                                         break 
    483                                 if len(cmd) > 0: 
    484                                         handleCommand(cmd) 
    485                                         #if cmd == '?': 
    486                                         printHelp() 
    487                         #if PORT.inWaiting() > 0: 
    488                         #       print PORT.readline() 
     460        #def main(): 
     461                #while True: 
     462                        #PORT.write("Pz") 
     463                        #cmd = "" #cmd = MYGETCH() 
     464                        #if isData(): 
     465                                #cmd = sys.stdin.read(1) 
     466                                #if cmd == 'x': 
     467                                        #break 
     468                                #if len(cmd) > 0: 
     469                                        #handleCommand(cmd) 
     470                                        ##if cmd == '?': 
     471                                        #printHelp() 
     472                        ##if PORT.inWaiting() > 0: 
     473                        ##      print PORT.readline() 
    489474         
    490475         
     
    493478        def consoleControl(self): 
    494479                 
    495                 print "DEBUG: consoleControl()" 
     480                if self.DEBUG: 
     481                        print "DEBUG: consoleControl()" 
    496482                 
    497483                if (sys.platform == 'win32'): 
     
    527513         
    528514         
    529                 CHECKSUM = "Pz" 
     515                #CHECKSUM = "Pz" 
    530516                 
    531517                while True: 
     
    533519                         
    534520                        try: 
    535                                 self.device.write(CHECKSUM) 
     521                                self.device.write(DEVICE_KEEPALIVE_CHECKSUM) 
     522                         
    536523                        except: 
    537524                                if self.DEBUG: 
    538525                                        print "ERROR: Failed to write to serial device:", 
    539                                         print CHECKSUM 
     526                                        print DEVICE_KEEPALIVE_CHECKSUM 
    540527                                         
    541528                                         
     
    577564        ################################################################## 
    578565         
     566        def manualControl(self): 
     567                 
     568                if self.DEBUG: 
     569                        print "DEBUG: manualControl()" 
     570                 
     571                if (sys.platform == 'win32'): 
     572                        if self.DEBUG: 
     573                                print "---> Wheelchair Control: Console mode unavailable under Windows" 
     574                         
     575                        self.exitThread() 
     576         
     577         
     578                while True: 
     579                        self.device.write(DEVICE_KEEPALIVE_CHECKSUM) 
     580                        cmd = "" #cmd = MYGETCH() 
     581                        if self.isData(): 
     582                                cmd = sys.stdin.read(1) 
     583                                if cmd == 'x': 
     584                                        break 
     585                                if len(cmd) > 0: 
     586                                        self.handleCommand(cmd) 
     587                                        #if cmd == '?': 
     588                                        self.printHelp() 
     589                        #if PORT.inWaiting() > 0: 
     590                        #       print PORT.readline() 
     591         
     592        #def isData(self): 
     593                #return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) 
     594         
     595         
     596        #def handleCommand(self, cmd): 
     597                ##global speed 
     598                #if cmd in DIGITS: # 0-9 
     599                        #setSpeed(int(cmd)*255/9) 
     600                #if cmd in SPEED_COMMANDS.keys(): 
     601                        #setSpeed(speed + SPEED_COMMANDS[cmd]) 
     602                #elif cmd in MOVE_COMMANDS.keys(): 
     603                        #if MOVE_COMMANDS[cmd] == 'stop': 
     604                                #stopBot() 
     605                        #else: 
     606                                #print MOVE_COMMANDS[cmd] 
     607                                #moveBot(MOVE_COMMANDS[cmd], speed) 
     608 
     609                #elif cmd == 'a': 
     610                        #setDrive('A') 
     611                #elif cmd == 'b': 
     612                        #setDrive('B') 
     613         
     614         
     615        ################################################################## 
     616         
     617        def testControl(self): 
     618                 
     619                #self.stopBot() 
     620                 
     621                #speed = (DEFAULT_WHEELCHAIR_SPEED) * 255/9 
     622                 
     623                #self.setSpeed(speed) 
     624                 
     625                ##self.moveBot('fwd', speed) 
     626                 
     627                #self.timer.start(DEVICE_KEEPALIVE_TIMER) 
     628                 
     629                pass 
     630         
     631         
     632        ################################################################## 
     633         
    579634        #def guiControl(self): 
    580635                 
     
    590645                if (self.command == 'console'): 
    591646                        self.consoleControl() 
     647                 
     648                elif (self.command == 'manual'): 
     649                        self.manualControl() 
     650                 
     651                elif (self.command == 'test'): 
     652                        self.testControl() 
    592653                 
    593654                #elif (self.command == 'gui'): 
     
    723784 
    724785def isData(): 
    725         return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) 
     786        return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) 
    726787 
    727788def main(): 
     789         
     790        port = serial.Serial(DEFAULT_SERIAL_DEVICE,baudrate=DEFAULT_SERIAL_BAUD,bytesize=serial.EIGHTBITS) 
     791         
    728792        while True: 
    729                 PORT.write("Pz") 
     793                #port.write("Pz") 
     794                port.write(DEVICE_KEEPALIVE_CHECKSUM) 
    730795                cmd = "" #cmd = MYGETCH() 
    731796                if isData(): 
     
    746811 
    747812if __name__ == "__main__": 
    748          
    749         PORT = serial.Serial("/dev/arduino1",baudrate=57600,bytesize=serial.EIGHTBITS) 
    750          
    751         old_settings = termios.tcgetattr(sys.stdin) 
    752         stopBot() 
    753         speed = 0 
    754          
    755         try: 
    756                 tty.setcbreak(sys.stdin.fileno()) 
    757                 main() 
    758         finally: 
    759                 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) 
    760  
     813 
     814         
     815        # Perform correct KeyboardInterrupt handling 
     816        # (allows you to hit control-c to quit) 
     817        signal.signal(signal.SIGINT, signal.SIG_DFL) 
     818         
     819        # Collect default settings and command line parameters 
     820        device = DEFAULT_SERIAL_DEVICE 
     821        command = DEFAULT_COMMAND 
     822         
     823        for each in sys.argv: 
     824         
     825                if each.startswith("--device="): 
     826                        device = each[ len("--device="): ] 
     827                elif each.startswith("--command="): 
     828                        command = each[ len("--command="): ] 
     829         
     830         
     831        #app = QtGui.QApplication(sys.argv) 
     832        app = QtCore.QCoreApplication(sys.argv) 
     833         
     834        wheelchair = puzzlebox_brainstorms_wheelchair_control( \ 
     835                        device_address=DEFAULT_SERIAL_DEVICE, \ 
     836                        command=DEFAULT_COMMAND, \ 
     837                        DEBUG=DEBUG) 
     838         
     839        #wheelchair.start() 
     840         
     841        sys.exit(app.exec_()) 
     842         
     843         
     844        #PORT = serial.Serial("/dev/arduino1",baudrate=DEFAULT_SERIAL_BAUD,bytesize=serial.EIGHTBITS) 
     845         
     846        #old_settings = termios.tcgetattr(sys.stdin) 
     847        #stopBot() 
     848        #speed = 0 
     849         
     850        #try: 
     851                #tty.setcbreak(sys.stdin.fileno()) 
     852                #main() 
     853        #finally: 
     854                #termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) 
     855 
Note: See TracChangeset for help on using the changeset viewer.