New Invention AI logic for finding a path

Only been tested between two floors with no walls