Fixed car path finding, implemented one test case
Removed debug code
Merge pull request #36 from mibewh/master

Finished Car pathfinding