Add right wall follow algorithm

Updated backwardWallFollow to be able to follow the right wall based on
a boolean flag.