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

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

Brainstorms/Helicopter_Control.py:

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