Changeset 9757141 in robotics


Ignore:
Timestamp:
08/11/13 17:58:39 (8 years ago)
Author:
Steve Castellotti <sc@…>
Branches:
master
Children:
3716c66
Parents:
d75471a (diff), 24bf8e2 (diff)
Note: this is a merge changeset, the changes displayed below correspond to the merge itself.
Use the (diff) links above to see all the changes relative to each parent.
Message:

Merge branch 'master' of ssh://puzzlebox.info:22/repo/robotics

Location:
wheelchair
Files:
2 added
4 edited

Legend:

Unmodified
Added
Removed
  • wheelchair/python/DoraRecieve.py

    • Property mode changed from 100644 to 100755
    radf4715 r24bf8e2  
    11#!/usr/bin/env python 
     2# -*- coding: utf-8 -*- 
     3# 
     4# Puzzlebox - Robotics - Wheelchair - Receiver 
     5# 
     6# Copyright Puzzlebox Productions, LLC (2013) 
     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 
     11import sys 
     12import time 
     13import signal 
     14import serial 
     15import select 
    216 
    317from PySide import QtCore, QtGui, QtNetwork 
    418 
     19import Wheelchair_Control as wheelchair_control 
     20 
     21##################################################################### 
     22# Globals 
     23##################################################################### 
     24 
     25DEBUG=1 
     26 
    527SOCKET = 20320 
     28 
     29DEFAULT_SPEED = 5 
     30 
    631currentTarget= 'i' 
    732previousTarget= 'i' 
     33 
     34DIRECTION_CODES = { 
     35        '0': '', \ 
     36        '1': 'left', \ 
     37        '2': 'right', \ 
     38        '3': 'reverse', \ 
     39        '4': 'forward', \ 
     40        '5': 'stop' 
     41} 
     42 
     43#KEY_CODES = { 
     44        #'':        '', \ 
     45        #'left':    'j', \ 
     46        #'right':   'l', \ 
     47        #'reverse': 'k', \ 
     48        #'forward': 'i', \ 
     49        #'stop':    ' ' 
     50#} 
     51 
     52KEY_CODES = { 
     53        'left': 'left', \ 
     54        'right': 'right', \ 
     55        'reverse': 'rev', \ 
     56        'forward': 'fwd', \ 
     57        'stop': 'stop' 
     58} 
     59 
     60#CONTROLS = { 'fwd': 'F', 
     61             #'rev': 'B', 
     62             #'right': 'R', 
     63             #'left': 'L', 
     64#} 
     65 
     66CONTROLS = { 
     67        'forward': 'F', \ 
     68        'left': 'L', \ 
     69        'right': 'R', \ 
     70        'reverse': 'B' 
     71} 
     72 
     73 
     74##################################################################### 
     75# Classes 
     76##################################################################### 
     77 
    878class Receiver(QtGui.QDialog): 
     79         
    980        def __init__(self, parent=None): 
     81                 
    1082                super(Receiver, self).__init__(parent) 
    1183 
     
    29101                self.setLayout(mainLayout) 
    30102 
    31  
    32103                self.setWindowTitle("Broadcast Receiver") 
     104                 
     105                 
     106                self.wheelchair = wheelchair_control.puzzlebox_brainstorms_wheelchair_control( \ 
     107                        device_address=wheelchair_control.DEFAULT_SERIAL_DEVICE, \ 
     108                        command=wheelchair_control.DEFAULT_COMMAND, \ 
     109                        DEBUG=DEBUG) 
     110         
     111         
     112                self.wheelchair.start() 
     113                 
     114                if DEBUG: 
     115                        print "Wheelchair Started" 
     116 
     117 
     118        ################################################################## 
    33119 
    34120        def processPendingDatagrams(self): 
     121                 
    35122                while self.udpSocket.hasPendingDatagrams(): 
    36                                 datagram, host, port = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize()) 
    37  
    38                                 try: 
    39                                         # Python v3. 
    40                                         datagram = str(datagram, encoding='ascii') 
    41                                         try: 
    42                                                 wheelchair.sendCommand('1','i') 
    43                                         except TypeError: 
    44                                                 Print "Wheelchair does not blah."  
    45                                          
    46                                         #if datagram is '0' 
    47 ##                                              currentTarget = 'i' 
    48 ##                                      elif datagram is '1     ' 
    49 ##                                              currentTarget ='m' 
    50 ##                                      elif datagram is '-1' 
    51 ##                                              currentTarget = PreviousTarget 
    52 ##                                      previousTarget=currentTarget 
    53                                         #wheelchair.sendCommand('1', currentTarget) 
    54  
    55  
    56                                 except TypeError: 
    57                                         # Python v2. 
    58                                         pass 
    59  
    60                                 self.statusLabel.setText("Received datagram: \"%s\"" % datagram) 
    61  
     123 
     124 
     125                        datagram, host, port = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize()) 
     126 
     127 
     128                        try: 
     129                                # Python v3. 
     130                                datagram = str(datagram, encoding='ascii') 
     131                        except: 
     132                                # Python v2. 
     133                                pass 
     134 
     135 
     136                        #if DEBUG: 
     137                                #print "Datagram: %s" % datagram 
     138                                #print datagram 
     139                                #print 
     140                                #print type(datagram) 
     141                                #print 
     142 
     143 
     144 
     145                        try: 
     146                                 
     147                        #if True: 
     148                                #self.wheelchair.sendCommand('1','i') 
     149                                 
     150                                datagram = str(datagram) 
     151                                 
     152                                #if DEBUG: 
     153                                        #print "Datagram str():", 
     154                                        #print datagram 
     155                                        #print 
     156                                        #print type(datagram) 
     157                                        #print 
     158                                 
     159                                #DIRECTION_CODES = { 
     160                                        #'0': '', \ 
     161                                        #'1': 'left', \ 
     162                                        #'2': 'right', \ 
     163                                        #'3': 'reverse', \ 
     164                                        #'4': 'forward', \ 
     165                                        #'5': 'stop' 
     166                                #} 
     167                                 
     168                                #KEY_CODES = { 
     169                                        #'':        '', \ 
     170                                        #'left':    'j', \ 
     171                                        #'right':   'l', \ 
     172                                        #'reverse': 'k', \ 
     173                                        #'forward': 'i', \ 
     174                                        #'stop':    ' ' 
     175                                #} 
     176                                 
     177                                 
     178                                if (datagram in DIRECTION_CODES.keys()): 
     179                                         
     180                                        direction = DIRECTION_CODES[datagram] 
     181                                         
     182                                        #print direction 
     183                                         
     184                                        if ((direction in KEY_CODES.keys()) and \ 
     185                                            (KEY_CODES[direction] != '')): 
     186                                         
     187                                                key_code = KEY_CODES[direction] 
     188                                                 
     189                                                 
     190                                                if DEBUG: 
     191                                                        print "[Dora] Direction: %s Sending: %s" % \ 
     192                                                                (direction, key_code) 
     193                                                 
     194                                                 
     195                                                self.wheelchair.sendCommand('%i' % DEFAULT_SPEED, key_code) 
     196                         
     197                                else: 
     198                                         
     199                                        if DEBUG: 
     200                                                print "[Dora] Direction not recognized:", 
     201                                                print datagram 
     202                         
     203                        except Exception, e: 
     204                                if DEBUG: 
     205                                        print "ERROR: Exception:", 
     206                                        print e 
     207                                 
     208 
     209 
     210                        self.statusLabel.setText("Received datagram: \"%s\"" % datagram) 
     211 
     212 
     213##################################################################### 
     214# Globals 
     215##################################################################### 
    62216 
    63217if __name__ == '__main__': 
    64  
    65         import sys 
    66         import time 
    67         import signal 
    68         import serial 
    69         import select 
    70  
    71  
    72218 
    73219        app = QtGui.QApplication(sys.argv) 
    74220        receiver = Receiver() 
    75 #       receiver.show() 
     221        #receiver.show() 
    76222        sys.exit(receiver.exec_()) 
    77223 
    78         #This code is not really needed. Just gives us keyboard control also 
    79         # Perform correct KeyboardInterrupt handling 
     224        # Perform correct KeyboardInterrupt handling 
     225        # (allows you to hit control-c to quit) 
    80226        signal.signal(signal.SIGINT, signal.SIG_DFL) 
    81227 
     
    92238 
    93239 
    94         app1 = QtCore.QCoreApplication(sys.argv) 
    95  
    96         wheelchair = puzzlebox_brainstorms_wheelchair_control( \ 
    97                         device_address=device, \ 
    98                         command=command, \ 
    99                         DEBUG=DEBUG) 
     240        #app1 = QtCore.QCoreApplication(sys.argv) 
     241 
     242        #wheelchair = puzzlebox_brainstorms_wheelchair_control( \ 
     243                        #device_address=device, \ 
     244                        #command=command, \ 
     245                        #DEBUG=DEBUG) 
    100246 
    101247        receiver.show() 
    102         print "Receiver Showing" 
    103         wheelchair.start() 
    104         print "Wheelchair Started" 
    105  
    106  
     248         
     249        if DEBUG: 
     250                print "Receiver Showing" 
     251        #wheelchair.start() 
     252        #print "Wheelchair Started" 
    107253 
    108254        sys.exit(app.exec_()) 
    109         sys.exit(app1.exec_()) 
     255        #sys.exit(app1.exec_()) 
  • wheelchair/python/Wheelchair_Control.py

    radf4715 r9bf1fe2  
    44# Puzzlebox - Brainstorms - Wheelchair Control 
    55# 
    6 # Copyright Puzzlebox Productions, LLC (2010-2012) 
     6# Copyright Puzzlebox Productions, LLC (2010-2013) 
    77# 
    88# This code is released under the GNU Pulic License (GPL) version 2 
     
    1010 
    1111__changelog__ = """ 
    12 Last Update: 2012.08.05 
     12Last Update: 2013.08.11 
    1313 
    1414""" 
     
    1919import serial 
    2020import select 
    21  
    22 import Configuration as configuration 
    23  
    24 if configuration.ENABLE_PYSIDE: 
     21import termios, select, sys, tty 
     22 
     23#import Configuration as configuration 
     24 
     25#if configuration.ENABLE_PYSIDE: 
     26try: 
    2527        try: 
    2628                import PySide as PyQt4 
     
    2931                print "ERROR: Exception importing PySide:", 
    3032                print e 
    31                 configuration.ENABLE_PYSIDE = False 
     33                #configuration.ENABLE_PYSIDE = False 
    3234        else: 
    3335                print "INFO: [Brainstorms:Wheelchair_Control] Using PySide module" 
    3436 
    35 if not configuration.ENABLE_PYSIDE: 
     37#if not configuration.ENABLE_PYSIDE: 
     38except: 
    3639        print "INFO: [Brainstorms:Wheelchair_Control] Using PyQt4 module" 
    3740        from PyQt4 import QtCore 
     
    99102} 
    100103 
     104# 2013.08.11 
     105#MOVE_COMMANDS ={ 
     106                #' ' : 'stop', 
     107                #'i' : 'fwd', 
     108                #'m' : 'rev', 
     109                #'j' : 'left', 
     110                #'k' : 'right', 
     111                #} 
     112 
     113 
    101114MOVE_COMMANDS ={ 
    102                 ' ' : 'stop', 
    103                 'i' : 'fwd', 
    104                 'm' : 'rev', 
    105                 'j' : 'left', 
    106                 'k' : 'right', 
    107                 } 
     115        ' ' : 'stop', 
     116        'i' : 'fwd', 
     117        'k' : 'rev', 
     118        'j' : 'left', 
     119        'l' : 'right', 
     120} 
     121 
    108122 
    109123#SPEED_COMMANDS = { 
     
    120134 
    121135DIGITS = map(str,range(10)) 
     136 
     137#PORT = serial.Serial("/dev/arduino1",baudrate=57600,bytesize=serial.EIGHTBITS) 
     138PORT = None 
    122139 
    123140STOP_TIME = 0 
     
    226243                        init_rts_cts_flow_control = 0 
    227244                 
    228                 self.device = serial.Serial(port = self.device_address, \ 
     245                try: 
     246                        self.device = serial.Serial(port = self.device_address, \ 
    229247                                                                                baudrate = baudrate, \ 
    230248                                                                                bytesize = init_byte_size, \ 
     
    234252                                                                                rtscts = init_rts_cts_flow_control, \ 
    235253                                                                                timeout = timeout) 
    236                  
     254                except: 
     255                        if self.DEBUG: 
     256                                print "ERROR: FATAL: Serial device not initialized:", 
     257                                print self.device_address 
    237258                 
    238259                # NOTE disabled 2012-08-04 due to console not responding 
     
    246267        def sendCommand(self, speed, command): 
    247268                 
    248                 output = '%s%s' % (COMMAND_CHARACTER, \ 
    249                         WHEELCHAIR_COMMANDS[speed][command]) 
    250                  
    251                 self.device.write(output) 
     269                #output = '%s%s' % (COMMAND_CHARACTER, \ 
     270                        #WHEELCHAIR_COMMANDS[speed][command]) 
     271                 
     272                 
     273                if speed != self.wheelchair_speed: 
     274                        self.setSpeed(speed) 
     275                 
     276                 
     277                self.moveBot(command) 
     278                 
     279                #self.device.write(output) 
     280                 
     281                #if self.DEBUG: 
     282                        #print "--> Wheelchair Command: %s (Speed %i) [%s]" % \ 
     283                           #(command, speed, output) 
    252284                 
    253285                if self.DEBUG: 
    254                         print "--> Wheelchair Command: %s (Speed %i) [%s]" % \ 
    255                            (command, speed, output) 
    256                  
    257                 self.wheelchair_speed = speed 
     286                        print "--> Wheelchair Command: %s (Speed %i)" % \ 
     287                           (command, self.speed) 
     288                 
     289                #self.wheelchair_speed = speed 
     290                self.wheelchair_speed = self.speed 
     291                 
    258292                self.wheelchair_command = command 
     293         
     294         
     295        ################################################################## 
     296         
     297        #def writeOutput(self, output): 
     298                 
     299                #self.device.write(output) 
    259300         
    260301         
     
    308349         
    309350        def setOutput(self, data): 
    310                 self.device.write(data) 
    311                 self.device.write("Pz") 
     351                 
     352                try: 
     353                        self.device.write(data) 
     354                        self.device.write("Pz") 
     355                 
     356                except: 
     357                        if self.DEBUG: 
     358                                print "ERROR: Failed to write to serial device:", 
     359                                print data 
     360                 
     361                 
    312362        #       print 'Output set to:', data[0], ord(data[1]) 
    313363        #       printHelp() 
    314          
    315364         
    316365        def setDrive(self, value): 
     
    321370         
    322371         
    323         #def moveBot(self, dir, speed): 
     372        # 2013.08.11 
     373        ##def moveBot(self, dir, speed): 
     374        #def moveBot(self, dir): 
     375                #if (dir in ['left','right']): 
     376                        ##output = CONTROLS[dir] + chr(int(speed * steeringsensitivity)) 
     377                        #output = CONTROLS[dir] + chr(int(self.speed * STEERING_SENSITIVITY)) 
     378                #else: 
     379                        #output = CONTROLS[dir] + chr(self.speed) 
     380                #self.setOutput(output) 
     381         
     382         
    324383        def moveBot(self, dir): 
    325                 if (dir in ['left','right']): 
    326                         #output = CONTROLS[dir] + chr(int(speed * steeringsensitivity)) 
    327                         output = CONTROLS[dir] + chr(int(self.speed * STEERING_SENSITIVITY)) 
     384                 
     385                if (dir == 'stop'): 
     386                        output = 'SS' 
     387                 
    328388                else: 
    329389                        output = CONTROLS[dir] + chr(self.speed) 
     390                 
     391                if self.DEBUG: 
     392                        print "moveBot: Output:", 
     393                        print output 
     394                 
    330395                self.setOutput(output) 
    331396         
     
    386451         
    387452         
     453        def main(): 
     454                while True: 
     455                        PORT.write("Pz") 
     456                        cmd = "" #cmd = MYGETCH() 
     457                        if isData(): 
     458                                cmd = sys.stdin.read(1) 
     459                                if cmd == 'x': 
     460                                        break 
     461                                if len(cmd) > 0: 
     462                                        handleCommand(cmd) 
     463                                        #if cmd == '?': 
     464                                        printHelp() 
     465                        #if PORT.inWaiting() > 0: 
     466                        #       print PORT.readline() 
     467         
     468         
    388469        ################################################################# 
    389470         
     
    423504                                        #STOP_TIME = time.time() + STOP_INTERVAL 
    424505         
     506         
     507                CHECKSUM = "Pz" 
     508                 
    425509                while True: 
    426510                        #PORT.write("Pz") 
    427                         self.device.write("Pz") 
    428511                         
     512                        try: 
     513                                self.device.write(CHECKSUM) 
     514                        except: 
     515                                if self.DEBUG: 
     516                                        print "ERROR: Failed to write to serial device:", 
     517                                        print CHECKSUM 
     518                                         
     519                                         
    429520                        cmd = MYGETCH() 
    430521                         
     
    541632##################################################################### 
    542633 
    543  
     634def setOutput(data): 
     635        PORT.write(data) 
     636        PORT.write("Pz") 
     637#       print 'Output set to:', data[0], ord(data[1]) 
     638#       printHelp() 
     639 
     640 
     641def setDrive(value): 
     642        if value == 'A': 
     643                setOutput('DA') 
     644        if value == 'B': 
     645                setOutput('DB') 
     646 
     647 
     648def moveBot(dir, speed): 
     649        output = CONTROLS[dir] + chr(speed) 
     650        setOutput(output) 
     651 
     652 
     653def stopBot(): 
     654        setOutput('SS') 
     655 
     656 
     657def setSpeed(value): 
     658        global speed 
     659        speed = value 
     660        if speed > 255: 
     661                speed = 255 
     662        if speed < 0: 
     663                speed = 0 
     664        print 'speed:', speed, '/ 255' 
     665 
     666 
     667def handleCommand(cmd): 
     668        global speed 
     669        if cmd in DIGITS: # 0-9 
     670                setSpeed(int(cmd)*255/9) 
     671        if cmd in SPEED_COMMANDS.keys(): 
     672                setSpeed(speed + SPEED_COMMANDS[cmd]) 
     673        elif cmd in MOVE_COMMANDS.keys(): 
     674                if MOVE_COMMANDS[cmd] == 'stop': 
     675                        stopBot() 
     676                else: 
     677                        print MOVE_COMMANDS[cmd] 
     678                        moveBot(MOVE_COMMANDS[cmd], speed) 
     679        elif cmd == 'w': 
     680                rockets.up() 
     681        elif cmd == 'e': 
     682                rockets.down() 
     683        elif cmd == 'q': 
     684                rockets.left() 
     685        elif cmd == 'r': 
     686                rockets.right()  
     687        elif cmd == 't': 
     688                rockets.laser() 
     689        elif cmd == 'y': 
     690                rockets.fire() 
     691        elif cmd == 'a': 
     692                setDrive('A') 
     693        elif cmd == 'b': 
     694                setDrive('B') 
     695  
     696def printHelp(): 
     697        print 'commands: ' 
     698        print 'i j k l to move, SPACE = stop, x = quit' 
     699        print 's d f g or 0-9 to adjust speed between 0 and 255.' 
     700 
     701 
     702def isData(): 
     703        return select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []) 
     704 
     705def main(): 
     706        while True: 
     707                PORT.write("Pz") 
     708                cmd = "" #cmd = MYGETCH() 
     709                if isData(): 
     710                        cmd = sys.stdin.read(1) 
     711                        if cmd == 'x': 
     712                                break 
     713                        if len(cmd) > 0: 
     714                                handleCommand(cmd) 
     715                                #if cmd == '?': 
     716                                printHelp() 
     717                #if PORT.inWaiting() > 0: 
     718                #       print PORT.readline() 
     719 
     720 
     721##################################################################### 
     722# Main 
     723##################################################################### 
     724 
     725if __name__ == "__main__": 
     726         
     727        PORT = serial.Serial("/dev/arduino1",baudrate=57600,bytesize=serial.EIGHTBITS) 
     728         
     729        old_settings = termios.tcgetattr(sys.stdin) 
     730        stopBot() 
     731        speed = 0 
     732         
     733        try: 
     734                tty.setcbreak(sys.stdin.fileno()) 
     735                main() 
     736        finally: 
     737                termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) 
     738 
  • wheelchair/udp/broadcastreceiver.py

    • Property mode changed from 100644 to 100755
    r6a76f6a r9bf1fe2  
    3232        def processPendingDatagrams(self): 
    3333                while self.udpSocket.hasPendingDatagrams(): 
    34                                 datagram, host, port = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize()) 
     34                         
     35                        datagram, host, port = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize()) 
    3536 
    36                                 try: 
    37                                         # Python v3. 
    38                                         datagram = str(datagram, encoding='ascii') 
    39                                 except TypeError: 
    40                                         # Python v2. 
    41                                         pass 
     37                        try: 
     38                                # Python v3. 
     39                                datagram = str(datagram, encoding='ascii') 
     40                        except TypeError: 
     41                                # Python v2. 
     42                                pass 
    4243 
    43                                 self.statusLabel.setText("Received datagram: \"%s\"" % datagram) 
     44                        self.statusLabel.setText("Received datagram: \"%s\"" % datagram) 
    4445 
    4546 
  • wheelchair/udp/broadcastsender.py

    • Property mode changed from 100644 to 100755
    r6a76f6a r9bf1fe2  
    1010                QtGui.QDialog.__init__(self, parent) 
    1111                 
    12                 self.statusLabel = QtGui.QLabel(self.tr("Ready to broadcast datagramms on port 45454")) 
     12                self.statusLabel = QtGui.QLabel(self.tr("Ready to broadcast datagramms on port %i" %SOCKET)) 
    1313                self.startButton = QtGui.QPushButton(self.tr("&Start")) 
    1414                self.quitButton = QtGui.QPushButton(self.tr("&Quit")) 
Note: See TracChangeset for help on using the changeset viewer.