Added obstacle avoidance, replaced swap! with reset!, general refactoring.