&ACCESS RVP
&REL 32
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF friokc( )
;FOLD INI
  ;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

  ;ENDFOLD (USER INI)
;ENDFOLD (INI)

bas (#tool,1)

ptp $POS_ACT
ptp {A1 -90, A2 100, A3 -85,A4 0, A5 40, A6 0, E1 0}

;Setting Stiffness to some decent defaults
;FOLD SET_STIFFNESS
USERSTIFF = $STIFFNESS
USERSTIFF.FRAMETYPE=#TOOL
USERSTIFF.CPSTIFFNESS = {x 500, Y 500, Z 500, A 50, B 50, C 50}
USERSTIFF.CPDAMPING = {X 0.7, Y 0.7, Z 0.7, A 0.7, B 0.7, C 0.7}
USERSTIFF.AXISSTIFFNESS = {A1 150, A2 150, A3 150, A4 50, A5 50, A6 50, E1 50} 
USERSTIFF.AXISDAMPING = {A1 0.7, A2 0.7, A3 0.7, A4 0.7, A5 0.7, A6 0.7, E1 0.7}
USERSTIFF.MAXFORCE = {X 50, Y 50, Z 50, A 50, B 50, C 50}
USERSTIFF.AXISMAXDELTATRQ = {A1 50, A2 50, A3 50, A4 50, A5 50, A6 50, E1 50}
$STIFFNESS = USERSTIFF
$STIFFNESS.STRATEGY = 10
$STIFFNESS.COMMIT = TRUE
;ENDFOLD (SET_STIFFNESS)

;FOLD Declare Interrupts
$FRI_TO_INT[16] = 0
$FRI_TO_INT[15] = 0
$FRI_TO_INT[14] = 0

INTERRUPT DECL 20 WHEN $FRI_FRM_INT[16] == 1 DO FRI_START()
INTERRUPT DECL 21 WHEN $FRI_FRM_INT[16] == 2 DO FRI_STOP()
INTERRUPT DECL 22 WHEN $FRI_FRM_INT[16] == 3 DO RESET_STATUS()
INTERRUPT DECL 23 WHEN $FRI_FRM_INT[16] == 4 DO SET_CPSTIFFNESS_DAMPING()
INTERRUPT DECL 24 WHEN $FRI_FRM_INT[16] == 5 DO SETAXISSTIFFNESSDAMPING()
INTERRUPT DECL 25 WHEN $FRI_FRM_INT[16] == 6 DO SWITCH_CP_CONTROL()
INTERRUPT DECL 26 WHEN $FRI_FRM_INT[16] == 7 DO SWITCH_AXIS_CONTROL()
INTERRUPT DECL 27 WHEN $FRI_FRM_INT[16] == 8 DO SWITCH_GRAVCOMP()
INTERRUPT DECL 28 WHEN $FRI_FRM_INT[16] == 9 DO SWITCH_POSITION()
INTERRUPT DECL 29 WHEN $FRI_FRM_INT[16] == 10 DO MOVE_START_POSITION()
INTERRUPT DECL 30 WHEN $FRI_FRM_INT[16] == 11 DO RESET_COUNTER() 

;ENDFOLD

retVal = friopen (2)
INTERRUPT ON
;retVal = fristart (1.0)
wait for $FRI_FRM_INT[16] == 99
retVal = fristop()
retVal = friclose()
END

DEF RESET_COUNTER()
INTERRUPT OFF
$FRI_TO_INT[16] = 111
$FRI_TO_INT[15] = 11
$FRI_TO_INT[14] = 0
INTERRUPT ON
END

DEF FRI_START()
INTERRUPT OFF
$FRI_TO_INT[15] = 1
retVal = fristart (1.0)
$FRI_TO_INT[16] = 100
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF FRI_STOP()
INTERRUPT OFF
$FRI_TO_INT[15] = 2
retVal = fristop()
$FRI_TO_INT[16] = 101
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF RESET_STATUS()
INTERRUPT OFF
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
$FRI_TO_INT[15] = 0
$FRI_TO_INT[16] = 0
INTERRUPT ON
END

DEF SET_CPSTIFFNESS_DAMPING()
INTERRUPT OFF
$FRI_TO_INT[15] = 4
USERSTIFF.CPSTIFFNESS.X = $FRI_FRM_INT[10]
USERSTIFF.CPSTIFFNESS.Y = $FRI_FRM_INT[11]
USERSTIFF.CPSTIFFNESS.Z = $FRI_FRM_INT[12]
USERSTIFF.CPSTIFFNESS.A = $FRI_FRM_INT[13]
USERSTIFF.CPSTIFFNESS.B = $FRI_FRM_INT[14]
USERSTIFF.CPSTIFFNESS.C = $FRI_FRM_INT[15]
USERSTIFF.CPDAMPING.X = $FRI_FRM_REA[11]
USERSTIFF.CPDAMPING.Y = $FRI_FRM_REA[12]
USERSTIFF.CPDAMPING.Z = $FRI_FRM_REA[13]
USERSTIFF.CPDAMPING.A = $FRI_FRM_REA[14]
USERSTIFF.CPDAMPING.B = $FRI_FRM_REA[15]
USERSTIFF.CPDAMPING.C = $FRI_FRM_REA[16]
USERSTIFF.COMMIT = TRUE
$STIFFNESS=USERSTIFF
WAIT FOR ( $STIFFNESS.COMMIT == FALSE )
$FRI_TO_INT[16] = 102
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF SETAXISSTIFFNESSDAMPING()
INTERRUPT OFF
$FRI_TO_INT[15] = 5
USERSTIFF.AXISSTIFFNESS.A1 = $FRI_FRM_INT[9]
USERSTIFF.AXISSTIFFNESS.A2 = $FRI_FRM_INT[10]
USERSTIFF.AXISSTIFFNESS.E1 = $FRI_FRM_INT[11]
USERSTIFF.AXISSTIFFNESS.A3 = $FRI_FRM_INT[12]
USERSTIFF.AXISSTIFFNESS.A4 = $FRI_FRM_INT[13]
USERSTIFF.AXISSTIFFNESS.A5 = $FRI_FRM_INT[14]
USERSTIFF.AXISSTIFFNESS.A6 = $FRI_FRM_INT[15]
USERSTIFF.AXISDAMPING.A1 = $FRI_FRM_REA[10]
USERSTIFF.AXISDAMPING.A2 = $FRI_FRM_REA[11]
USERSTIFF.AXISDAMPING.E1 = $FRI_FRM_REA[12]
USERSTIFF.AXISDAMPING.A3 = $FRI_FRM_REA[13]
USERSTIFF.AXISDAMPING.A4 = $FRI_FRM_REA[14]
USERSTIFF.AXISDAMPING.A5 = $FRI_FRM_REA[15]
USERSTIFF.AXISDAMPING.A6 = $FRI_FRM_REA[16]
USERSTIFF.COMMIT = TRUE
$STIFFNESS=USERSTIFF
WAIT FOR ( $STIFFNESS.COMMIT == FALSE )
$FRI_TO_INT[16] = 103
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END


DEF SWITCH_CP_CONTROL()
INTERRUPT OFF
$FRI_TO_INT[15] = 6
USERSTIFF.STRATEGY=20
USERSTIFF.COMMIT=TRUE
;BRAKE F
$STIFFNESS=USERSTIFF
WAIT FOR ( $STIFFNESS.COMMIT == FALSE )
$FRI_TO_INT[16] = 104
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF SWITCH_AXIS_CONTROL()
INTERRUPT OFF
$FRI_TO_INT[15] = 7
USERSTIFF.STRATEGY=30
USERSTIFF.COMMIT=TRUE
;BRAKE F
$STIFFNESS=USERSTIFF
WAIT FOR ( $STIFFNESS.COMMIT == FALSE )
$FRI_TO_INT[16] = 105
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF SWITCH_GRAVCOMP()
INTERRUPT OFF
$FRI_TO_INT[15] = 8
USERSTIFF.STRATEGY=101
USERSTIFF.COMMIT=TRUE
;BRAKE F
$STIFFNESS=USERSTIFF
WAIT FOR ( $STIFFNESS.COMMIT == FALSE )
$FRI_TO_INT[16] = 106
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF SWITCH_POSITION()
INTERRUPT OFF
$FRI_TO_INT[15] = 9
USERSTIFF.STRATEGY=10
USERSTIFF.COMMIT=TRUE
;BRAKE F
$STIFFNESS=USERSTIFF
WAIT FOR ( $STIFFNESS.COMMIT == FALSE )
$FRI_TO_INT[16] = 107
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END

DEF MOVE_START_POSITION()
INTERRUPT OFF
$FRI_TO_INT[15] = 10
PTP {A1 -90, A2 100, A3 -85,A4 0, A5 40, A6 0, E1 0}
$FRI_TO_INT[16] = 108
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
INTERRUPT ON
END