[Arduino] Program improved, should work, you have to test if the speed is good because debug with int to string convertion is not working so I'm not sure it's well parsed!