Refactored obstacle avoidance