Komplettes Positionsregister X nach Y kopieren?

PROGRAM MovePosReg
  CONST
    ERR_WARN  = 0 -- Severity constant for use in POST_ERR
    SUCCESS   = 0 -- success constant for status check
    PARA_INT  = 1 -- Parameter is an INTEGER
    POS_TYPE_POS   = 1 -- Position is of type POSITION
    POS_TYPE_XYZ   = 2 -- Position is of type XYZWPR
    POS_TYPE_XYZE  = 6 -- Position is of type XYZWPREXT
    POS_TYPE_JOINT = 9 -- Position is of type JOINTPOS
  
  VAR
    data_type     :INTEGER
    posRegSource  :INTEGER
    posRegTarget  :INTEGER
    temp_real     :REAL
    temp_string   :STRING[16]
    status        :INTEGER
    
    posn_type     :INTEGER
    num_axes      :INTEGER
    
    srcPosComment :STRING[16]
    xyz           :XYZWPR
    jpos          :JOINTPOS
    
  
BEGIN -- MovePosReg
  -- Handle first parameter (mandatory)
  GET_TPE_PRM(1, data_type, posRegSource, temp_real, temp_string, status)
  
  IF (status <> SUCCESS) THEN
    -- if rec_stat is not SUCCESS then post the error to the system.
    -- 17042 "ROUT-042 TPE parameters do not exist"
    POST_ERR(status, '', 0, ERR_WARN)
    RETURN
  ELSE
    -- Check if parmeter is an integer
    IF data_type <> PARA_INT THEN
      POST_ERR(status, '', 0, ERR_WARN)
      RETURN
    ELSE
      -- Check for correct range
      IF (posRegSource < 1) OR (posRegSource > 100) THEN
        POST_ERR(status, '', 0, ERR_WARN)
        RETURN
      ENDIF
    ENDIF
  ENDIF
  
  -- Handle second parameter (mandatory)
  GET_TPE_PRM(2, data_type, posRegTarget, temp_real, temp_string, status)
  
  IF status <> SUCCESS THEN
    -- if rec_stat is not SUCCESS then post the error to the system.
    -- 17042 "ROUT-042 TPE parameters do not exist"
    POST_ERR(status, '', 0, ERR_WARN)
    RETURN
  ELSE
    -- Check if parmeter is an integer
    IF data_type <> PARA_INT THEN
      POST_ERR(status, '', 0, ERR_WARN)
      RETURN
    ELSE
        -- Check for correct range
      IF (posRegTarget < 1) OR (posRegTarget > 100) THEN
        POST_ERR(status, '', 0, ERR_WARN)
        RETURN
      ENDIF
    ENDIF
  ENDIF
  
  -- Source and target are the same.
  IF (posRegSource = posRegTarget) THEN
    POST_ERR(status, '', 0, ERR_WARN)
    RETURN
  ENDIF
  
  POS_REG_TYPE(posRegSource, 1, posn_type, num_axes, status)
  
  SELECT posn_type OF
    CASE (POS_TYPE_POS):
      RETURN
    
    CASE (POS_TYPE_XYZ):
      xyz = GET_POS_REG(posRegSource, status)
      SET_POS_REG (posRegTarget, xyz, status)
      GET_PREG_CMT(posRegSource, srcPosComment, status)
      SET_PREG_CMT(posRegTarget, srcPosComment, status)
      SET_PREG_CMT(posRegSource, '', status)
      CLR_POS_REG (posRegSource, 1, status)
    
    CASE (POS_TYPE_XYZE):
      RETURN
    
    CASE (POS_TYPE_JOINT):
      jpos = GET_JPOS_REG(posRegSource, status)
      SET_JPOS_REG(posRegTarget, jpos, status)
      GET_PREG_CMT(posRegSource, srcPosComment, status)
      SET_PREG_CMT(posRegTarget, srcPosComment, status)
      SET_PREG_CMT(posRegSource, '', status)
      CLR_POS_REG (posRegSource, 1, status)
    
    ELSE:
      RETURN
  ENDSELECT

END MovePosReg