Finished wall follow code. Works like the python version.
Implemented steering trimming in the controller class, works. Also added a directory for testing code.