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)