&ACCESS RVO
&REL 1
DEF MainModule()
   ;FOLD INI
      ;FOLD BASISTECH INI
         GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS == TRUE DO IR_STOPM()
         INTERRUPT ON 3
      ;ENDFOLD
      ;FOLD USER INI
         ;Make your modifications here
      ;ENDFOLD
   ;ENDFOLD
   
 $CYCFLAG[1]=(I_GripSafty1 == FALSE) OR (I_GripSafty2==FALSE)
 INTERRUPT DECL 24 WHEN $CYCFLAG[1] DO StopSafty()
 INTERRUPT ON 24

   ;FOLD PTP $POS_ACT Tool[1] Base[1]
      BAS(#INITMOV, 0)
      BAS(#BASE, 1)
      BAS(#TOOL, 1)
      PTP $POS_ACT
   ;ENDFOLD
   $ov_pro=40
   
   LOOP
      AutoRun()
   ENDLOOP
END

DEF AutoRun()
   IF I_ProductType <> PdtTypeLast THEN
      MsgNotify("PdtType is Change", "SafeErr")
      Q_Error1=TRUE
      $FLAG[10]=TRUE
      HALT
   ENDIF
   PdtTypeLast=I_ProductType   
   WAIT FOR (I_Cmd > 0) AND (I_CmdCount <> Q_CmdCountRet) AND (Q_Cmd_Running==0)
   CmdSave = I_Cmd
   CmdCountLast=I_CmdCount
   Q_Cmd_Running=CmdSave
   PickOffsetX_Save=GetInt16(I_PickOffsetX)
   PickOffsetY_Save=GetInt16(I_PickOffsetY)
   PickOffsetZ_Save=GetInt16(I_PickOffsetZ)
   PlaceOffsetX_Save=GetInt16(I_PlaceOffsetX)
   PlaceOffsetY_Save=GetInt16(I_PlaceOffsetY)
   PlaceOffsetZ_Save=GetInt16(I_PlaceOffsetZ)
   CountX_Save=I_CountX
   CountY_Save=I_CountY

   SWITCH CmdSave       
      CASE 1
         FeedBelt1Pick()
      CASE 2
         FeedBelt2Pick()            
      CASE 3
         ReFeedBeltPick()    
      CASE 4
         WaterBeltPick()    
      CASE 5
         NgBeltPlace()    
      CASE 10
         Pallet1Place()
      CASE 11
         Pallet2Place()
   ENDSWITCH        
   
END


DEF StopSafty()
   INTERRUPT OFF 24
   BRAKE F
   BAS(#BASE, 1)
   BAS(#TOOL, 1)
   LIN_REL {Z 20}
   INTERRUPT ON 24
END