source: robotics/car/python/Car_Demo.py @ b821883

Last change on this file since b821883 was b821883, checked in by Steve Castellotti <sc@…>, 8 years ago

Car Demo:

  • initial commit (ported from Protocol_Orbit)
  • Property mode set to 100755
File size: 11.4 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Robotics - Car - Demo
5#
6# Copyright Puzzlebox Productions, LLC (2014)
7
8__changelog__ = """\
9Last Update: 2014.06.09
10"""
11
12__todo__ = """
13"""
14
15import string, time
16import serial
17
18#import Configuration as configuration
19
20#if configuration.ENABLE_PYSIDE:
21        #try:
22                #import PySide as PyQt4
23                #from PySide import QtCore
24        #except Exception, e:
25                #print "ERROR: Exception importing PySide:",
26                #print e
27                #configuration.ENABLE_PYSIDE = False
28        #else:
29                #print "INFO: [TAG] Using PySide module"
30
31#if not configuration.ENABLE_PYSIDE:
32        #print "INFO: [TAG] Using PyQt4 module"
33        #from PyQt4 import QtCore
34
35#import Puzzlebox.Jigsaw.Configuration as configuration
36##import puzzlebox_logger
37
38
39#####################################################################
40# Globals
41#####################################################################
42
43DEBUG = 1
44
45TAG = 'Robotics:Car'
46
47#DEFAULT_COMMAND = 'neutral'
48#DEFAULT_SERIAL_DEVICE = '/dev/ttyACM0'
49DEFAULT_SERIAL_DEVICE_STEPPER = '/dev/ttyACM0'
50DEFAULT_SERIAL_DEVICE_PYRAMID = '/dev/ttyUSB0'
51
52DEFAULT_MODE = "pyramid"
53#DEFAULT_MODE = "pair"
54
55DEFAULT_ARDUINO_BAUD_RATE = 9600
56DEFAULT_STEPPER_BAUD_RATE = 115200
57DEFAULT_PYRAMID_BAUD_RATE = 115200
58
59ARDUINO_INITIALIZATION_TIME = 2
60#ARDUINO_INITIALIZATION_TIME = 12
61
62#THROTTLE_POWER_MODIFIER = 0.25
63#STEERING_POWER_MODIFIER = 0.75
64
65LOOP_TIMER = 80 # 80 ms
66
67
68#####################################################################
69# Classes
70#####################################################################
71
72class puzzlebox_robotics_car_demo(QtCore.QThread):
73       
74        def __init__(self, log, \
75                     serial_port_stepper=DEFAULT_SERIAL_DEVICE_STEPPER, \
76                     serial_port_pyramid=DEFAULT_SERIAL_DEVICE_PYRAMID, \
77                     mode=DEFAULT_MODE, \
78                     command=DEFAULT_COMMAND, \
79                     DEBUG=DEBUG, parent=None):
80               
81                QtCore.QThread.__init__(self,parent)
82               
83                self.log = log
84                self.DEBUG = DEBUG
85                self.parent = parent
86                self.serial_port_stepper = serial_port_stepper
87                self.serial_port_pyramid = serial_port_pyramid
88                self.mode = mode
89               
90                self.command = command
91               
92                self.device_stepper = None
93                self.device_pyramid = None
94               
95                self.configuration = configuration
96               
97                self.name = "Robotics-Car-Demo"
98               
99                self.command_queue = []
100               
101               
102                try:
103                        self.initializeSerial()
104                except Exception, e:
105                        if self.DEBUG:
106                                print "<-- [TAG] Device connection failed"
107                                print "ERROR [TAG]:",
108                                print e
109               
110               
111                self.keep_running = True
112       
113       
114        ##################################################################
115       
116        def initializeSerial(self):
117               
118                if self.mode == 'pyramid':
119                        baudrate = DEFAULT_PYRAMID_BAUD_RATE
120                        rts_cts_flow_control = 'f'
121                else:
122                        baudrate = DEFAULT_ARDUINO_BAUD_RATE
123                        rts_cts_flow_control = 't'
124                       
125                if self.DEBUG:
126                        print "INFO [TAG]: Stepper serial device:",
127                        print self.serial_port_stepper
128                        print "INFO [TAG]: Connecting at baud rate",
129                        print baudrate
130                       
131                        print "INFO [TAG]: Pyramid serial device:",
132                        print self.serial_port_pyramid
133                        print "INFO [TAG]: Pyramid at baud rate",
134                        print baudrate
135                       
136                bytesize = 8
137                parity = 'NONE'
138                stopbits = 1
139                software_flow_control = 'f'
140                #timeout = 15
141                timeout = 5
142               
143                # convert bytesize
144                if (bytesize == 5):
145                        init_byte_size = serial.FIVEBITS
146                elif (bytesize == 6):
147                        init_byte_size = serial.SIXBITS
148                elif (bytesize == 7):
149                        init_byte_size = serial.SEVENBITS
150                elif (bytesize == 8):
151                        init_byte_size = serial.EIGHTBITS
152                else:
153                        #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
154                        init_byte_size = serial.EIGHTBITS
155               
156                # convert parity
157                if (parity == 'NONE'):
158                        init_parity = serial.PARITY_NONE
159                elif (parity == 'EVEN'):
160                        init_parity = serial.PARITY_EVEN
161                elif (parity == 'ODD'):
162                        init_parity = serial.PARITY_ODD
163                else:
164                        #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
165                        init_parity = serial.PARITY_NONE
166               
167                # convert stopbits
168                if (stopbits == 1):
169                        init_stopbits = serial.STOPBITS_ONE
170                elif (stopbits == 2):
171                        init_stopbits = serial.STOPBITS_TWO
172                else:
173                        #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
174                        init_byte_size = serial.STOPBITS_ONE
175               
176                # convert software flow control
177                if (software_flow_control == 't'):
178                        init_software_flow_control = 1
179                else:
180                        init_software_flow_control = 0
181               
182                # convert rts cts flow control
183                if (rts_cts_flow_control == 't'):
184                        init_rts_cts_flow_control = 1
185                else:
186                        init_rts_cts_flow_control = 0
187               
188                self.device_stepper = serial.Serial(port = self.serial_port_stepper, \
189                                            baudrate = baudrate, \
190                                            bytesize = init_byte_size, \
191                                            parity = init_parity, \
192                                            stopbits = init_stopbits, \
193                                            xonxoff = init_software_flow_control, \
194                                            rtscts = init_rts_cts_flow_control, \
195                                            timeout = timeout)
196               
197                self.device_pyramid = serial.Serial(port = self.serial_port_pyramid, \
198                                            baudrate = baudrate, \
199                                            bytesize = init_byte_size, \
200                                            parity = init_parity, \
201                                            stopbits = init_stopbits, \
202                                            xonxoff = init_software_flow_control, \
203                                            rtscts = init_rts_cts_flow_control, \
204                                            timeout = timeout)
205                #self.land()
206               
207                time.sleep(ARDUINO_INITIALIZATION_TIME)
208       
209       
210        ##################################################################
211       
212        def get_status(self, connection):
213               
214                status = 'Connected'
215               
216                return(status)
217       
218       
219        ##################################################################
220       
221        def hover(self):
222               
223                "Set throttle to hover"
224               
225                # Issue command
226                if self.DEBUG:
227                        print '--> [TAG] Hover ("P")'
228               
229                #self.device.write("P")
230                self.command_queue.append(command)
231       
232       
233        ##################################################################
234       
235        def land(self):
236               
237                "Set throttle to zero"
238               
239                # Issue command
240                if self.DEBUG:
241                        print '--> [TAG] Land ("O")'
242               
243                self.command_queue.append("O")
244                #self.device.write("O")
245       
246       
247        ##################################################################
248       
249        def padValue(self, value):
250               
251                while len(value) < 3:
252                        value = '0%s' % value
253               
254                return value
255       
256       
257        ##################################################################
258       
259        def setThrottle(self, throttle):
260               
261                value = self.padValue( str(throttle) )
262               
263                command = 't%s' % value
264               
265                if self.DEBUG:
266                        print "--> [TAG] Sending Orbit Command:",
267                        print command
268               
269                #self.device.write(command)
270                self.command_queue.append(command)
271       
272       
273        ##################################################################
274       
275        def setYaw(self, yaw):
276               
277                value = self.padValue( str(yaw) )
278               
279                command = 'y%s' % value
280               
281                if self.DEBUG:
282                        print "--> [TAG] Sending Orbit Command:",
283                        print command
284               
285                #self.device.write(command)
286                self.command_queue.append(command)
287       
288       
289        ##################################################################
290       
291        def setPitch(self, pitch):
292               
293                value = self.padValue( str(pitch) )
294               
295                command = 'p%s' % value
296               
297                if self.DEBUG:
298                        print "--> [TAG] Sending Orbit Command:",
299                        print command
300               
301                #self.device.write(command)
302                self.command_queue.append(command)
303       
304       
305        ##################################################################
306       
307        def sendCommand(self, command, power=None):
308               
309                if self.DEBUG:
310                        print "INFO: [TAG] Sending Orbit Command:",
311                        print command
312               
313                self.command_queue.append(command)
314       
315       
316        ##################################################################
317       
318        def convertColorToString(self, color):
319               
320                if len("%s" % color) == 1:
321                        output = "00%i" % color
322                elif len("%s" % color) == 2:
323                        output = "0%i" % color
324                elif len("%s" % color) == 3:
325                        output = "%i" % color
326                else:
327                        output = "%i" % color
328                        output = output[:3]
329               
330               
331                return(output)
332       
333       
334        ##################################################################
335       
336        def setColorWheel(self, red, green, blue):
337               
338                red = self.convertColorToString(red)
339                green = self.convertColorToString(green)
340                blue = self.convertColorToString(blue)
341               
342                command = "w%s%s%s" % (red, green, blue)
343               
344                if self.DEBUG:
345                        print "INFO: [TAG] Sending Orbit Command:",
346                        print command
347               
348                self.command_queue.append(command)
349                #self.device.write(command)
350       
351       
352        ##################################################################
353       
354        def setColor(self, position, red, green, blue):
355               
356                position = position - 1 # convert from counting number
357               
358                if len("%s" % position) == 1:
359                        position = "0%s" % position
360                else:
361                        position = "%s" % position
362               
363                red = convertColorToString(red)
364                green = convertColorToString(green)
365                blue = convertColorToString(blue)
366               
367                command = "c%s%s%s%s" % (position, red, green, blue)
368               
369                if self.DEBUG:
370                        print "INFO: [TAG] Sending Orbit Command:",
371                        print command
372               
373                self.command_queue.append(command)
374                #self.device.write(command)
375               
376               
377        ##################################################################
378       
379        def processCommand(self, command):
380               
381                if (command == 'hover'):
382                        self.hover()
383               
384                elif (command == 'land'):
385                        self.land()
386               
387                else:
388                        self.device.write(command)
389       
390       
391        ##################################################################
392       
393        def run(self):
394               
395                while self.keep_running:
396                       
397                        #if self.command_queue != []:
398                        while self.command_queue != []:
399                               
400                                command = self.command_queue[0]
401                                self.command_queue = self.command_queue[1:]
402                               
403                                try:
404                                        self.processCommand(command)
405                                        pass
406                                except Exception, e:
407                                        print "ERROR: [TAG] Exception sending car commands:",
408                                        print e
409                       
410                       
411                        # Sleep 80 ms
412                        QtCore.QThread.msleep(LOOP_TIMER)
413       
414       
415        ##################################################################
416       
417        def stop(self):
418               
419                #self.land()
420               
421                self.keep_running = False
422               
423                self.command_queue = []
424               
425                try:
426                        self.device.close()
427                except Exception, e:
428                        if self.DEBUG:
429                                print "ERROR: [TAG] failed to close device in stop():",
430                                print e
431       
432       
433        ##################################################################
434       
435        def exitThread(self, callThreadQuit=True):
436               
437                self.stop()
438               
439                if callThreadQuit:
440                        try:
441                                QtCore.QThread.quit(self)
442                        except Exception, e:
443                                if self.DEBUG:
444                                        print "ERROR: [TAG] failed to call QtCore.QThread.quit(self) in exitThread():",
445                                        print e
446
447
448#####################################################################
449# Functions
450#####################################################################
451
452#####################################################################
453# Main
454#####################################################################
455
456if __name__ == '__main__':
457       
458        # Perform correct KeyboardInterrupt handling
459        signal.signal(signal.SIGINT, signal.SIG_DFL)
460       
461        # Collect default settings and command line parameters
462        device = DEFAULT_SERIAL_DEVICE
463        mode = DEFAULT_MODE
464        command = DEFAULT_COMMAND
465       
466        for each in sys.argv:
467               
468                if each.startswith("--device="):
469                        device = each[ len("--device="): ]
470                elif each.startswith("--mode="):
471                        mode = each[ len("--mode="): ]
472                elif each.startswith("--command="):
473                        command = each[ len("--command="): ]
474       
475       
476        app = QtCore.QCoreApplication(sys.argv)
477       
478        demo = puzzlebox_robotics_car_demo( \
479                        None, \
480                        serial_port_stepper=DEFAULT_SERIAL_DEVICE_STEPPER, \
481                        serial_port_pyramid=DEFAULT_SERIAL_DEVICE_PYRAMID, \
482                        mode=mode, \
483                        command=command, \
484                        DEBUG=DEBUG)
485       
486        demo.start()
487       
488        sys.exit(app.exec_())
Note: See TracBrowser for help on using the repository browser.