This code works for Wall following AI. If you think it doesn't check the wiring.
Merge branch 'master' of https://github.com/Yosh17/Rasware2013