class StandAloneRadioA(object): def __init__(self): self.kb = KnowledgeBase() self.lock = Lock() self.data = NodeAData() self.packet = Packet('A') self.radio = RadioAPI() self.tx_packet_number = 1 self.ack_packet_number = 0 self.goodput = 0 # self.data = [] # for i in range(50): # self.data.append(0xff) def _configure_radio(self, power, frequency, data_rate, modulation): """ Configure radio for operation. """ self.radio.configure_radio(power, frequency, data_rate, modulation) def _send_packet(self): """ Transmit packet. """ self.packet.set_flags_node_a() location = self.kb.get_state()['current_location'] data = self.data.pack_data() tx_packet = self.packet.make_packet(self.tx_packet_number, location, data) self.radio.transmit(tx_packet) def _receive_packet(self): """ Receive packet. """ rx_packet = self.radio.receive(rx_fifo_threshold=64, timeout=1.0) if rx_packet == []: # this occurs when timeout has been exceeded return else: packet_number, time_stamp, location, flags, data = self.packet.parse_packet(rx_packet) self.ack_packet_number, self.goodput = self.data.unpack_data(data) # print "packet_number=%d time_stamp=%f location=%d flags=0x%x" %(packet_number, time_stamp, # location, flags) print "goodput for acknowledged packet #%d = %f bits/second" %(self.ack_packet_number, self.goodput) def _listen(self): """ Listen before talk. """ status = self.radio.listen(rssi_threshold=100, timeout=1.0) if status == 'clear': print "channel clear" def _fsm(self): self._listen() time.sleep(0.01) self._send_packet() time.sleep(0.01) self._receive_packet() time.sleep(0.01) def run(self): """ Run the radio subsystem. """ self.radio.startup() default_radio_profile = location = self.kb.get_state()['default_radio_profile'] power = default_radio_profile['power'] frequency = default_radio_profile['frequency'] data_rate = default_radio_profile['data_rate'] modulation = default_radio_profile['modulation'] self._configure_radio(power, frequency, data_rate, modulation) # state = "listen" while True: self._fsm() self.lock.acquire() self.kb.sent_packets.append(self.tx_packet_number) self.kb.ack_packets.append((self.ack_packet_number, self.goodput)) self.lock.release() self.tx_packet_number += 1 def shutdown(self): self.kb.save_kb() self.radio.shutdown()
class Controller(object): def __init__(self): self.lock = Lock() self.kb = KnowledgeBase() self.location = Location(self.kb, self.lock) self.motion = MotionSubsystem(self.kb, self.lock, self.motion_callback) self.rf = RadioSubsystem(self.kb, self.lock, self.radio_data_callback) self.fsm_state = 'at_beginning' self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) self.color_values = np.array([]) self.arrived = False def run(self): """ Start the controller. """ self.kb.build_route() self.location.start() self.motion.start() self.rf.start() # time.sleep(0.1) self.fsm() def motion_callback(self, has_arrived): self.arrived = has_arrived def radio_data_callback(self, sent_packet, ack_packet, goodput): """ Radio Subsystem IPC. """ self.sent_packet = np.append(self.sent_packet, sent_packet) self.ack_packet = np.append(self.ack_packet, ack_packet) self.goodput = np.append(self.goodput, goodput) def reset_radio_data(self): """ Reset local radio data storage. """ self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def fsm(self): while True: if self.fsm_state == 'at_beginning': before_start = 0 start_node = self.kb.get_start_node() self.motion.set_source_destination(before_start, start_node) self.motion.set_speed(25) self.motion.control_motion_operation('go') while not self.arrived: time.sleep(0.1) next_node = self.kb.get_next_node(start_node) self.kb.set_current_node(start_node) self.kb.set_next_node(next_node) self.kb.set_next_edge((start_node, next_node)) self.fsm_state = 'traversing_edge' continue elif self.fsm_state == 'traversing_edge': if DEBUG: print "traversing_edge" kb_state = self.kb.get_state() print kb_state current_edge = kb_state['next_edge'] next_edge = None last_node = kb_state['current_node'] current_node = None next_node = kb_state['next_node'] self.kb.set_current_edge(current_edge) self.kb.set_next_edge(next_edge) self.kb.set_last_node(last_node) self.kb.set_current_node(current_node) self.arrived = False self.motion.set_source_destination(current_edge[0], current_edge[1]) self.motion.set_speed(45) tic = time.time() self.motion.control_motion_operation('go') while not self.arrived: self.color_values = np.append(self.color_values, self.motion.color_reading()) time.sleep(0.1) toc = time.time() - tic # weight = toc * np.average(self.goodput) # targets = self.count_targets(self.color_values) # if DEBUG: # print "values for edge %s: weight = %0.2f, targets = %d" %(str(current_edge), # weight, # targets) # self.kb.set_edge_values(current_edge, weight, targets) metric_b = toc * np.average(self.goodput) targets = self.count_targets(self.color_values) if DEBUG: print "values for edge %s: targets = %d, metric_b = %f" % ( str(current_edge), targets, metric_b) self.kb.set_edge_values(current_edge, targets, metric_b) self.fsm_state = 'at_a_node' continue elif self.fsm_state == 'at_a_node': self.rf.control_radio_operation('pause') kb_state = self.kb.get_state() current_node = kb_state['next_node'] next_node = self.kb.get_next_node(current_node) current_edge = None last_edge = kb_state['current_edge'] next_edge = (current_node, next_node) self.kb.set_current_node(current_node) self.kb.set_next_node(next_node) self.kb.set_current_edge(current_edge) self.kb.set_last_edge(last_edge) self.kb.set_next_edge(next_edge) self.fsm_state = 'traversing_edge' self.reset_radio_data() self.color_values = np.array([]) self.rf.control_radio_operation('continue') continue else: print "controller.fsm() error" print "state == ", state break def count_targets(self, a): a[a != 5] = 0 j = 0 for i in range(len(a) - 1): if a[i] == 0 and a[i + 1] == 5: j += 1 return j def shutdown(self): self.kb.save_kb() self.motion.join() self.rf.join() time.sleep(0.1) self.location.join() # shut this down last
class StandAloneRadioA(object): def __init__(self): self.kb = KnowledgeBase() self.lock = Lock() self.data = NodeAData() self.packet = Packet('A') self.radio = RadioAPI() self.tx_packet_number = 1 self.ack_packet_number = 0 self.goodput = 0 # self.data = [] # for i in range(50): # self.data.append(0xff) def _configure_radio(self, power, frequency, data_rate, modulation): """ Configure radio for operation. """ self.radio.configure_radio(power, frequency, data_rate, modulation) def _send_packet(self): """ Transmit packet. """ self.packet.set_flags_node_a() location = self.kb.get_state()['current_location'] data = self.data.pack_data() tx_packet = self.packet.make_packet(self.tx_packet_number, location, data) self.radio.transmit(tx_packet) def _receive_packet(self): """ Receive packet. """ rx_packet = self.radio.receive(rx_fifo_threshold=64, timeout=1.0) if rx_packet == []: # this occurs when timeout has been exceeded return else: packet_number, time_stamp, location, flags, data = self.packet.parse_packet( rx_packet) self.ack_packet_number, self.goodput = self.data.unpack_data(data) # print "packet_number=%d time_stamp=%f location=%d flags=0x%x" %(packet_number, time_stamp, # location, flags) print "goodput for acknowledged packet #%d = %f bits/second" % ( self.ack_packet_number, self.goodput) def _listen(self): """ Listen before talk. """ status = self.radio.listen(rssi_threshold=100, timeout=1.0) if status == 'clear': print "channel clear" def _fsm(self): self._listen() time.sleep(0.01) self._send_packet() time.sleep(0.01) self._receive_packet() time.sleep(0.01) def run(self): """ Run the radio subsystem. """ self.radio.startup() default_radio_profile = location = self.kb.get_state( )['default_radio_profile'] power = default_radio_profile['power'] frequency = default_radio_profile['frequency'] data_rate = default_radio_profile['data_rate'] modulation = default_radio_profile['modulation'] self._configure_radio(power, frequency, data_rate, modulation) # state = "listen" while True: self._fsm() self.lock.acquire() self.kb.sent_packets.append(self.tx_packet_number) self.kb.ack_packets.append((self.ack_packet_number, self.goodput)) self.lock.release() self.tx_packet_number += 1 def shutdown(self): self.kb.save_kb() self.radio.shutdown()
class Controller(object): def __init__(self): self.lock = Lock() self.kb = KnowledgeBase() self.location = Location(self.kb, self.lock) self.motion = MotionSubsystem(self.kb, self.lock, self.motion_callback) self.rf = RadioSubsystem(self.kb, self.lock, self.radio_data_callback) self.fsm_state = 'at_beginning' self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) self.color_values = np.array([]) self.arrived = False def run(self): """ Start the controller. """ self.kb.build_route() self.location.start() self.motion.start() self.rf.start() # time.sleep(0.1) self.fsm() def motion_callback(self, has_arrived): self.arrived = has_arrived def radio_data_callback(self, sent_packet, ack_packet, goodput): """ Radio Subsystem IPC. """ self.sent_packet = np.append(self.sent_packet, sent_packet) self.ack_packet = np.append(self.ack_packet, ack_packet) self.goodput = np.append(self.goodput, goodput) def reset_radio_data(self): """ Reset local radio data storage. """ self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def fsm(self): while True: if self.fsm_state == 'at_beginning': before_start = 0 start_node = self.kb.get_start_node() self.motion.set_source_destination(before_start, start_node) self.motion.set_speed(25) self.motion.control_motion_operation('go') while not self.arrived: time.sleep(0.1) next_node = self.kb.get_next_node(start_node) self.kb.set_current_node(start_node) self.kb.set_next_node(next_node) self.kb.set_next_edge((start_node, next_node)) self.fsm_state = 'traversing_edge' continue elif self.fsm_state == 'traversing_edge': if DEBUG: print "traversing_edge" kb_state = self.kb.get_state() print kb_state current_edge = kb_state['next_edge'] next_edge = None last_node = kb_state['current_node'] current_node = None next_node = kb_state['next_node'] self.kb.set_current_edge(current_edge) self.kb.set_next_edge(next_edge) self.kb.set_last_node(last_node) self.kb.set_current_node(current_node) self.arrived = False self.motion.set_source_destination(current_edge[0], current_edge[1]) self.motion.set_speed(45) tic = time.time() self.motion.control_motion_operation('go') while not self.arrived: self.color_values = np.append(self.color_values, self.motion.color_reading()) time.sleep(0.1) toc = time.time() - tic # weight = toc * np.average(self.goodput) # targets = self.count_targets(self.color_values) # if DEBUG: # print "values for edge %s: weight = %0.2f, targets = %d" %(str(current_edge), # weight, # targets) # self.kb.set_edge_values(current_edge, weight, targets) metric_b = toc * np.average(self.goodput) targets = self.count_targets(self.color_values) if DEBUG: print "values for edge %s: targets = %d, metric_b = %f" %(str(current_edge), targets, metric_b) self.kb.set_edge_values(current_edge, targets, metric_b) self.fsm_state = 'at_a_node' continue elif self.fsm_state == 'at_a_node': self.rf.control_radio_operation('pause') kb_state = self.kb.get_state() current_node = kb_state['next_node'] next_node = self.kb.get_next_node(current_node) current_edge = None last_edge = kb_state['current_edge'] next_edge = (current_node, next_node) self.kb.set_current_node(current_node) self.kb.set_next_node(next_node) self.kb.set_current_edge(current_edge) self.kb.set_last_edge(last_edge) self.kb.set_next_edge(next_edge) self.fsm_state = 'traversing_edge' self.reset_radio_data() self.color_values = np.array([]) self.rf.control_radio_operation('continue') continue else: print "controller.fsm() error" print "state == ", state break def count_targets(self, a): a[a!=5] = 0 j = 0 for i in range(len(a)-1): if a[i] == 0 and a[i+1] == 5: j += 1 return j def shutdown(self): self.kb.save_kb() self.motion.join() self.rf.join() time.sleep(0.1) self.location.join() # shut this down last
class Controller(object): def __init__(self): self.kb = KnowledgeBase() self.lock = threading.Lock() self.location = Location(self.kb, self.lock) self.motion = SimpleMotion(self.kb, self.lock) self.rf = RadioSubsystem(self.kb, self.lock, self.radio_data_callback) self.fsm_state = 'at_beginning' self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def run(self): """ Start the controller. """ self.kb.build_route() self.location.start() self.rf.start() while True: self.fsm() def radio_data_callback(self, sent_packet, ack_packet, goodput): self.sent_packet = np.append(self.sent_packet, sent_packet) self.ack_packet = np.append(self.ack_packet, ack_packet) self.goodput = np.append(self.goodput, goodput) def reset_radio_data(self): self.sent_packet = np.array([]) self.ack_packet = np.array([]) self.goodput = np.array([]) def fsm(self): if self.fsm_state == 'at_beginning': print "\n\n\n" print "at_beginning" start_node = self.kb.get_start_node() self.motion.move_until_location(start_node, speed = 25) while not self.kb.get_state()['current_location'] == start_node: time.sleep(0.2) next_node = self.kb.get_next_node(start_node) # self.lock.acquire() self.kb.set_current_node(start_node) self.kb.set_next_node(next_node) self.kb.set_next_edge((start_node, next_node)) # self.lock.release() self.fsm_state = 'traversing_edge' return elif self.fsm_state == 'traversing_edge': print "\n\n\n" print "traversing_edge" kb_state = self.kb.get_state() current_edge = kb_state['next_edge'] next_edge = None last_node = kb_state['current_node'] current_node = None next_node = kb_state['next_node'] # self.lock.acquire() self.kb.set_current_edge(current_edge) self.kb.set_next_edge(next_edge) self.kb.set_last_node(last_node) self.kb.set_current_node(current_node) # self.lock.release() tic = time.time() # this is not threaded!!!!! self.motion.move_from_here_to_there(last_node, next_node, speed = 45) # print "self.kb.get_state()['current_location'] = ", self.kb.get_state()['current_location'] # print "next_node = ", next_node # print "self.kb.get_state()['current_location'] == next_node : ", self.kb.get_state()['current_location'] == next_node print "traversing_eddge, current_node = ", current_node print "traversing_edge, next_node = ", next_node while not self.kb.get_state()['current_location'] == next_node: # color = motion.get_color_reading() print "\n\n\n\n" print "Blah blah blah" # print "Detected color: ", color print "\n\n\n\n" time.sleep(0.01) toc = time.time() - tic weight = toc * np.average(self.goodput) print "weight value for edge %s = %0.2f" %(str(current_edge), weight) self.kb.set_edge_weight(current_edge, weight) # print self.goodput # print "average goodput for edge %s = %0.2f" %(str(current_edge), np.average(self.goodput)) self.fsm_state = 'at_a_node' return elif self.fsm_state == 'at_a_node': print "\n\n\n" print "at_a_node" self.rf.control_radio_operation('pause') kb_state = self.kb.get_state() current_node = kb_state['next_node'] next_node = self.kb.get_next_node(current_node) print "at_a_node, current_node = ", current_node print "at_a_node, next_node = ", next_node current_edge = None last_edge = kb_state['current_edge'] next_edge = (current_node, next_node) # self.lock.acquire() self.kb.set_current_node(current_node) self.kb.set_next_node(next_node) self.kb.set_current_edge(current_edge) self.kb.set_last_edge(last_edge) self.kb.set_next_edge(next_edge) # self.lock.release() self.fsm_state = 'traversing_edge' self.rf.control_radio_operation('continue') self.reset_radio_data() return else: if DEBUG: print "controller.fsm() error" print "state == ", state else: pass def shutdown(self): self.kb.save_kb() # pprint.pprint(self.kb.get_state()) # print "\n\n\n\n\n" # self.kb.route_debug() # print "\n\n\n\n\n" self.motion.shutdown() self.rf.join() time.sleep(0.1) self.location.join() # shut this down last