Merge pull request #1 from antoneri/master

Some refactoring
Merge pull request #1 from robotverkstan/master

Merge pull request #1 from antoneri/master