Much faster init
Removed some cout
fixes ipa320/ipa_canopen#45 for the groovy_dev branch
Merge pull request #44 from thiagodefreitas/groovy_dev

Fixes init bug
Merge branch 'groovy_dev' of github.com:ipa320/ipa_canopen into groovy