&ACCESS RVO1
&REL 45
&PARAM DISKPATH = KRC:\R1\Program
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF SAKRL_Ethernet( )
END

;FOLD DEF SAKRL_HandleError(ErrorCode:IN)
DEF SAKRL_HandleError(ErrorCode:IN)
  INT ErrorCode
  INT answer

  CHAR  StatusStr[81]
  DECL  STATE_T state
  INT   offset
  BOOL  b

  IF (ErrorCode == 0) THEN
    RETURN
  ENDIF

  ; This will raise the communications event handler

  $CYCFLAG[1] = TRUE

  ; Generate a message to display error code

  offset = 0
  b = STRCLEAR(StatusStr[])
  SWRITE(StatusStr[], state, offset, "EKI Communications Error: %d", ErrorCode)
  answer = SAKRL_PromptDialog(1, StatusStr[], 1, "Ack")

  ; Currently there is no recovery unless it is
  ; accomplished by the communications event handler
  ; with a RESUME command.

  answer = SAKRL_PromptDialog(1, "Stop program and restart to continue", 1, "Ack")

  ; Loop forever

  LOOP
    HALT
  ENDLOOP
END
;ENDFOLD DEF SAKRL_HandleError(ErrorCode:IN)

;FOLD DEF SAKRL_WriteMessage()
DEF SAKRL_WriteMessage()

  IF (NOT SAKRL_CONNECTED) THEN
    RETURN
  ENDIF

  SAKRL_RET = EKI_SetInt(SAKRL_SENSOR[], "RACData/MsgType", SAKRL_TX.MsgType)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetInt(SAKRL_SENSOR[], "RACData/ProgNum", SAKRL_TX.ProgNum)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetInt(SAKRL_SENSOR[], "RACData/InspectionIndex", SAKRL_TX.InspIndex)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Position/XPos", SAKRL_TX.Position.X)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Position/YPos", SAKRL_TX.Position.Y)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Position/ZPos", SAKRL_TX.Position.Z)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Position/APos", SAKRL_TX.Position.A)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Position/BPos", SAKRL_TX.Position.B)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Position/CPos", SAKRL_TX.Position.C)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetInt(SAKRL_SENSOR[], "RACData/Closure/Status", SAKRL_TX.Position.S)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetInt(SAKRL_SENSOR[], "RACData/Closure/Turn", SAKRL_TX.Position.T)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Joints/A1", SAKRL_TX.Joints.A1)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Joints/A2", SAKRL_TX.Joints.A2)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Joints/A3", SAKRL_TX.Joints.A3)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Joints/A4", SAKRL_TX.Joints.A4)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Joints/A5", SAKRL_TX.Joints.A5)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Joints/A6", SAKRL_TX.Joints.A6)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex1", SAKRL_TX.Joints.E1)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex2", SAKRL_TX.Joints.E2)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex3", SAKRL_TX.Joints.E3)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex4", SAKRL_TX.Joints.E4)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex5", SAKRL_TX.Joints.E5)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex6", SAKRL_TX.Joints.E6)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Velocity", SAKRL_TX.Velocity)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/Acceleration", SAKRL_TX.Acceleration)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/JtVelocity", SAKRL_TX.JtVelocity)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/JtAcceleration", SAKRL_TX.JtAcceleration)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExAxisVelocity", SAKRL_TX.ExAxisVelocity)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetReal(SAKRL_SENSOR[], "RACData/ExAxisAcceleration", SAKRL_TX.ExAxisAcceleration)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetBool(SAKRL_SENSOR[], "RACData/AtHome", SAKRL_TX.AtHome)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetBool(SAKRL_SENSOR[], "RACData/AtStart", SAKRL_TX.AtStart)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_SetString(SAKRL_SENSOR[], "RACData/Error", SAKRL_TX.Error[])
  SAKRL_HandleError(SAKRL_RET.Msg_No)
END
;ENDFOLD DEF SAKRL_WriteMessage()

;FOLD DEF SAKRL_ReadMessage()
DEF SAKRL_ReadMessage()
  IF (NOT SAKRL_CONNECTED) THEN
    RETURN
  ENDIF

  FOR i = 1 TO 81
    SAKRL_RX.Error[i] = 0
  ENDFOR

  SAKRL_RET = EKI_GetInt(SAKRL_SENSOR[], "RACData/MsgType", SAKRL_RX.MsgType)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  ; If SARKL_RET.Buff > 0, this means that at least one
  ; more message than the one currently being read is
  ; still in the input buffer and that it should be
  ; read on the next read cycle.  This is accomplished
  ; by setting $FLAG[2] to TRUE indicating that there
  ; is an incoming message ready to read.

  IF (SAKRL_RET.Buff > 0) THEN
    $FLAG[2] = TRUE
  ENDIF

  SAKRL_RET = EKI_GetInt(SAKRL_SENSOR[], "RACData/ProgNum", SAKRL_RX.ProgNum)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetInt(SAKRL_SENSOR[], "RACData/InspectionIndex", SAKRL_RX.InspIndex)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Position/XPos", SAKRL_RX.Position.X)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Position/YPos", SAKRL_RX.Position.Y)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Position/ZPos", SAKRL_RX.Position.Z)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Position/APos", SAKRL_RX.Position.A)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Position/BPos", SAKRL_RX.Position.B)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Position/CPos", SAKRL_RX.Position.C)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetInt(SAKRL_SENSOR[], "RACData/Closure/Status", SAKRL_RX.Position.S)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetInt(SAKRL_SENSOR[], "RACData/Closure/Turn", SAKRL_RX.Position.T)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Joints/A1", SAKRL_RX.Joints.A1)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Joints/A2", SAKRL_RX.Joints.A2)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Joints/A3", SAKRL_RX.Joints.A3)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Joints/A4", SAKRL_RX.Joints.A4)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Joints/A5", SAKRL_RX.Joints.A5)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Joints/A6", SAKRL_RX.Joints.A6)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex1", SAKRL_RX.Joints.E1)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex2", SAKRL_RX.Joints.E2)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex3", SAKRL_RX.Joints.E3)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex4", SAKRL_RX.Joints.E4)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex5", SAKRL_RX.Joints.E5)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExternalAxes/Ex6", SAKRL_RX.Joints.E6)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Velocity", SAKRL_RX.Velocity)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/Acceleration", SAKRL_RX.Acceleration)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/JtVelocity", SAKRL_RX.JtVelocity)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/JtAcceleration", SAKRL_RX.JtAcceleration)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExAxisVelocity", SAKRL_RX.ExAxisVelocity)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetReal(SAKRL_SENSOR[], "RACData/ExAxisAcceleration", SAKRL_RX.ExAxisAcceleration)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetBool(SAKRL_SENSOR[], "RACData/AtHome", SAKRL_RX.AtHome)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetBool(SAKRL_SENSOR[], "RACData/AtStart", SAKRL_RX.AtStart)
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetString(SAKRL_SENSOR[], "RACData/Error", SAKRL_RX.Error[])
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_GetInt(SAKRL_SENSOR[], "RACData/MsgCount", SAKRL_RX.MsgCount)
  SAKRL_HandleError(SAKRL_RET.Msg_No)
END
;ENDFOLD DEF SAKRL_ReadMessage()

;FOLD GLOBAL DEF SAKRL_Connect(Sensor[]:IN)
GLOBAL DEF SAKRL_Connect(Sensor[]:IN)
  CHAR Sensor[]
  
  SAKRL_SENSOR[] = Sensor[]
  SAKRL_CONNECTED = FALSE

  SAKRL_RET = EKI_Init(Sensor[])
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  SAKRL_RET = EKI_Open(Sensor[])
  SAKRL_HandleError(SAKRL_RET.Msg_No)

  $FLAG[2] = FALSE
  SAKRL_CONNECTED = TRUE
END
;ENDFOLD GLOBAL DEF SAKRL_Connect(Sensor[]:IN)

;FOLD GLOBAL DEF SAKRL_Disconnect(Sensor[]:IN)
GLOBAL DEF SAKRL_Disconnect(Sensor[]:IN)
  CHAR Sensor[]

  IF (NOT SAKRL_CONNECTED) THEN
    RETURN
  ENDIF

  SAKRL_CONNECTED = FALSE
  SAKRL_RET = EKI_Close(Sensor[])
  SAKRL_RET = EKI_Clear(Sensor[])
END
;ENDFOLD GLOBAL DEF SAKRL_Disconnect(Sensor[]:IN)

;FOLD GLOBAL DEFFCT BOOL SAKRL_CheckForMessage()
GLOBAL DEFFCT BOOL SAKRL_CheckForMessage()
  
  IF (NOT SAKRL_CONNECTED) THEN
    RETURN (FALSE)
  ENDIF
  
  IF ($FLAG[2] == TRUE) THEN
    $FLAG[2] = FALSE
    SAKRL_ReadMessage()
    RETURN (TRUE)
  ENDIF

  RETURN (FALSE)
ENDFCT
;ENDFOLD GLOBAL DEFFCT BOOL SAKRL_CheckForMessage()

;FOLD GLOBAL DEFFCT BOOL SARKL_SendMessage()
GLOBAL DEFFCT BOOL SAKRL_SendMessage()

  IF (NOT SAKRL_CONNECTED) THEN
    RETURN (FALSE)
  ENDIF

  SAKRL_WriteMessage()
  SAKRL_RET = EKI_Send(SAKRL_SENSOR[], "RACData")

  IF (SAKRL_RET.Msg_No <> 0) THEN
    SAKRL_HandleError(SAKRL_RET.Msg_No)
    RETURN (FALSE)
  ENDIF

  RETURN (TRUE)
ENDFCT
;ENDFOLD GLOBAL DEFCT BOOL SARKL_SendMessage()
