Finished wall follow code. Works like the python version.