&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)

;TODO EDIT THE NEXT LINE TO YOUR ACTUAL TOOL CONFIG!!! VERY IMPORTANT!!!
bas (#tool,1)


ptp $POS_ACT
;TODO EDIT THE NEXT LINE TO A POSITION THAT IS GOOD IN YOUR SETUP!!!
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}
USERSTIFF.AXISMAXDELTA = {A1 400, A2 400, A3 400, A4 400, A5 400, A6 400, E1 400}
USERSTIFF.CPMAXDELTA= {X 99,Y 99,Z 99,A 380,B 380,C 380}
$STIFFNESS = USERSTIFF
$STIFFNESS.STRATEGY = 10
$STIFFNESS.COMMIT = TRUE
;ENDFOLD (SET_STIFFNESS)

;FOLD Init FRI_TO Variables
$FRI_TO_INT[16] = 0
$FRI_TO_INT[15] = 0
$FRI_TO_INT[14] = 0



;ENDFOLD

retVal = friopen (2)
;INTERRUPT ON
;retVal = fristart (1.0)

old_cmd=0
LOOP
Wait for (($FRI_FRM_INT[16] <> 0) and ($FRI_FRM_INT[16] <> old_cmd))
old_cmd=$FRI_FRM_INT[16]
Switch $FRI_FRM_INT[16]
Case 1
     FRI_START()
Case 2
     FRI_STOP()
Case 3
     RESET_STATUS()
Case 4
     SET_CPSTIFFNESS_DAMPING()
Case 5
     SETAXISSTIFFNESSDAMPING()
Case 6
     SWITCH_CP_CONTROL()
Case 7
     SWITCH_AXIS_CONTROL()
Case 8
     SWITCH_GRAVCOMP()
Case 9
     SWITCH_POSITION()
Case 10
     MOVE_START_POSITION()
Case 11
     RESET_COUNTER()
Case 99
 retVal = fristop()
 retVal = friclose()
 Exit
ENDSwitch

ENDLOOP


wait for $FRI_FRM_INT[16] == 99
retVal = fristop()
retVal = friclose()
END

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

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

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

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

DEF SET_CPSTIFFNESS_DAMPING()
$FRI_TO_INT[14] = $FRI_TO_INT[14] + 1
$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
END

DEF SETAXISSTIFFNESSDAMPING()
$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[14] = $FRI_TO_INT[14] + 1
$FRI_TO_INT[16] = 103
END


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

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

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

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

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