Basically working obstacle avoidance