Toggle waypoints implemented
Merge pull request #39 from sli-fox/steph_branch

Toggle waypoints implemented