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

  SWITCH  COMMAND
  CASE #INITMOV
    INITMOV ( )
	RESET_CD_PARAMS ()
  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 ( )
  CASE #CP_DAT
    VEL_CP (LDAT_ACT.VEL )
    CP_DAT ( )
  CASE #PTP_DAT
    VEL_PTP (PDAT_ACT.VEL )
    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 )
  CASE #CP_PARAMS
    CP_DAT ( )
    FRAMES ( )
    VEL_CP (REAL_PAR )
  ENDSWITCH
END
;ENDFOLD BAS ()
;FOLD INITMOV ()

DEFFCT CHAR[128] IntToStrWithPrefix(tVar:IN, bPrefixComma:IN)
DECL INT tVar
DECL CHAR tStr[128]
DECL STATE_T STATE
DECL INT OFFSET
DECL BOOL bPrefixComma

OFFSET = 0
IF bPrefixComma THEN
   SWRITE(tStr[],STATE,OFFSET,",%d",tVar)
ELSE
   SWRITE(tStr[],STATE,OFFSET,"%d",tVar)
ENDIF
RETURN tStr[]
ENDFCT

DEF UpdateLoadModes ( )
   INT I
   FOR I=1 TO MAX_TOOL STEP 1
      IF (LOAD_MODE[I]==#NONE) AND (LOAD_DATA[I].M <> -1) THEN
         LOAD_MODE[I]=#MANUAL
      ENDIF      
   ENDFOR   
END

DEF CheckLoadModes ( )
   INT I,iRet
   CHAR MsgPar[256]
   BOOL first, bResult,foundT1Modes
   DECL KrlMsgOpt_T MsgOpt
  
   IF ($MODE_OP <> #T1) THEN
      first = TRUE
      foundT1Modes=FALSE
      bResult = STRCLEAR(MsgPar[])
      
      FOR I=1 TO MAX_TOOL STEP 1
         IF (LOAD_MODE[I]==#T1) THEN
            foundT1Modes=TRUE
            IF first THEN
               first = FALSE
               iRet = STRADD(MsgPar[], IntToStrWithPrefix(I, FALSE))
            ELSE
               iRet = STRADD(MsgPar[], IntToStrWithPrefix(I, TRUE))
            ENDIF
         ENDIF
      ENDFOR
    
      IF (foundT1Modes) THEN
	     MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
         IF ($MODE_OP==#EX) THEN
            MsgNotify("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
         ELSE
            MsgQuit ("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
         ENDIF
      ENDIF
   ENDIF
END

DEF CheckLoadModeForTool(tool_no:IN)
   INT iRet,tool_no
   CHAR MsgPar[256]
   DECL KrlMsgOpt_T MsgOpt

   CONTINUE
   IF (($ON_PATH==FALSE) AND ($MODE_OP <> #T1) AND (LOAD_MODE[tool_no]==#T1)) THEN
      MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
      iRet = STRADD(MsgPar[], IntToStrWithPrefix(tool_no, FALSE))
	  IF ($MODE_OP==#EX) THEN
         MsgNotify("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
      ELSE
         MsgQuit ("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
      ENDIF
   ENDIF
END


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=SJERK(LDEFAULT)
  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
  bJointOffsetHasBeenSet=FALSE
  IF iLastAxisNo > 0 THEN
    $JOINT_OFFSET[iLastAxisNo] = 0
  ENDIF
  iLastAxisNo=0
  
  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
      IF ((LOAD_A3_DATA.M<>0) AND (LOAD_A3_DATA.J.X==0) AND (LOAD_A3_DATA.J.Y==0) AND (LOAD_A3_DATA.J.Z==0)) THEN
        $LOAD_A3.J=$DEF_LA3_J
      ELSE
        $LOAD_A3.J=LOAD_A3_DATA.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

  STOPM_FLAG=FALSE ;reflects state of interrupt 3
  ; Set $Gear_Jerk[] with defaults
  FOR I = 1 TO 12
    $GEAR_JERK[I] = DEF_GEAR_JERK
  ENDFOR
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
    IF bJointOffsetHasBeenSet THEN
      bJointOffsetHasBeenSet = FALSE
      IF iLastAxisNo > 0 THEN
        $JOINT_OFFSET[iLastAxisNo] = 0
      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,CONV_PART_NBR[MACH_IDX])
  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,CONV_PART_NBR[MACH_IDX])
    ELSE
      IF BASE_CORR_ON THEN
        $BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
      ELSE
        $BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
      ENDIF
    ENDIF  
  ENDIF
  
  CONTINUE
  $ACT_BASE=BASE_NO
END
;ENDFOLD EB_BASE ()
;FOLD EB_ABS_BASE ()
DEF EB_ABS_BASE (MACH_IDX :IN, BASE_NO :IN)
  INT MACH_IDX, BASE_NO

  IF BASE_CORR_ON AND M_BAS_COR_ON THEN
    $BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR,CONV_PART_NBR[MACH_IDX])
  ELSE
    IF M_BAS_COR_ON THEN
      $BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO]:M_BASE_CORR,CONV_PART_NBR[MACH_IDX])
    ELSE
      IF BASE_CORR_ON THEN
        $BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
      ELSE
        $BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
      ENDIF
    ENDIF  
  ENDIF
  
  CONTINUE
  $ACT_BASE=BASE_NO
END
;ENDFOLD EB_ABS_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
    CHECK_REDEXAX_ACC(I)  
    $ACC_EXTAX[I]=ACC*RedAccExAx[I]/100.0
  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>$VEL_MA.CP THEN
    $VEL.CP=$VEL_MA.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
  
  FOR I=1 TO  $EX_AX_NUM
    CHECK_REDEXAX_VEL(I) 
    $VEL_EXTAX[I]=DEF_VEL_PTP*RedVelExAx[I]/100.0
  ENDFOR
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
   CHECK_REDEXAX_VEL(I)  
   $VEL_EXTAX[I]=VEL*RedVelExAx[I]/100.0
  ENDFOR
END
;ENDFOLD VEL_PTP ()
;FOLD {H}
DEF  CHECK_REDEXAX_ACC(I :IN )
  INT I
  IF RedAccExAx[I]>100.0 THEN
    RedAccExAx[I]=100.0
  ENDIF
  IF RedAccExAx[I]<1.0 THEN
    RedAccExAx[I]=1.0
  ENDIF
END
DEF CHECK_REDEXAX_VEL(I :IN )
  INT I
  IF RedVelExAx[I]>100.0 THEN
    RedVelExAx[I]=100.0
  ENDIF
  IF RedVelExAx[I]<1.0 THEN
    RedVelExAx[I]=1.0
  ENDIF
END
;ENDFOLD
;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
        IF ((LOAD_DATA[TOOL_NO].M<>0) AND (LOAD_DATA[TOOL_NO].J.X==0) AND (LOAD_DATA[TOOL_NO].J.Y==0) AND (LOAD_DATA[TOOL_NO].J.Z==0)) THEN
          $LOAD.J=$DEF_L_J
        ELSE  
          $LOAD.J=LOAD_DATA[TOOL_NO].J
        ENDIF
      ENDIF
    ENDIF
  ENDIF

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


  IF STOPM_FLAG==TRUE THEN ;no correct finish of IR_STOPMESS
    STOPM_FLAG=FALSE ;reflects state of interrupt 3
    ON_ERROR_PROCEED
    INTERRUPT ON 3
    SWITCH($ERR.NUMBER)
      CASE 0
        ;ok
      CASE 1430
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ()
        INTERRUPT ON 3
        ERR_CLEAR($ERR)
      DEFAULT
        ERR_RAISE($ERR)
    ENDSWITCH
  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
        MsgQuit ("IllegalUseOfRemoteTool", "BAS")
        RETURN
      ENDIF
    ENDIF
  ENDIF

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

  ;Set $JOINT_OFFSET
  IF JOINT_DEVICE_INDEX[FDAT_ACT.BASE_NO] > 0 THEN
    JointDeviceIndex = JOINT_DEVICE_INDEX[FDAT_ACT.BASE_NO]
    IF JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX > 0 THEN
      IF IsDeviceDefined(JOINT_DEVICE[JointDeviceIndex].NAME[]) THEN
        $JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = EO(JOINT_DEVICE[JointDeviceIndex].NAME[], JOINT_DEVICE[JointDeviceIndex].OFFSET )
      ELSE
        $JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = JOINT_DEVICE[JointDeviceIndex].OFFSET
      ENDIF
      bJointOffsetHasBeenSet = TRUE
      iLastAxisNo = JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX
    ENDIF
  ELSE  
    IF bJointOffsetHasBeenSet THEN
      bJointOffsetHasBeenSet = FALSE
      IF iLastAxisNo > 0 THEN
        $JOINT_OFFSET[iLastAxisNo] = 0
      ENDIF
    ENDIF
  ENDIF
  
  ;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
        IF (ActualConveyorMode == #Absolute) THEN
          EB_ABS_BASE(MACH_INDEX, FDAT_ACT.BASE_NO)
        ELSE
          EB_BASE(MACH_INDEX, FDAT_ACT.BASE_NO)
          CONV_PART_NBR[MACH_INDEX] = 0
        ENDIF
      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
  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
      IF PDAT_ACT.APO_DIST > 100 THEN
        $APO.CPTP=100
      ELSE
        $APO.CPTP=PDAT_ACT.APO_DIST
      ENDIF
    ENDIF
  ELSE
    IF PDAT_ACT.APO_DIST > 100 THEN
      $APO.CPTP=100
    ELSE
      $APO.CPTP=PDAT_ACT.APO_DIST
    ENDIF
    ERR_CLEAR($ERR)
  ENDIF
END
;ENDFOLD PTP_DAT ()
;FOLD CP_DAT ()
DEF  CP_DAT ( )
  DECL ORI_TYPE ORI_CHECK
  DECL CIRC_TYPE CIRC_CHECK
  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
  ON_ERROR_PROCEED
  CIRC_CHECK = LDAT_ACT.CIRC_TYP
  IF $ERR.NUMBER == 0 THEN
    $CIRC_TYPE=LDAT_ACT.CIRC_TYP
  ELSE
    $CIRC_TYPE=DEF_CIRC_TYP
    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 {H}
GLOBAL DEFFCT CP SVEL_CP (CP_VALUE :IN, ORI_VALUE :IN, PARAMS :IN )
  REAL AX_VEL
  REAL CP_VALUE
  REAL ORI_VALUE
  DECL CP RET_VEL
  DECL LDAT PARAMS
  IF CP_VALUE>$VEL_MA.CP THEN
    RET_VEL.CP=$VEL_MA.CP
  ELSE
    RET_VEL.CP=CP_VALUE
  ENDIF
  RET_VEL.ORI1=DEF_VEL_ORIS
  ;for Spline-HFP
  ON_ERROR_PROCEED
  AX_VEL=PARAMS.AXIS_VEL
  IF $ERR.NUMBER>0 THEN
    ERR_CLEAR($ERR)
  ELSE
    IF PARAMS.AXIS_VEL>100.0 THEN
      AX_VEL=DEF_VEL_PTP
    ELSE
      AX_VEL=DEF_VEL_PTP*PARAMS.AXIS_VEL/100.0
    ENDIF
    ;APP// control structure not allowed here
    $VEL_AXIS[1]=AX_VEL
    $VEL_AXIS[2]=AX_VEL
    $VEL_AXIS[3]=AX_VEL
    $VEL_AXIS[4]=AX_VEL
    $VEL_AXIS[5]=AX_VEL
    $VEL_AXIS[6]=AX_VEL
    IF RedVelExAx[1]>100.0 THEN
      RedVelExAx[1]=100.0
    ENDIF
    IF RedVelExAx[1]<1.0 THEN
      RedVelExAx[1]=1.0
    ENDIF
    IF RedVelExAx[2]>100.0 THEN
      RedVelExAx[2]=100.0
    ENDIF
    IF RedVelExAx[2]<1.0 THEN
      RedVelExAx[2]=1.0
    ENDIF
    IF RedVelExAx[3]>100.0 THEN
      RedVelExAx[3]=100.0
    ENDIF
    IF RedVelExAx[3]<1.0 THEN
      RedVelExAx[3]=1.0
    ENDIF
    IF RedVelExAx[4]>100.0 THEN
      RedVelExAx[4]=100.0
    ENDIF
    IF RedVelExAx[4]<1.0 THEN
      RedVelExAx[4]=1.0
    ENDIF
    IF RedVelExAx[5]>100.0 THEN
      RedVelExAx[5]=100.0
    ENDIF
    IF RedVelExAx[5]<1.0 THEN
      RedVelExAx[5]=1.0
    ENDIF
    IF RedVelExAx[6]>100.0 THEN
      RedVelExAx[6]=100.0
    ENDIF
    IF RedVelExAx[6]<1.0 THEN
      RedVelExAx[6]=1.0
    ENDIF
    $VEL_EXTAX[1]=AX_VEL*RedVelExAx[1]/100.0
    $VEL_EXTAX[2]=AX_VEL*RedVelExAx[2]/100.0
    $VEL_EXTAX[3]=AX_VEL*RedVelExAx[3]/100.0
    $VEL_EXTAX[4]=AX_VEL*RedVelExAx[4]/100.0
    $VEL_EXTAX[5]=AX_VEL*RedVelExAx[5]/100.0
    $VEL_EXTAX[6]=AX_VEL*RedVelExAx[6]/100.0
  ENDIF
  ;return CP-structure
  RETURN RET_VEL
ENDFCT
GLOBAL DEFFCT REAL SVEL_JOINT (VEL_FACTOR :IN)
  REAL VEL_FACTOR ; [%]
  REAL VEL
  IF VEL_FACTOR>100.0 THEN
    VEL=DEF_VEL_PTP
  ELSE
    VEL=DEF_VEL_PTP*VEL_FACTOR/100.0
  ENDIF
  ;$VEL_AXIS[1]=VEL
  $VEL_AXIS[2]=VEL
  $VEL_AXIS[3]=VEL
  $VEL_AXIS[4]=VEL
  $VEL_AXIS[5]=VEL
  $VEL_AXIS[6]=VEL
  IF RedVelExAx[1]>100.0 THEN
    RedVelExAx[1]=100.0
  ENDIF
  IF RedVelExAx[1]<1.0 THEN
    RedVelExAx[1]=1.0
  ENDIF
  IF RedVelExAx[2]>100.0 THEN
    RedVelExAx[2]=100.0
  ENDIF
  IF RedVelExAx[2]<1.0 THEN
    RedVelExAx[2]=1.0
  ENDIF
  IF RedVelExAx[3]>100.0 THEN
    RedVelExAx[3]=100.0
  ENDIF
  IF RedVelExAx[3]<1.0 THEN
    RedVelExAx[3]=1.0
  ENDIF
  IF RedVelExAx[4]>100.0 THEN
    RedVelExAx[4]=100.0
  ENDIF
  IF RedVelExAx[4]<1.0 THEN
    RedVelExAx[4]=1.0
  ENDIF
  IF RedVelExAx[5]>100.0 THEN
    RedVelExAx[5]=100.0
  ENDIF
  IF RedVelExAx[5]<1.0 THEN
    RedVelExAx[5]=1.0
  ENDIF
  IF RedVelExAx[6]>100.0 THEN
    RedVelExAx[6]=100.0
  ENDIF
  IF RedVelExAx[6]<1.0 THEN
    RedVelExAx[6]=1.0
  ENDIF
  $VEL_EXTAX[1]=VEL*RedVelExAx[1]/100.0
  $VEL_EXTAX[2]=VEL*RedVelExAx[2]/100.0
  $VEL_EXTAX[3]=VEL*RedVelExAx[3]/100.0
  $VEL_EXTAX[4]=VEL*RedVelExAx[4]/100.0
  $VEL_EXTAX[5]=VEL*RedVelExAx[5]/100.0
  $VEL_EXTAX[6]=VEL*RedVelExAx[6]/100.0
  RETURN VEL
ENDFCT
GLOBAL DEFFCT CP SACC_CP (PARAMS :IN)
  REAL AX_ACC
  DECL LDAT PARAMS	
  DECL CP RET_ACC
  PARAMS.ACC=PARAMS.AXIS_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
  ;for Spline-HFP
  IF $ADAP_ACC==#NONE THEN;
    IF PARAMS.AXIS_ACC>100.0 THEN
      AX_ACC=DEF_ACC_SPTP
    ELSE
      AX_ACC=DEF_ACC_SPTP*PARAMS.AXIS_ACC/100.0
    ENDIF
  ELSE
    AX_ACC=DEF_ACC_SPTP*PARAMS.AXIS_ACC/100.0
  ENDIF
  ;APP// control structure not allowed here
  $ACC_AXIS[1]=AX_ACC
  $ACC_AXIS[2]=AX_ACC
  $ACC_AXIS[3]=AX_ACC
  $ACC_AXIS[4]=AX_ACC
  $ACC_AXIS[5]=AX_ACC
  $ACC_AXIS[6]=AX_ACC
  IF RedAccExAx[1]>100.0 THEN
    RedAccExAx[1]=100.0
  ENDIF
  IF RedAccExAx[1]<1.0 THEN
    RedAccExAx[1]=1.0
  ENDIF
  IF RedAccExAx[2]>100.0 THEN
    RedAccExAx[2]=100.0
  ENDIF
  IF RedAccExAx[2]<1.0 THEN
    RedAccExAx[2]=1.0
  ENDIF
  IF RedAccExAx[3]>100.0 THEN
    RedAccExAx[3]=100.0
  ENDIF
  IF RedAccExAx[3]<1.0 THEN
    RedAccExAx[3]=1.0
  ENDIF
  IF RedAccExAx[4]>100.0 THEN
    RedAccExAx[4]=100.0
  ENDIF
  IF RedAccExAx[4]<1.0 THEN
    RedAccExAx[4]=1.0
  ENDIF
  IF RedAccExAx[5]>100.0 THEN
    RedAccExAx[5]=100.0
  ENDIF
  IF RedAccExAx[5]<1.0 THEN
    RedAccExAx[5]=1.0
  ENDIF
  IF RedAccExAx[6]>100.0 THEN
    RedAccExAx[6]=100.0
  ENDIF
  IF RedAccExAx[6]<1.0 THEN
    RedAccExAx[6]=1.0
  ENDIF
  $ACC_EXTAX[1]=AX_ACC*RedAccExAx[1]/100.0
  $ACC_EXTAX[2]=AX_ACC*RedAccExAx[2]/100.0
  $ACC_EXTAX[3]=AX_ACC*RedAccExAx[3]/100.0
  $ACC_EXTAX[4]=AX_ACC*RedAccExAx[4]/100.0
  $ACC_EXTAX[5]=AX_ACC*RedAccExAx[5]/100.0
  $ACC_EXTAX[6]=AX_ACC*RedAccExAx[6]/100.0
  ;return CP-structure
  RETURN RET_ACC
ENDFCT
GLOBAL DEFFCT REAL SACC_JOINT (PARAMS :IN)
  DECL PDAT PARAMS
  REAL AX_ACC
  IF PARAMS.ACC>100.0 THEN
    AX_ACC=DEF_ACC_SPTP
  ELSE
    AX_ACC=DEF_ACC_SPTP*PARAMS.ACC/100.0
  ENDIF
  ;$ACC_AXIS[1]=AX_ACC
  $ACC_AXIS[2]=AX_ACC
  $ACC_AXIS[3]=AX_ACC
  $ACC_AXIS[4]=AX_ACC
  $ACC_AXIS[5]=AX_ACC
  $ACC_AXIS[6]=AX_ACC
  IF RedAccExAx[1]>100.0 THEN
    RedAccExAx[1]=100.0
  ENDIF
  IF RedAccExAx[1]<1.0 THEN
    RedAccExAx[1]=1.0
  ENDIF
  IF RedAccExAx[2]>100.0 THEN
    RedAccExAx[2]=100.0
  ENDIF
  IF RedAccExAx[2]<1.0 THEN
    RedAccExAx[2]=1.0
  ENDIF
  IF RedAccExAx[3]>100.0 THEN
    RedAccExAx[3]=100.0
  ENDIF
  IF RedAccExAx[3]<1.0 THEN
    RedAccExAx[3]=1.0
  ENDIF
  IF RedAccExAx[4]>100.0 THEN
    RedAccExAx[4]=100.0
  ENDIF
  IF RedAccExAx[4]<1.0 THEN
    RedAccExAx[4]=1.0
  ENDIF
  IF RedAccExAx[5]>100.0 THEN
    RedAccExAx[5]=100.0
  ENDIF
  IF RedAccExAx[5]<1.0 THEN
    RedAccExAx[5]=1.0
  ENDIF
  IF RedAccExAx[6]>100.0 THEN
    RedAccExAx[6]=100.0
  ENDIF
  IF RedAccExAx[6]<1.0 THEN
    RedAccExAx[6]=1.0
  ENDIF
  $ACC_EXTAX[1]=AX_ACC*RedAccExAx[1]/100.0
  $ACC_EXTAX[2]=AX_ACC*RedAccExAx[2]/100.0
  $ACC_EXTAX[3]=AX_ACC*RedAccExAx[3]/100.0
  $ACC_EXTAX[4]=AX_ACC*RedAccExAx[4]/100.0
  $ACC_EXTAX[5]=AX_ACC*RedAccExAx[5]/100.0
  $ACC_EXTAX[6]=AX_ACC*RedAccExAx[6]/100.0
  RETURN AX_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 CONT SAPO_PTP (PARAMS :IN)
  DECL PDAT PARAMS
  DECL CONT RET_APO
  DECL APO_MODE_T MODE_CHECK

  ON_ERROR_PROCEED
  MODE_CHECK = PARAMS.APO_MODE
  IF $ERR.NUMBER == 0 THEN
    IF PARAMS.APO_MODE == #CPTP THEN
      RET_APO.CDIS=1E6
      RET_APO.CPTP=PARAMS.APO_DIST
    ELSE
      RET_APO.CDIS=PARAMS.APO_DIST
      RET_APO.CPTP=100
    ENDIF
  ELSE
    RET_APO.CDIS=PARAMS.APO_DIST
    RET_APO.CPTP=DEF_APO_CPTP
    ERR_CLEAR($ERR)
  ENDIF

  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 FRAME STOOL2 (FSP :IN)
  DECL FDAT FSP
  DECL FRAME RET_TOOL
  $ACT_TOOL = FSP.TOOL_NO
  $ACT_BASE = FSP.BASE_NO
  RET_TOOL = STOOL(FSP.TOOL_NO)
  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, JointDeviceIndex
  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
    IF bJointOffsetHasBeenSet THEN
      bJointOffsetHasBeenSet = FALSE
      IF iLastAxisNo > 0 THEN
        $JOINT_OFFSET[iLastAxisNo] = 0
      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
    ;Set $JOINT_OFFSET 
    IF JOINT_DEVICE_INDEX[BASE_NO] > 0 THEN
      JointDeviceIndex = JOINT_DEVICE_INDEX[BASE_NO]
      IF JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX > 0 THEN
        IF IsDeviceDefinedSPL(JOINT_DEVICE[JointDeviceIndex].NAME[]) THEN
          $JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = EO(JOINT_DEVICE[JointDeviceIndex].NAME[], JOINT_DEVICE[JointDeviceIndex].OFFSET )
        ELSE
          $JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = JOINT_DEVICE[JointDeviceIndex].OFFSET
        ENDIF
        bJointOffsetHasBeenSet = TRUE
        iLastAxisNo = JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX
      ENDIF
    ELSE  
      IF bJointOffsetHasBeenSet THEN
        bJointOffsetHasBeenSet = FALSE
        IF iLastAxisNo > 0 THEN
          $JOINT_OFFSET[iLastAxisNo] = 0
        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 REAL SGEAR_JERK (PARAMS :IN )
  DECL PDAT PARAMS
  REAL GEAR_JERK
  IF PARAMS.GEAR_JERK>100.0 THEN
    GEAR_JERK=DEF_GEAR_JERK
  ELSE
    GEAR_JERK=DEF_GEAR_JERK*PARAMS.GEAR_JERK/100.0
  ENDIF
  ;$GEAR_JERK[1]=GEAR_JERK
  $GEAR_JERK[2]=GEAR_JERK
  $GEAR_JERK[3]=GEAR_JERK
  $GEAR_JERK[4]=GEAR_JERK
  $GEAR_JERK[5]=GEAR_JERK
  $GEAR_JERK[6]=GEAR_JERK
  IF $EX_AX_NUM>0 THEN
    $GEAR_JERK[7]=GEAR_JERK
    $GEAR_JERK[8]=GEAR_JERK
    $GEAR_JERK[9]=GEAR_JERK
    $GEAR_JERK[10]=GEAR_JERK
    $GEAR_JERK[11]=GEAR_JERK
    $GEAR_JERK[12]=GEAR_JERK
  ENDIF
  RETURN GEAR_JERK
ENDFCT
GLOBAL DEFFCT JERK_STRUC SJERK (PARAMS :IN )
  DECL JERK_STRUC RET_JERK
  DECL LDAT PARAMS
  REAL GEAR_JERK, J_FACTOR, J_C0, J_F0, J_D3
  
  PARAMS.JERK_FAC=PARAMS.GEAR_JERK
  ;GEAR_JERK[1..6]for Spline-HFP
  IF PARAMS.GEAR_JERK>100.0 THEN
    GEAR_JERK=DEF_GEAR_JERK
  ELSE
    GEAR_JERK=DEF_GEAR_JERK*PARAMS.GEAR_JERK/100.0
  ENDIF
  ;APP// control structure not allowed here
  $GEAR_JERK[1]=GEAR_JERK
  $GEAR_JERK[2]=GEAR_JERK
  $GEAR_JERK[3]=GEAR_JERK
  $GEAR_JERK[4]=GEAR_JERK
  $GEAR_JERK[5]=GEAR_JERK
  $GEAR_JERK[6]=GEAR_JERK
  IF $EX_AX_NUM>0 THEN
    $GEAR_JERK[7]=GEAR_JERK
    $GEAR_JERK[8]=GEAR_JERK
    $GEAR_JERK[9]=GEAR_JERK
    $GEAR_JERK[10]=GEAR_JERK
    $GEAR_JERK[11]=GEAR_JERK
    $GEAR_JERK[12]=GEAR_JERK
  ENDIF
  ;JERK
  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
    ;APPBlock/*
    ;f(J_Factor) = linear for 0 <= J_Factor <= J_C0 
    ;f(J_Factor) = cubic for  J_C0 < J_Factor <= 1
    ;f(J_C0) = J_F0 with J_C0[0,1] and J_F0[0,1]
    ;J_C0 >= J_F0 >= 0
    ;APPBlock*/
    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 JERK-structure
    RETURN RET_JERK
  ENDIF
ENDFCT
GLOBAL DEFFCT INT S_EXAX_IGN(PARAMS :IN)
  DECL LDAT PARAMS
  RETURN PARAMS.EXAX_IGN
ENDFCT
GLOBAL DEFFCT INT S_EXAX_IGN2(PARAMS :IN)
  DECL PDAT 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[40] 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
DEFFCT BOOL IsDeviceDefined(StringVar :IN)

DECL CHAR StringVar[]
INT StringLen, StringPos, CharValue

  StringLen=STRLEN(StringVar[])
  IF StringLen>0 THEN
    FOR StringPos=1 TO StringLen
      CharValue=StringVar[StringPos]
      IF CharValue > 32 THEN
        RETURN (TRUE)
      ENDIF
    ENDFOR
    RETURN(FALSE)
  ELSE
    RETURN(FALSE)
  ENDIF

ENDFCT
DEFFCT BOOL IsDeviceDefinedSPL(StringVar :IN)

DECL CHAR StringVar[]
INT CharValue

  IF StringVar[1] > 32 THEN
    IF StringVar[2] > 32 THEN
      IF StringVar[3] > 32 THEN
        RETURN (TRUE)
      ENDIF
    ENDIF
  ELSE
    RETURN(FALSE)
  ENDIF

ENDFCT
GLOBAL DEF setFixedBaseIfLinked()
  IF (FDAT_ACT.BASE_NO == $ACT_BASE) THEN
    IF (STRCOMP($LK_MASTER[], " ", #CASE_SENS) == FALSE) THEN
      $BASE=$NULLFRAME
      $ACT_BASE=0
      LIN_REL {X 0}
    ENDIF
  ENDIF
END

GLOBAL DEF TraceStart(StringDef :IN, StringName :IN, UseRingBuffer :IN)
  
DECL CHAR StringDef[], StringName[], tempName[64]
DECL INT counter, index, OFFSET, act_iter, max_iter
DECL BOOL compErg, UseRingBuffer, DummyBool
DECL STATE_T STATE

   ON_ERROR_PROCEED
   DummyBool = UseRingBuffer
   IF $ERR.NUMBER > 0 THEN
     UseRingBuffer = FALSE
     ERR_CLEAR($ERR)
   ENDIF

   index = -1
   FOR counter = 1 TO 10 STEP 1
      compErg = STRCOMP(StringName[], TRACE_CACHE[counter].NAME[], #NOT_CASE_SENS)
      IF compErg THEN
         index = counter
         continue
      ENDIF
   ENDFOR
   
   IF index > 0 THEN
      act_iter = TRACE_CACHE[index].ACT_ITER
      max_iter = TRACE_CACHE[index].MAX_ITER
      
      act_iter = act_iter + 1
      
      IF act_iter > max_iter THEN
	     IF UseRingBuffer THEN
		    act_iter = 1
		 ELSE
            RETURN
	     ENDIF		
      ENDIF
      
      tempName[] = StringName[]
      OFFSET = STRLEN(tempName[])
      SWRITE(tempName[],STATE,OFFSET,"%d",act_iter)
            
      TRACE_CACHE[index].ACT_ITER = act_iter
   ELSE
      tempName[] = StringName[]
   ENDIF
   
   $TRACE.MODE = #T_STOP
   WAIT FOR $TRACE.STATE == #T_END
   $TRACE.CONFIG[] = StringDef[]
   $TRACE.NAME[] = tempName[]
   $TRACE.MODE = #T_START
   WAIT FOR $TRACE.STATE <> #T_END
   WAIT SEC 0.012
END

GLOBAL DEF TraceStop()
   WAIT SEC 0.012
   $TRACE.MODE = #T_STOP
   WAIT FOR $TRACE.STATE == #T_END
END

GLOBAL DEF TimerAction (Action :IN,TimerIndex :IN,TimerValue :IN)
INT TimerIndex, TimerValue
DECL TIMER_ACTIONTYPE Action

SWITCH Action
   CASE #TimerStart
      IF ($TIMER_STOP[TimerIndex]==FALSE) THEN
         $TIMER_STOP[TimerIndex]=TRUE
         MsgNotify("RunningTimerHasBeenStopped","BasisTech",TimerIndex,,26)
      ENDIF
      $TIMER[TimerIndex]=0
      $TIMER_STOP[TimerIndex]=FALSE
   CASE #TimerStop
      $TIMER_STOP[TimerIndex]=TRUE
   CASE #TimeOut
      IF ($TIMER_STOP[TimerIndex]==FALSE) THEN
         $TIMER_STOP[TimerIndex]=TRUE
         MsgNotify("RunningTimerHasBeenStopped","BasisTech",TimerIndex,,26)
      ENDIF
      $TIMER[TimerIndex]=TimerValue
      $TIMER_STOP[TimerIndex]=FALSE

ENDSWITCH
END

GLOBAL DEF BasisTech_PLC_INIT()
   UpdateLoadModes ( )
   InitHomePositions ( )   
END

GLOBAL DEF BasisTech_PLC_LOOP()
   DECL KRLMSG_T MSG
   DECL KRLMSGPAR_T MsgPar[4]
   DECL KRLMSGOPT_T Opt
   DECL BOOL bDummy
  
   MSG.Nr = 6789
   MSG.MODUL[] = "TechHandler"
   MSG.MSG_TXT[] = "EasyTeachKeepOriWarning"
   Opt = {VL_STOP FALSE, CLEAR_P_RESET FALSE, LOG_TO_DB TRUE}

   IF EasyTeachKeepOriWarning THEN
      IF EasyTeachKeepOriHandle == 0 THEN
         EasyTeachKeepOriHandle = SET_KRLMSG(#State,MSG,MsgPar[],Opt)
	  ENDIF
   ELSE
      IF EasyTeachKeepOriHandle > 0 THEN
	     bDummy=Clear_KrlMsg(EasyTeachKeepOriHandle)
		 IF	bDummy THEN
		    EasyTeachKeepOriHandle = 0
		 ENDIF
	  ENDIF
   ENDIF

END

GLOBAL DEFFCT BOOL IsMovingBase(BaseNumber:IN )

  INT MACH_INDEX, COOP_KRC_INDEX, BaseNumber
  DECL ESYS MECH_TYPE

  IF (NOT (VARSTATE("BaseNumber") == #INITIALIZED)) THEN
    BaseNumber = $ACT_BASE
  ENDIF
  IF (BaseNumber == 0) THEN
    RETURN (FALSE)
  ENDIF
  MACH_INDEX = MACHINE_FRAME_DAT[BaseNumber].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
    RETURN (FALSE)
  ENDIF
    
  IF (MECH_TYPE == #ROBOT) THEN
    IF (COOP_KRC_INDEX > 1) THEN
      ;LK Base
      RETURN (TRUE)
    ELSE
      RETURN (FALSE)
    ENDIF
  ELSE
    IF (MECH_TYPE == #NONE) THEN
      RETURN (FALSE)
    ELSE
      IF (MECH_TYPE == #CONVEYOR) THEN
        ;EB Base
        RETURN (TRUE)
      ELSE
        ;EK Base
        RETURN (TRUE)
      ENDIF
    ENDIF
  ENDIF
ENDFCT


GLOBAL DEF HopMove (StartPos :IN, TargetPos :IN, VelTransfer :IN,HopData :IN, MoveNotDirect :IN, TransferedType :OUT, StartBase :IN, TargetFdat:IN, isWithTrigger:IN)
  
  BOOL isSCARA, isDELTA, isPtpPossible, MoveNotDirect, isPtpDesiredByCustomer, isMovingStart, isMovingTarget, isWithTrigger
  BOOL MoveInBase, DummyBool
  E6POS StartPos, TargetPos, MidPos, PosOverSP, PosOverTP 
  E6AXIS AxisOverSP, AxisOverTP, AxisMidPt
  REAL StartVel, TargetVel, StartAcc, TargetAcc, TransferVel, VelTransfer
  DECL HDAT HopData
  INT TransferedType, DummyInt
  DECL KrlMsgOpt_T MsgOpt
  FRAME StartBase
  DECL FDAT TargetFdat

  MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
  MoveInBase = FALSE

  ON_ERROR_PROCEED
  DummyInt = TargetFdat.Base_No
  IF $ERR.NUMBER > 0 THEN
    MoveInBase = TRUE
    TargetFdat.BASE_NO = $ACT_BASE     
    ERR_CLEAR($ERR)
  ENDIF
  ON_ERROR_PROCEED
  DummyInt = StartBase.X
  IF $ERR.NUMBER > 0 THEN
    StartBase = $BASE
    MoveInBase = TRUE
    ERR_CLEAR($ERR)
  ENDIF

  ON_ERROR_PROCEED
  DummyBool = isWithTrigger
  IF $ERR.NUMBER > 0 THEN
    isWithTrigger = FALSE
    ERR_CLEAR($ERR)
  ENDIF

  $CP_VEL_MODE = 5
  
  isSCARA = (($MAIN_AXIS == #CC) AND ($NUM_AX == 4))
  isDELTA = (($KINCLASS == #DELTA_KIN) AND ($NUM_AX == 4))
  isPtpPossible = isSCARA AND NOT isWithTrigger
  isPtpDesiredByCustomer = FALSE

  isMovingStart = IsMovingBase($ACT_BASE)
  isMovingTarget = IsMovingBase(TargetFdat.BASE_NO)

  IF MoveNotDirect AND (isMovingStart OR isMovingTarget) THEN            
    MsgQuit ("HopIndirectAndMovingBaseNotPossible", "BasisTech", , ,39 ,MsgOpt)
  ENDIF

  IF HopData.VEL_START > $VEL_MA.CP * $CP_VEL_MODE THEN
    StartVel = $VEL_MA.CP * $CP_VEL_MODE
  ELSE
    StartVel = HopData.VEL_START
  ENDIF
  IF HopData.VEL_DEST > $VEL_MA.CP * $CP_VEL_MODE THEN
    TargetVel = $VEL_MA.CP * $CP_VEL_MODE
  ELSE
    TargetVel = HopData.VEL_DEST
  ENDIF
  
  IF HopData.ACC_START/100.0 > $CP_VEL_MODE THEN
    StartAcc = $ACC_MA.CP * $CP_VEL_MODE
  ELSE
    StartAcc = $ACC_MA.CP * HopData.ACC_START/100.0
  ENDIF
  IF HopData.ACC_DEST/100.0 > $CP_VEL_MODE THEN
    TargetAcc = $ACC_MA.CP * $CP_VEL_MODE
  ELSE
    TargetAcc = $ACC_MA.CP * HopData.ACC_DEST/100.0
  ENDIF

  IF VelTransfer > $VEL_MA.CP * $CP_VEL_MODE THEN
    TransferVel = $VEL_MA.CP * $CP_VEL_MODE
  ELSE
    TransferVel = VelTransfer
  ENDIF

  IF $ON_PATH THEN
    IF MoveInBase THEN
      PosOverSP = StartPos
      PosOverSP.Z = PosOverSP.Z + HopData.HT_Start
  
      PosOverTP = TargetPos
      PosOverTP.Z = PosOverTP.Z + HopData.HT_Dest
    ELSE
      PosOverSP = PositionToWorld(StartPos, $ACT_BASE)
      PosOverSP.Z = PosOverSP.Z + HopData.HT_Start
  
      PosOverTP = PositionToWorld(TargetPos, TargetFdat.BASE_NO)
      PosOverTP.Z = PosOverTP.Z + HopData.HT_Dest
    ENDIF  

    IF NOT ((isMovingStart OR isMovingTarget) OR (isPtpPossible OR isPtpDesiredByCustomer)) THEN
      ;change current Base to World..
      IF NOT MoveInBase THEN
        $BASE = $NULLFRAME
      ENDIF    
    
      ERR_STATUS = 0
      AxisOverSP = INVERSE (PosOverSP,$AXIS_ACT,ERR_STATUS)
      CheckERRStatus()
      AxisOverTP = INVERSE (PosOverTP,$AXIS_ACT,ERR_STATUS)
      CheckERRStatus()
      AxisMidPt.A1  = (AxisOverSP.A1 + AxisOverTP.A1) / 2
      AxisMidPt.A2  = (AxisOverSP.A2 + AxisOverTP.A2) / 2
      AxisMidPt.A3  = (AxisOverSP.A3 + AxisOverTP.A3) / 2
      AxisMidPt.A4  = (AxisOverSP.A4 + AxisOverTP.A4) / 2
      AxisMidPt.A5  = (AxisOverSP.A5 + AxisOverTP.A5) / 2
      AxisMidPt.A6  = (AxisOverSP.A6 + AxisOverTP.A6) / 2
      AxisMidPt.E1  = 0
      AxisMidPt.E2  = 0
      AxisMidPt.E3  = 0
      AxisMidPt.E4  = 0
      AxisMidPt.E5  = 0
      AxisMidPt.E6  = 0
  
      MidPos = FORWARD (AxisMidPt,ERR_STATUS)
      CheckERRStatus()
      IF MoveInBase THEN
        MidPos.A = StartPos.A
        MidPos.B = StartPos.B
        MidPos.C = StartPos.C
      ELSE
        MidPos = PositionFromWorld(MidPos, TargetFdat.BASE_NO)
        ; Set Base to Current Base
        $BASE = StartBase
      ENDIF    
    ENDIF

    IF NOT MoveInBase THEN
      PosOverSP = PositionFromWorld(PosOverSP, $ACT_BASE)
      PosOverTP = PositionFromWorld(PosOverTP, TargetFdat.BASE_NO)
    ENDIF
  ENDIF
  
  IF ((NOT isMovingTarget) AND (NOT isMovingStart) AND (isPtpPossible)) THEN
    IF $ON_PATH THEN
      $VEL.CP = StartVel
      $VEL.ORI1 = $CP_VEL_MODE * $VEL_MA.ORI1
      $VEL.ORI2 = $CP_VEL_MODE * $VEL_MA.ORI2
      $ACC.CP = StartAcc
      $ACC.ORI1 = $CP_VEL_MODE * $ACC_MA.ORI1
      $ACC.ORI2 = $CP_VEL_MODE * $ACC_MA.ORI2
      IF (HopData.HT_Start - HopData.APO_START > 0) THEN
        $APO.CDIS = HopData.HT_Start - HopData.APO_START
        PTP PosOverSP C_DIS
      ELSE
        PTP PosOverSP
      ENDIF

      IF NOT MoveInBase THEN
        ;-----------------------------      
        ; Set TOOL,BASE to Target Point   
        FDAT_ACT = TargetFdat
        BAS(#FRAMES)
        ;-----------------------------
      ENDIF		
	  
      IF (HopData.HT_Dest - HopData.APO_DEST > 0) THEN
        $APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
        ON_ERROR_PROCEED        
        PTP PosOverTP C_DIS
        IF $ERR.NUMBER > 0 THEN
          SWITCH $ERR.NUMBER
            CASE 1447
              ;APP// software limit switch point cannot be reached
              LIN PosOverTP
            DEFAULT
              MsgQuit("UnexpectedErrorDuringHopMovement","BasisTech",$ERR.NUMBER, ,38 ,MsgOpt)
          ENDSWITCH             
          ERR_CLEAR($ERR)
        ENDIF          
      ELSE
        ON_ERROR_PROCEED        
        PTP PosOverTP
        IF $ERR.NUMBER > 0 THEN
          SWITCH $ERR.NUMBER
            CASE 1447
              ;APP// software limit switch point cannot be reached
              LIN PosOverTP
            DEFAULT
              MsgQuit("UnexpectedErrorDuringHopMovement","BasisTech",$ERR.NUMBER, ,38 ,MsgOpt)
          ENDSWITCH             
          ERR_CLEAR($ERR)
        ENDIF          
      ENDIF    
      $VEL.CP = TargetVel
      $ACC.CP = TargetAcc
      IF MoveInBase THEN
        PTP TargetPos
      ENDIF  
    ELSE
      ;APP// not on path (block selection)
      IF MoveInBase THEN
        LIN TargetPos
      ENDIF  
	 ENDIF
  ELSE
    ;APP// no ptp movement
    IF MoveNotDirect THEN
      IF $ON_PATH THEN
        TransferedType = 2 ;Forced Indirect
        $VEL.CP = StartVel
        $VEL.ORI1 = $CP_VEL_MODE * $VEL_MA.ORI1
        $VEL.ORI2 = $CP_VEL_MODE * $VEL_MA.ORI2
        $ACC.CP = StartAcc
        $ACC.ORI1 = $CP_VEL_MODE * $ACC_MA.ORI1
        $ACC.ORI2 = $CP_VEL_MODE * $ACC_MA.ORI2
        IF (HopData.HT_Start - HopData.APO_START > 0) THEN
          $APO.CDIS = HopData.HT_Start - HopData.APO_START
          LIN PosOverSP C_DIS
        ELSE
          LIN PosOverSP
        ENDIF    

        IF NOT MoveInBase THEN
          ;-----------------------------      
          ; Set TOOL,BASE to Target Point   
          FDAT_ACT = TargetFdat
          BAS(#FRAMES)
          ;-----------------------------
        ENDIF		

        $VEL.CP = TransferVel
        $APO.CDIS = 1000.0
        LIN MidPos C_DIS
        IF (HopData.HT_Dest - HopData.APO_DEST > 0) THEN
          $APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
          LIN PosOverTP C_DIS
        ELSE
          LIN PosOverTP
        ENDIF    
        $VEL.CP = TargetVel
        $ACC.CP = TargetAcc
        IF MoveInBase THEN
          LIN TargetPos
        ENDIF  
      ELSE
        IF MoveInBase THEN
          LIN TargetPos
		ELSE
          ;-----------------------------      
          ; Set TOOL,BASE to Target Point   
          FDAT_ACT = TargetFdat
          BAS(#FRAMES)
          ;-----------------------------          
        ENDIF  
      ENDIF
    ;APP// no ptp movement, direct
    ELSE
      IF $ON_PATH THEN
        $VEL.CP = StartVel
        $VEL.ORI1 = $CP_VEL_MODE * $VEL_MA.ORI1
        $VEL.ORI2 = $CP_VEL_MODE * $VEL_MA.ORI2
        $ACC.CP = StartAcc
        $ACC.ORI1 = $CP_VEL_MODE * $ACC_MA.ORI1
        $ACC.ORI2 = $CP_VEL_MODE * $ACC_MA.ORI2
        IF (HopData.HT_Start - HopData.APO_START > 0) THEN
          $APO.CDIS = HopData.HT_Start - HopData.APO_START
          LIN PosOverSP C_DIS
        ELSE
          LIN PosOverSP
        ENDIF    
        IF NOT MoveInBase THEN
          ;-----------------------------      
          ; Set TOOL,BASE to Target Point   
          FDAT_ACT = TargetFdat
          BAS(#FRAMES)
          ;-----------------------------
        ENDIF		
        $VEL.CP = TransferVel
        IF (HopData.HT_Dest - HopData.APO_DEST > 0) THEN
          $APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
          ON_ERROR_PROCEED
          LIN PosOverTP C_DIS
          IF $ERR.NUMBER > 0 THEN
		    TransferedType = 1 ;Indirect 
            $APO.CDIS = 1000.0
            IF NOT (isMovingStart OR isMovingTarget) THEN
               LIN MidPos C_DIS
            ENDIF
            $APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
            LIN PosOverTP C_DIS
            ERR_CLEAR($ERR)
	      ELSE
		    TransferedType = 0 ;Direct
          ENDIF    
        ELSE
          ON_ERROR_PROCEED
          LIN PosOverTP
          IF $ERR.NUMBER > 0 THEN
		    TransferedType = 1 ; Indirect
            $APO.CDIS = 1000.0
            IF NOT (isMovingStart OR isMovingTarget) THEN
               LIN MidPos C_DIS
            ENDIF
            LIN PosOverTP
            ERR_CLEAR($ERR)
		  ELSE
		    TransferedType = 0 ;Direct
          ENDIF    
        ENDIF    
        $VEL.CP = TargetVel
        $ACC.CP = TargetAcc
        IF MoveInBase THEN
          LIN TargetPos
        ENDIF  
      ELSE
        ;APP// not on path (block selection)
        IF MoveInBase THEN
          LIN TargetPos
		ELSE
          ;-----------------------------      
          ; Set TOOL,BASE to Target Point   
          FDAT_ACT = TargetFdat
          BAS(#FRAMES)
          ;-----------------------------          
        ENDIF  
      ENDIF
    ENDIF
  ENDIF

  IF MoveInBase THEN
    $ACC.CP = DEF_ACC_CP
    $ACC.ORI1 = DEF_ACC_ORI1
    $ACC.ORI2 = DEF_ACC_ORI2
    $VEL.CP = DEF_VEL_CP
    $VEL.ORI1 = DEF_VEL_ORI1
    $VEL.ORI2 = DEF_VEL_ORI2
    $CP_VEL_MODE = 0
  ENDIF
END

DEF CheckERRStatus()
  DECL INT ERR_STAT
  DECL KrlMsgOpt_T MsgOpt

  MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
  ERR_STAT = ERR_STATUS
  ERR_STATUS = 0
  REPEAT
    SWITCH ERR_STAT
    CASE 0
	  RETURN
    CASE 1
      MsgQuit ("HopPositionWrongCalculation1", "BasisTech", , ,36 ,MsgOpt)
    CASE 2,3
      MsgQuit ("HopPositionWrongCalculation2", "BasisTech", , ,37 ,MsgOpt)
    ENDSWITCH
  UNTIL FALSE
END

DEF InitHomePositions()
  DECL KrlMsgOpt_T MsgOpt
  INT nHandle
  BOOL bDummy
  
  MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}

  IF ($KINCLASS == #DELTA_KIN) OR ($MAIN_AXIS == #CC) THEN
    IF (XHOME.A2 == -90) AND (XHOME.A3 == 90) THEN
      XHOME = $H_POS
      MsgState (nHandle,"XHomeInitialized", "BasisTech", , ,38 ,MsgOpt)
      bDummy=CLEAR_KRLMSG (nHandle)
    ENDIF
    IF (XHOME1.A2 == -90) AND (XHOME1.A3 == 90) THEN
      XHOME1 = $AXIS_HOME[1]
      MsgState (nHandle,"XHome1Initialized", "BasisTech", , ,39 ,MsgOpt)
      bDummy=CLEAR_KRLMSG (nHandle)
    ENDIF
    IF (XHOME2.A2 == -90) AND (XHOME2.A3 == 90) THEN
      XHOME2 = $AXIS_HOME[2]
      MsgState (nHandle,"XHome2Initialized", "BasisTech", , ,40 ,MsgOpt)
      bDummy=CLEAR_KRLMSG (nHandle)
    ENDIF
    IF (XHOME3.A2 == -90) AND (XHOME3.A3 == 90) THEN
      XHOME3 = $AXIS_HOME[3]
      MsgState (nHandle,"XHome3Initialized", "BasisTech", , ,41 ,MsgOpt)
      bDummy=CLEAR_KRLMSG (nHandle)
    ENDIF
    IF (XHOME4.A2 == -90) AND (XHOME4.A3 == 90) THEN
      XHOME4 = $AXIS_HOME[4]
      MsgState (nHandle,"XHome4Initialized", "BasisTech", , ,42 ,MsgOpt)
      bDummy=CLEAR_KRLMSG (nHandle)
    ENDIF
    IF (XHOME5.A2 == -90) AND (XHOME5.A3 == 90) THEN
      XHOME5 = $AXIS_HOME[5]
      MsgState (nHandle,"XHome5Initialized", "BasisTech", , ,43 ,MsgOpt)
      bDummy=CLEAR_KRLMSG (nHandle)
    ENDIF
  ENDIF  
END

GLOBAL DEF SetDefaultValues()
    $ACC.CP = DEF_ACC_CP
    $ACC.ORI1 = DEF_ACC_ORI1
    $ACC.ORI2 = DEF_ACC_ORI2
    $VEL.CP = DEF_VEL_CP
    $VEL.ORI1 = DEF_VEL_ORI1
    $VEL.ORI2 = DEF_VEL_ORI2
    $CP_VEL_MODE = 0
END

DEFFCT E6POS PositionToWorld(Position:IN, PositionBaseNumber:IN)
  DECL E6POS Position, Result
  DECL INT PositionBaseNumber, MACH_INDEX, PartNumber
  DECL FRAME MachineFrame, PositionBase
  DECL BOOL Part_Present
  DECL CHAR ConveyorName[40]
         
  MachineFrame = $NULLFRAME
         
  IF(PositionBaseNumber > 0) THEN
    PositionBase = BASE_DATA[PositionBaseNumber]
    MACH_INDEX = MACHINE_FRAME_DAT[PositionBaseNumber].MACH_DEF_INDEX
    IF MACH_INDEX > 0 THEN
      MachineFrame = MACHINE_DEF[MACH_INDEX].ROOT
      IF MACHINE_DEF[MACH_INDEX].MECH_TYPE == #CONVEYOR THEN
        ConveyorName[] = MACHINE_DEF[MACH_INDEX].Name[]
        PartNumber = CONV_PART_NBR[MACH_INDEX]
        Result = EB_TEST(MachineFrame, ConveyorName[], PositionBase, PartNumber, Part_Present)
        Result = Result : Position
        RETURN RESULT
      ENDIF
    ENDIF            
  ELSE
    PositionBase = $NULLFRAME
  ENDIF
         
  ;calculate the position in world
  Result = MachineFrame:PositionBase:Position
         
  RETURN Result 
         
ENDFCT

DEFFCT E6POS PositionFromWorld(Position:IN, PositionBaseNumber:IN)
  DECL FRAME PositionBase, MachineFrame
  DECL E6POS Position, Result
  DECL INT PositionBaseNumber , MACH_INDEX, PartNumber
  DECL BOOL Part_Present
  DECL CHAR ConveyorName[40]
        
  MachineFrame = $NULLFRAME
         
  IF(PositionBaseNumber > 0) THEN
    PositionBase = BASE_DATA[PositionBaseNumber]
    MACH_INDEX = MACHINE_FRAME_DAT[PositionBaseNumber].MACH_DEF_INDEX
    IF MACH_INDEX > 0 THEN
      MachineFrame = MACHINE_DEF[MACH_INDEX].ROOT
      IF MACHINE_DEF[MACH_INDEX].MECH_TYPE == #CONVEYOR THEN
        ConveyorName[] = MACHINE_DEF[MACH_INDEX].Name[]
        PartNumber = CONV_PART_NBR[MACH_INDEX]
        Result = EB_TEST(MachineFrame, ConveyorName[], PositionBase, PartNumber, Part_Present)
        Result = INV_POS(Result) : Position
        RETURN RESULT
      ENDIF
    ENDIF
  ELSE
    PositionBase = $NULLFRAME
  ENDIF
         
  ;Position in World => Position in Base..
  Result = INV_POS(PositionBase): INV_POS(MachineFrame) :Position
         
  RETURN Result
         
ENDFCT

;ENDFOLD