Calculating the wall angle

Now that we have a reliable sensor platform, we need to calculate the actual angle that the wall is at.  The biggest problem here is bad sensor data.  The IR sensors spit out values that can vary widely, even when nothing is changing.  You'll see 40cm, 20cm, 38cm, 35cm, 80cm from the same sensor.  So first that needs some smoothing.  I simply took the last 8 readings, threw out the max and min, and then averaged the rest.  The sensors run at 500Hz so the delay shouldn't be noticable.

Second, when the sensor doesn't see anything it spits out weird values.  So even though the sensors are rated for 80cm, I found they didn't give reliable data (they give values when there's nothing there) past 50cm.

So, first smooth out the values, then throw out the readings that are too far, then you can calculate the angles they all make.  Take the average of that, and that gives you the "best guess" of the current angle the wall makes.  First I wrote and debugged all that using the Processing gui since it was *much* easier to visualize the geometry.  Then I converted that code into the Arduino code so that it could run directly on-board the bot.

It worked great!