Create filter for potentially unreliable SonarSensor object.

[mcolinj]

As it is written, and will soon be committed to master, the SonarSensor listens for range updates from the sonar unit, and it provides them to the PID controller to help control driving, or elevator, or whatever.

I think we should provide a filtering behavior where we keep track of the last 5 values received, you can use list/array slicing to push a new value and remove a value each time.

Then, the filter should compute a value to give to the pid that is the average of the middle 3 of the 5 values. (this is most easily done by sorting/slicing/and math.mean ing.

Take some care in the code so that it will work properly as it is starting up.

[mcolinj]

This is a feature to work on after we have pushed the sonar and vision sensor objects to master.

[mcolinj]

You can insert the new reading at the front of the window and then trim with slicing.

window [1, 2, 3, 4, 5] window.insert(0, 7) window [7, 1, 2, 3, 4, 5] window = window[:5] window [7, 1, 2, 3, 4]

That works okay while you are filling up the window as well.

window = [1, 2] window = window[:5] window [1, 2]

[mcolinj]

Here's the code I think we need for the filter operation. See if you can test it out to make sure it does what we need.

from statistics import mean

def filterWindow(self, new_value):
        wsize = self.wsize

        # drop in the new value, trim to wsize elements
        self.range_window.insert(0, new_value)
        self.range_window = range_window[:wsize]

        # just return the reading if we do not have a full window
        if len(self.range_window) < wsize:
            return new_value
        else:
            # sort and then return the average w/o the hi and lo
            sorted_window = sort(self.range_window)
            return mean(sorted_window[1:wsize-1])