Obstacle avoidance implemented. Triggered some debugging problems... take care with this commit.
pathFinder fully implemented. "Worst case" runtime: 365ms