Testing Pathfinding

Tested pathfinding on longer map and tried tweaking the algorithm
slightly