* added files for wall following demoä

* got wallfollowing working on latest firmware

* code cleanup

* move files to source folder

* fixed unlocking bug

* variable renaming and states handling with switch case

* formatting

* make the wall following state machine file truely seperate from firmware

* fixed some hardcoded values