Added linear interpolation code to waypoint selection.