Changeset 210


Ignore:
Timestamp:
11/15/10 22:00:22 (11 years ago)
Author:
sc
Message:

Brainstorms/Helicopter_Control.py:

  • read/write mode added
  • COMMAND_PACKET data added
  • neutral command added
  • hover command added
  • fly_forward command added
  • pickled packet testing added to comments
  • commented legacy code cleanup
File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/brainstorms/Puzzlebox/Brainstorms/Helicopter_Control.py

    r209 r210  
    3636##################################################################### 
    3737 
    38 DEBUG = 1 
     38DEBUG = 2 
    3939 
    4040DEFAULT_COMMAND = 'dump_packets' 
     
    4242SERIAL_DEVICE = '/dev/ttyUSB0' 
    4343DEFAULT_SERIAL_BAUDRATE = 115200 
     44DEFAULT_MODE = 'read' 
    4445 
    4546PROTOCOL_SYNC = '\x80' 
     
    4748PAYLOAD_MINIMUM_LENGTH = 8 
    4849 
    49 DEVICE_BUFFER_CHECK_TIMER = 20 # Check buffer size once every 20ms 
    50  
     50DEVICE_BUFFER_TIMER = 20 # Check buffer size once every 20ms 
     51 
     52COMMAND_PACKET = { 
     53        'neutral': '\x80\x53\x68\x81\x86\xd1\xa4\x29', \ 
     54        'no_thrust': '\x80\xac\xdf\xa2\x9a\xb0\x8b\x8d\x54', \ 
     55        'minimum_thrust': '\x80\x59\x68\x81\x86\xd1\xa4\x29', \ 
     56        'maximum_thrust': '\x83\xad\xdf\x91\xe0\x83\x12\x8d\x54', \ 
     57        'maximum_forward': '\x83\xad\xde\x11\xcd\xb0\x8b\x8d\x54', \ 
     58} 
     59 
     60DEFAULT_COMMAND_PACKET = COMMAND_PACKET['neutral'] 
    5161 
    5262##################################################################### 
     
    7080                self.device_address = device_address 
    7181                self.command = command 
    72                  
    73                 self.connection = None 
    74                  
    75                 #try: 
    76                         #self.connection = self.connect_to_nxt(self.device) 
    77                 #except Exception, e: 
    78                         #if self.DEBUG: 
    79                                 #print "<-- [RC] Connection failed to NXT device [%s]" % self.device 
    80                                 #print "ERROR [RC]:", 
    81                                 #print e 
    82          
     82                self.mode = DEFAULT_MODE 
     83                 
    8384                self.serial_device = None 
    8485                self.protocol = None 
     
    8687                 
    8788                #self.configureRemote() 
    88          
    89          
    90         ################################################################## 
    91          
    92         #def connect_to_nxt(self, device): 
    93                  
    94                 #connection = jaraco.nxt.Connection(self.device) 
    95                  
    96                 #if self.DEBUG: 
    97                         #battery_voltage = self.get_battery_voltage(connection) 
    98                         #print "--> [RC] Battery voltage:", 
    99                         #print battery_voltage 
    100                  
    101                  
    102                 #return(connection) 
    103          
    104          
    105         ################################################################## 
    106          
    107         #def connect_to_port(self, device_port): 
    108          
    109                 #if isinstance(device_port, basestring): 
    110                         #port = getattr(jaraco.nxt.messages.OutputPort, device_port) 
    111                  
    112                 #assert port in jaraco.nxt.messages.OutputPort.values() 
    113                  
    114                 #return(port) 
    11589         
    11690         
     
    12397                                self.log, \ 
    12498                                device_address=self.device_address, \ 
     99                                mode=self.mode, \ 
    125100                                DEBUG=self.DEBUG, \ 
    126101                                parent=self) 
    127102                 
    128103                self.serial_device.start() 
    129          
    130          
     104                 
     105                 
    131106                self.protocol = \ 
    132107                        ProtocolHandler( \ 
    133108                                self.log, \ 
    134109                                self.serial_device, \ 
     110                                mode=self.mode, \ 
    135111                                DEBUG=self.DEBUG, \ 
    136112                                parent=self) 
     
    146122                        print "data_payload:", 
    147123                        print packet['data_payload'].encode("hex") 
     124                        #print packet['data_payload'] 
     125                         
     126                        #if packet['data_payload'].encode("hex") == '80acdf22cdb08b8d54': 
     127                                #print True 
     128                                #import cPickle as pickle 
     129                                #file = open('packet.data', 'w') 
     130                                #pickle.dump(packet['data_payload'], file) 
     131                                #file.close() 
     132                                #sys.exit(app.exec_()) 
    148133                 
    149134                #if (packet != {}): 
    150135                        #self.packet_queue.append(packet) 
    151                         #self.sendPacketQueue() 
     136                        ###self.sendPacketQueue() 
    152137                 
    153138                if (self.parent != None): 
     
    159144        def dump_packets(self): 
    160145                 
     146                pass 
     147         
     148         
     149        ################################################################## 
     150         
     151        def neutral(self): 
     152                 
     153                if self.DEBUG: 
     154                        print "--> Command: neutral" 
     155                 
     156                self.protocol.command_packet = COMMAND_PACKET['neutral'] 
     157         
     158         
     159        ################################################################## 
     160         
     161        def hover(self): 
     162                 
     163                if self.DEBUG: 
     164                        print "--> Command: hover" 
     165                 
     166                self.protocol.command_packet = COMMAND_PACKET['neutral'] 
     167                QtCore.QThread.msleep(1 * 1000) # 1 second 
     168                 
     169                self.protocol.command_packet = COMMAND_PACKET['minimum_thrust'] 
     170                QtCore.QThread.msleep(1 * 1000) # 1 second 
     171                 
     172                self.protocol.command_packet = COMMAND_PACKET['neutral'] 
     173         
     174         
     175        ################################################################## 
     176         
     177        def fly_forward(self, duration): 
     178                 
     179                if self.DEBUG: 
     180                        print "--> Command: fly_forward" 
     181                 
     182                self.protocol.command_packet = COMMAND_PACKET['no_thrust'] 
     183                QtCore.QThread.msleep(1 * 1000) # 1 second 
     184                 
     185                self.protocol.command_packet = COMMAND_PACKET['maximum_forward'] 
     186                QtCore.QThread.msleep(duration * 1000) # 1 second 
     187                 
     188                self.protocol.command_packet = COMMAND_PACKET['no_thrust'] 
     189         
     190         
     191        ################################################################## 
     192         
     193        def processCommand(self): 
     194                 
     195                if (self.command == 'dump_packets'): 
     196                        self.mode = 'read' 
     197                else: 
     198                        self.mode = 'write' 
     199                 
     200                 
    161201                self.configureRemote() 
     202                 
     203                 
     204                if (self.command == 'dump_packets'): 
     205                        self.mode = 'read' 
     206                        self.dump_packets() 
     207                 
     208                elif (self.command == 'neutral'): 
     209                        self.mode = 'write' 
     210                        self.neutral() 
     211                 
     212                elif (self.command == 'hover'): 
     213                        self.mode = 'write' 
     214                        self.hover() 
     215                 
     216                elif (self.command == 'fly_forward'): 
     217                        self.mode = 'write' 
     218                        self.fly_forward(duration=3) 
     219         
     220         
     221        ################################################################## 
     222         
     223        def stop(self): 
     224                 
     225                #self.connection.close() 
     226                pass 
    162227         
    163228         
     
    166231        def run(self): 
    167232                 
    168                 if (self.command == 'dump_packets'): 
    169                         self.dump_packets() 
    170                  
    171                 #elif (self.command == 'hover'): 
    172                         #self.hover(self.connection, power=power) 
    173                  
    174                 #elif (self.command == 'fly_forward'): 
    175                         #self.fly_forward(self.connection, power=power, duration=3) 
     233                if self.DEBUG: 
     234                        print "<---- [%s] Main thread running" % "Helicopter Remote" 
     235                 
     236                 
     237                self.processCommand() 
    176238                 
    177239                self.exec_() 
    178          
    179          
    180         ################################################################## 
    181          
    182         def stop(self): 
    183                  
    184                 self.connection.close() 
    185          
    186          
    187         ################################################################## 
    188          
    189         #def run(self): 
    190                  
    191                 #if self.DEBUG: 
    192                         #print "<---- [%s] Main thread running" % "Helicopter Remote" 
    193                  
    194                 #self.exec_() 
    195240         
    196241         
     
    226271        def __init__(self, log, \ 
    227272                               serial_device, \ 
     273                               mode=DEFAULT_MODE, \ 
    228274                               DEBUG=DEBUG, \ 
    229275                               parent=None): 
     
    236282                 
    237283                self.device = None 
    238                 self.buffer = '' 
     284                self.mode = mode 
    239285                 
    240286                self.device = serial_device 
     
    244290                 
    245291                self.keep_running = True 
     292                 
     293                self.command_packet = DEFAULT_COMMAND_PACKET 
    246294         
    247295         
     
    249297         
    250298        def processDataPayload(self, data_payload, payload_timestamp): 
    251          
    252                 #if self.DEBUG: 
    253                         #print "data_payload:", 
    254                         #print data_payload 
    255                         #print data_payload.encode("hex") 
    256                  
    257299                 
    258300                packet_update = {} 
     
    269311        def parseStream(self): 
    270312                 
    271                 # Loop forever, parsing one Packet per loop... 
     313                # Loop forever, parsing one packet per loop 
    272314                packet_count = 0 
    273315                 
     
    278320                        if (byte != PROTOCOL_SYNC): 
    279321                                continue 
    280                                  
     322                         
    281323                         
    282324                        payload_timestamp = time.time() 
    283325                         
    284                         data_payload = self.device.readBuffer() 
     326                        data_payload = self.device.getBuffer() 
    285327                         
    286328                        # Remove PROTOCOL_SYNC_TAIL 
     
    288330                         
    289331                         
    290                         #print len(data_payload) 
    291332                        if len(data_payload) < PAYLOAD_MINIMUM_LENGTH: 
    292333                                continue 
     
    308349        ################################################################## 
    309350         
    310         #def updateDataPacket(self, packet_update): 
    311                  
    312                 #if 'eSense' in packet_update.keys(): 
    313                         #process_packet = {'eSense': {}} 
    314                         #for key in packet_update['eSense'].keys(): 
    315                                 #self.data_packet['eSense'][key] = packet_update['eSense'][key] 
    316                                 #process_packet['eSense'][key] = packet_update['eSense'][key] 
    317                  
    318                 #else: 
    319                         #self.data_packet.update(packet_update) 
    320                         #process_packet = packet_update 
    321                  
    322                  
    323                 #process_packet['timestamp'] = packet_update['timestamp'] 
    324                  
    325                  
    326                 #if self.DEBUG > 3: 
    327                         #print self.data_packet 
    328                  
    329                  
    330                 #if (self.parent != None): 
    331                         #self.parent.processPacket(process_packet) 
     351        def writeStream(self): 
     352                 
     353                # Loop forever, writing one packet per loop 
     354                packet_count = 0 
     355                 
     356                #import cPickle as pickle 
     357                #file = open('packet.data', 'r') 
     358                #packet = pickle.loads(file.read()) 
     359                #file.close() 
     360                 
     361                while self.keep_running: 
     362                         
     363                        # Synchronize on [SYNC] bytes 
     364                        buffer = PROTOCOL_SYNC 
     365                         
     366                        buffer += self.command_packet 
     367                         
     368                         
     369                        self.device.buffer = buffer 
     370                        #self.device.buffer = packet 
     371                        #print packet.encode("hex") 
     372                         
     373                        # Sleep for 20 ms 
     374                        # Based on 50 Hz refresh rate of Blade MLP4DSM RC device 
     375                        # (1/50) * 1000 = 20 
     376                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER) 
    332377         
    333378         
     
    340385                self.session_start_timestamp = time.time() 
    341386                 
    342                 self.parseStream() 
     387                if self.mode == 'read': 
     388                        self.parseStream() 
     389                 
     390                elif self.mode == 'write': 
     391                        self.writeStream() 
    343392         
    344393         
     
    364413        def __init__(self, log, \ 
    365414                               device_address=SERIAL_DEVICE, \ 
     415                               mode=DEFAULT_MODE, \ 
    366416                               DEBUG=DEBUG, \ 
    367417                               parent=None): 
     
    373423                 
    374424                self.device_address = device_address 
     425                self.mode = mode 
    375426                self.device = None 
    376427                self.buffer = '' 
     
    387438                                       #QtCore.SIGNAL("timeout()"), \ 
    388439                                       #self.checkBuffer) 
    389                 #self.buffer_check_timer.start(DEVICE_BUFFER_CHECK_TIMER) 
     440                #self.buffer_check_timer.start(DEVICE_BUFFER_TIMER) 
    390441                 
    391442                self.keep_running = True 
     
    471522                 
    472523                try: 
    473 ##                      device = serial.Serial(port = self.device_address, \ 
    474 ##                                                  baudrate = baudrate, \ 
    475 ##                                                  bytesize = init_byte_size, \ 
    476 ##                                                  parity = init_parity, \ 
    477 ##                                                  stopbits = init_stopbits, \ 
    478 ##                                                  xonxoff = init_software_flow_control, \ 
    479 ##                                                  rtscts = init_rts_cts_flow_control, \ 
    480 ##                                                  timeout = timeout) 
    481524                         
    482525                        device = serialWrapper(port = self.device_address, \ 
     
    523566        ################################################################### 
    524567         
    525         def readBuffer(self): 
     568        def getBuffer(self): 
    526569                 
    527570                data_payload = self.buffer 
     
    548591                # (1/50) * 1000 = 20 
    549592                while len(self.buffer) < length: 
    550                         QtCore.QThread.msleep(DEVICE_BUFFER_CHECK_TIMER) 
     593                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER) 
    551594                         
    552595                bytes = self.buffer[:length] 
     
    585628        ################################################################### 
    586629         
    587         def run(self): 
    588                  
     630        def readBuffer(self): 
     631         
    589632                self.buffer = '' 
    590633                 
     
    609652                 
    610653                self.exitThread() 
     654         
     655         
     656        ################################################################### 
     657         
     658        def writeBuffer(self): 
     659         
     660                self.buffer = '' 
     661                 
     662                while self.keep_running: 
     663                         
     664                        if (len(self.buffer) != 0): 
     665                                buffer = self.buffer 
     666                                self.buffer = '' 
     667                                 
     668                                try: 
     669                                        self.device.write(buffer) 
     670                                         
     671                                        if self.DEBUG > 1: 
     672                                                print "Device wrote:", 
     673                                                print buffer 
     674                                 
     675                                except: 
     676                                        if self.DEBUG: 
     677                                                print "ERROR: failed to write to serial device" 
     678                                        break 
     679                         
     680                         
     681                        # Sleep for 20 ms if buffer is empty 
     682                        # Based on 50 Hz refresh rate of Blade MLP4DSM RC device 
     683                        # (1/50) * 1000 = 20 
     684                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER) 
     685                 
     686                self.exitThread() 
     687         
     688         
     689        ################################################################### 
     690         
     691        def run(self): 
     692                 
     693                if self.mode == 'read': 
     694                        self.readBuffer() 
     695                 
     696                elif self.mode == 'write': 
     697                        self.writeBuffer() 
    611698 
    612699 
     
    652739        rc = puzzlebox_brainstorms_helicopter_control(device_address=device, command=command, DEBUG=DEBUG) 
    653740         
    654         #if rc.connection == None: 
    655                 ##sys.exit() 
    656                 #sys.exit(app.exec_()) 
    657          
    658         #rc.run(rc.command) 
    659741        rc.start() 
    660742         
    661         #rc.stop() 
    662743        sys.exit(app.exec_()) 
    663744 
Note: See TracChangeset for help on using the changeset viewer.