Added obstacle avoidance code to master