&ACCESS RVP
&REL 269
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF SAKRL( )
;FOLD INI
;BOOL bContinue

  ;FOLD BASISTECH INI
    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
    INTERRUPT ON 3
    BAS (#INITMOV,0 )
  ;ENDFOLD (BASISTECH INI)
  ;FOLD USER INI
;Make your modifications here
  AtHome = 0
  AtStart = 0
  motionProgramRunning = FALSE
  bGo = FALSE
  bAbort = FALSE
  velocityPct = 20.0
  accelerationPct = 20.0
  jtVelocityPct = 20.0
  jtAccelerationPct = 20.0
  GoodMsgCount = 0
  NoMsgCount = 0
  tstMsgType = 23
  MsgType = 23
 
  sardEStop = FALSE
  GLOBAL INTERRUPT DECL 5 WHEN sardEStop==TRUE DO  HitEStop( )

  cmd8InMainLoop = FALSE
  cmd8InInterruptLoop = FALSE
  MoveCalled = FALSE
  InMoveFcn = FALSE
  GotToEndofGetMessages = FALSE
  cmd8B4EndOfGetMessages = FALSE
  cmd8AtEndOfGetMessages = FALSE
  ;ENDFOLD (USER INI)
;ENDFOLD (INI)
;FOLD Declare Timer-based interrupts for communication

  ; Interrupt to check for incoming messages every 250ms
  INTERRUPT DECL 6 WHEN $TIMER[4] >= 250 DO GetMessages()
  ; Init the timer for the interrupt
  $TIMER_STOP[4] = TRUE
  $TIMER[4] = 0
  
  ; Interrupt to write robot position during execution of move commands
  ;GLOBAL INTERRUPT DECL 7 WHEN $TIMER[5] >= 250 DO SendStatus()
  ; Init the timer for the interrupt
  ;$TIMER_STOP[5] = TRUE
  ;$TIMER[5] = 0
;ENDFOLD

;FOLD PTP HOME  Vel= 100 % DEFAULT;%{PE}%R 5.4.24,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS(#PTP_PARAMS,100)
$H_POS=XHOME
PTP XHOME
;ENDFOLD

;FOLD Declare sensorname and open a channel
bRet = strClear(sensorname[]) 
sensorname[]="RAC" 
errint = EKX_open(sensorname[])
EKX_handleerror(errint)


;ENDFOLD
;FOLD Build Dataset for messages received
;receive structure - Matches RACData.xml on server & robot
rectext[1].s[] = "RAC.RACData.MsgType"
rectext[2].s[] = "RAC.RACData.ProgNum"
rectext[3].s[] = "RAC.RACData.InspectionIndex"
rectext[4].s[] = "RAC.RACData.Position.XPos"
rectext[5].s[] = "RAC.RACData.Position.YPos"
rectext[6].s[] = "RAC.RACData.Position.ZPos"
rectext[7].s[] = "RAC.RACData.Position.APos"
rectext[8].s[] = "RAC.RACData.Position.BPos"
rectext[9].s[] = "RAC.RACData.Position.CPos"
rectext[10].s[] = "RAC.RACData.Closure.Status"
rectext[11].s[] = "RAC.RACData.Closure.Turn"
rectext[12].s[] = "RAC.RACData.Joints.A1"
rectext[13].s[] = "RAC.RACData.Joints.A2"
rectext[14].s[] = "RAC.RACData.Joints.A3"
rectext[15].s[] = "RAC.RACData.Joints.A4"
rectext[16].s[] = "RAC.RACData.Joints.A5"
rectext[17].s[] = "RAC.RACData.Joints.A6"
rectext[18].s[] = "RAC.RACData.Velocity"
rectext[19].s[] = "RAC.RACData.Acceleration"
rectext[20].s[] = "RAC.RACData.JtVelocity"
rectext[21].s[] = "RAC.RACData.JtAcceleration"
rectext[22].s[]= "RAC.RACData"
;ENDFOLD
; Fold Initialize Variables

bContinue = TRUE
MsgCount = 0
Command = #NONE
bRet = strClear(ErrorText[])
ErrorText[] = "No errors"
; ENDFOLD
;FOLD Start the Timer based interrupt for reading incoming data
INTERRUPT ON 6
$TIMER_STOP[4] = false
; ENDFOLD

; Sent this robot's joint ranges
SendJointRanges()

; Since we just moved home, send a status message
AtHome = 1
SendStatus()

; *****************
; MAIN PROGRAM LOOP
; *****************
WHILE bContinue
  ; "Long" commands are handled in the main loop
  ; "Short" commands are handled in the message receiver interrupt
  SWITCH (Command)
    CASE #NONE
    CASE #LOAD_PROG
      Command = #NONE
      RunMotionProgram()

    CASE #MOVE_HOME
    CASE #MOVE_START
    CASE #MOVE_CART
      cmd8InMainLoop = TRUE
      ;Command = #NONE
      ;MoveToCartesianPosition()
      ;Command = #NONE

    CASE #MOVE_JTS
      ;Command = #NONE
      ;MoveToJointPose()
      ;Command = #NONE

  ENDSWITCH
Command = #NONE

if tstMsgType == 8 then
  tstMsgType = 0
  MoveToCartesianPosition()
endif

if tstMsgType == 9 then
  tstMsgType = 0
  MoveToJointPose()
endif

if tstMsgType == 6 then
  tstMsgType = 0
  bContinue = FALSE
endif

if tstMsgType == 3 then
  tstMsgType = 0
  SendStatus()
endif

if tstMsgType == 7 then
  tstMsgType = 0
  SendStatus()
endif

if tstMsgType == 12 then
  tstMsgType = 0
  SendJointRanges()
endif

if tstMsgType == 13 then
  tstMsgType = 0
  DoForwardKinematics()
endif

if tstMsgType == 14 then
  tstMsgType = 0
  DoInverseKinematics()
endif

if tstMsgType == 15 then
  tstMsgType = 0
  GetToolFrame()
endif

if tstMsgType == 16 then
  tstMsgType = 0
  SetToolFrame()
endif
      
  ; Throttle
  WAIT SEC 0.1
  ;Command = #NONE
ENDWHILE

;FOLD Close channel
EKX_OK = EKX_close(sensorname[])
if EKX_OK == false then
  halt
endif
;ENDFOLD
;End Program
;FOLD PTP HOME  Vel= 100 % DEFAULT;%{PE}%R 5.4.24,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
;$BWDSTART = FALSE
;PDAT_ACT=PDEFAULT
;FDAT_ACT=FHOME
;BAS(#PTP_PARAMS,100)
;$H_POS=XHOME
;PTP XHOME
;ENDFOLD
END

;FOLD GetMessages
GLOBAL DEF GetMessages()
  BOOL bNew
  ; Disable the interrupt to handle without interruption
  INTERRUPT OFF 6
  ; Reset the timer
  $TIMER_STOP[4] = TRUE
  $TIMER[4] = 0
  intcount = intcount + 1


  ; Check for an incoming message - timeout is 12ms (1 IPO)
  EKX_OK = EKX_waitforsensordata(0,rectext[22].s[], 12)
  if EKX_OK == false then
     ; No messages available or a bad or partial message
    NoMsgCount = NoMsgCount + 1
  ELSE
    ; Get the message Type
    EKX_OK = EKX_getintegerelement(0, rectext[1].s[], MsgType, bNew)
    tstMsgType = MsgType
    if EKX_OK == false then
       halt
    endif
    ; Get the Program number
    EKX_OK = EKX_getintegerelement(0, rectext[2].s[], ProgNum, bNew)
    if EKX_OK == false then
      halt
    endif
    ; Get the inspection index
    EKX_OK = EKX_getintegerelement(0, rectext[3].s[], InspIndex, bNew)
    if EKX_OK == false then
      halt
    endif
    ;get position
    FOR i=1 TO 6
      EKX_OK = EKX_getrealelement(0, rectext[3+i].s[], position[i], bNew)
      if EKX_OK == false then
        halt
      endif
    ENDFOR
    ; Get Closure: Status
    EKX_OK = EKX_getintegerelement(0, rectext[10].s[], closure[1], bNew)
    if EKX_OK == false then
      halt
    endif
    ; Get the Closure: Turn
    EKX_OK = EKX_getintegerelement(0, rectext[11].s[], closure[2], bNew)
    if EKX_OK == false then
      halt
    endif
    ;get joint values
    FOR j=1 TO 6
      EKX_OK = EKX_getrealelement(0, rectext[11+j].s[], joints[j], bNew)
      if EKX_OK == false then
        halt
      endif
    ENDFOR
    ; Get velocity
    EKX_OK = EKX_getrealelement(0, rectext[18].s[], velocityPct, bNew)
    if EKX_OK == false then
      halt
    endif
    ; Get acceleration
    EKX_OK = EKX_getrealelement(0, rectext[19].s[], accelerationPct, bNew)
    if EKX_OK == false then
      halt
    endif
    
    ; Get Axis velocity
    EKX_OK = EKX_getrealelement(0, rectext[20].s[], jtVelocityPct, bNew)
    if EKX_OK == false then
      halt
    endif
    ; Get Axis acceleration
    EKX_OK = EKX_getrealelement(0, rectext[21].s[], jtAccelerationPct, bNew)
    if EKX_OK == false then
      halt
    endif
    
  ENDIF

  ; Transform the message into a program command
  IF Command <> #NONE THEN
    ; mcc69 Error - cant' handle multiple requests simultaneously - or need to wait...???
  ENDIF
  
  SWITCH MsgType
    CASE 1
      Command = #LOAD_PROG
    CASE 2
      Command = #EXEC_MOVE
    CASE 3
      ;if motionProgramRunning == TRUE then
        SendStatus()
      ;endif
      Command = #GET_POS
    CASE 4
      Command = #MOVE_HOME
    CASE 5
      Command = #MOVE_START
    CASE 6
      Command = #ABORT_PROG
    CASE 7
      Command = #GET_STATUS
    CASE 8
      Command = #MOVE_CART
      cmd8InInterruptLoop = TRUE
    CASE 9
      Command = #MOVE_JTS
    CASE 10
      Command = #SET_VEL_ACCEL
    CASE 11
      ;if motionProgramRunning == TRUE then
        Command = #HIT_E_STOP
        sardEStop = TRUE
      ;endif
    CASE 12
      SendJointRanges()
    CASE 13
      DoForwardKinematics()
    CASE 14
      DoInverseKinematics()
    CASE 15
      GetToolFrame()
      Command = #NONE
    CASE 16
      SetToolFrame()
      Command = #NONE
  ENDSWITCH
  
   IF Command == #MOVE_CART THEN
    cmd8B4EndOfGetMessages= TRUE
  ENDIF

  ; IF A PROGRAM is already loaded and running, then not
  ; all commands are allowed.  Some Commands must be 
  ; handled immediately (GET_POS, GET_STATUS, EXEC_MOVE)
  SWITCH (Command)
    CASE #EXEC_MOVE
      bGo = TRUE
      Command = #NONE
    CASE #ABORT_PROG
      bAbort = TRUE
      bContinue = FALSE
      bGo = TRUE
      Command = #NONE
    CASE #MOVE_CART
    CASE #MOVE_JTS
    CASE #GET_STATUS
      SendStatus()
      ;Command = #NONE
    CASE #GET_POS
      if motionProgramRunning == TRUE then
        SendStatus()
      endif
      ;Command = #NONE
    CASE #SET_VEL_ACCEL
      ;do nothing, values get set from receipt of xml






  ENDSWITCH
  
  GotToEndofGetMessages = TRUE
  IF Command == #MOVE_CART THEN
    cmd8AtEndOfGetMessages= TRUE
  ENDIF

  ; Enable the interrupt and start the timer
  INTERRUPT ON 6
  $TIMER_STOP[4] = FALSE
  GoodMsgCount = GoodMsgCount + 1
END

;ENDFOLD

;FOLD WriteStatusInfo
DEF WriteStatusInfo()
  ; Set the atHome, atStart and Error and position fields
  ; Write the data to the XML structure
  errint = EKX_WriteReal("RAC.RACData.Position.XPos", $POS_ACT.X)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.YPos", $POS_ACT.Y)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.ZPos", $POS_ACT.Z)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.APos", $POS_ACT.A)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.BPos", $POS_ACT.B)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.CPos", $POS_ACT.C)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Status", $POS_ACT.S)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Turn", $POS_ACT.T)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A1", $AXIS_ACT.A1)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A2", $AXIS_ACT.A2)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A3", $AXIS_ACT.A3)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A4", $AXIS_ACT.A4)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A5", $AXIS_ACT.A5)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A6", $AXIS_ACT.A6)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Velocity", velocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Acceleration", accelerationPct)
  EKX_HandleError(errint)
    errint = EKX_WriteReal("RAC.RACData.JtVelocity", jtVelocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.JtAcceleration", jtAccelerationPct)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtHome", AtHome)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtStart", AtStart)
  EKX_HandleError(errint)
  errint = EKX_WriteString("RAC.RACData.Error", ErrorText[])
  EKX_HandleError(errint)
END
;ENDFOLD

;FOLD WriteJointRangeInfo
DEF WriteJointRangeInfo()
  ; Set the atHome, atStart and Error and position fields
  ; Write the data to the XML structure
  errint = EKX_WriteReal("RAC.RACData.Position.XPos", $SOFTN_END[1])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.YPos", $SOFTP_END[1])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.ZPos", $SOFTN_END[2])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.APos", $SOFTP_END[2])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.BPos", $SOFTN_END[3])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.CPos", $SOFTP_END[3])
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Status", $POS_ACT.S)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Turn", $POS_ACT.T)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A1", $SOFTN_END[4])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A2", $SOFTP_END[4])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A3", $SOFTN_END[5])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A4", $SOFTP_END[5])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A5", $SOFTN_END[6])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A6", $SOFTP_END[6])
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Velocity", velocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Acceleration", accelerationPct)
  EKX_HandleError(errint)
    errint = EKX_WriteReal("RAC.RACData.JtVelocity", jtVelocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.JtAcceleration", jtAccelerationPct)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtHome", AtHome)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtStart", AtStart)
  EKX_HandleError(errint)
  errint = EKX_WriteString("RAC.RACData.Error", ErrorText[])
  EKX_HandleError(errint)
;inSendRanges = true
END
;ENDFOLD

;FOLD WriteTOOLFrameInfo
DEF WriteTOOLFrameInfo()
  ; Write the data to the XML structure
  errint = EKX_WriteReal("RAC.RACData.Position.XPos", TOOL_DATA[1].X)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.YPos", TOOL_DATA[1].Y)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.ZPos", TOOL_DATA[1].Z)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.APos", TOOL_DATA[1].A)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.BPos", TOOL_DATA[1].B)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.CPos", TOOL_DATA[1].C)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Status", $POS_ACT.S)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Turn", $POS_ACT.T)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A1", $AXIS_ACT.A1)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A2", $AXIS_ACT.A2)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A3", $AXIS_ACT.A3)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A4", $AXIS_ACT.A4)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A5", $AXIS_ACT.A5)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A6", $AXIS_ACT.A6)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Velocity", velocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Acceleration", accelerationPct)
  EKX_HandleError(errint)
    errint = EKX_WriteReal("RAC.RACData.JtVelocity", jtVelocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.JtAcceleration", jtAccelerationPct)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtHome", AtHome)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtStart", AtStart)
  EKX_HandleError(errint)
  errint = EKX_WriteString("RAC.RACData.Error", ErrorText[])
  EKX_HandleError(errint)
;inSendRanges = true
END
;ENDFOLD

;FOLD WriteKinematicInfo
DEF WriteKinematicInfo()
  ; Set the atHome, atStart and Error and position fields
  ; Write the data to the XML structure
  errint = EKX_WriteReal("RAC.RACData.Position.XPos", PPOS.X)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.YPos", PPOS.Y)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.ZPos", PPOS.Z)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.APos", PPOS.A)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.BPos", PPOS.B)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Position.CPos", PPOS.C)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Status", PPOS.S)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.Closure.Turn", PPOS.T)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A1", PAXIS.A1)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A2", PAXIS.A2)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A3", PAXIS.A3)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A4", PAXIS.A4)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A5", PAXIS.A5)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Joints.A6", PAXIS.A6)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Velocity", velocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.Acceleration", accelerationPct)
  EKX_HandleError(errint)
    errint = EKX_WriteReal("RAC.RACData.JtVelocity", jtVelocityPct)
  EKX_HandleError(errint)
  errint = EKX_WriteReal("RAC.RACData.JtAcceleration", jtAccelerationPct)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtHome", AtHome)
  EKX_HandleError(errint)
  errint = EKX_WriteInteger("RAC.RACData.AtStart", AtStart)
  EKX_HandleError(errint)
  errint = EKX_WriteString("RAC.RACData.Error", ErrorText[])
  EKX_HandleError(errint)
END
;ENDFOLD

;FOLD SendStatus
GLOBAL DEF SendStatus()
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 3)
  EKX_HAndleError(errint)
  WriteStatusInfo()
  SendMsg()
  ;$TIMER[5] = 0
END
;ENDFOLD

;FOLD SendReady
GLOBAL DEF SendReady()
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 1)
  EKX_HAndleError(errint)
  WriteStatusInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SendJointRanges
GLOBAL DEF SendJointRanges()
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 12)
  EKX_HAndleError(errint)
  WriteJointRangeInfo()
  SendMsg()
END
;ENDFOLD

;FOLD DoForwardKinematics
GLOBAL DEF DoForwardKinematics()
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 13)
  EKX_HAndleError(errint)
  PAXIS.A1=joints[1]
  PAXIS.A2=joints[2]
  PAXIS.A3=joints[3]
  PAXIS.A4=joints[4]
  PAXIS.A5=joints[5]
  PAXIS.A6=joints[6]
  PPOS = FORWARD (PAXIS, errint)
  WriteKinematicInfo()
  SendMsg()
END
;ENDFOLD

;FOLD DoInverseKinematics -- FROM CURRENT JOINT POSITION
GLOBAL DEF DoInverseKinematics()
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 14)
  EKX_HAndleError(errint)
  PPOS.X=position[1]
  PPOS.Y=position[2]
  PPOS.Z=position[3]
  PPOS.A=position[4]
  PPOS.B=position[5]
  PPOS.C=position[6]
  ;PPOS.S=closure[1]
  ;PPOS.T=closure[2]
  PAXIS = INVERSE (PPOS, $AXIS_ACT,  errint) ; always from current frame, $AXIS_ACT
  WriteKinematicInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SetToolFrame -- W.R.T. Flange Frame
GLOBAL DEF SetToolFrame()
  ; advance run...
  $TOOL.X=position[1]
  $TOOL.Y=position[2]
  $TOOL.Z=position[3]
  $TOOL.A=position[4]
  $TOOL.B=position[5]
  $TOOL.C=position[6]
  TOOL_DATA[1].X=position[1]
  TOOL_DATA[1].Y=position[2]
  TOOL_DATA[1].Z=position[3]
  TOOL_DATA[1].A=position[4]
  TOOL_DATA[1].B=position[5]
  TOOL_DATA[1].C=position[6]
END
;ENDFOLD

;FOLD GetToolFrame -- W.R.T. Flange Frame
GLOBAL DEF GetToolFrame()
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 15)
  EKX_HAndleError(errint)
  WriteToolFrameInfo()
  SendMsg()
END
;ENDFOLD

;FOLD PTP Move to commanded Cartesian Position
DEF MoveToCartesianPosition()
  Command = #NONE
  MoveCalled = TRUE
  IF motionProgramRunning == FALSE THEN
    bGo = TRUE
    bAbort = FALSE
    sardEStop = FALSE
    motionProgramRunning = TRUE
  
    MoveToFrame()
  ELSE
    bRet = strClear(ErrorText[])
    ;ErrorText[] = "Motion already running"
    ;SendStatus()  
  ENDIF
END
;ENDFOLD

;FOLD PTP Move to commanded Joint Pose
DEF MoveToJointPose()
  Command = #NONE
  MoveCalled = TRUE
  IF motionProgramRunning == FALSE THEN
    bGo = TRUE
    bAbort = FALSE
    sardEStop = FALSE
    motionProgramRunning = TRUE
    MoveToJointSet()
  ELSE
    bRet = strClear(ErrorText[])
    ;ErrorText[] = "Motion already running"
    ;SendStatus()  
  ENDIF
END
;ENDFOLD

;FOLD SendMotionComplete
GLOBAL DEF SendMotionComplete()
  ; Disable the interrupt that sends position data during moves
  ;INTERRUPT OFF 7
  ; Reset the timer
  ;$TIMER_STOP[5] = TRUE
  ;$TIMER[5] = 0
  ; Send the current status, because this info should be updated
  AtHome = 0
  AtStart = 0
  motionProgramRunning = FALSE
  ; Set the message type and current inspection point
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 2)
  EKX_HAndleError(errint)
  ;errint = EKX_WriteInteger("RAC.RACData.InspectionIndex", CurInspPoint)
  ;EKX_HandleError(errint)
  WriteStatusInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SendProgramComplete
GLOBAL DEF SendProgramComplete()
  ; Send the current status, because this info should be updated
  AtHome = 0
  AtStart = 1
  motionProgramRunning = FALSE
  ; Set the message type
  ; Write the data to the XML structure
  errint = EKX_WriteInteger("RAC.RACData.MsgType", 4)
  EKX_HAndleError(errint)
  WriteStatusInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SendMsg
GLOBAL DEF SendMsg()
  ;FOLD Increase message counter
  MsgCount = MsgCount + 1
  errint = EKX_WriteInteger("RAC.RACData.MsgCount", MsgCount)
  EKX_HandleError(errint)
  ;ENDFOLD
  errint = EKX_Send("RAC")
  EKX_HandleError(errint)
END
;ENDFOLD

;FOLD RunMotionProgram
DEF RunMotionProgram()
  IF motionProgramRunning == FALSE THEN
    bGo = TRUE
    bAbort = FALSE
    motionProgramRunning = TRUE
    SWITCH ProgNum
      CASE 1
        ;Insp1()
      CASE 2
        ;Insp2() 
    ENDSWITCH
  ELSE
    bRet = strClear(ErrorText[])
    ErrorText[] = "Program already running"
    SendStatus()  
  ENDIF
END
;ENDFOLD

;FOLD ProgramIsReady
GLOBAL DEF ProgramIsReady()
  AtHome = 0
  AtStart = 1
  bGo = FALSE
  SendReady()
END
;ENDFOLD

;FOLD AtInspPoint
GLOBAL DEF AtInspPoint(PointNum : IN)
  INT PointNum
  CurInspPoint = PointNum
  SendMotionComplete()
END
;ENDFOLD

;FOLD ProgramComplete
GLOBAL DEF ProgramComplete()
  SendProgramComplete()
END
;ENDFOLD