Issues with obstacle avoidance code