Waypoints implemented

Waypoints implemented