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 __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 __init__(self): logging.basicConfig(filename='basic.log', filemode='w', level=logging.DEBUG) self.current_location = 0 brick = utils.connect_to_brick() self.cognition = DecisionMaker() self.location = Location(self.location_callback) self.motion = MotionSubsystem(brick) self.radio = RadioSubsystem(self.radio_update_flag, self.radio_update_data, self.radio_reconfig_flag) self.tracker = TargetTracker(brick) self.radio_update_flag = False self.reconfig_flag = False self.iteration = 1 self.f = open('track_data', 'w')
def __init__(self): self.current_location = 0 brick = utils.connect_to_brick() self.cognition = DecisionMaker() self.location = Location(self.location_callback) self.motion = MotionSubsystem(brick) self.radio = RadioSubsystem(self.radio_update_flag, self.radio_update_data, self.radio_reconfig_flag) self.tracker = TargetTracker(brick) self.radio_update_flag = False self.reconfig_flag = False # self.iteration = 1 self.current_m = {} self.previous_m = {} self.re_explore = False self.f = open('track_data', 'w')
class Controller(object): def __init__(self): self.current_location = 0 brick = utils.connect_to_brick() self.cognition = DecisionMaker() self.location = Location(self.location_callback) self.motion = MotionSubsystem(brick) self.radio = RadioSubsystem(self.radio_update_flag, self.radio_update_data, self.radio_reconfig_flag) self.tracker = TargetTracker(brick) self.radio_update_flag = False self.reconfig_flag = False # self.iteration = 1 self.current_m = {} self.previous_m = {} self.re_explore = False self.f = open('track_data', 'w') def location_callback(self, current_location): """ Callback for location module. This function is used by the location module to relay the current location--as read by the barcode reader--back to the controller. Parameters ---------- current_location : int Value of barcode representing current location. """ self.current_location = current_location def radio_update_flag(self, flag=False): """ Callback for radio subsystem. This function simply sets a flag true or false, to indicate if there is data from the radio subsystem. Parameters ---------- flag : bool `True` if radio subsystem has passed data through the `radio_callback_data`. """ self.radio_update_flag = flag def radio_update_data(self, tx_packets=0, rx_packets=0, signal_strength=0): """ Callback for radio subsystem. This function is used for the radio susbsytem to pass data back to the controller. Parameters ---------- tx_packets : int Number of streamed packets sent by radio. rx_packets : int Number of streamed packets received by Node B. rssi : int """ self.tx_packets = tx_packets self.rx_packets = rx_packets self.rssi = signal_strength def radio_reconfig_flag(self, flag=False): """ Callback for radio subsystem. This function is use by the radio subsystem to indicate that a reconfiguration request has been acknowledged by Node B. Parameters ---------- flag : bool `True` if radio subsystem has received an acknowledgment from Node B for a reconfiguration request. """ self.reconfig_flag = flag def build_route(self): """ Build route graph. """ path_a = Path(name='A', distance=62.0, direction='left') path_b = Path(name='B', distance=48.0, direction='straight') path_c = Path(name='C', distance=87.5, direction='right') # preload values for path, bypass initial exploration path_a.current_meters['X'] = 5 path_a.current_meters['Y'] = 1 path_a.current_meters['RSSI'] = -86 path_b.current_meters['X'] = 3 path_b.current_meters['Y'] = 0 path_b.current_meters['RSSI'] = -86 path_c.current_meters['X'] = 5 path_c.current_meters['Y'] = 3 path_c.current_meters['RSSI'] = -86 self.path_names = ['A', 'B', 'C'] # this is a hack self.paths = [path_a, path_b, path_c] def run(self): """ Run AV controller. """ parser = argparse.ArgumentParser() parser.add_argument("-f", type=float, default=434e6, metavar='frequency', dest='frequency', nargs=1, help="Transmit frequency (default: %(default)s)") parser.add_argument("-m", type=str, default='gfsk', metavar='modulation', dest='modulation', choices=['gfsk', 'fsk', 'ask'], help="Select modulation from [%(choices)s] (default: %(default)s)") parser.add_argument("-p" "--power", type=int, default=17, metavar='power', dest='power', choices=[8, 11, 14, 17], help="Select transmit power from [%(choices)s] (default: %(default)s)") parser.add_argument("-r" "--bitrate", type=float, default=4.8e3, metavar='bitrate', dest='bitrate', help="Set bitrate (default: %(default)s)") args = parser.parse_args() self.frequency = args.frequency self.modulation = args.modulation self.eirp = args.power self.bitrate = args.bitrate self.build_route() self.location.start() self.motion.start() self.radio.start() self.fsm() print self.path_history print self.choice_history print self.score_history print self.soln_idx print self.current_m print self.previous_m self.shutdown() def shutdown(self): """ Shutdown subsytems before stopping. """ # self.f.close() self.tracker.kill_sensor() self.motion.join() self.location.join() # shut this down last def fsm(self): """ AV finite state machine. """ fsm_state = 'first_time' start = 1 destination = 2 convergence_iterator = 0 self.path_history = [] self.choice_history = [] self.score_history = [] self.prev_score = -10 self.prev_param = {} self.prev_soln = [] self.soln_idx = [] iteration = 1 while iteration < 11: if fsm_state == 'first_time': ################################################################### self.motion.set_direction('straight') self.motion.set_speed(25) self.motion.set_state('go') while not self.current_location == start: time.sleep(0.01) else: self.motion.set_state('stop') time.sleep(0.1) self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(self.modulation, self.eirp, self.bitrate, self.frequency) fsm_state = 'before_traverse' continue ################################################################### if fsm_state == 'before_traverse': ################################################################### choice = self.cognition.choose_path(self.paths) if choice != -1: current_path = self.paths[choice] self.soln_idx.append('Explore') current_path.current_knobs['Modulation'] = self.modulation current_path.current_knobs['Rs'] = self.bitrate current_path.current_knobs['EIRP'] = self.eirp current_path.current_knobs['Speed'] = 25 self.motion.set_speed(25) else: score, param, soln, s_i = self.cognition.solution(self.paths, iteration) print "score: ", score print "prev_score: ", self.prev_score if score > self.prev_score: print "current solution is better" self.soln_idx.append(s_i) self.score_history.append(score) self.prev_score = score self.prev_param = param self.prev_soln = soln name_of_chosen_path = param['name'] choice = self.path_names.index(name_of_chosen_path) current_path = self.paths[choice] self.choice_history.append(current_path.name) current_path.current_knobs['Modulation'] = 'fsk' current_path.current_knobs['EIRP'] = param['EIRP'] current_path.current_knobs['Rs'] = param['Rs'] current_path.current_knobs['Speed'] = param['rotor_power'] self.radio.set_config_packet_data(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs']) self.radio.set_state('reconfigure') while not self.reconfig_flag: time.sleep(0.1) else: self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs'], self.frequency) self.motion.set_speed(current_path.current_knobs['Speed']) else: print "previous solution is better" try: name_of_chosen_path = self.prev_param['name'] except KeyError: print "KeyError" print self.prev_param sys.exit(1) comparison = self.compare(self.prev_param) if comparison == True: print "prev solution is better and old environment is unchanged" convergence_iterator += 1 if convergence_iterator == 3: convergence_iterator = 0 self.re_explore = True self.soln_idx.append('prev result') self.score_history.append(self.score_history[-1]) choice = self.path_names.index(name_of_chosen_path) current_path = self.paths[choice] self.choice_history.append(current_path.name) current_path.current_knobs['Modulation'] = 'fsk' current_path.current_knobs['EIRP'] = self.prev_param['EIRP'] current_path.current_knobs['Rs'] = self.prev_param['Rs'] current_path.current_knobs['Speed'] = self.prev_param['rotor_power'] self.radio.set_config_packet_data(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs']) self.radio.set_state('reconfigure') while not self.reconfig_flag: time.sleep(0.1) else: self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs'], self.frequency) self.motion.set_speed(current_path.current_knobs['Speed']) else: print "prev solution is better but old environment has changed" print "use current solution" self.soln_idx.append(s_i) self.score_history.append(score) self.prev_score = score self.prev_param = param self.prev_soln = soln name_of_chosen_path = param['name'] choice = self.path_names.index(name_of_chosen_path) current_path = self.paths[choice] self.choice_history.append(current_path.name) current_path.current_knobs['Modulation'] = 'fsk' current_path.current_knobs['EIRP'] = param['EIRP'] current_path.current_knobs['Rs'] = param['Rs'] current_path.current_knobs['Speed'] = param['rotor_power'] self.radio.set_config_packet_data(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs']) self.radio.set_state('reconfigure') while not self.reconfig_flag: time.sleep(0.1) else: self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs'], self.frequency) self.motion.set_speed(current_path.current_knobs['Speed']) self.motion.set_direction(current_path.direction) self.path_history.append(current_path.name) fsm_state = 'traverse_path' continue ################################################################### if fsm_state == 'traverse_path': ################################################################### name = current_path.name if not current_path.current_meters == {}: # print "how about here?" self.previous_m[name] = copy.deepcopy(current_path.current_meters) # print "previous_m[name]: ", self.previous_m[name] # print "maybe here?" if self.re_explore == True: for p in self.paths: p.has_been_explored = False self.re_explore = False if not current_path.has_been_explored: print "Exploring path" self.radio.set_state('listen') else: print "Exploiting path" self.radio.set_state('stream') self.motion.set_state('go') tic = time.time() while not self.current_location == destination: self.tracker.run() time.sleep(0.1) else: self.motion.set_state('stop') self.radio.set_state('stop') toc = time.time() time.sleep(1) x, y = self.tracker.tally_results() print "x = %d y = %d" %(x, y) self.tracker.reset() name = current_path.name current_path.current_meters['X'] = x current_path.current_meters['Y'] = y current_path.solution_as_observed['T'] = toc - tic fsm_state = 'after_traverse' continue ################################################################### if fsm_state == 'after_traverse': ################################################################### print "updating meters" # for p in self.paths: # p.update_meters() if not current_path.has_been_explored: print "marking current path as explored" current_path.has_been_explored = True # fsm_state = 'go_to_beginning' # continue else: self.radio.set_state('update') while not self.radio_update_flag: time.sleep(0.1) else: current_path.current_meters['RSSI'] = self.rssi # current_path.solution_as_observed['G'] = self.rx_packets # current_path.solution_as_observed['Z'] = self.cognition.calculate_z(x, y) # current_path.solution_as_observed['B'] = self.cognition.estimate_ber(self.tx_packets, # self.rx_packets) name = current_path.name self.current_m[name] = current_path.current_meters iteration += 1 fsm_state = 'go_to_beginning' continue ################################################################### if fsm_state == 'go_to_beginning': ################################################################### name = current_path.name print "current_m: ", self.current_m[name] print "previous_m: ", self.previous_m[name] print "current_m: ", self.current_m print "previous_m: ", self.previous_m print "" print "Iteration %d finished" %(iteration-1,) s = raw_input("AVEP has completed an iteration, press Y/y to continue ") self.motion.set_direction('straight') self.motion.set_speed(25) self.motion.set_state('go') while not self.current_location == start: time.sleep(0.01) else: self.motion.set_state('stop') time.sleep(0.1) fsm_state = 'before_traverse' continue ################################################################### def compare(self, param): """ Determine if the environment has changed from one iteration to the next. """ name = param['name'] c_meters = self.current_m[name] p_meters = self.previous_m[name] if c_meters['X'] != p_meters['X']: print "current_meters['X'] != previous_meters['X']" return False elif c_meters['Y'] != p_meters['Y']: print "current_meters['Y'] != previous_meters['Y']" return False # elif current_meters['RSSI'] == previous_meters['RSSI']: # print "current_meters['Noise'] == previous_meters['Y']" # return False else: return True
class Controller(object): def __init__(self): logging.basicConfig(filename='basic.log', filemode='w', level=logging.DEBUG) self.current_location = 0 brick = utils.connect_to_brick() self.cognition = DecisionMaker() self.location = Location(self.location_callback) self.motion = MotionSubsystem(brick) self.radio = RadioSubsystem(self.radio_update_flag, self.radio_update_data, self.radio_reconfig_flag) self.tracker = TargetTracker(brick) self.radio_update_flag = False self.reconfig_flag = False self.iteration = 1 self.f = open('track_data', 'w') def location_callback(self, current_location): """ Callback for location module. This function is used by the location module to relay the current location--as read by the barcode reader--back to the controller. Parameters ---------- current_location : int Value of barcode representing current location. """ self.current_location = current_location def radio_update_flag(self, flag=False): """ Callback for radio subsystem. This function simply sets a flag true or false, to indicate if there is data from the radio subsystem. Parameters ---------- flag : bool `True` if radio subsystem has passed data through the `radio_callback_data`. """ self.radio_update_flag = flag def radio_update_data(self, tx_packets=0, rx_packets=0, signal_strength=0): """ Callback for radio subsystem. This function is used for the radio susbsytem to pass data back to the controller. Parameters ---------- tx_packets : int Number of streamed packets sent by radio. rx_packets : int Number of streamed packets received by Node B. rssi : int """ self.tx_packets = tx_packets self.rx_packets = rx_packets self.rssi = signal_strength def radio_reconfig_flag(self, flag=False): """ Callback for radio subsystem. This function is use by the radio subsystem to indicate that a reconfiguration request has been acknowledged by Node B. Parameters ---------- flag : bool `True` if radio subsystem has received an acknowledgment from Node B for a reconfiguration request. """ self.reconfig_flag = flag def build_route(self): """ Build route graph. """ path_a = Path(name='A', distance=62.0, direction='left') path_b = Path(name='B', distance=48.0, direction='straight') path_c = Path(name='C', distance=87.5, direction='right') self.paths = [path_a, path_b, path_c] def run(self): """ Run AV controller. """ parser = argparse.ArgumentParser() parser.add_argument("-f", type=float, default=434e6, metavar='frequency', dest='frequency', nargs=1, help="Transmit frequency (default: %(default)s)") parser.add_argument( "-m", type=str, default='gfsk', metavar='modulation', dest='modulation', choices=['gfsk', 'fsk', 'ask'], help="Select modulation from [%(choices)s] (default: %(default)s)") parser.add_argument( "-p" "--power", type=int, default=17, metavar='power', dest='power', choices=[8, 11, 14, 17], help= "Select transmit power from [%(choices)s] (default: %(default)s)") parser.add_argument("-r" "--bitrate", type=float, default=4.8e3, metavar='bitrate', dest='bitrate', help="Set bitrate (default: %(default)s)") args = parser.parse_args() self.frequency = args.frequency self.modulation = args.modulation self.eirp = args.power self.bitrate = args.bitrate self.build_route() self.location.start() self.motion.start() self.radio.start() self.fsm() def shutdown(self): """ Shutdown subsytems before stopping. """ # self.f.close() logging.close() self.tracker.kill_sensor() self.motion.join() self.location.join() # shut this down last def fsm(self): """ AV finite state machine. """ fsm_state = 'first_time' start = 1 destination = 2 while True: ################################################################### if fsm_state == 'first_time': # logging.info("v3_controller::fsm:first_time") self.motion.set_direction('straight') self.motion.set_speed(25) self.motion.set_state('go') while not self.current_location == start: time.sleep(0.01) else: self.motion.set_state('stop') time.sleep(0.1) self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(self.modulation, self.eirp, self.bitrate, self.frequency) fsm_state = 'before_traverse' continue ################################################################### ################################################################### if fsm_state == 'before_traverse': # logging.info("v3_controller::fsm: before_traverse") # for p in self.paths: # p.update_knobs() i = self.cognition.choose_path(self.paths) current_path = self.paths[i] current_path.iteration = self.iteration if not current_path.has_been_explored: # use default values current_path.current_knobs['Modulation'] = self.modulation current_path.current_knobs['Rs'] = self.bitrate current_path.current_knobs['EIRP'] = self.eirp current_path.current_knobs['Speed'] = 25 self.motion.set_speed(25) else: # notify base station of new configuration self.radio.set_config_packet_data( current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs']) self.radio.set_state('reconfigure') while not self.reconfig_flag: # wait for acknowledgment time.sleep(0.1) else: # use new configuration self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration( current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs'], self.frequency) self.motion.set_speed( current_path.current_knobs['Speed']) self.motion.set_direction(current_path.direction) s = "\n\n" s += "Before traverse.\n" s += "==================================================\n" s += "Iteration %d.\n" % (self.iteration, ) for p in self.paths: s += "\n\nPath %s information:\n" % (p.name, ) s += "Path explored yet? " + str( p.has_been_explored) + "\n" s += "solution_parameters: " + str( p.solution_parameters) + "\n" s += "solution_as_implemented: " + str( p.solution_as_implemented) + "\n" s += "previous_meters: " + str(p.previous_meters) + "\n" s += "current_knobs: " + str(p.current_knobs) + "\n" s += "\n\nChosen path is %s.\n" % (current_path.name, ) s += "==================================================" logging.info(s) # logging.info("\n\nChosen path is %s." %(current_path.name,)) # logging.info("==================================================") fsm_state = 'traverse_path' continue ################################################################### ################################################################### if fsm_state == 'traverse_path': logging.info("v3_controller::fsm: traverse_path") self.radio.set_state('stream') self.motion.set_state('go') tic = time.time() while not self.current_location == destination: self.tracker.run() time.sleep(0.1) else: self.motion.set_state('stop') self.radio.set_state('stop') toc = time.time() time.sleep(1) x, y = self.tracker.tally_results() logging.info("v3_controller::fsm:traverse_path: x = %d" % (x, )) logging.info("v3_controller::fsm:traverse_path: y = %d" % (y, )) # self.shutdown() # print "x = ", x # print "y = ", y self.tracker.reset() current_path.current_meters['X'] = x current_path.current_meters['Y'] = y current_path.solution_as_observed['T'] = toc - tic fsm_state = 'after_traverse' # self.shutdown() continue ################################################################### ################################################################### if fsm_state == 'after_traverse': self.iteration += 1 current_path.has_been_explored = True for p in self.paths: p.update_meters() self.radio.set_state('update') while not self.radio_update_flag: time.sleep(0.1) else: current_path.current_meters['RSSI'] = self.rssi current_path.solution_as_observed['G'] = self.rx_packets current_path.solution_as_observed[ 'Z'] = self.cognition.calculate_z(x, y) current_path.solution_as_observed[ 'B'] = self.cognition.estimate_ber( self.tx_packets, self.rx_packets) # TODO: add the part where we determine if the # solution we used wasn any good fsm_state = 'go_to_beginning' continue ################################################################### ################################################################### if fsm_state == 'go_to_beginning': self.motion.set_direction('straight') self.motion.set_speed(55) self.motion.set_state('go') while not self.current_location == start: time.sleep(0.01) else: self.motion.set_state('stop') time.sleep(0.1) fsm_state = 'before_traverse' continue
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): logging.basicConfig(filename='basic.log', filemode='w', level=logging.DEBUG) self.current_location = 0 brick = utils.connect_to_brick() self.cognition = DecisionMaker() self.location = Location(self.location_callback) self.motion = MotionSubsystem(brick) self.radio = RadioSubsystem(self.radio_update_flag, self.radio_update_data, self.radio_reconfig_flag) self.tracker = TargetTracker(brick) self.radio_update_flag = False self.reconfig_flag = False self.iteration = 1 self.f = open('track_data', 'w') def location_callback(self, current_location): """ Callback for location module. This function is used by the location module to relay the current location--as read by the barcode reader--back to the controller. Parameters ---------- current_location : int Value of barcode representing current location. """ self.current_location = current_location def radio_update_flag(self, flag=False): """ Callback for radio subsystem. This function simply sets a flag true or false, to indicate if there is data from the radio subsystem. Parameters ---------- flag : bool `True` if radio subsystem has passed data through the `radio_callback_data`. """ self.radio_update_flag = flag def radio_update_data(self, tx_packets=0, rx_packets=0, signal_strength=0): """ Callback for radio subsystem. This function is used for the radio susbsytem to pass data back to the controller. Parameters ---------- tx_packets : int Number of streamed packets sent by radio. rx_packets : int Number of streamed packets received by Node B. rssi : int """ self.tx_packets = tx_packets self.rx_packets = rx_packets self.rssi = signal_strength def radio_reconfig_flag(self, flag=False): """ Callback for radio subsystem. This function is use by the radio subsystem to indicate that a reconfiguration request has been acknowledged by Node B. Parameters ---------- flag : bool `True` if radio subsystem has received an acknowledgment from Node B for a reconfiguration request. """ self.reconfig_flag = flag def build_route(self): """ Build route graph. """ path_a = Path(name='A', distance=62.0, direction='left') path_b = Path(name='B', distance=48.0, direction='straight') path_c = Path(name='C', distance=87.5, direction='right') self.paths = [path_a, path_b, path_c] def run(self): """ Run AV controller. """ parser = argparse.ArgumentParser() parser.add_argument("-f", type=float, default=434e6, metavar='frequency', dest='frequency', nargs=1, help="Transmit frequency (default: %(default)s)") parser.add_argument("-m", type=str, default='gfsk', metavar='modulation', dest='modulation', choices=['gfsk', 'fsk', 'ask'], help="Select modulation from [%(choices)s] (default: %(default)s)") parser.add_argument("-p" "--power", type=int, default=17, metavar='power', dest='power', choices=[8, 11, 14, 17], help="Select transmit power from [%(choices)s] (default: %(default)s)") parser.add_argument("-r" "--bitrate", type=float, default=4.8e3, metavar='bitrate', dest='bitrate', help="Set bitrate (default: %(default)s)") args = parser.parse_args() self.frequency = args.frequency self.modulation = args.modulation self.eirp = args.power self.bitrate = args.bitrate self.build_route() self.location.start() self.motion.start() self.radio.start() self.fsm() def shutdown(self): """ Shutdown subsytems before stopping. """ # self.f.close() logging.close() self.tracker.kill_sensor() self.motion.join() self.location.join() # shut this down last def fsm(self): """ AV finite state machine. """ fsm_state = 'first_time' start = 1 destination = 2 while True: ################################################################### if fsm_state == 'first_time': # logging.info("v3_controller::fsm:first_time") self.motion.set_direction('straight') self.motion.set_speed(25) self.motion.set_state('go') while not self.current_location == start: time.sleep(0.01) else: self.motion.set_state('stop') time.sleep(0.1) self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(self.modulation, self.eirp, self.bitrate, self.frequency) fsm_state = 'before_traverse' continue ################################################################### ################################################################### if fsm_state == 'before_traverse': # logging.info("v3_controller::fsm: before_traverse") # for p in self.paths: # p.update_knobs() i = self.cognition.choose_path(self.paths) current_path = self.paths[i] current_path.iteration = self.iteration if not current_path.has_been_explored: # use default values current_path.current_knobs['Modulation'] = self.modulation current_path.current_knobs['Rs'] = self.bitrate current_path.current_knobs['EIRP'] = self.eirp current_path.current_knobs['Speed'] = 25 self.motion.set_speed(25) else: # notify base station of new configuration self.radio.set_config_packet_data(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs']) self.radio.set_state('reconfigure') while not self.reconfig_flag: # wait for acknowledgment time.sleep(0.1) else: # use new configuration self.radio.set_current_location(self.current_location) self.radio.set_radio_configuration(current_path.current_knobs['Modulation'], current_path.current_knobs['EIRP'], current_path.current_knobs['Rs'], self.frequency) self.motion.set_speed(current_path.current_knobs['Speed']) self.motion.set_direction(current_path.direction) s = "\n\n" s += "Before traverse.\n" s += "==================================================\n" s += "Iteration %d.\n" %(self.iteration,) for p in self.paths: s += "\n\nPath %s information:\n" %(p.name,) s += "Path explored yet? " + str(p.has_been_explored) + "\n" s += "solution_parameters: " + str(p.solution_parameters) + "\n" s += "solution_as_implemented: " + str(p.solution_as_implemented) + "\n" s += "previous_meters: " + str(p.previous_meters) + "\n" s += "current_knobs: " + str(p.current_knobs) + "\n" s += "\n\nChosen path is %s.\n" %(current_path.name,) s += "==================================================" logging.info(s) # logging.info("\n\nChosen path is %s." %(current_path.name,)) # logging.info("==================================================") fsm_state = 'traverse_path' continue ################################################################### ################################################################### if fsm_state == 'traverse_path': logging.info("v3_controller::fsm: traverse_path") self.radio.set_state('stream') self.motion.set_state('go') tic = time.time() while not self.current_location == destination: self.tracker.run() time.sleep(0.1) else: self.motion.set_state('stop') self.radio.set_state('stop') toc = time.time() time.sleep(1) x, y = self.tracker.tally_results() logging.info("v3_controller::fsm:traverse_path: x = %d" %(x,)) logging.info("v3_controller::fsm:traverse_path: y = %d" %(y,)) # self.shutdown() # print "x = ", x # print "y = ", y self.tracker.reset() current_path.current_meters['X'] = x current_path.current_meters['Y'] = y current_path.solution_as_observed['T'] = toc - tic fsm_state = 'after_traverse' # self.shutdown() continue ################################################################### ################################################################### if fsm_state == 'after_traverse': self.iteration += 1 current_path.has_been_explored = True for p in self.paths: p.update_meters() self.radio.set_state('update') while not self.radio_update_flag: time.sleep(0.1) else: current_path.current_meters['RSSI'] = self.rssi current_path.solution_as_observed['G'] = self.rx_packets current_path.solution_as_observed['Z'] = self.cognition.calculate_z(x, y) current_path.solution_as_observed['B'] = self.cognition.estimate_ber(self.tx_packets, self.rx_packets) # TODO: add the part where we determine if the # solution we used wasn any good fsm_state = 'go_to_beginning' continue ################################################################### ################################################################### if fsm_state == 'go_to_beginning': self.motion.set_direction('straight') self.motion.set_speed(55) self.motion.set_state('go') while not self.current_location == start: time.sleep(0.01) else: self.motion.set_state('stop') time.sleep(0.1) fsm_state = 'before_traverse' continue
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