Improved path finding decision

Made it so that the regular pathfinding algorithm doesn't always have to
be used on every iteration.