PLAN1 测试OK

app/user/MotorCtrl.c

@@ -140,7 +140,7 @@ void setMotorState(uint8_t motorid,uint8_t act)
         MotorState[motorid] = act;
         if (act!=ACT_NOACT)
         {
-            for (i = 0; i < 4; i++)
+            for (i = 0; i < 3; i++)
             {
                 MotorTarget[i] = 0;
             }
@@ -158,6 +158,10 @@ void setMotorTarget(uint8_t motorid,uint16_t target)
     MotorTarget[motorid] = target;
 }
  
+ uint16_t getMotorTarget(uint8_t motorid)
+ {
+    return MotorTarget[motorid];
+ }
 
 
 
@@ -320,7 +324,7 @@ void MotorMemoryKeyM3Release(void)
     {
         flagMotorMemoryKeyStart = 0;
         buzzer_start();
-        for (i = 0; i < 6; i++)
+        for (i = 0; i < 3; i++)
         {
             if (MotorHardStop1[i]!=0 && MotorHardStop2[i]!=0)
             {
@@ -332,7 +336,7 @@ void MotorMemoryKeyM3Release(void)
     }
     else
     {
-        for (i = 0; i < 6; i++)
+        for (i = 0; i < 3; i++)
         {
             setMotorTarget(i,MemoryLoc[2][i]);
         }
@@ -413,11 +417,11 @@ void MotorCtrl(void)//10ms
             MotorHardStop2[3] = MotorHallLoc[3];
         }
         MotorState[MOTOR3] = ACT_NOACT;
-        MotorState[MOTOR4] = ACT_NOACT;
+        //MotorState[MOTOR4] = ACT_NOACT;
     }
 
 
-    for (i = 0; i < 4; i++)
+    for (i = 0; i < 3; i++)
     {
         MotorStateReal[i] = MotorState[i];
         if (MotorState[i] == ACT_NOACT  && MotorHardStop1[i] != 0 && MotorHardStop2[i] != 0 )

app/user/MotorCtrl.h

@@ -26,6 +26,9 @@ void TP_act(void);
 
 void setMotorState(uint8_t motorid,uint8_t act);
 uint8_t getMotorState(uint8_t motorid);
+void setMotorTarget(uint8_t motorid,uint16_t target);
+uint16_t getMotorTarget(uint8_t motorid);
+
 
 
 void MotorMemoryKeyM1Press(void);

app/user/appTask.c

@@ -352,7 +352,7 @@ void fw_tp_ctrl(void)//50ms
     if (fw_tp_pressed)
     {
         fw_tp_cnt++;
-        if (fw_tp_cnt >= 100)//5s
+        if (fw_tp_cnt >= 60)//5s
         {
             fw_tp_cnt = 0;
             fw_tp_pressed = 0;
@@ -373,27 +373,42 @@ void fw_tp_ctrl(void)//50ms
     case FW_TP_STATE_START:
         if (fw_tp_switch)
         {
-            TP_act();
+            FW_act();
         }
         else
         {
-            FW_act();
+            TP_act();
         }
         fw_tp_switch = !fw_tp_switch;
         fw_tp_stop = 0;
+        fw_tp_state = FW_TP_STATE_RUNNING;
         break;
     case FW_TP_STATE_RUNNING:
-        if (fw_tp_stop == 1)
+        if (fw_tp_stop == 1 || fw_tp_pressed == 1)
         {
             fw_tp_state = FW_TP_STATE_STOP;
             fw_tp_stop = 0;
+            fw_tp_pressed = 0;
         }
+        if (getMotorTarget(MOTOR1) == 0 &&
+            getMotorTarget(MOTOR2) == 0 &&
+            getMotorTarget(MOTOR3) == 0 )
+        {
+            fw_tp_state = FW_TP_STATE_STOP;
+        }
+        
         
         break;
     case FW_TP_STATE_STOP:
-        setMotorState(MOTOR1,ACT_NOACT);
+        fw_tp_cnt = 0;
-        setMotorState(MOTOR2,ACT_NOACT);
+        fw_tp_pressed = 0;
-        setMotorState(MOTOR3,ACT_NOACT);
+        //setMotorState(MOTOR1,ACT_NOACT);
+        //setMotorState(MOTOR2,ACT_NOACT);
+        //setMotorState(MOTOR3,ACT_NOACT);
+        setMotorTarget(MOTOR1,0);
+        setMotorTarget(MOTOR2,0);
+        setMotorTarget(MOTOR3,0);
+
         fw_tp_state = FW_TP_STATE_IDLE;
         break;
 
@@ -412,6 +427,7 @@ void js_ctrl(void)//50ms
 {
     static uint8_t js_state=JS_STATE_IDLE;
     static uint16_t js_running_cnt;
+    static uint8_t motor1_flag = 0;
     switch (js_state)
     {
     case JS_STATE_IDLE:
@@ -447,8 +463,9 @@ void js_ctrl(void)//50ms
     case JS_STATE_START:
 #if M26B_PLAN == PLAN1
         //setMotorState(MOTOR1,ACT_XQ);
+        motor1_flag = 0;
         setMotorState(MOTOR2,ACT_XQ);
-        setMotorState(MOTOR3,ACT_XH);
+        setMotorState(MOTOR3,ACT_XQ);
 #else
         gotoJSLoc();
 #endif
@@ -457,9 +474,10 @@ void js_ctrl(void)//50ms
         break;
     case JS_STATE_RUNNING:
 #if M26B_PLAN == PLAN1
-        if (getMotorState(MOTOR2) == ACT_NOACT)
+        if (getMotorState(MOTOR2) == ACT_NOACT && motor1_flag == 0)
         {
             setMotorState(MOTOR1,ACT_XQ);
+            motor1_flag = 1;
         }
 #endif        
         js_running_cnt++;
@@ -467,11 +485,11 @@ void js_ctrl(void)//50ms
         {
             MOTOR4Ctrl(ACT_XQ);
         }
-        else if (js_running_cnt >= 40 && js_running_cnt < 840)//2-42s
+        else if (js_running_cnt >= 40 && js_running_cnt < 1000)//2-50s
         {
             MOTOR4Ctrl(ACT_NOACT);
         }
-        else if (js_running_cnt >= 840 && js_running_cnt < 880)//42-44s
+        else if (js_running_cnt >= 1000 && js_running_cnt < 1020)//50-52s
         {
             MOTOR4Ctrl(ACT_XH);
         }
@@ -482,7 +500,9 @@ void js_ctrl(void)//50ms
         }
         if (js_pressed == 1)
         {
-            js_state = JS_STATE_STOP;
+            js_pressed = 0;
+            js_running_cnt = 999;
+            //js_state = JS_STATE_STOP;
             setMotorState(MOTOR1,ACT_NOACT);
             setMotorState(MOTOR2,ACT_NOACT);
             setMotorState(MOTOR3,ACT_NOACT);

app/user/hwCtrl.c

@@ -134,9 +134,9 @@ uint8_t GetSigState(uint8_t keyno)
     case SIGID_HALL_HG:
         return getIOstate(IOID_P61);
     case SIGID_HALL_ZD:
-        return getIOstate(IOID_P62);
-    case SIGID_HALL_TT:
         return getIOstate(IOID_P63);
+    case SIGID_HALL_TT:
+        return getIOstate(IOID_P62);
     default:
         return 0;
     }