[Tom Line]

One comment - from your diagram it should not be "impossible" to get to the next waypoint if you come into that one wrong.

Your PID loop for steering should be adjusted such that above a certain angle, or when there is an obstacle ahead of you (a single sensor will easily tell you this) then the PID loop will increase the turning amount so that the robot tank steers to head directly to the next target. This is the way we have ours set up.

We ended up dispensing with the waypoint system. This year's game doesn't lend itself to it as much as previous years. We have a constantly updating position working off the walls and correlating with the encoders. The sole directive in the program is to continue moving forward when possible.