Final wall following code

Final version of the wall following code