With the Sharp GP2D02 sensor output linearized and converted into 10's of an inch it becomes easy to implement wall following logic. If you remember your high school geometry you can see that the angles, hence the ratios of rise to run (slope) are the same for both the vehicle wall and the amount of turning needed. I ignored the fact that the turns are not crisp (I could stop the bot first, then spin the required amount...) and that R' is really an arc, not a straight line - but in practice these errors are less than my resolution of the encoder and can be safely ignored.

Having PID control of the wheel position also helps. I simply add/subtract the desired rotation (R') from the desired setpoint and the PID drives the wheels where they need to be. Forward motion is simply adding a constant amount to the desired position at a known rate (20hz in my case) - the constant is, guess what! Velocity! This is all detailed in MainLogic.asm