Initial algorithm to code
Added directional dependent readings;temp move to .cpp for ease of edit
Added placeholders for missing functions
Merge branch 'master' into singleUnobstructed
Added in goHome fn
Setting up test framework