         initState = 1;
     }
 
-
-    int delta = 150;
-    if (initState == 1 && initTimestamp + delta <= time && initSpeed == 255)
+    uint8_t delta = 150;
+    if (initState == 1 && initTimestamp + delta*2 <= time)
+    {
+        motor1_RPMCount = 0;
+        motor2_RPMCount = 0;
+        motor3_RPMCount = 0;
+
+        initTimestamp = time;
+       initState = 2;
+    }
+
+
+    if (initState == 2 && initTimestamp + delta <= time && initSpeed == 255)
     {
         motor1_MaxRPM = motor1_RPMCount;
         motor2_MaxRPM = motor2_RPMCount;
         motor3_MaxRPM = motor3_RPMCount;
     }
 
-    if (initState == 1 && initTimestamp + delta <= time && initSpeed > 0)
-    {
-        SpeedMapping_Motor1[initSpeed] = 255 / motor1_MaxRPM * motor1_RPMCount;
-        SpeedMapping_Motor2[initSpeed] = 255 / motor2_MaxRPM * motor2_RPMCount;
-        SpeedMapping_Motor3[initSpeed] = 255 / motor3_MaxRPM * motor3_RPMCount;
+    if (initState == 2 && initTimestamp + delta <= time && initSpeed > 0)
+    {
+        //SpeedMapping_Motor1[initSpeed] = 255 * motor1_RPMCount / motor1_MaxRPM;
+        //SpeedMapping_Motor2[initSpeed] = 255 * motor2_RPMCount / motor2_MaxRPM;
+        //SpeedMapping_Motor3[initSpeed] = 255 * motor3_RPMCount / motor3_MaxRPM;
+
+       if(SpeedMapping_Motor1[255 * motor1_RPMCount / motor1_MaxRPM] == 0)
+       {
+             SpeedMapping_Motor1[255 * motor1_RPMCount / motor1_MaxRPM] = initSpeed;
+       }
+
+       if(SpeedMapping_Motor2[255 * motor2_RPMCount / motor2_MaxRPM] == 0)
+       {
+             SpeedMapping_Motor2[255 * motor2_RPMCount / motor2_MaxRPM] = initSpeed;
+       }
+
+       if(SpeedMapping_Motor3[255 * motor3_RPMCount / motor3_MaxRPM] == 0)
+       {
+             SpeedMapping_Motor3[255 * motor3_RPMCount / motor3_MaxRPM] = initSpeed;
+       }
+
 
         // 12:  255 - 245 - 235 - 225 - 215 - 205 - 195 - 185 - 175 - 165 - 155 - 145
         if (initSpeed > 150)


void Calibration()
{
    if (initState >= 2)
    {
        return;
    }

    int time = millis();

    if(initState == 0)
    {

    }


    if (initState == 2 && initSpeed == 0)
    {
        initState = 3;
        for(int i = 0; i < SpeedMappingSize; i++)
        {
            Serial.print(i);
