Wall following works
Merge pull request #25 from RC93/master

Wall following working