diff --git a/src/main.cpp b/src/main.cpp
index 1af44f2..21f9b7b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -29,18 +29,26 @@ Module_GRBL _GRBL = Module_GRBL(STEPMOTOR_I2C_ADDR);
  * @param mode mode de fonctionnement (defaut = distance) absolute
  */
 void init(int speed, double pas, int accel ,String mode = "distance"){
+    char s[1024];
+
     _GRBL.Init(&Wire);
     _GRBL.setMode(mode);
-    Serial.print("3");
-    char* s = "";
-    Serial.print("4");
-    printf(s,"$0=%d", pas);  // step/mm
-    Serial.print("5");
+
+    sprintf(s,"$0=%f", pas);  // step/mm
     _GRBL.sendGcode(s);
-    printf(s,"$4=%f", speed);  // speed
+    Serial.println(s);
+
+    sprintf(s,"$4=%d", speed);  // speed
     _GRBL.sendGcode(s);
-    printf(s,"$8=%d", pas);  // acceleration, mm/sec^2
+    Serial.println(s);
+
+    sprintf(s,"$8=%d", accel);  // acceleration, mm/sec^2
     _GRBL.sendGcode(s);
+    Serial.println(s);
+
+    sprintf(s,"$3=%d", 500);  // puse/µsec
+    _GRBL.sendGcode(s);
+    Serial.println(s);
 }
 
 void setup() {
@@ -62,8 +70,8 @@ void setup() {
 }
 
 void mouveForward(int mm = 5){
-    char* s = "";
-    printf(s, "G1 X%d", mm);
+    char s[1024];
+    sprintf(s, "G1 X%d", mm);
     _GRBL.sendGcode(s);
 }
 
@@ -71,13 +79,11 @@ void loop() {
     if (M5.BtnA.wasPressed())
     {
         Serial.print(_GRBL.readStatus());
-        _GRBL.setMotor(5, 5, 5, 200);
-        _GRBL.setMotor(0, 0, 0, 200);
+        mouveForward(50);
     }
 
     if (M5.BtnB.wasPressed()) {
-        _GRBL.sendGcode("G1 X5Y5Z5");
-        _GRBL.sendGcode("G1 X0Y0Z0");
+        mouveForward(50);
     }
 
     if (M5.BtnC.wasReleased()) {