Step 2 : some refactoring and test on robot.
Merge pull request #2 from fabriceambert/step2

Step 2 : some refactoring and test on robot.