Fixed obstacle avoidance. Everything now works as needed except for leader selection, which will be implemented next.