Obstacle Avoidance v1

Created obstacle avoidance logic