Path finding and obstacle avoidance solved. To discuss