diff --git a/BCZT/user/MotorCtrl.c b/BCZT/user/MotorCtrl.c
index de793d9..77451e7 100644
--- a/BCZT/user/MotorCtrl.c
+++ b/BCZT/user/MotorCtrl.c
@@ -212,8 +212,8 @@ void AutoCalCtrl(void)
         MotorArr2state = 1;
         MotorArr3state = 1;
         setMotorState(MOTOR1,ACT_XQ);
-        setMotorState(MOTOR3,ACT_XQ);
-        setMotorState(MOTOR5,ACT_XQ);
+        //setMotorState(MOTOR3,ACT_XQ);
+        //setMotorState(MOTOR5,ACT_XQ);
         autocalcounter[0] = 0;
         autocalcounter[1] = 0;
         autocalcounter[2] = 0;
@@ -284,7 +284,7 @@ void AutoCalCtrl(void)
         }
         if (autocalcounter[0] > 3000 || MotorErr[MOTOR1] != 0)
         {
-            MotorArr1state = 4;
+            MotorArr1state++;
             wait1 = 0;
             setMotorState(MOTOR1,ACT_NOACT);
         }
@@ -342,7 +342,23 @@ void AutoCalCtrl(void)
     //2
      switch (MotorArr2state)
     {
-    case 1://motor3 xq
+    case 1:
+        if (MotorArr1state == 0)
+        {
+            wait2++;
+            if (wait2 > 50)
+            {
+                wait2 = 0;
+                MotorArr2state++;
+                setMotorState(MOTOR3,ACT_XQ);
+            }
+        }
+        else
+        {
+            wait2 = 0;
+        }
+        break;
+    case 2://motor3 xq
         autocalcounter[1]++;
         if (MotorHardStop1[MOTOR3] != 0)
         {
@@ -352,12 +368,12 @@ void AutoCalCtrl(void)
         }
         if (autocalcounter[1] > 3000 || MotorErr[MOTOR3] != 0)
         {
-            MotorArr2state = 4;
+            MotorArr2state = 5;
             wait2 = 0;
             setMotorState(MOTOR3,ACT_NOACT);
         }
         break;
-    case 2://wait
+    case 3://wait
         wait2++;
         if (wait2 > 50)
         {
@@ -366,7 +382,7 @@ void AutoCalCtrl(void)
             autocalcounter[1] = 0;
         }
         break;
-    case 3://motor3 xh 
+    case 4://motor3 xh 
         autocalcounter[1]++;
         if (MotorHardStop2[MOTOR3] != 0)
         {
@@ -376,12 +392,12 @@ void AutoCalCtrl(void)
         }
         if (autocalcounter[1] > 3000 || MotorErr[MOTOR3] != 0)
         {
-            MotorArr2state = 4;
+            MotorArr2state++;
             wait2 = 0;
             setMotorState(MOTOR3,ACT_NOACT);
         }
         break;
-    case 4://wait
+    case 5://wait
         wait2++;
         if (wait2 > 50)
         {
@@ -390,7 +406,7 @@ void AutoCalCtrl(void)
             autocalcounter[1] = 0;
         }
         break;
-    case 5://motor4 xq
+    case 6://motor4 xq
         autocalcounter[1]++;
         if (MotorHardStop1[MOTOR4] != 0)
         {
@@ -404,7 +420,7 @@ void AutoCalCtrl(void)
             setMotorState(MOTOR4,ACT_NOACT);
         }
         break;
-    case 6://wait
+    case 7://wait
         wait2++;
         if (wait2 > 50)
         {
@@ -413,7 +429,7 @@ void AutoCalCtrl(void)
             autocalcounter[1] = 0;
         }
         break;
-    case 7:
+    case 8:
         autocalcounter[1]++;
         if (MotorHardStop2[MOTOR4] != 0)
         {
@@ -432,22 +448,38 @@ void AutoCalCtrl(void)
     }
     switch (MotorArr3state)
     {
-    case 1://motor5 xq
+    case 1:
+        if (MotorArr2state == 0)
+        {
+            wait3++;
+            if (wait3 > 50)
+            {
+                wait3 = 0;
+                MotorArr3state++;
+                setMotorState(MOTOR5,ACT_XQ);
+            }
+        }
+        else
+        {
+            wait3 = 0;
+        }
+        break;
+    case 2://motor5 xq
         autocalcounter[2]++;
         if (MotorHardStop1[MOTOR5] != 0)
         {
             MotorArr3state++;
             wait3 = 0;
-	    setMotorState(MOTOR5,ACT_NOACT);
+	        setMotorState(MOTOR5,ACT_NOACT);
         }
         if (autocalcounter[2] > 3000 || MotorErr[MOTOR5] != 0)
         {
-            MotorArr3state = 4;
+            MotorArr3state = 5;
             wait3 = 0;
             setMotorState(MOTOR5,ACT_NOACT);
         }
         break;
-    case 2://wait
+    case 3://wait
         wait3++;
         if (wait3 > 50)
         {
@@ -456,7 +488,7 @@ void AutoCalCtrl(void)
             autocalcounter[2] = 0;
         }
         break;
-    case 3://motor5 xh 
+    case 4://motor5 xh 
         autocalcounter[2]++;
         if (MotorHardStop2[MOTOR5] != 0)
         {
@@ -466,12 +498,12 @@ void AutoCalCtrl(void)
         }
         if (autocalcounter[2] > 3000 || MotorErr[MOTOR5] != 0)
         {
-            MotorArr3state = 4;
+            MotorArr3state = 5;
             wait3 = 0;
             setMotorState(MOTOR5,ACT_NOACT);
         }
         break;
-    case 4://wait
+    case 5://wait
         wait3++;
         if (wait3 > 50)
         {
@@ -480,7 +512,7 @@ void AutoCalCtrl(void)
             autocalcounter[2] = 0;
         }
         break;
-    case 5://motor6 xq
+    case 6://motor6 xq
         autocalcounter[2]++;
         if (MotorHardStop1[MOTOR6] != 0)
         {
@@ -494,7 +526,7 @@ void AutoCalCtrl(void)
             setMotorState(MOTOR6,ACT_NOACT);
         }
         break;
-    case 6://wait
+    case 7://wait
         wait3++;
         if (wait3 > 50)
         {
@@ -503,7 +535,7 @@ void AutoCalCtrl(void)
             autocalcounter[2] = 0;
         }
         break;
-    case 7:
+    case 8:
         autocalcounter[2]++;
         if (MotorHardStop2[MOTOR6] != 0)
         {
@@ -759,10 +791,14 @@ void MotorCtrl(void)//10ms
         MotorState[MOTOR5] = ACT_NOACT;
         MotorState[MOTOR6] = ACT_NOACT;
     }
-
     for (i = 0; i < 6; i++)
     {
         MotorStateReal[i] = MotorState[i];
+    }
+    
+    for (i = 0; i < 6; i++)
+    {
+        
         if (MotorState[i] == ACT_NOACT  && MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0 )
         {
             if (MotorTarget[i]!=0 && MotorHardStop1[i] > MotorTarget[i] && MotorTarget[i] > MotorHardStop2[i])
@@ -770,10 +806,12 @@ void MotorCtrl(void)//10ms
                 if (MotorTarget[i] > MotorHallLoc[i]+10)
                 {
                     MotorStateReal[i] = ACT_XQ;
+                    break;
                 }
                 else if (MotorTarget[i] < MotorHallLoc[i]-10)
                 {
                     MotorStateReal[i] = ACT_XH;
+                    break;
                 }
                 else
                 {
@@ -792,9 +830,7 @@ void MotorCtrl(void)//10ms
             {
                 MotorStateReal[i] = ACT_NOACT;
             }
-            
         }
-        
         else
         {
             MotorTarget[i] = 0;