Correct major bug in ev3 api and update algo

Start to implement the memory of the robot to know where it is already
pass