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