Broke out navigation system into parts for better organization. Now tick motor things and update state.
Fixed bug in FOLLOW command that made it only follow variable 0.
Navigator now guesses the current position of the robot using the encoders and heading when the VPS is not giving good information.
Test to see if motor current draw can be used to detect obstructions. It can.
Navigator now knows if the robot is obstructed.