Step 2 : some refactoring and test on robot.