&ACCESS RVP
&REL 199
&PARAM DISKPATH = KRC:\R1\Program
&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
    extAxisVelocityPct = 5.0
    extAxisAccelerationPct = 10.0
    GoodMsgCount = 0
    NoMsgCount = 0
    tstMsgType = 23
    MsgType = 23
    frameNum = 0
    splineApproximation = 5
    linearApproximation = 5
 
    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 100ms
  INTERRUPT DECL 7 WHEN $TIMER[4] >= 100 DO GetMessages()

  ; Init the timer for the interrupt
  $TIMER_STOP[4] = TRUE
  $TIMER[4] = 0
  
;ENDFOLD

;FOLD Define Touch Trigger Interrupt
  
  ;FLAG to set use of trigger interrupt to true or false
  ;If set to true robot will stop on trigger intput
  $FLAG[20]=FALSE

;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
  PTP $AXIS_ACT ;Move to currrent position instead
;ENDFOLD

;FOLD Declare sensorname and open a channel
  bRet = strClear(SAKRLsensorname[]) 
  SAKRLsensorname[]="SAKRL" 
  SAKRL_Connect(SAKRLsensorname[])
;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 7
  $TIMER_STOP[4] = false
;ENDFOLD

;FOLD Setup interrupt for communication error handling
  $CYCFLAG[1] = FALSE
  INTERRUPT DECL 10 WHEN $CYCFLAG[1] DO CommunicationError()
  INTERRUPT ON 10
;ENDFOLD

; Sent this robot's joint ranges
SendJointRanges()

; Since we just moved home, send a status message
AtHome = 1
SendStatus()
BAS(#BASE,16) ;Set correct base
BAS(#TOOL,16) ;Set correct tool

; *****************
; 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

    CASE #MOVE_JTS

  ENDSWITCH
  Command = #NONE

  SWITCH  (tstMsgType)
      
    CASE 3
      tstMsgType = 0
      SendStatus()
      
    CASE 6
      tstMsgType = 0
      bContinue = FALSE

    CASE 7
      tstMsgType = 0
      SendStatus()
      
    CASE 8
      tstMsgType = 0
      ;MoveToCartesianPosition()
      MoveWithTrigger(1)
      
    CASE 9
      tstMsgType = 0
      ;MoveToJointPose()
      MoveWithTrigger(2)
    
    CASE 12
      tstMsgType = 0
      SendJointRanges()

    CASE 13
      tstMsgType = 0
      DoForwardKinematics()

    CASE 14
      tstMsgType = 0
      ;DoInverseKinematics() ;Actually never called

    CASE 15
      tstMsgType = 0
      GetToolFrame()

    CASE 16
      tstMsgType = 0
      SetToolFrame()

    CASE 17
      tstMsgType = 0
      ;adds a frame to the frame array
      AddFrameToArray()

    CASE 18
      tstMsgType = 0
      ;DoSpline()
      MoveWithTrigger(4)

    CASE 19
      tstMsgType = 0
      ;DoMultiLin()
      MoveWithTrigger(5)

    CASE 20
      tstMsgType = 0
      ;MoveExternalAxes()
      MoveWithTrigger(3)

    CASE 25
      tstMsgType = 0
      ;New command to directly add joint set to frame array
      AddJointSetToFrameArray()

  ENDSWITCH

      
  ; Throttle
  WAIT SEC 0.1

ENDWHILE

; **********************
; MAIN PROGRAM LOOP END
; **********************

;FOLD Close channel
  SAKRL_Disconnect(SAKRLsensorname[])
;ENDFOLD

END

;FOLD GetMessages
GLOBAL DEF GetMessages()

  BOOL bNew

  ; Disable the interrupt to handle without interruption
  INTERRUPT OFF 7
  ; Reset the timer
  $TIMER_STOP[4] = TRUE
  $TIMER[4] = 0

  intcount = intcount + 1

  ; Check for an incoming message
  SAKRL_RX.StatusOk = SAKRL_CheckForMessage()

  IF (SAKRL_RX.StatusOK == FALSE) THEN
    ; No messages available
    NoMsgCount = NoMsgCount + 1
  ELSE
    ; Get the message Type
    MsgType = SAKRL_RX.MsgType
    tstMsgType = MsgType

    ; Get the Program number
    ProgNum = SAKRL_RX.ProgNum
    frameNum = ProgNum

    ; Get the inspection index
    InspIndex = SAKRL_RX.InspIndex

    ;get position
    position[1] = SAKRL_RX.Position.X
    position[2] = SAKRL_RX.Position.Y
    position[3] = SAKRL_RX.Position.Z
    position[4] = SAKRL_RX.Position.A
    position[5] = SAKRL_RX.Position.B
    position[6] = SAKRL_RX.Position.C

    ; Get Closure: Status
    closure[1]  = SAKRL_RX.Position.S
    closure[2]  = SAKRL_RX.Position.T
    
    ;get joint values
    joints[1] = SAKRL_RX.Joints.A1    
    joints[2] = SAKRL_RX.Joints.A2
    joints[3] = SAKRL_RX.Joints.A3
    joints[4] = SAKRL_RX.Joints.A4
    joints[5] = SAKRL_RX.Joints.A5
    joints[6] = SAKRL_RX.Joints.A6

    ;get external axis values
    extAxes[1] = SAKRL_RX.Joints.E1
    extAxes[2] = SAKRL_RX.Joints.E2    
    extAxes[3] = SAKRL_RX.Joints.E3
    extAxes[4] = SAKRL_RX.Joints.E4
    extAxes[5] = SAKRL_RX.Joints.E5
    extAxes[6] = SAKRL_RX.Joints.E6

    ; Get velocity
    velocityPct = SAKRL_RX.Velocity
    
    ; Get acceleration
    accelerationPct = SAKRL_RX.Acceleration

    ; Get Axis velocity
    jtVelocityPct = SAKRL_RX.JtVelocity

    ; Get Axis acceleration
    jtAccelerationPct = SAKRL_RX.JtAcceleration

    ; Get External Axis velocity
    extAxisVelocityPct = SAKRL_RX.ExAxisVelocity

    ; Get Axis acceleration
    extAxisAccelerationPct = SAKRL_RX.ExAxisAcceleration
  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
      ;SendStatus() ;Thats done in the main loop
      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
      Command = #HIT_E_STOP
      sardEStop = TRUE
    CASE 12
      Command = #NONE
    CASE 13
      Command = #NONE
    CASE 14
      Command = #NONE
    CASE 15
      Command = #NONE
    CASE 16
      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()
    CASE #GET_POS
      if motionProgramRunning == TRUE then
        SendStatus()
      endif
    CASE #SET_VEL_ACCEL

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

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

END

;ENDFOLD

;FOLD WriteStatusInfo
GLOBAL DEF WriteStatusInfo()

  ; Set the AtHome, AtStart, Error and position fields
  SAKRL_TX.Position           = $POS_ACT
  SAKRL_TX.Joints             = $AXIS_ACT
  SAKRL_TX.Velocity           = velocityPct
  SAKRL_TX.Acceleration       = accelerationPct
  SAKRL_TX.JtVelocity         = jtVelocityPct
  SAKRL_TX.JtAcceleration     = jtAccelerationPct
  SAKRL_TX.ExAxisVelocity     = extAxisVelocityPct
  SAKRL_TX.ExAxisAcceleration = extAxisAccelerationPct
  SAKRL_TX.AtHome             = (AtHome <> 0)
  SAKRL_TX.AtStart            = (AtStart <> 0)
  SAKRL_TX.Error[]            = ErrorText[]

END
;ENDFOLD

;FOLD WriteJointRangeInfo
GLOBAL DEF WriteJointRangeInfo()
  ; Set the AtHome, AtStart, Error and position fields
  SAKRL_TX.Position.X         = $SOFTN_END[1]
  SAKRL_TX.Position.Y         = $SOFTP_END[1]
  SAKRL_TX.Position.Z         = $SOFTN_END[2]
  SAKRL_TX.Position.A         = $SOFTP_END[2]
  SAKRL_TX.Position.B         = $SOFTN_END[3]
  SAKRL_TX.Position.C         = $SOFTP_END[3]
  SAKRL_TX.Position.S         = $POS_ACT.S
  SAKRL_TX.Position.T         = $POS_ACT.T
  SAKRL_TX.Joints.A1          = $SOFTN_END[4]
  SAKRL_TX.Joints.A2          = $SOFTP_END[4]
  SAKRL_TX.Joints.A3          = $SOFTN_END[5]
  SAKRL_TX.Joints.A4          = $SOFTP_END[5]
  SAKRL_TX.Joints.A5          = $SOFTN_END[6]
  SAKRL_TX.Joints.A6          = $SOFTP_END[6]
  SAKRL_TX.Joints.E1          = $SOFTN_END[7]
  SAKRL_TX.Joints.E2          = $SOFTP_END[7]
  SAKRL_TX.Joints.E3          = $SOFTN_END[8]
  SAKRL_TX.Joints.E4          = $SOFTP_END[8]
  SAKRL_TX.Joints.E5          = $SOFTN_END[9]
  SAKRL_TX.Joints.E6          = $SOFTP_END[9]
  SAKRL_TX.Velocity           = $SOFTN_END[10]
  SAKRL_TX.Acceleration       = $SOFTP_END[10]
  SAKRL_TX.JtVelocity         = $SOFTN_END[11]
  SAKRL_TX.JtAcceleration     = $SOFTP_END[11]
  SAKRL_TX.ExAxisVelocity     = $SOFTN_END[12]
  SAKRL_TX.ExAxisAcceleration = $SOFTP_END[12]
  SAKRL_TX.AtHome             = (AtHome <> 0)
  SAKRL_TX.AtStart            = (AtStart <> 0)
  SAKRL_TX.Error[]            =  ErrorText[]
END
;ENDFOLD

;FOLD WriteTOOLFrameInfo
GLOBAL DEF WriteTOOLFrameInfo()
  WriteStatusInfo()
  SAKRL_TX.Position.X = TOOL_DATA[16].X
  SAKRL_TX.Position.Y = TOOL_DATA[16].Y
  SAKRL_TX.Position.Z = TOOL_DATA[16].Z
  SAKRL_TX.Position.A = TOOL_DATA[16].A
  SAKRL_TX.Position.B = TOOL_DATA[16].B
  SAKRL_TX.Position.C = TOOL_DATA[16].C
END
;ENDFOLD

;FOLD WriteKinematicInfo
GLOBAL DEF WriteKinematicInfo()
  ; Set the AtHome, AtStart, Error and position fields
  SAKRL_TX.Position           = PPOS
  SAKRL_TX.Joints             = PAXIS
  SAKRL_TX.Joints.E1          = PEXTAXIS.E1
  SAKRL_TX.Joints.E2          = PEXTAXIS.E2
  SAKRL_TX.Joints.E3          = PEXTAXIS.E3
  SAKRL_TX.Joints.E4          = PEXTAXIS.E4
  SAKRL_TX.Joints.E5          = PEXTAXIS.E5
  SAKRL_TX.Joints.E6          = PEXTAXIS.E6
  SAKRL_TX.Velocity           = velocityPct
  SAKRL_TX.Acceleration       = accelerationPct
  SAKRL_TX.JtVelocity         = jtVelocityPct
  SAKRL_TX.JtAcceleration     = jtAccelerationPct
  SAKRL_TX.ExAxisVelocity     = extAxisVelocityPct
  SAKRL_TX.ExAxisAcceleration = extAxisAccelerationPct
  SAKRL_TX.AtHome             = (AtHome <> 0)
  SAKRL_TX.AtStart            = (AtStart <> 0)
  SAKRL_TX.Error[]            = ErrorText[]
END
;ENDFOLD

;FOLD SendStatus
GLOBAL DEF SendStatus()
  ; Set the message type
  SAKRL_TX.MsgType =  3
  WriteStatusInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SendReady
GLOBAL DEF SendReady()
  ; Set the message type
  SAKRL_TX.MsgType = 1
  WriteStatusInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SendJointRanges
GLOBAL DEF SendJointRanges()
  ; Set the message type
  SAKRL_TX.MsgType = 12
  WriteJointRangeInfo()
  SendMsg()
END
;ENDFOLD

;FOLD DoForwardKinematics
GLOBAL DEF DoForwardKinematics()

  ; Set the message type
  SAKRL_TX.MsgType = 13

  ;Set joint values for FORWARD() kineamtics calulation
  ;-->remark: ext axis are not set on purpose
  ;-->PAXIS is a POs variable
  PAXIS.A1=joints[1]
  PAXIS.A2=joints[2]
  PAXIS.A3=joints[3]
  PAXIS.A4=joints[4]
  PAXIS.A5=joints[5]
  PAXIS.A6=joints[6]
  PAXIS.E1=$AXIS_ACT.E1
  
  ;Do Forward() twice, because it sometimes fails on the first try
  errint=0
  
  PPOS = FORWARD(PAXIS, errint)
  
  IF(errint <> 0 ) THEN
      PPOS = FORWARD(PAXIS, errint)
      IF(errint <> 0 ) THEN
          RETURN
      ENDIF
  ENDIF
  
  ;Send calculated position back to SA
  WriteKinematicInfo()
  SendMsg()

END
;ENDFOLD

;FOLD DoInverseKinematics -- FROM CURRENT JOINT POSITION
GLOBAL DEF DoInverseKinematics()
  ; Set the message type
  SAKRL_TX.MsgType = 14
  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.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_DATA[16].X=position[1]
  TOOL_DATA[16].Y=position[2]
  TOOL_DATA[16].Z=position[3]
  TOOL_DATA[16].A=position[4]
  TOOL_DATA[16].B=position[5]
  TOOL_DATA[16].C=position[6]
END
;ENDFOLD

;FOLD GetToolFrame -- W.R.T. Flange Frame
GLOBAL DEF GetToolFrame()
  ; Set the message type
  SAKRL_TX.MsgType = 15
  WriteToolFrameInfo()
  SendMsg()
END
;ENDFOLD

;FOLD Move Any Command --> With Trigger Intrrupt
GLOBAL DEF MoveWithTrigger(moveCommandNum: IN)

  DECL INT moveCommandNum
  
  ;Disable Communication
  INTERRUPT OFF 7
  $TIMER_STOP[4]=TRUE
  $TIMER[4]=0

  ;trigger Interrupt whenever Trigger Input is false
  ;Must have higher priority that timer interrupt, for safety issues
  INTERRUPT DECL 6 WHEN NOT $IN[12] AND $FLAG[20]  DO OnTriggerInterrupt()
  ;Enable Trigger Interrupt (will only fire if FLAG[20] is set to true
  INTERRUPT ON 6

  SWITCH (moveCommandNum)
    CASE 1
      MoveToCartesianPosition()
    CASE 2
      MoveToJointPose()
    CASE 3
      MoveExternalAxes()
    CASE 4
      DoSpline()
    CASE 5
      DoMultiLin()
  ENDSWITCH

  ;DISABLE Trigger Interrupt
  INTERRUPT OFF 6

  ;Enable Communication
  INTERRUPT ON 7
  $TIMER_STOP[4]=FALSE
  
END
;ENDFOLD

;FOLD LIN Move to commanded Cartesian Position
GLOBAL DEF MoveToCartesianPosition()
  Command = #NONE
  MoveCalled = TRUE
  IF motionProgramRunning == FALSE THEN
    bGo = TRUE
    bAbort = FALSE
    sardEStop = FALSE
    motionProgramRunning = TRUE
  
    ; Move to Commanded Frame position
    InMoveFcn = TRUE
    $BWDSTART=FALSE
    ;Set Base, Tool, and velocities
    PDAT_ACT=PPDAT
    FDAT_ACT=FPDAT
    LDAT_ACT=LPDAT
    BAS(#CP_PARAMS,0.02 * velocityPct) ; Meters/sec, 2 is max
    BAS(#ACC_CP,0.25 * accelerationPct) ; Meters/sec^2, 25 is max

    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]
    PPOS.E1=$AXIS_ACT.E1
    
    LIN PPOS

    ; Inform the RAC that the motion is complete
    SendMotionComplete()
    
  ELSE
    bRet = strClear(ErrorText[])
    ;ErrorText[] = "Motion already running"
    ;SendStatus()  
  ENDIF
END
;ENDFOLD

;FOLD PTP Move to commanded Joint Pose
GLOBAL DEF MoveToJointPose()
  Command = #NONE
  MoveCalled = TRUE
  IF motionProgramRunning == FALSE THEN
    bGo = TRUE
    bAbort = FALSE
    sardEStop = FALSE
    motionProgramRunning = TRUE
    
    ; Move to Commanded Joint Set
    InMoveFcn = TRUE
    $BWDSTART=FALSE
    ;Set Base, Tool, and velocities
    PDAT_ACT=PPDAT
    FDAT_ACT=FPDAT
    LDAT_ACT=LPDAT
    BAS(#PTP_PARAMS,jtVelocityPct)
    BAS(#ACC_PTP,jtAccelerationPct)

    PAXIS.A1=joints[1]
    PAXIS.A2=joints[2]
    PAXIS.A3=joints[3]
    PAXIS.A4=joints[4]
    PAXIS.A5=joints[5]
    PAXIS.A6=joints[6]
    PAXIS.E1=$AXIS_ACT.E1

    PTP PAXIS

    ;Inform the RAC that the motion is complete
    SendMotionComplete()

  ELSE
    bRet = strClear(ErrorText[])
    ;ErrorText[] = "Motion already running"
    ;SendStatus()  
  ENDIF
END
;ENDFOLD

;FOLD PTP Move External Axes
GLOBAL DEF MoveExternalAxes()
  Command = #NONE
  MoveCalled = TRUE
  IF motionProgramRunning == FALSE THEN
    bGo = TRUE
    bAbort = FALSE
    sardEStop = FALSE
    motionProgramRunning = TRUE
    
    ; Move to Commanded Ext Axis Set
    InMoveFcn = TRUE
    $BWDSTART=FALSE
    ;Set Base, Tool, and velocities
    PDAT_ACT=PPDAT
    FDAT_ACT=FPDAT
    LDAT_ACT=LPDAT
    BAS(#PTP_PARAMS,extAxisVelocityPct)
    BAS(#ACC_PTP,extAxisAccelerationPct)

    PEXTAXIS.A1=$AXIS_ACT.A1
    PEXTAXIS.A2=$AXIS_ACT.A2
    PEXTAXIS.A3=$AXIS_ACT.A3
    PEXTAXIS.A4=$AXIS_ACT.A4
    PEXTAXIS.A5=$AXIS_ACT.A5
    PEXTAXIS.A6=$AXIS_ACT.A6
    PEXTAXIS.E1=extAxes[1]
    PEXTAXIS.E2=extAxes[2]
    PEXTAXIS.E3=extAxes[3]
    PEXTAXIS.E4=extAxes[4]
    PEXTAXIS.E5=extAxes[5]
    PEXTAXIS.E6=extAxes[6]

    PTP PEXTAXIS

   ; Inform the RAC that the motion is complete
   SendMotionComplete()
   
  ELSE
    bRet = strClear(ErrorText[])
    ;ErrorText[] = "Motion already running"
    ;SendStatus()  
  ENDIF
END
;ENDFOLD

;FOLD AddFrameToArray
GLOBAL DEF AddFrameToArray()

  ARRAYPOS[frameNum].X=position[1]
  ARRAYPOS[frameNum].Y=position[2]
  ARRAYPOS[frameNum].Z=position[3]
  ARRAYPOS[frameNum].A=position[4]
  ARRAYPOS[frameNum].B=position[5]
  ARRAYPOS[frameNum].C=position[6]
  ARRAYPOS[frameNum].E1=$AXIS_ACT.E1
  
  SendMotionComplete()
END
;ENDFOLD

;FOLD AddJointSetToFrameArray
GLOBAL DEF AddJointSetToFrameArray()

  ;Set joint values for FORWARD() kineamtics calulation
  ;-->remark: ext axis are not set on purpose
  ;-->PAXIS is a POs variable
  PAXIS.A1=joints[1]
  PAXIS.A2=joints[2]
  PAXIS.A3=joints[3]
  PAXIS.A4=joints[4]
  PAXIS.A5=joints[5]
  PAXIS.A6=joints[6]
  PAXIS.E1=$AXIS_ACT.E1
  
  ;Do Forward() twice, because it sometimes fails on the first try
  errint=0
  
  PPOS = FORWARD(PAXIS, errint)
  
  IF(errint <> 0 ) THEN
      PPOS = FORWARD(PAXIS, errint)
      IF(errint <> 0 ) THEN
          SAKRL_DisplayMessage(1, "FORWARD failed in AddToArray")
      ENDIF
  ENDIF
  
  ;Add calculated position to frame array
  ARRAYPOS[frameNum].X=PPOS.X
  ARRAYPOS[frameNum].Y=PPOS.Y
  ARRAYPOS[frameNum].Z=PPOS.Z
  ARRAYPOS[frameNum].A=PPOS.A
  ARRAYPOS[frameNum].B=PPOS.B
  ARRAYPOS[frameNum].C=PPOS.C
  ARRAYPOS[frameNum].E1=$AXIS_ACT.E1
  
  ;send motion complete to robot driver
  SendMotionComplete()

END
;ENDFOLD

;FOLD DoMultiLIN
GLOBAL DEF DoMultiLIN()

  DECL INT COUNT

  $ADVANCE = 3

  ;Set Base, Tool, and velocities
  PDAT_ACT=PPDAT
  FDAT_ACT=FPDAT
  LDAT_ACT=LPDAT
  BAS(#CP_PARAMS,0.02 * velocityPct) ; Meters/sec, 2 is max
  BAS(#ACC_CP,0.25 * accelerationPct) ; Meters/sec^2, 25 is max

  ;Set small approximation distance and advance run
  $APO.CDIS = linearApproximation
  $APO.CPTP = splineApproximation 

  ;Start motion
  FOR COUNT=1 TO frameNum
    LIN ARRAYPOS[COUNT] C_DIS
  ENDFOR

  ;Reset Advance run
  $ADVANCE=0

  ;Send Motion complete
  SendStatus()
  SendMotionComplete()

END
;ENDFOLD

;FOLD DoSpline
GLOBAL DEF DoSpline()
  
  DECL INT COUNT

  ;Three steps are calculated in advance
  $ADVANCE = 3

  ;Set Base, Tool, and velocities
  PDAT_ACT=PPDAT
  FDAT_ACT=FPDAT
  LDAT_ACT=LPDAT
  BAS(#PTP_PARAMS,jtVelocityPct)
  BAS(#ACC_PTP,jtAccelerationPct)

  ;Set small approximation distance and advance run
  $APO.CDIS = linearApproximation
  $APO.CPTP = splineApproximation 

  ;Start motion
  IF frameNum == 1 THEN
    LIN ARRAYPOS[frameNum] C_DIS
  ELSE
    FOR COUNT=1 TO frameNum
      ;Used PTP istead of spline (much faster)
      PTP ARRAYPOS[COUNT] C_PTP 
    ENDFOR
  ENDIF


  ;Reset Advance run
  $ADVANCE=0

  ;Send Motion complete
  SendStatus()
  SendMotionComplete()

END
;ENDFOLD

;FOLD SendMotionComplete
GLOBAL DEF SendMotionComplete()
  AtHome = 0
  AtStart = 0
  motionProgramRunning = FALSE
  ; Set the message type and current inspection point
  SAKRL_TX.MsgType = 2
  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
  SAKRL_TX.MsgType = 4
  WriteStatusInfo()
  SendMsg()
END
;ENDFOLD

;FOLD SendMsg
GLOBAL DEF SendMsg()
  MsgCount = MsgCount + 1
  SAKRL_TX.MsgCount = MsgCount

  IF (NOT SAKRL_SendMessage()) THEN
    ; Do something appropriate
  ENDIF
END
;ENDFOLD

;FOLD RunMotionProgram
GLOBAL 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

;FOLD CommunicationError()
GLOBAL DEF CommunicationError()
  BRAKE

  ; The function that invoked this interrupt will not
  ; return until $CYCFLAG[1] is set to FALSE.
END
;ENDFOLD

;FOLD OnTriggerInterrupt()
GLOBAL DEF OnTriggerInterrupt( )

  ;Disable Touch Trigger Interrupt to prevent contact bouncing
  INTERRUPT OFF 6

  ;Break motion
  BRAKE F

  ;Send Position reached
  SendMotionComplete()

  ; If necessary a rectrat motion could be added here in order
  ; to achive fast retract on a touch signal
  ; E.G. DO:
  ; POS_RETRACT = $POS_ACT before the critical motion
  ; LIN POS_RETRACT here

  ;Resume Program
  RESUME

END
;ENDFOLD