V1.01-005-debug

调试版本，目前方块前定位，需要修改

Machine/FuXiaoWeiMachine/FXWChuantou_HeLian.c

@@ -136,7 +136,7 @@ void CTHL_AutoStepAction(void)
     CTHL_TestLED = !CTHL_TestLED;   
     CTHL_Test = 0;
   }
-  
+  if(CTHL_bHandWork_MODE)CTHL_bHandWork_State=1;else CTHL_bHandWork_State=0;
   if(bRunning)
   {
     switch(CTHL_AutoStep)
@@ -145,14 +145,22 @@ void CTHL_AutoStepAction(void)
       if(dwTickCount >= CTHL_AutoDelay)
       {    
         if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
-        {         
+        {
+
           CTHL_YXC_VAVLE=1;
           CTHL_YFK_VAVLE=1;
-          CTHL_TXM_VAVLE=0;
-          CTHL_AutoStep = 2;
+          CTHL_TXM_VAVLE=0;          
           //设置反转电机的扭矩和速度的设置       
           freq=CTHL_PARAM_FZ_Speed+SERVO_PARAM_SPEED_DIFFER;
-          set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);                    
+          if(CTHL_cZipCnt >=1)
+          {
+            set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMIN,freq,freq,SERVO_TARR_CW); 
+          }
+          else 
+          {
+            set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);
+          }
+          //set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);                    
           Set_Ctrlmode_trans(Y_AXIS,POS_MODE);          
           bCxcAuto=0;
           if(CTHL_cZipCnt > 1)  CTHL_AutoDelay = dwTickCount + 10;//拉头检测时间            
@@ -163,6 +171,7 @@ void CTHL_AutoStepAction(void)
             CTHL_TryCnt = 0;
           } 
           CTHL_GF_Change = 0;
+          CTHL_AutoStep = 2;          
         }
       }
       break;
@@ -486,7 +495,12 @@ void CTHL_AutoStepAction(void)
           if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
           {
             CTHL_TFK_VAVLE=0;
-            CTHL_TD_MotorStep =10;
+            if(!CTHL_bHandWork_MODE)CTHL_TD_MotorStep =10; 
+            else
+            {
+              CTHL_YXC_VAVLE=0;
+              CTHL_AutoDelay = dwTickCount + 1000 ;
+            }
             CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCLD_DELAY;
             CTHL_AutoStep = 18;
           }
@@ -494,51 +508,58 @@ void CTHL_AutoStepAction(void)
       }
       break;  
     case 18:
-      if(!CTHL_CTM_Limit_IN)
-      {        
-        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
-        {
-          CTHL_JLTou_VAVLE = 0;
-          //CTHL_AutoStep1=1;
-          CTHL_AutoStep = 19;          
-         
+      if(!CTHL_bHandWork_MODE)
+      {
+        //if(!CTHL_CTM_Limit_IN)
+        {        
+          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
+          {
+            CTHL_JLTou_VAVLE = 0;
+            //CTHL_AutoStep1=1;
+            CTHL_AutoStep = 19;          
+           
+          }
         }
       }
+      else if(dwTickCount>=CTHL_AutoDelay)
+      {
+         CTHL_MGuo_VAVLE=0;
+         CTHL_CTXM_VAVLE =0; 
+         CTHL_CTXMSSStep=1;
+         CTHL_AutoStep = 19; 
+      }
       break;
     case 19:
       if((CTHL_TD_MotorStep == 0))
-      {   
-        //               if(CTHL_START_IN)
-        {  
-          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
+      { 
+        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
+        {
+          CTHL_AutoStep = 1; 
+          if(CTHL_cStopMode==1 || bSingOneFlg)
           {
-            CTHL_AutoStep = 1; 
-            if(CTHL_cStopMode==1 || bSingOneFlg)
-            {
-              CTHL_cStopMode=2;
-              CTHL_ZhouWantwo=1;
-              CTHL_CTXM_VAVLE =0; 
-            }
-            AddToTal(CTHL_TOTAL_ADDR);    
-            AddToTal(CTHL_TRUST_ALARM_ADDR);                 
-            CalProSP(CTHL_SPEED_ADDR);
-            //公分节那条也不加
-            if((CTHL_cZipCnt<3) && !CTHL_GF_Change) 
-              CTHL_cZipCnt++;
-            if((CTHL_NoLaLian_IN))
-              CTHL_cZipCnt = 0;
-            if(bSingOneFlg || CTHL_bHandWork_MODE)
-            {
-              CTHL_AutoStep = 0;
-              bRunning = 0;
-              bSingOneFlg = 0;
-            } 
-            CTHL_GF_Change = 0;
+            CTHL_cStopMode=2;
+            CTHL_ZhouWantwo=1;
+            CTHL_CTXM_VAVLE =0; 
           }
-        }     
+          AddToTal(CTHL_TOTAL_ADDR);    
+          AddToTal(CTHL_TRUST_ALARM_ADDR);                 
+          CalProSP(CTHL_SPEED_ADDR);
+          //公分节那条也不加
+          if((CTHL_cZipCnt<3) && !CTHL_GF_Change) 
+            CTHL_cZipCnt++;
+          if((CTHL_NoLaLian_IN))
+            CTHL_cZipCnt = 0;
+          if(bSingOneFlg || CTHL_bHandWork_MODE)
+          {
+            CTHL_AutoStep = 0;
+            bRunning = 0;
+            bSingOneFlg = 0;
+            CTHL_bHandWork_MODE=0;
+          } 
+          CTHL_GF_Change = 0;
+        }             
       }
-      break;
-      
+      break;      
     }    
     
     //退下模后启动送拉头
@@ -623,7 +644,7 @@ void CTHL_AutoStepAction(void)
         if(!bSingOneFlg)
         {
           CTHL_CTXM_VAVLE=1;
-          CTHL_CTXMSSStep = 21;
+          CTHL_CTXMSSStep = 22;
         }
         else
         {
@@ -713,7 +734,7 @@ void CTHL_CheckStart(void)
   
 #if 1  
   //启动    自动启动 单一自动	 			
-  if((CTHL_START_IN_UP || bStart || CTHL_bSingOne  ) && !CTHL_wltzi)
+  if((CTHL_START_IN_UP || bStart || CTHL_bSingOne ||CTHL_bHandWork_Run ) && !CTHL_wltzi)
   {    
    
     if(!bRunning)
@@ -726,6 +747,11 @@ void CTHL_CheckStart(void)
         bSingOneFlg=1;
         
       }
+      if(CTHL_bHandWork_Run)
+      {
+        CTHL_bHandWork_MODE=1;
+        CTHL_bHandWork_Run=0;
+      }
       bFZ_LengthOK = 0;
       SetEn(X_AXIS, MOTOR_DISEN);
       SetEn(Y_AXIS, MOTOR_DISEN);
@@ -830,6 +856,8 @@ void CTHL_CheckStart(void)
       Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
       SetEn(X_AXIS, MOTOR_EN);
       SetEn(Y_AXIS, MOTOR_EN);
+      CTHL_bHandWork_MODE=0;
+      CTHL_bHandWork_Run=0;
       
       CTHL_AutoStep = 0; 
       CTHL_stop=0;
@@ -1158,28 +1186,25 @@ void CTHL_TD_Motor(void)	 //
         else
           CTHL_TXM_VAVLE    = 1; 
     }
-    if(!CTHL_bHandWork_MODE && !CTHL_NoLaLian_IN)
+    if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
     {
-      if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
+      //松码钩后输出合链长度
+      if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
       {
-        //松码钩后输出合链长度
-        if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
-        {
-          CTHL_XSavePosBuff2=dwXRealPos;
-          CTHL_HL_VAVLE=1;
-        }
+        CTHL_XSavePosBuff2=dwXRealPos;
+        CTHL_HL_VAVLE=1;
       }
-      else
-      {                  
-        //松码钩后输出合链长度
-        if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
-        {
-          CTHL_XSavePosBuff2=dwXRealPos;
-          CTHL_HL_VAVLE=1;
-        }
-        
-      }  
     }
+    else
+    {                  
+      //松码钩后输出合链长度
+      if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
+      {
+        CTHL_XSavePosBuff2=dwXRealPos;
+        CTHL_HL_VAVLE=1;
+      }
+      
+    }    
     //   方块感应到了之后，最长15公分也要退
     if(((dwXRealPos+1700)>((CTHL_LianLengthAutoFZCRcheck+2700)-CTHL_PARAM_HELIAN_LENGTH)) || (bFKCheck_Flag && (dwXRealPos >= (CTHL_XSavePosBuff3 + 2500))))
     {
@@ -1221,6 +1246,8 @@ void CTHL_TD_Motor(void)	 //
             CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;
             CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld  - dwXRealPos;
           }
+          CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;
+          CTHL_LENTH=CTHL_LianLengthCheck+CTHL_LianLengthAutoFZcheck- dwXRealPos;
           AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH,1000,CTHL_PARAM_FKDW_Speed,250,250,200); 
         }
         else
@@ -1324,28 +1351,24 @@ void CTHL_TD_Motor(void)	 //
     break;   
   case 14:
     CTHL_NoLaLian_Speed_Proc();
-    if(!CTHL_bHandWork_MODE)
+    if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
     {
-      if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
+      //松码钩后输出合链长度
+      if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
       {
-        //松码钩后输出合链长度
-        if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
-        {
-          CTHL_XSavePosBuff2=dwXRealPos;
-          CTHL_HL_VAVLE=1;
-        }
+        CTHL_XSavePosBuff2=dwXRealPos;
+        CTHL_HL_VAVLE=1;
       }
-      else
-      {                  
-        //松码钩后输出合链长度
-        if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
-        {
-          CTHL_XSavePosBuff2=dwXRealPos;
-          CTHL_HL_VAVLE=1;
-        }
-      }  
     }
-    
+    else
+    {                  
+      //松码钩后输出合链长度
+      if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
+      {
+        CTHL_XSavePosBuff2=dwXRealPos;
+        CTHL_HL_VAVLE=1;
+      }
+    }    
     //   方块感应到了之后，最长15公分也要退
     if(((dwXRealPos+1700)>((CTHL_LianLengthAutoFZCRcheck+2700)-CTHL_PARAM_HELIAN_LENGTH)) || (bFKCheck_Flag && (dwXRealPos >= (CTHL_XSavePosBuff3 + 2500))))
     {
@@ -1373,9 +1396,7 @@ void CTHL_TD_Motor(void)	 //
     {
       
       CTHL_TD_MotorStep = 15; 
-      
     } 
-    
     if(CTHL_cZipCnt > 1)
     {
       if(dwXRealPos >= (CTHL_PARAM_WLCDSHEZHI_LENTH+CTHL_LianLengthAutoCheck)) 
@@ -1387,7 +1408,7 @@ void CTHL_TD_Motor(void)	 //
     }
     else
     {
-      if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+ CTHL_LianLengthAutoFZcheck) 
+      if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+ CTHL_LianLengthAutoFZCRcheck) 
       {
         AxisEgmStop(X_AXIS);
         CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
@@ -1397,154 +1418,82 @@ void CTHL_TD_Motor(void)	 //
   case 15:
     CTHL_NoLaLian_Speed_Proc();
     //在最后的几条接链只用下模合链
-    if(!CTHL_bHandWork_MODE && !CTHL_NoLaLian_IN)
+    if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
     {
-      if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
+      //松码钩后输出合链长度
+      if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
       {
-        //松码钩后输出合链长度
-        if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
-        {
-          CTHL_XSavePosBuff2=dwXRealPos;
-          CTHL_HL_VAVLE=1;
-        }
+        CTHL_XSavePosBuff2=dwXRealPos;
+        CTHL_HL_VAVLE=1;
       }
-      else
-      {                  
-        //松码钩后输出合链长度
-        if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
-        {
-          CTHL_XSavePosBuff2=dwXRealPos;
-          CTHL_HL_VAVLE=1;
-        }
-      }  
-    }    
+    }
+    else
+    {                  
+      //松码钩后输出合链长度
+      if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
+      {
+        CTHL_XSavePosBuff2=dwXRealPos;
+        CTHL_HL_VAVLE=1;
+      }
+    }
     //   方块感应到了之后，最长15公分也要退
     if(((dwXRealPos+1700)>((CTHL_LianLengthAutoFZCRcheck+2700)-CTHL_PARAM_HELIAN_LENGTH)) || (bFKCheck_Flag && (dwXRealPos >= (CTHL_XSavePosBuff3 + 2500))))
     {
       CTHL_HL_VAVLE=0;
-    }        
-    
+    }
     //第二次感应方块， 要做定位
     if(CTHL_FK_Check_UP && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1800)))
     {
-      //     user_datas[127] = dwXRealPos;
-      
-      if(!CTHL_bHandWork_MODE)
+      // user_datas[127] = dwXRealPos;
+      if(!CTHL_NoLaLian_IN)
       {
         
-        if(!CTHL_NoLaLian_IN)
-        {
-          
-          CTHL_XSavePosBuff = dwXRealPos;
-          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
-                            1000,1000,30,80,50); 
-          AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_TDDW_LENGTH,
-                            1000,1000,30,80,50);  
-        }
-        if(((dwXRealPos-CTHL_XSavePosBuff3) <= CTHL_PARAM_GFJ_LENGTH) && (CTHL_cZipCnt>1))
-        {
-          CTHL_TD_MotorStep=50;
-          AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15);
-          AxisContinueMoveAcc(Y_AXIS,1600,DIR_P,1000,600,15,15);
-          CTHL_TD_MotorDelay = dwTickCount + 200;
-          bFZ_LengthOK= 0;
-          CTHL_cZipCnt = 0;
-        }
-        else
-        {
-          CTHL_TXM_VAVLE=0;
-          CTHL_TD_MotorStep = 16;
-          if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
-          CTHL_AutoDelay=dwTickCount;
-          /*if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0))
-          {
-            if(dwTickCount >= CTHL_AutoDelay )
-            {
-              if(CTHL_HSLiao_Origin_IN||CTHL_CTM_Minid_IN&&!CTHL_HSLiao_VAVLE)
-              {                
-                //有拉头，推斜码到位不能亮
-                if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)
-                {
-                  if(!CTHL_ZhouWanone && !bSingOneFlg)
-                  {
-                    CTHL_CTXM_VAVLE=1;
-                    
-                    //                                      CTHL_JXM_VAVLE=1;
-                  }
-                }
-                else if(dwTickCount >= (CTHL_AutoDelay+3000))
-                {
-                  CTHL_wltzi=1;
-                  user_datas[127]=2;
-                  CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);  // 没有拉头  
-                }
-              }
-              else if(dwTickCount >= (CTHL_AutoDelay+4000))
-              {
-                CTHL_wltzi=1;
-                CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);//横送料原位异常                         
-              }
-            }
-          }*/
-        }                        
-        bFZ_LengthOK = 1;                               
-        if(CTHL_cZipCnt)CTHL_LianLengthCheckOld = CTHL_LianLengthCheck;
-        CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3; 
-      }  
-      CTHL_XSavePosBuff = dwXRealPos; 
-    }    
-    if(!CTHL_bHandWork_MODE)
-    {
-      if(CTHL_cZipCnt > 1)
+        CTHL_XSavePosBuff = dwXRealPos;
+        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
+                          1000,1000,30,80,50); 
+        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_TDDW_LENGTH,
+                          1000,1000,30,80,50);  
+      }
+      if(((dwXRealPos-CTHL_XSavePosBuff3) <= CTHL_PARAM_GFJ_LENGTH) && (CTHL_cZipCnt>1))
       {
-        if((dwXRealPos >= (CTHL_PARAM_WLCDSHEZHI_LENTH+CTHL_LianLengthAutoCheck)))
-        {
-          AxisEgmStop(X_AXIS);
-          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
-        }
-        
+        CTHL_TD_MotorStep=50;
+        AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15);
+        AxisContinueMoveAcc(Y_AXIS,1600,DIR_P,1000,600,15,15);
+        CTHL_TD_MotorDelay = dwTickCount + 200;
+        bFZ_LengthOK= 0;
+        CTHL_cZipCnt = 0;
       }
-      else if(!CTHL_bHandWork_MODE)
+      else
       {
-        if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+ CTHL_LianLengthAutoFZcheck) 
-        {
-          AxisEgmStop(X_AXIS);
-          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
-        }
-      }    
-    }            
-    break;          
-  case 16:   
-   /* if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0))
+        CTHL_TXM_VAVLE=0;
+        CTHL_TD_MotorStep = 16;
+        if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
+        CTHL_AutoDelay=dwTickCount;
+      }                        
+      bFZ_LengthOK = 1;                               
+      if(CTHL_cZipCnt)CTHL_LianLengthCheckOld = CTHL_LianLengthCheck;
+      CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3; 
+    }  
+    CTHL_XSavePosBuff = dwXRealPos; 
+    if(CTHL_cZipCnt > 1)
     {
-      if(dwTickCount >= CTHL_AutoDelay )
+      if((dwXRealPos >= (CTHL_PARAM_WLCDSHEZHI_LENTH+CTHL_LianLengthAutoCheck)))
       {
-        if(CTHL_HSLiao_Origin_IN||CTHL_CTM_Minid_IN&&!CTHL_HSLiao_VAVLE)
-        {          
-          //有拉头，推斜码到位不能亮
-          if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)
-          {
-            if(!CTHL_ZhouWanone && !bSingOneFlg)
-            {
-              CTHL_CTXM_VAVLE=1;
-              //                          CTHL_JXM_VAVLE=1;
-            }
-          }
-          else if(dwTickCount >= (CTHL_AutoDelay+3000))
-          {
-            CTHL_wltzi=1;
-            user_datas[127]=3;
-            CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);  // 没有拉头  
-          }
-        }
-        else if(dwTickCount >= (CTHL_AutoDelay+4000))
-        {
-          CTHL_wltzi=1;
-          CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);//横送料原位异常                         
-        }
+        AxisEgmStop(X_AXIS);
+        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
       }
-    }*/
-    
+      
+    }
+    else
+    {
+      if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+ CTHL_LianLengthAutoFZCRcheck) 
+      {
+        AxisEgmStop(X_AXIS);
+        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
+      }
+    }         
+    break;          
+  case 16:  
     if((dwXRealPos+1700)>((CTHL_LianLengthAutoFZCRcheck+2700)-CTHL_PARAM_HELIAN_LENGTH))
     {
       CTHL_HL_VAVLE=0;
@@ -1616,11 +1565,8 @@ void CTHL_TD_Motor(void)	 //
       //    CTHL_MGuo_VAVLE=0;
       CTHL_CTXM_VAVLE=0;
       CTHL_TD_MotorStep = 52;  
-      
-      
     }  
     break;
-    
   case 52:  
     if(dwXRealPos>=CTHL_PARAM_GFJ_LENGTH)
     {
@@ -1693,10 +1639,10 @@ void CTHL_FZ_Motor(void)	 //
       SetPos(X_AXIS, 0);  //启动位置设为0点
       SetPos(Y_AXIS, 0);  //启动位置设为0点
       FZ_ALARM_FLG=0;
-      if(CTHL_cZipCnt<1)//前两次走
+      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)//前两次走
       {
-        AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50); 
-        AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED+1500,DIR_N,1500,1000,30,50);
+        AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_FZFirst_SPEED,DIR_N,1000,1000,30,50); 
+        AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FZFirst_SPEED+100,DIR_N,1500,1000,30,50);
       }
       else 
       {
@@ -1709,13 +1655,13 @@ void CTHL_FZ_Motor(void)	 //
     }
     break;
   case 2:
-    if(abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100){//提前转换模式
+    if((abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100)&&!CTHL_NoLaLian_IN&&!CTHL_bHandWork_MODE){//提前转换模式
       Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
     }
     if(abs(dwXRealPos) >= CTHL_PARAM_XMCR_SLOW_LENGTH)//穿链结束，开始走链加速
     {
       CTHL_FZ_MotorDelay = dwTickCount+0;
-      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
+      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)
       {                    
         AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100);
         AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100);
@@ -1732,19 +1678,24 @@ void CTHL_FZ_Motor(void)	 //
       CTHL_FZ_MotorStep++;      
     }
     //电机速度差太大
-    if(abs(dwXRealPos)>dwYRealPos_com+4500)
+    if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!CTHL_bHandWork_MODE)
     {
-      CTHL_FZ_MotorStep=100;
+       FZ_ALARM_FLG=1;
+       CTHL_FZ_MotorStep=100;
+       CTHL_XSavePosBuff1=dwYRealPos_com;
+       AxisEgmStop(X_AXIS);
+       AxisEgmStop(Y_AXIS);
       //bCxcAuto=0;//停轴
       //SetEn(Y_AXIS, MOTOR_DISEN);
       SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
-      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机卡滞   
+      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
+      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机  
     }
     break;
   case 3:
     if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
     if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10 ) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0;
-    if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
+    if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)
     {
       CTHL_FZ_MotorStep++; 
     }
@@ -1760,7 +1711,7 @@ void CTHL_FZ_Motor(void)	 //
       }
     }
     //长度超出报警
-    if(CTHL_cZipCnt>=1)
+    if(CTHL_cZipCnt>=1&&!CTHL_bHandWork_MODE)
     {
       if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
       {
@@ -1778,21 +1729,25 @@ void CTHL_FZ_Motor(void)	 //
     }
     else
     {
-      if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){  //位置大于第一条拉链长度    
+      if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length&&!CTHL_bHandWork_MODE){  //位置大于第一条拉链长度    
         bCxcAuto=1;//停轴
         SetEn(Y_AXIS, MOTOR_DISEN);
         CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);   //穿入方块超出预设长度                  
       }
     }
-    FZ_ALARM_FLG=0;
     //电机速度差太大
-    if(abs(dwXRealPos)>dwYRealPos_com+4500)
+    if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!CTHL_bHandWork_MODE)
     {
-        CTHL_FZ_MotorStep=100;
+       FZ_ALARM_FLG=1;
+       CTHL_FZ_MotorStep=100;
+       CTHL_XSavePosBuff1=dwYRealPos_com;
+       AxisEgmStop(X_AXIS);
+       AxisEgmStop(Y_AXIS);
       //bCxcAuto=0;//停轴
       //SetEn(Y_AXIS, MOTOR_DISEN);
       SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
-      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机   
+      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
+      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机  
     }
     break;
   case 4:
@@ -1801,7 +1756,7 @@ void CTHL_FZ_Motor(void)	 //
     if((abs(dwXRealPos) > 500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿
     {               
       CTHL_XSavePosBuff1 = abs(dwXRealPos);
-      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN){//记录前两条的方块感应长度；
+      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE){//记录前两条的方块感应长度；
         //停机距离调整
         CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;
         AxisMovePosAccDec(X_AXIS,1000,-CTHL_LENTH,
@@ -1822,7 +1777,7 @@ void CTHL_FZ_Motor(void)	 //
     else
     {
         //长度超出报警
-        if(CTHL_cZipCnt>=1)
+        if(CTHL_cZipCnt>=1&&!CTHL_bHandWork_MODE)
         {
           if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
           {
@@ -1840,13 +1795,13 @@ void CTHL_FZ_Motor(void)	 //
         }
         else
         {
-          if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){  //位置大于第一条拉链长度    
+          if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length&&!CTHL_bHandWork_MODE){  //位置大于第一条拉链长度    
             bCxcAuto=1;//停轴
             CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);   //穿入方块超出预设长度                  
           }
         }
         //电机速度差太大
-        if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!FZ_ALARM_FLG)
+        if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!FZ_ALARM_FLG&&!CTHL_bHandWork_MODE)
         {
            FZ_ALARM_FLG=1;
            CTHL_FZ_MotorStep=100;
@@ -1868,9 +1823,10 @@ void CTHL_FZ_Motor(void)	 //
       {
         CTHL_LianLengthCheck = abs(dwXRealPos);   //为第一条长度给值
         CTHL_LianLengthAutoFZCRcheck = abs(dwXRealPos);      
-        CTHL_LianLengthAutoFZcheck=abs(dwXRealPos);
+        //CTHL_LianLengthAutoFZcheck=abs(dwXRealPos);
       }
-      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);  
+      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
+      CTHL_LianLengthAutoFZcheck=abs(dwXRealPos);
       CTHL_FZ_MotorStep = 0; 
     } 
     break;
@@ -1906,7 +1862,7 @@ void CTHL_FZ_Motor(void)	 //
     }
     else
     {
-      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
+      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)
       {
          AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED/2,DIR_N,1000,1000,30,50); 
          AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50);
@@ -2661,7 +2617,6 @@ void CTHL_ManualAction(void)
     //X轴Y轴电机测试 
     if(CTHL_bCLMotor_N)
     { 
-      
       SetEn(Y_AXIS, MOTOR_DISEN);
       SetEn(X_AXIS, MOTOR_DISEN);
       CTHL_FKDW_VAVLE=0;
@@ -2670,23 +2625,25 @@ void CTHL_ManualAction(void)
         SetPos(X_AXIS, 0); 
         SetPos(Y_AXIS, 0); 
       }
-      
-      if(!X_DRV)
+      if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE)&&(!CTHL_FKGY_IN||CTHL_FKGY_IN&&CTHL_YXC_VAVLE&&!CTHL_YFK_VAVLE))
       {
         //                Y轴       运行速度                    启动速度        加速度  减速度                 
         AxisContinueMoveAcc(X_AXIS,600 ,DIR_N,600,600,15,15);
         AxisContinueMoveAcc(Y_AXIS,580,DIR_N,580,580,15,15); 
-      }      
+      } 
+      if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE||CTHL_FKGY_IN&&CTHL_YXC_VAVLE&&CTHL_YFK_VAVLE)
+      {
+        AxisEgmStop(Y_AXIS);
+        AxisEgmStop(X_AXIS);
+      }
     }
-    
     //在压方块电磁阀输出情况下，方块感应后电机要自动停止
-    if(CTHL_FKGY_IN && CTHL_YFK_VAVLE && CTHL_bCLMotor_P)
+    if((CTHL_XCDW_Limit_IN )&& CTHL_bCLMotor_P)
     {
       CTHL_bCLMotor_P=0;
       AxisEgmStop(Y_AXIS);
       AxisEgmStop(X_AXIS);
     }
-    
     if(CTHL_bCLMotor_P)  //后退限位已经取消
     {
       SetEn(Y_AXIS, MOTOR_DISEN);
@@ -2698,39 +2655,21 @@ void CTHL_ManualAction(void)
         SetPos(Y_AXIS, 0); 
       }
       //压方块电磁阀没有输出时，方块感应信号不限制
-      if(!CTHL_FKGY_IN || !CTHL_YFK_VAVLE)
+      if(!CTHL_XCDW_Limit_IN)
       {
-        if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE))
+        if(!X_DRV )
         {  
           //                Y轴       运行速度                    启动速度        加速度  减速度
           AxisContinueMoveAcc(Y_AXIS,630,DIR_P,630,630,15,15);
           AxisContinueMoveAcc(X_AXIS,600,DIR_P,600,600,15,15);
-        }
-        
-        if(CTHL_FKGY_IN && CTHL_YFK_VAVLE)
-        {
-          CTHL_bCLMotor_P=0;
-          AxisEgmStop(Y_AXIS);
-          AxisEgmStop(X_AXIS);
-        }
-        
-      }
-      
-      if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE)
-      {
-        AxisEgmStop(Y_AXIS);
-        AxisEgmStop(X_AXIS);
-        
-      }
-      
+        } 
+      } 
     }
-    
     if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0))
     {
       AxisEgmStop(Y_AXIS);
       AxisEgmStop(X_AXIS);
     }
-    
   }        
 }
 

Machine/FuXiaoWeiMachine/FXWChuantou_HeLian.h

@@ -114,7 +114,9 @@ extern void ExtiAcitionX02(void);
 
 #define cHELIANMG                   cCharBuff11
 #define cBAOjin                     cCharBuff12
-#define CTHL_CTXMSS_FLAG            cCharBuff13       
+#define CTHL_CTXMSS_FLAG            cCharBuff13  
+
+#define CTHL_bHandWork_MODE          cCharBuff14
 //位标志位
 #define bTuiLaTouOkFlg              bBitFlag0
 #define bZhuangLiaoOkFlg            bBitFlag1
@@ -222,6 +224,8 @@ extern void ExtiAcitionX02(void);
 #define CTHL_PARAM_XCCRBH_LENGTH             (user_datas[71]) //小插插入保护长度
 #define CTHL_PARAM_JMDW_LENGTH             (user_datas[72]) //前码定位距离
 
+#define CTHL_PARAM_SDDZ_STATE             (user_datas[75]) //手动状态
+
 //速度页面调试参数
 #define CTHL_PARAM_First_SPEED                  user_datas[150] //前两条速度
 #define CTHL_PARAM_Stop_length                  user_datas[151] //前两条定位长度
@@ -326,13 +330,13 @@ extern void ExtiAcitionX02(void);
 
 #define CTHL_bSingOne       M0020     //单循环
 #define CTHL_bDANBU_MODE              M0021      //单步调试
-#define CTHL_bHandWork_MODE              M0022      //手动穿链模式
-
+#define CTHL_bHandWork_Run              M0022      //手动穿链模式
+#define CTHL_bHandWork_State            M0023      //手动穿链模式
 
 #define CTHL_bAutoZhuangLiao        M0024       //自动装料
 
-#define CTHL_bCLMotor_P         M0025       //X轴电机测试
-#define CTHL_bCLMotor_N         M0026       //X轴电机测试
+#define CTHL_bCLMotor_P         M0026       //X轴电机测试
+#define CTHL_bCLMotor_N         M0025       //X轴电机测试
 #define CTHL_bCLMotor_Z        M0027       //Z轴电机测试
 #define CTHL_bBaoJin          M0028       //报警清除加松轴
 #define CTHL_bYJDW             M0029     //过链杆压带

Project/settings/Project.dnx

@@ -179,7 +179,7 @@
         <Bp0>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\YingXingMachine\YXTWOChuantou_HeLian.c}.811.1" 0 0 1 "" 0 "" 0</Bp0>
         <Bp1>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\YingXingMachine\YXTWOChuantou_HeLian.h}.96.1" 0 0 1 "" 0 "" 0</Bp1>
         <Bp2>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\YingXingMachine\YXTWOChuantou_HeLian.c}.845.1" 0 0 1 "" 0 "" 0</Bp2>
-        <Bp3>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\FuXiaoWeiMachine\FXWChuantou_HeLian.h}.350.1" 0 0 1 "" 0 "" 0</Bp3>
+        <Bp3>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\FuXiaoWeiMachine\FXWChuantou_HeLian.h}.353.1" 0 0 1 "" 0 "" 0</Bp3>
         <Bp4>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\User\Action.c}.2.14" 0 0 1 "" 0 "" 0</Bp4>
         <Bp5>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\ChuanChiMachine\CCChuantou_HeLian.c}.57.1" 0 0 1 "" 0 "" 0</Bp5>
         <Bp6>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\ChuanChiMachine\CCChuantou_HeLian.c}.625.1" 0 0 1 "" 0 "" 0</Bp6>
@@ -187,7 +187,7 @@
         <Bp8>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\YingXingMachine\YXChuantou_HeLian.c}.137.1" 0 0 1 "" 0 "" 0</Bp8>
         <Bp9>_ 0 "EMUL_CODE" "{$PROJ_DIR$\..\Machine\ChangShengGeiLiMachine\GLChuantou_HeLian.c}.1834.1" 0 0 1 "" 0 "" 0</Bp9>
         <Bp10>_ 1 "EMUL_DATA" "get_encode_value" 0 0 0 0 4294967295</Bp10>
-        <Bp11>_ 1 "EMUL_CODE" "{$PROJ_DIR$\..\User\servocom_app.c}.619.5" 0 0 1 "" 0 "" 0</Bp11>
+        <Bp11>_ 1 "EMUL_CODE" "{$PROJ_DIR$\..\User\servocom_app.c}.621.5" 0 0 1 "" 0 "" 0</Bp11>
         <Count>12</Count>
     </Breakpoints2>
     <Aliases>

User/servocom_app.c

@@ -149,6 +149,7 @@ int clr_com_servo_alarm(unsigned short axis)
   case X_AXIS:
     if(dwTickCount>=clr_com_servo_x_delay)
     {
+       servo_x.alarmrest=1;       
        clr_com_servo_x_delay=dwTickCount+1000;
        servo_com_cmd(axis,ALARM_REST);
     }
@@ -156,6 +157,7 @@ int clr_com_servo_alarm(unsigned short axis)
   case Y_AXIS:
     if(dwTickCount>=clr_com_servo_y_delay)
     {
+       servo_y.alarmrest=1; 
        servo_com_cmd(axis,ALARM_REST);
        clr_com_servo_y_delay=dwTickCount+1000;
     }