def run(self): """ Run the controller event loop :return: """ comm = Communication(SERIAL_PORT) print("Running MAPS controller on serial port: %s" % (SERIAL_PORT)) try: weights = np.ones(dtype=np.float32, shape=(5,1))/5 while True: val, pin_id = comm.read_packet() if not self.valid_pin(pin_id): continue aggregation_update(pin_id, val) floor_thresh = get_floor_threshold(pin_id) ceil_thresh = get_ceil_threshold(pin_id) if not self.eventManager.check_failures(pin_id, val, ceil_thresh, floor_thresh): # no failures vals = self.aggregation_buffers[pin_id].history.T if get_filter_mode() != FILTER_NONE: filtered_val = apply_filter(vals) else: filtered_val = val self.eventManager.publish_packet(pin_id, filtered_val) self.aggregation_buffers[pin_id].add(val) else: self.eventManager.publish_failure(pin_id) self.eventManager.sleep() except rospy.ROSInterruptException: pass
def test_all(): event_manager = EventManager() for i in range(16): set_threshhold(i, 0) aggregation_update(i, 0) test_range = range(-32,32) + range(32,0, -1) for i in range(16): for j in test_range: set_threshhold(i, j) time.sleep(0.01) for k in test_range: aggregation_update(i, k) time.sleep(0.01) set_threshhold(i, 15.0) aggregation_update(i, 16.0) time.sleep(0.01) event_manager.publish_packet(i, 16.0) time.sleep(0.5) aggregation_update(i, 11.0) event_manager.publish_failure(i) time.sleep(0.5)