source: trunk/brainstorms/Puzzlebox/Brainstorms/Helicopter_Control.py @ 206

Last change on this file since 206 was 206, checked in by sc, 11 years ago

Brainstorms/Helicopter_Control.py:

  • Qt threaded model added
  • Property svn:executable set to *
File size: 17.2 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Helicopter Control
5#
6# Copyright Puzzlebox Productions, LLC (2010)
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
11__changelog__ = """
12Last Update: 2010.11.15
13
14"""
15
16import sys, time
17import signal
18import serial
19
20try:
21        import PySide as PyQt4
22        from PySide import QtCore
23except:
24        print "Using PyQt4 module"
25        from PyQt4 import QtCore
26else:
27        print "Using PySide module"
28
29import Configuration as configuration
30
31#import Puzzlebox.Synapse.Protocol as protocol
32from Puzzlebox.Synapse import Protocol
33
34#####################################################################
35# Globals
36#####################################################################
37
38DEBUG = 3
39
40DEFAULT_COMMAND = 'hover'
41
42SERIAL_DEVICE = '/dev/ttyUSB0'
43DEFAULT_SERIAL_BAUDRATE = 115200
44
45DEVICE_BUFFER_CHECK_TIMER = 20 # Check buffer size once every 20ms
46
47
48#####################################################################
49# Classes
50#####################################################################
51
52#class puzzlebox_brainstorms_helicopter_control:
53class puzzlebox_brainstorms_helicopter_control(QtCore.QThread):
54       
55        def __init__(self, \
56                     device_address=SERIAL_DEVICE, \
57                     command=DEFAULT_COMMAND, \
58                     DEBUG=DEBUG, \
59                     parent=None):
60               
61                QtCore.QThread.__init__(self, parent)
62               
63                self.log = None
64                self.DEBUG = DEBUG
65               
66                self.device_address = device_address
67                self.command = command
68               
69                self.connection = None
70               
71                #try:
72                        #self.connection = self.connect_to_nxt(self.device)
73                #except Exception, e:
74                        #if self.DEBUG:
75                                #print "<-- [RC] Connection failed to NXT device [%s]" % self.device
76                                #print "ERROR [RC]:",
77                                #print e
78       
79                self.serial_device = None
80                self.protocol = None
81               
82               
83                self.configureRemote()
84       
85       
86        ##################################################################
87       
88        #def connect_to_nxt(self, device):
89               
90                #connection = jaraco.nxt.Connection(self.device)
91               
92                #if self.DEBUG:
93                        #battery_voltage = self.get_battery_voltage(connection)
94                        #print "--> [RC] Battery voltage:",
95                        #print battery_voltage
96               
97               
98                #return(connection)
99       
100       
101        ##################################################################
102       
103        #def connect_to_port(self, device_port):
104       
105                #if isinstance(device_port, basestring):
106                        #port = getattr(jaraco.nxt.messages.OutputPort, device_port)
107               
108                #assert port in jaraco.nxt.messages.OutputPort.values()
109               
110                #return(port)
111       
112       
113        ##################################################################
114       
115        def configureRemote(self):
116               
117                self.serial_device = \
118                        SerialDevice( \
119                                self.log, \
120                                device_address=self.device_address, \
121                                DEBUG=self.DEBUG, \
122                                parent=self)
123               
124                self.serial_device.start()
125               
126                #self.protocol = \
127                        #ProtocolHandler( \
128                                #self.log, \
129                                #self.serial_device, \
130                                #DEBUG=self.DEBUG, \
131                                #parent=self)
132               
133                #self.protocol.start()
134       
135       
136        ##################################################################
137       
138        def processPacket(self, packet):
139               
140                if self.DEBUG > 2:
141                        print packet
142               
143                if (packet != {}):
144                        self.packet_queue.append(packet)
145                        self.sendPacketQueue()
146               
147                #if (self.parent != None):
148                        #self.parent.processPacketThinkGear(self.protocol.data_packet)
149       
150        ##################################################################
151       
152        def dump_packets(self):
153               
154                pass
155       
156       
157        ##################################################################
158       
159        def run(self, command):
160               
161                #if (command == 'drive_forward'):
162                        #self.drive_forward(self.connection, power=power, duration=3)
163               
164                #elif (command == 'drive_reverse'):
165                        #self.drive_reverse(self.connection, power=power)
166               
167                #elif (command == 'turn_left'):
168                        #self.turn_left(self.connection, power=power)
169               
170                #elif (command == 'turn_right'):
171                        #self.turn_right(self.connection, power=power)
172               
173                #elif (command == 'turn_left_in_reverse'):
174                        #self.turn_left_in_reverse(self.connection, power=power)
175               
176                #elif (command == 'turn_right_in_reverse'):
177                        #self.turn_right_in_reverse(self.connection, power=power)
178               
179                #elif (command == 'stop_motors'):
180                        #self.stop_motors(self.connection)
181               
182                #elif (command == 'test_drive'):
183                        #self.test_drive(self.connection)
184               
185               
186                self.dump_packets()
187       
188       
189        ##################################################################
190       
191        def stop(self):
192               
193                self.connection.close()
194       
195       
196        ##################################################################
197       
198        def run(self):
199               
200                if self.DEBUG:
201                        print "<---- [%s] Main thread running" % "Helicopter Remote"
202               
203                self.exec_()
204       
205       
206        ##################################################################
207       
208        def exitThread(self, callThreadQuit=True):
209               
210                try:
211                        self.emulationTimer.stop()
212                except:
213                        pass
214               
215                if self.serial_device != None:
216                        self.serial_device.exitThread()
217               
218                if self.protocol != None:
219                        self.protocol.exitThread()
220               
221                self.socket.close()
222               
223                if callThreadQuit:
224                        QtCore.QThread.quit(self)
225               
226                if self.parent == None:
227                        sys.exit()
228
229
230#####################################################################
231#####################################################################
232
233class ProtocolHandler(QtCore.QThread):
234       
235        def __init__(self, log, \
236                               serial_device, \
237                               DEBUG=DEBUG, \
238                               parent=None):
239               
240                QtCore.QThread.__init__(self,parent)
241               
242                self.log = log
243                self.DEBUG = DEBUG
244                self.parent = parent
245               
246                self.device = None
247                self.buffer = ''
248               
249                self.device = serial_device
250               
251                self.packet_count = 0
252                self.bad_packets = 0
253               
254                self.keep_running = True
255       
256       
257        ##################################################################
258       
259        def parseStream(self):
260               
261                '''Each Packet begins with its Header, followed by its Data Payload,
262                and ends with the Payload's Check-sum Byte, as follows:
263                [SYNC] [SYNC] [PLENGTH]      [PAYLOAD...]         [CHKSUM]
264                _______________________      _____________     ____________
265                ^^^^^^^^(Header)^^^^^^^      ^^(Payload)^^     ^(Checksum)^'''
266               
267                # Loop forever, parsing one Packet per loop...
268                packet_count = 0
269               
270                while self.keep_running:
271                       
272                        # Synchronize on [SYNC] bytes
273                        # Read from stream until two consecutive [SYNC] bytes are found
274                        byte = self.device.read()
275                        if (byte != PROTOCOL_SYNC):
276                                continue
277                       
278                        byte = self.device.read()
279                        if (byte != PROTOCOL_SYNC):
280                                continue
281                       
282                        payload_timestamp = time.time()
283                       
284                        # Parse [PLENGTH] byte
285                       
286                        # SPEC: [PLENGTH] byte indicates the length, in bytes, of the
287                        # Packet's Data Payload [PAYLOAD...] section, and may be any value
288                        # from 0 up to 169. Any higher value indicates an error
289                        # (PLENGTH TOO LARGE). Be sure to note that [PLENGTH] is the length
290                        # of the Packet's Data Payload, NOT of the entire Packet.
291                        # The Packet's complete length will always be [PLENGTH] + 4.
292                       
293                        byte = self.device.read()
294                        packet_length = byte.encode("hex")
295                        packet_length = int(packet_length, 16)
296                       
297                        if (packet_length > 170):
298                                self.bad_packets += 1
299                                if self.DEBUG:
300                                        print "ERROR: packet length bad"
301                                        continue
302                       
303                       
304                        # Collect [PAYLOAD...] bytes
305                        data_payload = self.device.read(packet_length)
306                       
307                       
308                        # Calculate [PAYLOAD...] checksum
309                       
310                        # SPEC: The [CHKSUM] Byte must be used to verify the integrity of the
311                        # Packet's Data Payload. The Payload's Checksum is defined as:
312                        #  1. summing all the bytes of the Packet's Data Payload
313                        #  2. taking the lowest 8 bits of the sum
314                        #  3. performing the bit inverse (one's compliment inverse)
315                        #     on those lowest 8 bits
316                       
317                        payload_checksum = 0
318                        for byte in data_payload:
319                                value = byte.encode("hex")
320                                value = int(value, 16)
321                                payload_checksum += value
322                       
323                       
324                        # Take the lowest 8 bits of the calculated payload_checksum
325                        # and invert them. Serious C code mojo follows.
326                        payload_checksum &= 0xff
327                        payload_checksum = ~payload_checksum & 0xff
328                       
329                       
330                        # Parse [CKSUM] byte
331                        packet_checksum = self.device.read()
332                        packet_checksum = packet_checksum.encode("hex")
333                        packet_checksum = int(packet_checksum, 16)
334                       
335                       
336                        # Verify [CKSUM] byte against calculated [PAYLOAD...] checksum
337                        if packet_checksum != payload_checksum:
338                                self.bad_packets += 1
339                                if self.DEBUG > 1:
340                                        print "ERROR: packet checksum does not match"
341                                        print "       packet_checksum:",
342                                        print packet_checksum
343                                        print "       payload_checksum:",
344                                        print payload_checksum
345                                       
346                                        #self.device.checkBuffer()
347                               
348                                continue
349                       
350                       
351                        else:
352                                # Since [CKSUM] is OK, parse the Data Payload
353                                if self.DEBUG > 3:
354                                        print "packet checksum correct"
355                               
356                               
357                                self.processDataPayload(data_payload, payload_timestamp)
358                               
359                               
360                                #if self.DEBUG > 1:
361                                        #packet_count += 1
362                                        #if packet_count >= DEBUG_PACKET_COUNT:
363                                                #print "max debugging count reached, disconnecting"
364                                                #self.keep_running = False
365                                                #self.device.stop()
366                                                #QtCore.QThread.quit(self)
367                                                ##sys.exit()
368       
369       
370        ##################################################################
371       
372        def updateDataPacket(self, packet_update):
373               
374                if 'eSense' in packet_update.keys():
375                        process_packet = {'eSense': {}}
376                        for key in packet_update['eSense'].keys():
377                                self.data_packet['eSense'][key] = packet_update['eSense'][key]
378                                process_packet['eSense'][key] = packet_update['eSense'][key]
379               
380                else:
381                        self.data_packet.update(packet_update)
382                        process_packet = packet_update
383               
384               
385                process_packet['timestamp'] = packet_update['timestamp']
386               
387               
388                if self.DEBUG > 3:
389                        print self.data_packet
390               
391               
392                if (self.parent != None):
393                        self.parent.processPacketThinkGear(process_packet)
394       
395       
396        ##################################################################
397       
398        def run(self):
399               
400                self.packet_count = 0
401                self.bad_packets = 0
402                self.session_start_timestamp = time.time()
403               
404                self.parseStream()
405       
406       
407        ##################################################################
408       
409        def exitThread(self, callThreadQuit=True):
410               
411                try:
412                        self.device.stop()
413                except:
414                        pass
415               
416                #self.wait()
417                if callThreadQuit:
418                        QtCore.QThread.quit(self)
419
420
421#####################################################################
422#####################################################################
423
424#class SerialDevice(Protocol.SerialDevice):
425#class SerialDevice:
426class SerialDevice(QtCore.QThread):
427       
428        def __init__(self, log, \
429                               device_address=SERIAL_DEVICE, \
430                               DEBUG=DEBUG, \
431                               parent=None):
432               
433                QtCore.QThread.__init__(self, parent)
434               
435                self.log = log
436                self.DEBUG = DEBUG
437               
438                self.device_address = device_address
439                self.device = None
440                self.buffer = ''
441               
442                if (self.device_address.count(':') == 5):
443                        # Device address is a Bluetooth MAC address
444                        self.device = self.initializeBluetoothDevice()
445                else:
446                        # Device address is a serial port address
447                        self.device = self.initializeSerialDevice()
448               
449                #self.buffer_check_timer = QtCore.QTimer()
450                #QtCore.QObject.connect(self.buffer_check_timer, \
451                                       #QtCore.SIGNAL("timeout()"), \
452                                       #self.checkBuffer)
453                #self.buffer_check_timer.start(DEVICE_BUFFER_CHECK_TIMER)
454               
455                self.keep_running = True
456       
457       
458        ##################################################################
459       
460        #def initializeBluetoothDevice(self):
461               
462                #socket = bluetooth.BluetoothSocket( bluetooth.RFCOMM )
463               
464                #try:
465                        #socket.connect((self.device_address, THINKGEAR_DEVICE_BLUETOOTH_CHANNEL))
466               
467                #except Exception, e:
468                        #if self.DEBUG:
469                                #print "ERROR:",
470                                #print e
471                                #sys.exit()
472               
473               
474                #return socket
475       
476       
477        ###################################################################
478       
479        def initializeSerialDevice(self):
480               
481                baudrate = DEFAULT_SERIAL_BAUDRATE
482                bytesize = 8
483                parity = 'NONE'
484                stopbits = 1
485                software_flow_control = 'f'
486                rts_cts_flow_control = 'f'
487                #timeout = 15
488                timeout = 5
489               
490                # convert bytesize
491                if (bytesize == 5):
492                        init_byte_size = serial.FIVEBITS
493                elif (bytesize == 6):
494                        init_byte_size = serial.SIXBITS
495                elif (bytesize == 7):
496                        init_byte_size = serial.SEVENBITS
497                elif (bytesize == 8):
498                        init_byte_size = serial.EIGHTBITS
499                else:
500                        #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
501                        init_byte_size = serial.EIGHTBITS
502               
503                # convert parity
504                if (parity == 'NONE'):
505                        init_parity = serial.PARITY_NONE
506                elif (parity == 'EVEN'):
507                        init_parity = serial.PARITY_EVEN
508                elif (parity == 'ODD'):
509                        init_parity = serial.PARITY_ODD
510                else:
511                        #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
512                        init_parity = serial.PARITY_NONE
513               
514                # convert stopbits
515                if (stopbits == 1):
516                        init_stopbits = serial.STOPBITS_ONE
517                elif (stopbits == 2):
518                        init_stopbits = serial.STOPBITS_TWO
519                else:
520                        #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
521                        init_byte_size = serial.STOPBITS_ONE
522               
523                # convert software flow control
524                if (software_flow_control == 't'):
525                        init_software_flow_control = 1
526                else:
527                        init_software_flow_control = 0
528               
529                # convert rts cts flow control
530                if (rts_cts_flow_control == 't'):
531                        init_rts_cts_flow_control = 1
532                else:
533                        init_rts_cts_flow_control = 0
534               
535               
536                try:
537##                      device = serial.Serial(port = self.device_address, \
538##                                                  baudrate = baudrate, \
539##                                                  bytesize = init_byte_size, \
540##                                                  parity = init_parity, \
541##                                                  stopbits = init_stopbits, \
542##                                                  xonxoff = init_software_flow_control, \
543##                                                  rtscts = init_rts_cts_flow_control, \
544##                                                  timeout = timeout)
545                       
546                        device = serialWrapper(port = self.device_address, \
547                                                    baudrate = baudrate, \
548                                                    bytesize = init_byte_size, \
549                                                    parity = init_parity, \
550                                                    stopbits = init_stopbits, \
551                                                    xonxoff = init_software_flow_control, \
552                                                    rtscts = init_rts_cts_flow_control, \
553                                                    timeout = timeout)
554               
555                except Exception, e:
556                        if self.DEBUG:
557                                print "ERROR:",
558                                print e,
559                                print self.device_address
560                                sys.exit()
561               
562               
563                #device.flushInput()
564                ##device.flushOutput()
565               
566               
567                return(device)
568       
569       
570        ###################################################################
571       
572        #def checkBuffer(self):
573               
574                #if self.DEBUG > 1:
575                        #print "INFO: Buffer size check:",
576                        #print len(self.buffer),
577                        #print "(maximum before reset is %i)" % DEVICE_BUFFER_MAX_SIZE
578               
579                #if (DEVICE_BUFFER_MAX_SIZE <= len(self.buffer)):
580                       
581                        #if self.DEBUG:
582                                #print "ERROR: Buffer size has grown too large, resetting"
583                       
584                        #self.reset()
585       
586       
587        ###################################################################
588       
589        def reset(self):
590               
591                self.buffer = ''
592       
593       
594        ###################################################################
595       
596        #def read(self, length=1):
597               
598                ## Sleep for 2 ms if buffer is empty
599                ## Based on 512 Hz refresh rate of NeuroSky MindSet device
600                ## (1/512) * 1000 = 1.9531250
601                #while len(self.buffer) < length:
602                        #QtCore.QThread.msleep(2)
603                       
604                #bytes = self.buffer[:length]
605               
606                #self.buffer = self.buffer[length:]
607               
608                #return(bytes)
609       
610       
611        ###################################################################
612       
613        def stop(self):
614               
615                #self.buffer_check_timer.stop()
616                self.keep_running = False
617       
618       
619        ###################################################################
620       
621        def exitThread(self, callThreadQuit=True):
622               
623                self.stop()
624                self.close()
625               
626                if callThreadQuit:
627                        QtCore.QThread.quit(self)
628       
629       
630        ###################################################################
631       
632        def close(self):
633               
634                self.device.close()
635       
636       
637        ###################################################################
638       
639        #def run(self):
640        def start(self):
641               
642                self.buffer = ''
643               
644                while self.keep_running:
645                       
646                        try:
647                                #byte = self.device.read()
648                                byte = self.device.recv(1)
649                               
650                                if (len(byte) != 0):
651                                        if self.DEBUG > 2:
652                                                print "Device read:",
653                                                print byte
654                                               
655                                        self.buffer += byte
656                       
657                        except:
658                                if self.DEBUG:
659                                        print "ERROR: failed to read from serial device"
660                                break
661               
662               
663                #self.exitThread(callThreadQuit=False)
664                self.exitThread()
665
666
667#####################################################################
668#####################################################################
669
670class serialWrapper(serial.Serial):
671       
672        #__init__(port=None, baudrate=9600, bytesize=EIGHTBITS, parity=PARITY_NONE, stopbits=STOPBITS_ONE, timeout=None, xonxoff=False, rtscts=False, writeTimeout=None, dsrdtr=False, interCharTimeout=None)
673       
674        def recv(self, size=1):
675               
676                return(self.read(size))
677
678
679#####################################################################
680# Functions
681#####################################################################
682
683#####################################################################
684# Main
685#####################################################################
686
687if __name__ == '__main__':
688       
689        # Perform correct KeyboardInterrupt handling
690        signal.signal(signal.SIGINT, signal.SIG_DFL)
691       
692        # Collect default settings and command line parameters
693        device = SERIAL_DEVICE
694        command = DEFAULT_COMMAND
695       
696        for each in sys.argv:
697               
698                if each.startswith("--device="):
699                        device = each[ len("--device="): ]
700                elif each.startswith("--command="):
701                        command = each[ len("--command="): ]
702       
703       
704        app = QtCore.QCoreApplication(sys.argv)
705       
706        rc = puzzlebox_brainstorms_helicopter_control(device_address=device, command=command, DEBUG=DEBUG)
707       
708        if rc.connection == None:
709                #sys.exit()
710                sys.exit(app.exec_())
711       
712        #rc.run(rc.command)
713        rc.start()
714       
715        #rc.stop()
716        sys.exit(app.exec_())
717
Note: See TracBrowser for help on using the repository browser.