&ACCESS RO16
&REL 9
&COMMENT BASIS package
DEF  BAS (COMMAND :IN,REAL_PAR :IN )
;FOLD BAS () 
  DECL BAS_COMMAND COMMAND
  REAL REAL_PAR

  SWITCH  COMMAND
  CASE #INITMOV
    INITMOV ( )
  CASE #ACC_CP
    ACC_CP (REAL_PAR )
  CASE #ACC_GLUE
    ACC_GLUE (REAL_PAR )
  CASE #ACC_PTP
    ACC_PTP (REAL_PAR )
  CASE #VEL_CP
    VEL_CP (REAL_PAR )
  CASE #VEL_PTP
    VEL_PTP (REAL_PAR )
  CASE #TOOL
    TOOL (REAL_PAR )
  CASE #BASE
    BASE (REAL_PAR )
  CASE #EX_BASE
    EX_BASE (REAL_PAR, 0 )
  CASE #FRAMES
    FRAMES ( )
    TQMDETECTION ( )
  CASE #CP_DAT
    CP_DAT ( )
  CASE #PTP_DAT
    PTP_DAT ( )
  CASE #OUT_SYNC
    OUT_SYNC ( )
  CASE #OUT_ASYNC
    OUT_ASYNC ( )
  CASE #GROUP
    GROUP (REAL_PAR )
  CASE #PTP_PARAMS
    PTP_DAT ( )
    FRAMES ( )
    VEL_PTP (REAL_PAR )
    TQMDETECTION ( )
  CASE #CP_PARAMS
    CP_DAT ( )
    FRAMES ( )
    VEL_CP (REAL_PAR )
    TQMDETECTION ( )
  ENDSWITCH
END
;ENDFOLD BAS ()
;FOLD INITMOV () 
DEF  INITMOV ( )
  INT I

  ; PTP-Parameters:
  ACC_PTP (100.0 ) ; ptp acceleration 100% = DEF_ACC_PTP
  VEL_PTP (100.0 ) ; ptp velocity 100% = DEF_VEL_PTP

  ; CP-Parameters:
  ACC_CP (100.0 ) ; path acceleration 100%  =  DEF_ACC_CP
  $ACC.ORI1=DEF_ACC_ORI1 ; swivel acceleration
  $ACC.ORI2=DEF_ACC_ORI2 ; rotation acceleration

  VEL_CP (DEF_VEL_CP ) ; path velocity 
  $VEL.ORI1=DEF_VEL_ORI1 ; swivel velocity
  $VEL.ORI2=DEF_VEL_ORI2 ; rotation velocity

  ; Parameters for Approximation:
  $APO.CPTP=DEF_APO_CPTP ; PTP-Approximation
  $APO.CDIS=DEF_APO_CDIS ; Distance-Approximation
  $APO.CVEL=DEF_APO_CVEL ; Speed-Approximation
  $APO.CORI=DEF_APO_CORI ; Orientation-Approximation

  $FILTER=$DEF_FLT_PTP ; filter
  $ADVANCE=DEF_ADVANCE ; advance
  $ORI_TYPE=#VAR ; variable orientation
  $CIRC_TYPE=DEF_CIRC_TYP ; CIRC-orientation defined in $config.dat
  $JERK=DEF_JERK_STRUC ; JERK-structure defined in $config.dat
  TOOL_CORR=$NULLFRAME
  TOOL_CORR_ON=FALSE
  M_BASE_CORR=$NULLFRAME
  M_BAS_COR_ON=FALSE
  TOOL_CORR_W=$NULLFRAME
  TOOL_CORR_W_ON=FALSE
  BASE_CORR=$NULLFRAME
  BASE_CORR_ON=FALSE
  
  IF $KINCLASS<>#NONE THEN
    BASE (0 ) ; Activating Base $WORLD
    $TOOL=TOOL_DATA[1]
    $ACT_TOOL=1
  ENDIF

  IF $TCP_IPO==TRUE THEN
    $IPO_MODE=#BASE
  ENDIF

  IF $ADAP_ACC<>#NONE THEN
    IF LOAD_A1_DATA.M<0 THEN
      $LOAD_A1.M=0
      $LOAD_A1.CM={X 0.0, Y 0.0, Z 0.0, A 0.0, B 0.0, C 0.0}
      $LOAD_A1.J={X 0.0, Y 0.0, Z 0.0}
    ELSE
      $LOAD_A1.M=LOAD_A1_DATA.M
      $LOAD_A1.CM=LOAD_A1_DATA.CM
      $LOAD_A1.J=LOAD_A1_DATA.J
    ENDIF
    IF LOAD_A2_DATA.M<0 THEN
      $LOAD_A2.M=0
      $LOAD_A2.CM={X 0.0, Y 0.0, Z 0.0, A 0.0, B 0.0, C 0.0}
      $LOAD_A2.J={X 0.0, Y 0.0, Z 0.0}
    ELSE
      $LOAD_A2.M=LOAD_A2_DATA.M
      $LOAD_A2.CM=LOAD_A2_DATA.CM
      $LOAD_A2.J=LOAD_A2_DATA.J
    ENDIF
    IF LOAD_A3_DATA.M<0 THEN
      $LOAD_A3.M=$DEF_LA3_M
      $LOAD_A3.CM=$DEF_LA3_CM
      $LOAD_A3.J=$DEF_LA3_J
    ELSE
      $LOAD_A3.M=LOAD_A3_DATA.M
      $LOAD_A3.CM=LOAD_A3_DATA.CM
      $LOAD_A3.J=LOAD_A3_DATA.J
      IF (($LOAD_A3.M<>0) AND ($LOAD_A3.J.X==0) AND ($LOAD_A3.J.Y==0) AND ($LOAD_A3.J.Z==0)) THEN
        $LOAD_A3.J=$DEF_LA3_J
      ENDIF
    ENDIF
  ENDIF

  ; standard group definition
  COMPL_GROUP=1
  FOR I=1 TO  $EX_AX_NUM
    COMPL_GROUP=COMPL_GROUP*2+1
  ENDFOR
  $ACT_GROUP=COMPL_GROUP

  ; torque monitoring
  IF $ADAP_ACC<>#NONE THEN
    FOR I=1 TO  6
      $TORQMON[I]=$TORQMON_DEF[I]
    ENDFOR
  ENDIF
  TQM_INI ()

  STOPM_FLAG=FALSE ;reflects state of interrupt 3
END
;ENDFOLD INITMOV ()
;FOLD BASE ()
DEF BASE (BASE_NO :IN )
  INT BASE_NO

  IF (BASE_NO<=0) THEN
    IF BASE_CORR_ON AND M_BAS_COR_ON THEN
      $BASE=BASE_CORR:$WORLD:M_BASE_CORR
    ELSE
      IF M_BAS_COR_ON THEN
        $BASE=$WORLD:M_BASE_CORR
      ELSE
        IF BASE_CORR_ON THEN
          $BASE=BASE_CORR:$WORLD
        ELSE
          $BASE=$WORLD
        ENDIF
      ENDIF
    ENDIF
  ELSE
    IF BASE_CORR_ON AND M_BAS_COR_ON THEN
      $BASE=BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR
    ELSE
      IF M_BAS_COR_ON THEN
        $BASE=BASE_DATA[BASE_NO]:M_BASE_CORR
      ELSE
        IF BASE_CORR_ON THEN  
          $BASE=BASE_CORR:BASE_DATA[BASE_NO]
        ELSE
          $BASE=BASE_DATA[BASE_NO]
        ENDIF
      ENDIF
    ENDIF
  ENDIF

  CONTINUE
  $ACT_BASE=BASE_NO ; Storing actual Base-Number in global variable
  CONTINUE
  $ACT_EX_AX=-1
END
;ENDFOLD BASE ()
;FOLD EX_BASE ()
DEF  EX_BASE (BASE_NO :IN , MACH_IDX : IN)
  INT BASE_NO, MACH_IDX

  IF BASE_NO > 0 THEN
    IF BASE_CORR_ON AND M_BAS_COR_ON THEN
      $BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR)
    ELSE
      IF M_BAS_COR_ON THEN
        $BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_DATA[BASE_NO]:M_BASE_CORR)
      ELSE
        IF BASE_CORR_ON THEN  
          $BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_CORR:BASE_DATA[BASE_NO])
        ELSE
          $BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_DATA[BASE_NO])
        ENDIF
      ENDIF
    ENDIF    
  ENDIF
    
    CONTINUE
    $ACT_BASE=BASE_NO
END
;ENDFOLD EX_BASE ()
;FOLD LK_BASE ()
DEF  LK_BASE (MACH_IDX :IN, FRAME_IDX : IN, TOOL_IDX : IN )
  INT MACH_IDX, FRAME_IDX, TOOL_IDX

  IF FRAME_IDX>-1 THEN
    ; Linking to a frame - end-of-arm tool
    IF BASE_CORR_ON AND M_BAS_COR_ON THEN
      $BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_CORR:BASE_DATA[FRAME_IDX]:M_BASE_CORR, MACHINE_DEF[MACH_IDX].MECH_TYPE)
    ELSE
      IF M_BAS_COR_ON THEN
        $BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_DATA[FRAME_IDX]:M_BASE_CORR, MACHINE_DEF[MACH_IDX].MECH_TYPE)
      ELSE
        IF BASE_CORR_ON THEN
          $BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_CORR:BASE_DATA[FRAME_IDX], MACHINE_DEF[MACH_IDX].MECH_TYPE)
        ELSE
          $BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_DATA[FRAME_IDX], MACHINE_DEF[MACH_IDX].MECH_TYPE)
        ENDIF
      ENDIF
    ENDIF
  ELSE
    ; Linking to a tool - remote tool
    $BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], TOOL_DATA[TOOL_IDX], MACHINE_DEF[MACH_IDX].MECH_TYPE)
  ENDIF
  
  CONTINUE
  $ACT_EX_AX=-1
  CONTINUE
  $ACT_BASE=FRAME_IDX
END
;ENDFOLD LK_BASE ()
;FOLD EB_BASE ()
DEF EB_BASE (MACH_IDX :IN, BASE_NO :IN)
  INT MACH_IDX, BASE_NO

  IF BASE_CORR_ON AND M_BAS_COR_ON THEN
    $BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR,MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX)
  ELSE
    IF M_BAS_COR_ON THEN
      $BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO]:M_BASE_CORR,MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX)
    ELSE
      IF BASE_CORR_ON THEN
        $BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO],MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX)
      ELSE
        $BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO],MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX)
      ENDIF
    ENDIF  
  ENDIF
  
  CONTINUE
  $ACT_BASE=BASE_NO
END
;ENDFOLD EB_BASE ()
;FOLD ACC_CP ()
DEF  ACC_CP (ACC_FACTOR :IN )
  REAL ACC_FACTOR ; [%]

  IF ACC_FACTOR>100.0 THEN
    $ACC.CP=DEF_ACC_CP
    $ACC.ORI1=DEF_ACC_ORI1
    $ACC.ORI2=DEF_ACC_ORI2
  ELSE
    $ACC.CP=DEF_ACC_CP*ACC_FACTOR/100.0
    $ACC.ORI1=DEF_ACC_ORI1*ACC_FACTOR/100.0
    $ACC.ORI2=DEF_ACC_ORI2*ACC_FACTOR/100.0
  ENDIF
END
;ENDFOLD ACC_CP ()
;FOLD ACC_GLUE ()
DEF  ACC_GLUE (ACC_FACTOR :IN )
  REAL ACC_FACTOR ; [%]

  IF ACC_FACTOR>100.0 THEN
    $ACC.CP=$ACC_MA.CP
    $ACC.ORI1=$ACC_MA.ORI1
    $ACC.ORI2=$ACC_MA.ORI2
  ELSE
    $ACC.CP=$ACC_MA.CP*ACC_FACTOR/100.0
    $ACC.ORI1=$ACC_MA.ORI1*ACC_FACTOR/100.0
    $ACC.ORI2=$ACC_MA.ORI2*ACC_FACTOR/100.0
  ENDIF
END
;ENDFOLD ACC_GLUE ()
;FOLD ACC_PTP ()
DEF  ACC_PTP (ACC_FACTOR :IN )
  REAL ACC_FACTOR ; [%]
  REAL ACC
  INT I

  IF $ADAP_ACC==#NONE THEN
    IF ACC_FACTOR>100.0 THEN
      ACC=DEF_ACC_PTP
    ELSE
      ACC=DEF_ACC_PTP*ACC_FACTOR/100.0
    ENDIF
  ELSE
    ACC=ACC_FACTOR
  ENDIF

  FOR I=1 TO  $NUM_AX
    $ACC_AXIS[I]=ACC
  ENDFOR

  FOR I=1 TO  $EX_AX_NUM
    $ACC_EXTAX[I]=ACC / 2
  ENDFOR
END
;ENDFOLD ACC_PTP ()
;FOLD VEL_CP ()
DEF  VEL_CP (VEL_VALUE :IN )
  REAL VEL_VALUE, VEL_CHECK ; [m/s]
  INT I

  IF VEL_VALUE > DEF_VEL_CP THEN
    $VEL.CP=DEF_VEL_CP
  ELSE
    $VEL.CP=VEL_VALUE*DEF_VEL_FACT
  ENDIF

  ON_ERROR_PROCEED
  VEL_CHECK = $VEL.ORI1
  IF $ERR.NUMBER > 0 THEN
      $VEL.ORI1=DEF_VEL_ORI1 ; swivel velocity
      ERR_CLEAR($ERR)
  ENDIF

  ON_ERROR_PROCEED
  VEL_CHECK = $VEL.ORI2
  IF $ERR.NUMBER > 0 THEN
      $VEL.ORI2=DEF_VEL_ORI2 ; rotation velocity
      ERR_CLEAR($ERR)
  ENDIF
  
;    IF VEL_Value > 100.0 THEN
      $VEL_EXTAX[1]=DEF_VEL_PTP 
;    ELSE
;      $VEL_EXTAX[1]=VEL_VALUE
;    ENDIF

END
;ENDFOLD VEL_CP ()
;FOLD VEL_PTP ()
DEF  VEL_PTP (VEL_FACTOR :IN )
  REAL VEL_FACTOR ; [%]
  REAL VEL
  INT I

  IF VEL_FACTOR>100.0 THEN
    VEL=DEF_VEL_PTP
  ELSE
    VEL=DEF_VEL_PTP*VEL_FACTOR/100.0
  ENDIF

  FOR I=1 TO  $NUM_AX
    $VEL_AXIS[I]=VEL
  ENDFOR

 ; FOR I=1 TO  $EX_AX_NUM
 ;   IF VEL > 100 THEN
 ;     $VEL_EXTAX[I]=DEF_VEL_PTP
      $VEL_EXTAX[1]=DEF_VEL_PTP
 ;   ELSE
 ;     $VEL_EXTAX[I]=VEL
 ;   ENDIF
 ; ENDFOR
END
;ENDFOLD VEL_PTP ()
;FOLD TOOL ()
DEF  TOOL (TOOL_NO :IN )
  INT TOOL_NO

  IF (TOOL_NO<=0) THEN
    IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
      $TOOL=TOOL_CORR_W:TOOL_CORR
    ELSE
      IF TOOL_CORR_ON THEN
        $TOOL=TOOL_CORR
      ELSE
        IF TOOL_CORR_W_ON THEN
          $TOOL=TOOL_CORR_W
        ELSE
          $TOOL=$NULLFRAME
        ENDIF
      ENDIF
    ENDIF
        
    IF $ADAP_ACC<>#NONE THEN
      $LOAD.M=$DEF_L_M
      $LOAD.CM=$DEF_L_CM
      $LOAD.J=$DEF_L_J
    ENDIF
  ELSE
    IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
      $TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]:TOOL_CORR
    ELSE
      IF TOOL_CORR_ON THEN
        $TOOL=TOOL_DATA[TOOL_NO]:TOOL_CORR
      ELSE
        IF TOOL_CORR_W_ON THEN
          $TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]
        ELSE
          $TOOL=TOOL_DATA[TOOL_NO]
        ENDIF
      ENDIF
    ENDIF
    IF $ADAP_ACC<>#NONE THEN
      IF LOAD_DATA[TOOL_NO].M<0 THEN
        $LOAD.M=$DEF_L_M
        $LOAD.CM=$DEF_L_CM
        $LOAD.J=$DEF_L_J
      ELSE
        $LOAD.M=LOAD_DATA[TOOL_NO].M
        $LOAD.CM=LOAD_DATA[TOOL_NO].CM
        $LOAD.J=LOAD_DATA[TOOL_NO].J
        IF (($LOAD.M<>0) AND ($LOAD.J.X==0) AND ($LOAD.J.Y==0) AND ($LOAD.J.Z==0)) THEN
          $LOAD.J=$DEF_L_J
        ENDIF
      ENDIF
    ENDIF
  ENDIF

  CONTINUE
  $ACT_TOOL=TOOL_NO
END
;ENDFOLD TOOL ()
;FOLD FRAMES ()
DEF  FRAMES ( )
  INT MACH_INDEX, COOP_KRC_INDEX
  DECL ESYS MECH_TYPE
  DECL MSG_T IllegalUseOfRemoteTool
  BOOL Result

  IF STOPM_FLAG==TRUE THEN ;no correct finish of IR_STOPMESS
    STOPM_FLAG=FALSE ;reflects state of interrupt 3
    INTERRUPT ON 3
  ENDIF   

  ; Set the $COLL_LOAD and $COLL_TARGETLOAD
  IF ($COLLISAVOID == TRUE) THEN
    IF (FDAT_ACT.TOOL_NO <> 0) THEN
      Result = STRCOPY($COLL_LOAD[], MACHINE_TOOL_DAT[FDAT_ACT.TOOL_NO].GEOMETRY[])
    ELSE
      Result = STRCLEAR($COLL_LOAD[])
    ENDIF
    IF (FDAT_ACT.BASE_NO <> 0) THEN
      Result = STRCOPY($COLL_TARGETLOAD[], MACHINE_FRAME_DAT[FDAT_ACT.BASE_NO].GEOMETRY[])
    ELSE
      Result = STRCLEAR($COLL_TARGETLOAD[])
    ENDIF
  ENDIF

  ; Special case: Base=0
  IF (FDAT_ACT.BASE_NO == 0) THEN
    TOOL (FDAT_ACT.TOOL_NO)
    BASE (0)
    SET_IPO_MODE (FDAT_ACT.IPO_FRAME )
    RETURN
  ENDIF

  IF (FDAT_ACT.TOOL_NO <> 0) THEN
    IF (MACHINE_TOOL_DAT[FDAT_ACT.TOOL_NO].MACH_DEF_INDEX <> 0) THEN
      IF (MACHINE_DEF[MACHINE_TOOL_DAT[FDAT_ACT.TOOL_NO].MACH_DEF_INDEX].COOP_KRC_INDEX <> 1) THEN
        IllegalUseOfRemoteTool={VALID FALSE,RELEASE FALSE,TYP #QUIT,MODUL[] "BAS",KEY[] "IllegalUseOfRemoteTool",PARAM_TYP #VALUE,PARAM[] " ",DLG_FORMAT[] " ",ANSWER 0 }
        BAS_MSG (IllegalUseOfRemoteTool)
        RETURN
      ENDIF
    ENDIF
  ENDIF

  ;Set IPO_MODE, $TOOL and $LOAD
  SET_IPO_MODE (FDAT_ACT.IPO_FRAME )
  TOOL (FDAT_ACT.TOOL_NO )

  ;Get the frame's machine index, COOP_KRC index and mechanism type
  MACH_INDEX = MACHINE_FRAME_DAT[FDAT_ACT.BASE_NO].MACH_DEF_INDEX
  IF (MACH_INDEX > 0) Then
    MECH_TYPE = MACHINE_DEF[MACH_INDEX].MECH_TYPE
    COOP_KRC_INDEX = MACHINE_DEF[MACH_INDEX].COOP_KRC_INDEX
  ELSE
    MECH_TYPE = #NONE
    COOP_KRC_INDEX = 0
  ENDIF
    
  IF (MECH_TYPE == #ROBOT) THEN
    IF (COOP_KRC_INDEX > 1) THEN
      LK_BASE (MACH_INDEX, FDAT_ACT.BASE_NO, -1 )
    ELSE
      IF (COOP_KRC_INDEX == 1) THEN
        BASE (FDAT_ACT.BASE_NO )
      ENDIF
    ENDIF
  ELSE
    IF (MECH_TYPE == #NONE) THEN
      IF (FDAT_ACT.BASE_NO <= MAX_BASE) THEN
        BASE (FDAT_ACT.BASE_NO )
      ENDIF
    ELSE
      ; The base is relative to an external axis - need to determine if it is a local
      ; external axis or a remote one and call EK/LK appropriately.  A COOP_KRC_INDEX
      ; of 1 signifies it is a local external axis.
      IF (MECH_TYPE == #CONVEYOR) THEN
        EB_BASE(MACH_INDEX, FDAT_ACT.BASE_NO)
      ELSE
        IF (COOP_KRC_INDEX == 1) THEN
          EX_BASE (FDAT_ACT.BASE_NO, MACH_INDEX )
        ELSE
          LK_BASE (MACH_INDEX, FDAT_ACT.BASE_NO, -1 )
        ENDIF
      ENDIF
    ENDIF
  ENDIF
END
;ENDFOLD FRAMES ()
;FOLD PTP_DAT ()
DEF  PTP_DAT ( )
  DECL APO_MODE_T MODE_CHECK
  VEL_PTP (PDAT_ACT.VEL )
  ACC_PTP (PDAT_ACT.ACC )
  ON_ERROR_PROCEED
  MODE_CHECK = PDAT_ACT.APO_MODE
  IF $ERR.NUMBER == 0 THEN
    IF PDAT_ACT.APO_MODE==#CDIS THEN
      $APO.CDIS=PDAT_ACT.APO_DIST
    ELSE
      $APO.CPTP=PDAT_ACT.APO_DIST
    ENDIF
  ELSE
    $APO.CPTP=PDAT_ACT.APO_DIST
    ERR_CLEAR($ERR)
  ENDIF
END
;ENDFOLD PTP_DAT ()
;FOLD CP_DAT ()
DEF  CP_DAT ( )
  DECL ORI_TYPE ORI_CHECK
  VEL_CP (LDAT_ACT.VEL )
  ACC_CP (LDAT_ACT.ACC )
  $APO.CDIS=LDAT_ACT.APO_DIST
  ON_ERROR_PROCEED
  ORI_CHECK = LDAT_ACT.ORI_TYP
  IF $ERR.NUMBER == 0 THEN
    $ORI_TYPE=LDAT_ACT.ORI_TYP
  ELSE
    $ORI_TYPE=#VAR
    ERR_CLEAR($ERR)
  ENDIF
END
;ENDFOLD CP_DAT ()
;FOLD OUT_ASYNC ()
DEF  OUT_ASYNC ( )
  SWITCH  ODAT_ACT.OUT_MODE

  CASE #CONT_
    IF ODAT_ACT.PULSE_TIME>0 THEN
      CONTINUE
      PULSE ($OUT[ODAT_ACT.OUT_NO],ODAT_ACT.STATE,ODAT_ACT.PULSE_TIME )
    ELSE
      CONTINUE
      $OUT[ODAT_ACT.OUT_NO]=ODAT_ACT.STATE
    ENDIF

  CASE #STOP_
    IF ODAT_ACT.PULSE_TIME>0 THEN
      PULSE ($OUT[ODAT_ACT.OUT_NO],ODAT_ACT.STATE,ODAT_ACT.PULSE_TIME )
    ELSE
      $OUT[ODAT_ACT.OUT_NO]=ODAT_ACT.STATE
    ENDIF
  ENDSWITCH
END
;ENDFOLD OUT_ASYNC ()
;FOLD OUT_SYNC ()
DEF  OUT_SYNC ( )
  SWITCH  ODAT_ACT.OUT_MODE

  CASE #TRIGGER_
    IF ODAT_ACT.PULSE_TIME>0 THEN
      CONTINUE
      PULSE ($OUT[ODAT_ACT.OUT_NO],ODAT_ACT.STATE,ODAT_ACT.PULSE_TIME )
    ELSE
      CONTINUE
      $OUT[ODAT_ACT.OUT_NO]=ODAT_ACT.STATE
    ENDIF
  ENDSWITCH
END
;ENDFOLD OUT_SYNC ()
;FOLD GROUP ()
DEF  GROUP (GROUP_NO :IN )
  INT GROUP_NO
  $ACT_GROUP=DEF_GROUP[GROUP_NO]
END
;ENDFOLD GROUP ()
;FOLD SET_IPO_MODE ()
DEF  SET_IPO_MODE (IPO_FRAME :IN )
  DECL IPO_MODE IPO_FRAME
  IF $IPO_MODE==IPO_FRAME THEN
    RETURN 
  ELSE
    $IPO_MODE=IPO_FRAME
  ENDIF
END
;ENDFOLD SET_IPO_MODE ()
;FOLD TQMDETECTION ()
DEF TQMDETECTION ()
  BOOL STATE_CHECK
  ON_ERROR_PROCEED
  STATE_CHECK = FDAT_ACT.TQ_STATE
  IF $ERR.NUMBER == 0 THEN
    IF FDAT_ACT.TQ_STATE == FALSE THEN
      TQM_FIFO_READ(TM0)
    ENDIF
  ELSE
    TQM_FIFO_READ(TM0)
    ERR_CLEAR($ERR)
  ENDIF
END
;ENDFOLD TQMDETECTION ()
;FOLD SET_TQ_VALUES ()
GLOBAL DEF SET_TQ_VALUES ( TQ_AX1:IN, TQ_AX2:IN, TQ_AX3:IN, TQ_AX4:IN, TQ_AX5:IN, TQ_AX6:IN)
  INT I, TQ_AX1, TQ_AX2, TQ_AX3, TQ_AX4, TQ_AX5, TQ_AX6
  ON_ERROR_PROCEED
  $TORQMON[1]=TQ_AX1
  IF $ERR.NUMBER == 0 THEN
    $TORQMON[2]=TQ_AX2
    $TORQMON[3]=TQ_AX3
    $TORQMON[4]=TQ_AX4
    $TORQMON[5]=TQ_AX5
    $TORQMON[6]=TQ_AX6
  ELSE
    FOR I=1 TO 6    
      $TORQMON[I]=$TORQMON_DEF[I]
    ENDFOR  
    ERR_CLEAR($ERR)
  ENDIF
END
;ENDFOLD SET_TQ_VALUES ()
;FOLD BAS_MSG ()
GLOBAL DEF BAS_MSG (MESS_T:OUT)
  DECL MSG_T MESS_T
  DECL MSG_T EMPTY_MSG

  EMPTY_MSG={VALID FALSE,RELEASE FALSE,TYP #NOTIFY,MODUL[] " ",KEY[] " ",PARAM_TYP #VALUE,PARAM[] " ",DLG_FORMAT[] " ", ANSWER 0} 

  $MSG_T=MESS_T
  $MSG_T.VALID=TRUE

  WHILE $MSG_T.VALID
    WAIT SEC 0.05
  ENDWHILE

  WAIT SEC 0.2
  $MSG_T=EMPTY_MSG
  WAIT SEC 0.2
END
;ENDFOLD BAS_MSG ()
;FOLD {H}
GLOBAL DEFFCT CP SVEL_CP (CP_VALUE :IN, ORI_VALUE :IN )
  REAL CP_VALUE
  REAL ORI_VALUE
  DECL CP RET_VEL
  IF CP_VALUE>DEF_VEL_CP THEN
    RET_VEL.CP=DEF_VEL_CP
  ELSE
    RET_VEL.CP=CP_VALUE
  ENDIF
  RET_VEL.ORI1=DEF_VEL_ORIS
  RETURN RET_VEL
ENDFCT
GLOBAL DEFFCT CP SACC_CP (PARAMS :IN)
  DECL LDAT PARAMS	
  DECL CP RET_ACC
  IF PARAMS.ACC>100.0 THEN
    RET_ACC.CP=DEF_ACC_CP
    RET_ACC.ORI1=DEF_ACC_ORIS
  ELSE
    RET_ACC.CP=DEF_ACC_CP*PARAMS.ACC/100.0
    RET_ACC.ORI1=DEF_ACC_ORIS*PARAMS.ACC/100.0
  ENDIF
  RETURN RET_ACC
ENDFCT
GLOBAL DEFFCT CONT SAPO (PARAMS :IN)
  DECL LDAT PARAMS
  DECL CONT RET_APO
  RET_APO.CDIS=PARAMS.APO_DIST
  RET_APO.CPTP=DEF_APO_CPTP
  RET_APO.CORI=DEF_APO_CORIS
  RETURN RET_APO
ENDFCT
GLOBAL DEFFCT FRAME STOOL (TOOL_NO :IN)
  INT TOOL_NO
  DECL FRAME RET_TOOL
  IF (TOOL_NO<=0) THEN
    IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
      RET_TOOL=TOOL_CORR_W:TOOL_CORR
    ELSE
      IF TOOL_CORR_ON THEN
        RET_TOOL=TOOL_CORR
      ELSE
        IF TOOL_CORR_W_ON THEN
          RET_TOOL=TOOL_CORR_W
        ELSE
          RET_TOOL=$NULLFRAME
        ENDIF
      ENDIF
    ENDIF
  ELSE
    IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
      RET_TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]:TOOL_CORR
    ELSE
      IF TOOL_CORR_ON THEN
        RET_TOOL=TOOL_DATA[TOOL_NO]:TOOL_CORR
      ELSE
        IF TOOL_CORR_W_ON THEN
          RET_TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]
        ELSE
          RET_TOOL=TOOL_DATA[TOOL_NO]
        ENDIF
      ENDIF
    ENDIF
  ENDIF
  RETURN RET_TOOL
ENDFCT
GLOBAL DEFFCT LOAD SLOAD (TOOL_NO :IN)
  INT TOOL_NO
  DECL LOAD RET_LOAD
  IF (TOOL_NO<=0) THEN
    IF $ADAP_ACC<>#NONE THEN
      RET_LOAD.M=$DEF_L_M
      RET_LOAD.CM=$DEF_L_CM
      RET_LOAD.J=$DEF_L_J
      RETURN RET_LOAD
    ENDIF
  ELSE
    IF $ADAP_ACC<>#NONE THEN
      IF LOAD_DATA[TOOL_NO].M<0 THEN
        RET_LOAD.M=$DEF_L_M
        RET_LOAD.CM=$DEF_L_CM
        RET_LOAD.J=$DEF_L_J
        RETURN RET_LOAD
      ELSE
        RET_LOAD.M=LOAD_DATA[TOOL_NO].M
        RET_LOAD.CM=LOAD_DATA[TOOL_NO].CM
        RET_LOAD.J=LOAD_DATA[TOOL_NO].J
        IF ((RET_LOAD.M<>0) AND (RET_LOAD.J.X==0) AND (RET_LOAD.J.Y==0) AND (RET_LOAD.J.Z==0)) THEN
          RET_LOAD.J=$DEF_L_J
        ENDIF
        RETURN RET_LOAD
      ENDIF
    ENDIF
  ENDIF
  RETURN $LOAD 
ENDFCT
GLOBAL DEFFCT FRAME SBASE (BASE_NO :IN)
  INT BASE_NO
  DECL FRAME RET_BASE
  IF (BASE_NO<=0) THEN
    IF BASE_CORR_ON AND M_BAS_COR_ON THEN
      RET_BASE=BASE_CORR:$WORLD:M_BASE_CORR
    ELSE
      IF M_BAS_COR_ON THEN
        RET_BASE=$WORLD:M_BASE_CORR
      ELSE
        IF BASE_CORR_ON THEN
          RET_BASE=BASE_CORR:$WORLD
        ELSE
          RET_BASE=$WORLD
        ENDIF
      ENDIF
    ENDIF
  ELSE
    IF BASE_CORR_ON AND M_BAS_COR_ON THEN
      RET_BASE=BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR
    ELSE
      IF M_BAS_COR_ON THEN
        RET_BASE=BASE_DATA[BASE_NO]:M_BASE_CORR
      ELSE
        IF BASE_CORR_ON THEN  
          RET_BASE=BASE_CORR:BASE_DATA[BASE_NO]
        ELSE
          RET_BASE=BASE_DATA[BASE_NO]
        ENDIF
      ENDIF
    ENDIF
  ENDIF
  RETURN RET_BASE
ENDFCT
GLOBAL DEFFCT IPO_MODE SIPO_MODE (IPO_FRAME :IN)
  DECL IPO_MODE IPO_FRAME
  RETURN IPO_FRAME
ENDFCT
GLOBAL DEFFCT ORI_TYPE SORI_TYP (PARAMS :IN)
  DECL LDAT PARAMS
  RETURN PARAMS.ORI_TYP
ENDFCT
GLOBAL DEFFCT CIRC_TYPE SCIRC_TYP (PARAMS :IN)
  DECL LDAT PARAMS
  RETURN PARAMS.CIRC_TYP
ENDFCT
GLOBAL DEFFCT CIRC_MODE SCIRC_M (PARAMS :IN)
  DECL LDAT PARAMS
  RETURN PARAMS.CB
ENDFCT
GLOBAL DEFFCT JERK_STRUC SJERK (PARAMS :IN )
  DECL JERK_STRUC RET_JERK
  DECL LDAT PARAMS
  REAL J_FACTOR, J_C0, J_F0, J_D3
  IF PARAMS.JERK_FAC>=100.0 THEN
    RET_JERK.CP=$JERK_MA.CP
    RET_JERK.ORI=$JERK_MA.ORI
    RET_JERK.AX=$JERK_MA.AX
    RETURN RET_JERK
  ELSE
    J_F0=0.05
    J_C0=0.50
    J_FACTOR=PARAMS.JERK_FAC/100.0
    IF J_FACTOR<=J_C0 THEN
      J_FACTOR=J_F0/J_C0*J_FACTOR
    ELSE
      J_D3=(J_FACTOR-J_C0)/(1-J_C0)
      J_D3=J_D3*J_D3*J_D3
      J_FACTOR=J_F0/J_C0*J_FACTOR + (J_C0-J_F0)*J_D3/J_C0
    ENDIF
    RET_JERK.CP=$JERK_MA.CP*J_FACTOR
    RET_JERK.ORI=$JERK_MA.ORI*J_FACTOR
    RET_JERK.AX.A1=$JERK_MA.AX.A1*J_FACTOR
    RET_JERK.AX.A2=$JERK_MA.AX.A2*J_FACTOR
    RET_JERK.AX.A3=$JERK_MA.AX.A3*J_FACTOR
    RET_JERK.AX.A4=$JERK_MA.AX.A4*J_FACTOR
    RET_JERK.AX.A5=$JERK_MA.AX.A5*J_FACTOR
    RET_JERK.AX.A6=$JERK_MA.AX.A6*J_FACTOR
    IF $EX_AX_NUM>0 THEN
      RET_JERK.AX.E1=$JERK_MA.AX.E1*J_FACTOR
      RET_JERK.AX.E2=$JERK_MA.AX.E2*J_FACTOR
      RET_JERK.AX.E3=$JERK_MA.AX.E3*J_FACTOR
      RET_JERK.AX.E4=$JERK_MA.AX.E4*J_FACTOR
      RET_JERK.AX.E5=$JERK_MA.AX.E5*J_FACTOR
      RET_JERK.AX.E6=$JERK_MA.AX.E6*J_FACTOR
    ENDIF
    RETURN RET_JERK
  ENDIF
ENDFCT
GLOBAL DEFFCT INT S_EXAX_IGN(PARAMS :IN)
  DECL LDAT PARAMS
  RETURN PARAMS.EXAX_IGN
ENDFCT
GLOBAL DEFFCT FRAME K_ROOT (BASE_NO :IN)
  INT BASE_NO, IDX
  IF BASE_NO > 0 THEN
    IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
    IF IDX > 0 THEN
      RETURN MACHINE_DEF[IDX].ROOT
    ENDIF
  ENDIF
  RETURN $NULLFRAME
ENDFCT
GLOBAL DEFFCT ESYS K_TYPE (BASE_NO :IN)
  INT BASE_NO, IDX
  IF BASE_NO > 0 THEN
    IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
    IF IDX > 0 THEN
      RETURN MACHINE_DEF[IDX].MECH_TYPE
    ENDIF
  ENDIF
  RETURN #NONE
ENDFCT
GLOBAL DEFFCT FRAME K_OFFS(BASE_NO :IN)
  INT BASE_NO
  DECL FRAME RET_BASE
  IF (BASE_NO > 0) THEN
    RET_BASE=BASE_DATA[BASE_NO]
  ELSE
    RET_BASE=$WORLD
  ENDIF
  IF BASE_CORR_ON THEN
    RET_BASE=BASE_CORR:RET_BASE
  ENDIF
  IF M_BAS_COR_ON THEN
    RET_BASE=RET_BASE:M_BASE_CORR
  ENDIF
  RETURN RET_BASE
ENDFCT
GLOBAL DEFFCT FRAME LK_OFFS(BASE_NO :IN, TOOL_NO :IN)
  INT BASE_NO, TOOL_NO
  IF (BASE_NO > -1) THEN
    RETURN K_OFFS(BASE_NO)
  ELSE
    ;linking to remote tool
    IF (TOOL_NO > 0) THEN
      RETURN TOOL_DATA[TOOL_NO]
    ELSE
      RETURN $NULLFRAME
    ENDIF
  ENDIF
ENDFCT
GLOBAL DEFFCT CHAR[15] K_ADDR(BASE_NO :IN)
  INT BASE_NO, IDX
  IF BASE_NO > 0 THEN
    IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
    IF IDX > 0 THEN
      RETURN $COOP_KRC[MACHINE_DEF[IDX].COOP_KRC_INDEX].IP_ADDR[]
    ENDIF
    RETURN " "
  ENDIF
ENDFCT
GLOBAL DEFFCT CHAR[39] K_NAME(BASE_NO :IN)
  INT BASE_NO, IDX
  IF BASE_NO > 0 THEN
    IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
    IF IDX > 0 THEN
      RETURN MACHINE_DEF[IDX].NAME[]
    ENDIF
  ENDIF
  RETURN " "
ENDFCT
GLOBAL DEFFCT INT K_SHIFT(BASE_NO :IN)
  INT BASE_NO, IDX
  IF BASE_NO > 0 THEN
    IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
    IF IDX > 0 THEN
      RETURN MACHINE_DEF[IDX].COOP_KRC_INDEX
    ENDIF
  ENDIF
  RETURN 0
ENDFCT
;ENDFOLD