Issues with obstacle avoidance code! Please help!!!