cleaned up the code a little AND changed the final waypoint speed from 1500 to S_LOW.

Signed-off-by: rkburnside <richard@burnsides.org>