Much faster init
Removed some cout
fixes ipa320/ipa_canopen#45 for the groovy_dev branch
fixes ipa320/ipa_canopen#47 for the groovy_dev branch
Fixes homing for the groovy_dev
Merge branch 'groovy_dev' of https://github.com/thiagodefreitas/ipa_canopen into groovy_dev