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

Step3