First draft of obstacle avoidance with wall following