Merge pull request #5 from hasofanten/master

Using expected floor normal for more robust floor detection
now right ?
Merge pull request #3 from RoboGroup3/master

Merge yo