Started path finding

Began using the pathfinding alg.  Need to finish and debug.