Source Code for Module ckbot.wixeltdma

  1  """ 
  2  File: wixelTDMA 
  3   
  4  Implement TDMA interface via wixel boards 
  5  """ 
  6   
  7  from numpy import zeros 
  8  from copy import deepcopy 
  9  from multiprotocol import MultiProtocol 
 10  from port2port import Connection, newConnection 
 11  from struct import pack, unpack 
 12   
 13  DEFAULT_WIXEL_PORT = dict( 
 14    TYPE='tty', 
 15    glob='/dev/ttyACM*', 
 16    baudrate=115200 
 17    ) 
 18   
 19 -def generate_timeplan(tasks): 
 20      #Eventually, I'll build up the list "tasks" to be of the format [[node, period,  
 21      #duration, start]] 
 22      tasks = deepcopy(list(tasks)) 
 23       
 24      tasks[0].append(0) #Assigning first task to first time slot of 1/32 duration 
 25      slotNum = 0 # Start counting from slot number 0 
 26   
 27      # Nested for loop to assign starting slot numbers to all the nodes 
 28      for i in xrange(1, len(tasks)): # cycle through entire list, to assign starting  
 29          #slot values for each node 
 30   
 31          taskSlotAssigned = False # Boolean value to denote if node has been assigned  
 32          #a starting slot 
 33   
 34          while not taskSlotAssigned: # while starting slot number for current node  
 35              #isn't determined yet, do the following: 
 36   
 37              slotBusy = False # Boolean value denoting if slot 'slotNum' has already been 
 38              #assigned to an earlier node 
 39   
 40              for j in xrange(0,i): # Cycle through previous nodes 
 41   
 42                  if ((slotNum - tasks[j][3]) % tasks[j][1]) < tasks[j][2]: # Check if slot 
 43                      #'slotNum' is occupied by this earlier node, i.e. slotNum is within  
 44                      #'slotWidth' distance of any of the earlier nodes 
 45   
 46                      slotBusy = True # if yes, then update 'slotBusy' 
 47                      newSlotNum = slotNum + tasks[j][2] - (slotNum - tasks[j][3])%tasks[j][1] 
 48                      # update slotNum to (slotNum + {slotWidth of node} - (slotNum - {starting slot of node})%slotPeriod);  
 49                      #this moves the slotNum to the next slot number immediately following this node's transmission slots 
 50                      #print tasks[j] 
 51                      # print '\tslotNum {0} is occupied by {1}. Setting slotNum to {2}'.format(slotNum, tasks[j][0], newSlotNum) 
 52                      slotNum = newSlotNum 
 53   
 54   
 55              if not slotBusy: # if, after going through all previous nodes, we find that  
 56                  #the slot hasn't been assigned to any node yet, assign it to be the starting slot of this node. 
 57   
 58                  taskSlotAssigned = True 
 59                  tasks[i].append(slotNum) 
 60                  # print 'ASSIGNED: slot {0} to {1}'.format(slotNum, tasks[i][0]) 
 61                  break 
 62   
 63   
 64      # Populate 1 'period' of the timeplan; i.e., the minimum duration which can be  
 65      #repeated to obtain the entire timeplan for this frame 
 66   
 67      # That is, if we have the following slot assignments: [a,b,b,c,a,b,b,c,a,b,b,c...], then one period = [a,b,b,c] 
 68      periodDuration = 0 
 69      for i in xrange(0,len(tasks)): 
 70          if tasks[i][1] > periodDuration: # The periodicity of a sum of signals, which  
 71              #are a multiple of some common factor, is the periodicity of the lowest frequency signal 
 72              periodDuration = tasks[i][1] 
 73   
 74      period = zeros([1,periodDuration]).tolist()[0] 
 75   
 76   
 77      # Populate the slots in this 'period' with the node names. 
 78      for i in xrange(0,len(tasks)): 
 79          for j in xrange(tasks[i][3], periodDuration, tasks[i][1]): 
 80              for k in xrange(0, tasks[i][2]): 
 81                  if (period[j+k]): 
 82                      print '\nCollision!' 
 83                      period[j+k] = 'xxx' 
 84                  period[j+k] = tasks[i][0] 
 85   
 86      pkt = [255] # 0xFF 
 87      # To construct the packet to send across to the hub 
 88      for task in tasks: 
 89          start = task[3] 
 90          duration = task[2] 
 91          timePeriod = task[1] 
 92          pkt.extend([start,duration,timePeriod]) 
 93   
 94      return pkt 
 95       
 96 -class NodeConnection( Connection ): 
 97    """ 
 98    Concrete Connction subclass specialized to send and recieve dat via 
 99    a specific TMDA node 
100     
101    ALGORITHM: 
102      write -- prepend prefix and send via hub 
103      read -- pull from message queue 
104    """ 
105 -  def __init__(self, hubwrite, hubisopen ): 
106      self.isOpen = hubisopen 
107      self.write = hubwrite 
108      self.q = [] 
109       
110 -  def open( self ): 
111      """Make the connection active""" 
112      pass 
113   
114 -  def enqueue( self, msg ): 
115      self.q.append(msg) 
116       
117 -  def read( self, length ): 
118      if not self.q: 
119        return None 
120      return self.q.pop(0)[:length] 
121     
122 -  def close( self ): 
123      """Disconnect; further traffic may raise an exception 
124      """ 
125      pass 
126   
127 -  def reconnect( self, **changes ): 
128      """Try to reconnect with some configuration changes""" 
129      raise IOError('Cannot reconnect TDMA Nodes') 
130     
131 -class Hub( MultiProtocol ): 
132    """ 
133    Concrete MultiProtocol subclass for Wixel TDMA connections 
134     
135    TYPICAL USAGE: 
136      H = Hub() 
137      P1 = Dynamixel.Protocol( Dynamixel.Bus(  
138          port = H.getNewNodeConnection( tid1 ) 
139          )) 
140      H.addSubProtocol( P1 ) 
141      P2 = Dynamixel.Protocol( Dynamixel.Bus(  
142          port = H.getNewNodeConnection( tid2 ) 
143          )) 
144      H.addSubProtocol( P2 ) 
145      # start loop with H.update() called 
146    """ 
147 -  def __init__(self, wixelPort = None ): 
148      """ 
149      INPUT: 
150      """ 
151      MultiProtocol.__init__(self,*argv,**kw) 
152      # Make sure the protocols are initialized right w.r.t to nodes 
153       
154      # Initialize connection to the hub wixel 
155      if wixelPort is None: 
156        wixelPort = newConnection(DEFAULT_WIXEL_PORT) 
157      elif type(wixelPort) in [dict,str]: 
158        wixelPort = newConnection(wixelPort) 
159      elif assert isinstance(wixelPort,Connection) 
160      self.wix = wixelPort 
161      # Collection of TDMA nodes 
162      self.tdn = {} 
163      # Timeplan  
164      self.tp = [] 
165   
166 -  def getNewNodeConnection( self, tid ): 
167      """ 
168      INPUT: 
169         tid -- TDMA node ID 
170      OUTPUT: 
171         a NodeConnection instance that sends data via that node 
172         This is registered in self.tdn 
173      """ 
174      assert not self.tdn.has_key(tid), "TMDA node ID can only be initialized once" 
175      pfx = pack('B',tid)  
176      nc = NodeConnection( 
177        hubwrite = lambda msg : self.wix.write( pfx + msg ), 
178        hubisopen = self.wix.isOpen 
179      ) 
180      self.tdn[tid] = nc 
181      return nc 
182       
183 -  def _demuxIncoming( self ): 
184      """ 
185      (private) scan incoming messages and demultiplex them into 
186      the respective NodeConnection queues 
187      """ 
188      """ 
189      Using a buffer, reassemble messages from the wixel 
190      once a complete message is received, find which tdn it belongs  
191      to and queue it there. If an unknown tdn, record as error 
192      also detect any bad framing of data from wixel 
193      (note: wixel receives framed messages from HW) 
194      """ 
195      pass 
196     
197 -  def update( self ): 
198        """Update all sub-protocols and collect heartbeats""" 
199        hb = {} 
200        # Loop over all protocols contained in this one 
201        for p in self.nim.iterowners(): 
202            p.update() 
203            # Collect heartbeats from the protocols 
204            for inid,val in p.heartbeats.iteritems(): 
205                xnid = self.nim.mapI2X(p,inid) 
206                hb[xnid] = val 
207        self.heartbeats = hb 
208        """!!! 
209        if enough time has passed, self._emitTimeplan() 
210        """ 
211        return self._demuxIncoming() 
212   
213 -  def _emitTimeplan( self ): 
214      """(private) emit timeplan, generating a default one of needed""" 
215      """ 
216      if self.tp is empty, give a generic slot to each node 
217      otherwise use the self.tp to generate timeplan packet(s) 
218      and send them 
219      """ 
220      pass 
221