source: robotics/wheelchair/python/DoraRecieve.py

Last change on this file was 29b9de0, checked in by Steve Castellotti <sc@…>, 8 years ago
  • Final version for live control at Sudo Room 2013.08.11
  • Property mode set to 100755
File size: 5.0 KB
Line 
1#!/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
16
17from PySide import QtCore, QtGui, QtNetwork
18
19import Wheelchair_Control as wheelchair_control
20
21#####################################################################
22# Globals
23#####################################################################
24
25DEBUG=1
26
27SOCKET = 20320
28
29DEFAULT_SPEED = 5
30
31currentTarget= 'i'
32previousTarget= 'i'
33
34#DIRECTION_CODES = {
35        #'0': '', \
36        #'1': 'left', \
37        #'2': 'right', \
38        #'3': 'reverse', \
39        #'4': 'forward', \
40        #'5': 'stop'
41#}
42
43# '\n' is equivalent to C++ endl
44DIRECTION_CODES = {
45        '0': 'repeat', \
46        '1': 'forward', \
47        '2': 'reverse', \
48        '3': 'left', \
49        '4': 'right', \
50        '5': 'stop',
51        '0\n': 'repeat', \
52        '1\n': 'forward', \
53        '2\n': 'reverse', \
54        '3\n': 'left', \
55        '4\n': 'right', \
56        '5\n': 'stop'
57}
58
59#KEY_CODES = {
60        #'':        '', \
61        #'left':    'j', \
62        #'right':   'l', \
63        #'reverse': 'k', \
64        #'forward': 'i', \
65        #'stop':    ' '
66#}
67
68KEY_CODES = {
69        'left': 'left', \
70        'right': 'right', \
71        'reverse': 'rev', \
72        'forward': 'fwd', \
73        'stop': 'stop'
74}
75
76#CONTROLS = { 'fwd': 'F',
77             #'rev': 'B',
78             #'right': 'R',
79             #'left': 'L',
80#}
81
82CONTROLS = {
83        'forward': 'F', \
84        'left': 'L', \
85        'right': 'R', \
86        'reverse': 'B'
87}
88
89
90#####################################################################
91# Classes
92#####################################################################
93
94class Receiver(QtGui.QDialog):
95       
96        def __init__(self, parent=None):
97               
98                super(Receiver, self).__init__(parent)
99
100                self.statusLabel = QtGui.QLabel("Listening for broadcasted messages")
101                quitButton = QtGui.QPushButton("&Quit")
102
103                self.udpSocket = QtNetwork.QUdpSocket(self)
104                self.udpSocket.bind(SOCKET)
105
106                self.udpSocket.readyRead.connect(self.processPendingDatagrams)
107                quitButton.clicked.connect(self.close)
108
109                buttonLayout = QtGui.QHBoxLayout()
110                buttonLayout.addStretch(1)
111                buttonLayout.addWidget(quitButton)
112                buttonLayout.addStretch(1)
113
114                mainLayout = QtGui.QVBoxLayout()
115                mainLayout.addWidget(self.statusLabel)
116                mainLayout.addLayout(buttonLayout)
117                self.setLayout(mainLayout)
118
119                self.setWindowTitle("Broadcast Receiver")
120               
121               
122                self.last_direction = 'repeat'
123               
124               
125                self.wheelchair = wheelchair_control.puzzlebox_brainstorms_wheelchair_control( \
126                        device_address=wheelchair_control.DEFAULT_SERIAL_DEVICE, \
127                        command=wheelchair_control.DEFAULT_COMMAND, \
128                        DEBUG=DEBUG)
129       
130       
131                self.wheelchair.start()
132               
133               
134                if DEBUG:
135                        print "Wheelchair Started"
136
137
138        ##################################################################
139
140        def processPendingDatagrams(self):
141               
142                while self.udpSocket.hasPendingDatagrams():
143
144                        datagram, host, port = self.udpSocket.readDatagram(self.udpSocket.pendingDatagramSize())
145
146
147                        try:
148                                # Python v3.
149                                datagram = str(datagram, encoding='ascii')
150                        except:
151                                # Python v2.
152                                pass
153                       
154
155                        try:
156                               
157                               
158                                datagram = str(datagram)
159                               
160                               
161                                if (datagram in DIRECTION_CODES.keys()):
162                                       
163                                        direction = DIRECTION_CODES[datagram]
164                                       
165                                        print "NEW direction:",
166                                        print direction
167                                        print "last_direction:",
168                                        print self.last_direction
169                                       
170                                       
171                                        if (direction == 'repeat'):
172                                                direction = self.last_direction
173                                        else:
174                                                print
175                                                print "[====================================================]"
176                                                print
177                                       
178                                        if ((direction in KEY_CODES.keys()) and \
179                                            (KEY_CODES[direction] != '')):
180                                       
181                                                key_code = KEY_CODES[direction]
182                                               
183                                               
184                                                if DEBUG:
185                                                        print "[Dora] Direction: %s Sending: %s" % \
186                                                                (direction, key_code)
187                                               
188                                               
189                                                self.wheelchair.sendCommand('%i' % DEFAULT_SPEED, key_code)
190                                               
191                                               
192                                        self.last_direction = direction
193                                       
194                                       
195                                        #print "AFTER"
196                                        #print "last_direction:",
197                                        #print self.last_direction
198                               
199                               
200                                else:
201                                       
202                                        if DEBUG > 1:
203                                                print "[Dora] Direction not recognized:",
204                                                print datagram
205                       
206                        except Exception, e:
207                                if DEBUG:
208                                        print "ERROR: Exception:",
209                                        print e
210                               
211                       
212                       
213                        self.statusLabel.setText("Received datagram: \"%s\"" % datagram.strip())
214
215
216#####################################################################
217# Globals
218#####################################################################
219
220if __name__ == '__main__':
221
222        app = QtGui.QApplication(sys.argv)
223        receiver = Receiver()
224       
225        # Perform correct KeyboardInterrupt handling
226        # (allows you to hit control-c to quit)
227        signal.signal(signal.SIGINT, signal.SIG_DFL)
228       
229        # Collect default settings and command line parameters
230        #device = DEFAULT_SERIAL_DEVICE
231        #command = DEFAULT_COMMAND
232       
233        #for each in sys.argv:
234       
235                #if each.startswith("--device="):
236                        #device = each[ len("--device="): ]
237                #elif each.startswith("--command="):
238                        #command = each[ len("--command="): ]
239       
240       
241        receiver.show()
242       
243        if DEBUG > 1:
244                print "Receiver Showing"
245
246        sys.exit(app.exec_())
Note: See TracBrowser for help on using the repository browser.