Added pathfinding algorithm, sometimes works
Fixed path finding, occasionally bugs out, but doesn't oscillate anymore