Added ability to add a waypoint
Added ability to change waypoint