[Uberbots]

You should be telling the robot to correct for position over angle when the robot is far away from the waypoint.. As the robot gets closer to its desired position, angular correction has more of an effect over positional correction.

your angular correction should be a PID loop, except your P constant is acually a variable as a function of the distance from the waypoint.