Added wall follow

Added wall following. Still needs some work.
fix merge conflict, uncomment commands