Very buggy obstacle avoidance working