source: orbit/python/Puzzlebox/Orbit/Protocol_Orbit.py @ cb467b5

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