source: robotics/car/python/Car_Demo.py @ 3716c66

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