Added wall avoidance to approach target