Got wall follow working robustly