diff --git a/stm32/include/KJ2016Tempo.h b/stm32/include/KJ2016Tempo.h
index 55d3222ee469ee3f4762ff11c3ec50beb3f1c753..26f1cb5f03671e1cb47349b2d772a39c2b14cd97 100644
--- a/stm32/include/KJ2016Tempo.h
+++ b/stm32/include/KJ2016Tempo.h
@@ -8,8 +8,9 @@
  */
 class KJ2016Tempo
 {
-    unsigned int LEFT_SERVO_ID;
-    unsigned int RIGHT_SERVO_ID;
+    static const float KJ_INTERAXIS        = 100.f; // To update
+    static const float KJ_WHEEL_DIAMETER   = 60.f;
+    static const float SERVO_MAX_RPM       = 59.f;
 
     public:
         /**
@@ -20,6 +21,9 @@ class KJ2016Tempo
         ~KJ2016Tempo();
 
     private:
+        const unsigned int LEFT_SERVO_ID;
+        const unsigned int RIGHT_SERVO_ID;
+        const float SERVO_SPEED_FACTOR;
 
         void enginesStart();
         void enginesStop();
diff --git a/stm32/src/KJ2016Tempo.cpp b/stm32/src/KJ2016Tempo.cpp
index 75504fb664858ef714f7f1125665e82ab1c0e83f..d63b3f4bf42e2632db0ffc4382863a742701ef62 100644
--- a/stm32/src/KJ2016Tempo.cpp
+++ b/stm32/src/KJ2016Tempo.cpp
@@ -5,7 +5,8 @@
 
 #include "clock.h"
 
-KJ2016Tempo::KJ2016Tempo(unsigned int leftServoID, unsigned int rightServoID): LEFT_SERVO_ID(leftServoID), RIGHT_SERVO_ID(rightServoID)
+KJ2016Tempo::KJ2016Tempo(unsigned int leftServoID, unsigned int rightServoID):
+    LEFT_SERVO_ID(leftServoID), RIGHT_SERVO_ID(rightServoID), SERVO_SPEED_FACTOR((KJ2016Tempo::SERVO_MAX_RPM / 1024.f) * 60.f / (2.f*3.1415))
 {}
 
 KJ2016Tempo::~KJ2016Tempo()
@@ -60,36 +61,35 @@ void KJ2016Tempo::enginesStop()
 
 void KJ2016Tempo::turn90(bool toLeft)
 {
-    const float correctionFactor = 1.0f;
+    static const float alpha = (3.1415 * KJ2016Tempo::KJ_INTERAXIS * KJ2016Tempo::SERVO_SPEED_FACTOR) / (2 * KJ2016Tempo::KJ_WHEEL_DIAMETER);
 
-    const unsigned int servoOne = toLeft?LEFT_SERVO_ID:RIGHT_SERVO_ID;
-    const unsigned int servoTwo = toLeft?RIGHT_SERVO_ID:LEFT_SERVO_ID;
+    unsigned int angularSpeed = 0x0100;
 
-    const unsigned int speed = 0x0100;
+    angularSpeed = (toLeft)?angularSpeed:-angularSpeed;
 
     enginesStart();
 
-    ServosNumeriques::moveAtSpeed(0x0400 - speed, servoOne);
-    ServosNumeriques::moveAtSpeed(0x0400 + speed, servoTwo);
+    ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, LEFT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID);
 
-    Clock::delay(static_cast<unsigned int>( 1/((float)speed) * correctionFactor));
+    Clock::delay(static_cast<unsigned int>( alpha/(float)angularSpeed ));
 
     enginesStop();
 }
 
 void KJ2016Tempo::move(int distance)
 {
-    const float correctionFactor = 1.f;
+    static const float beta = KJ2016Tempo::SERVO_SPEED_FACTOR * 2.f / KJ2016Tempo::KJ_WHEEL_DIAMETER;
 
-    unsigned int speed = 0x0100;
+    unsigned int angularSpeed = 0x0100;
 
-    speed = (distance>0)?speed:-speed;
+    angularSpeed = (distance>0)?angularSpeed:-angularSpeed;
 
     enginesStart();
 
-    ServosNumeriques::moveAtSpeed(0x0400 + speed, LEFT_SERVO_ID);
-    ServosNumeriques::moveAtSpeed(0x0400 + speed, RIGHT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed(0x0400 - angularSpeed, LEFT_SERVO_ID);
+    ServosNumeriques::moveAtSpeed(0x0400 + angularSpeed, RIGHT_SERVO_ID);
 
-    Clock::delay(static_cast<unsigned int>((float)distance / (float)speed * correctionFactor));
+    Clock::delay(static_cast<unsigned int>( beta * (float)distance / (float)angularSpeed ));
     enginesStop();
 }