Waypoints

Waypoints for all states (theoretically) work, except for
ASSUME_POSITION