Merge pull request #11 from Stinktier/master

Added the Msgs
added parameter heightCorrection and rectangelPadding
 Debug messages + bugfixing. Still not determined the position
Merge pull request #6 from RoboGroup3/master

update my code