Removed empty duplicate moveToWaypoint function