[Tom Line]

Loubear - you may want to add a simple if/then into your code. This is how we control distance to the wall on our robot.

if angle to next waypoint is greater than it should be (i.e. - you'll turn into the wall like like your picture)

then current_steering_angle = current_steering_angle - 1;

if current_steering_angle < (optimum - 5)
current_steering_angle = optimum - 5;
else if current_steering_angle > (optimum + 5)
current_steering_angle = optimum + 5;

This will continually adjust your robot heading so it angles more towards the "wanted" or "intended" path to the next waypoint. Adjust the range (+5 and -5) to achieve the desired side no matter where you end up.

We use this but actually look at distance from the wall and adjust our heading to bring us to the correct distance (30 inches in our case).

Edit - I was just thinking that if you look at the desired angle to the waypoint beyond the one you're aiming at, you'll know which side of the current straight line to the waypoint you want to be on it. If you know you'll need to turn left at the waypoint, then you should try to be to the right of it, etc. You could add that if/then to what I've put in above.

We don't go that far. We simply floor our robot all the time and adjust the motor values with a pid only we want to turn.