Allowed minSpeed to equal 0, added getNextWaypoint and updated diagram