Wall Following Controller refactored. New version not tried yet