386 lines
10 KiB
Plaintext
386 lines
10 KiB
Plaintext
&ACCESS R1
|
|
&COMMENT Collision monitoring library
|
|
DEF CollMonLib( )
|
|
|
|
END
|
|
|
|
; Schreibt die 6 Parameter (fuer jede Roboter-Achse einen) in den
|
|
; entsprechenden Eintrag des $COLLMON_TOL PRO[]-Felds.
|
|
; Falls ein Parameter nicht SAVE_CM_PRO_SETTING wird, behaelt der entsprechende
|
|
; $COLLMON_TOL_PRO[]-Eintrag seinen Wert.
|
|
GLOBAL DEF SET_CM_PRO_VALUES(CM_AX1:IN, CM_AX2:IN, CM_AX3:IN, CM_AX4:IN, CM_AX5:IN, CM_AX6:IN)
|
|
INT TEST, CM_AX1, CM_AX2, CM_AX3, CM_AX4, CM_AX5, CM_AX6
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=CM_AX1
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[1]=CM_AX1
|
|
ENDIF
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=CM_AX2
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[2]=CM_AX2
|
|
ENDIF
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=CM_AX3
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[3]=CM_AX3
|
|
ENDIF
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=CM_AX4
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[4]=CM_AX4
|
|
ENDIF
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=CM_AX5
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[5]=CM_AX5
|
|
ENDIF
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=CM_AX6
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[6]=CM_AX6
|
|
ENDIF
|
|
|
|
ERR_CLEAR($ERR)
|
|
$COLLMON_IDX = 0
|
|
END
|
|
|
|
; Setzt $COLLMON_TOL_PRO[] auf die Defaultwerte aus.
|
|
; der $CUSTOM.DAT
|
|
GLOBAL DEF SET_CM_PRO_DEFAULT()
|
|
INT I
|
|
|
|
FOR I=1 TO 12
|
|
$COLLMON_TOL_PRO[I]=$COLLMON_TOL_PRO_DEF[I]
|
|
ENDFOR
|
|
$COLLMON_IDX = 0
|
|
END
|
|
; Setzt die globalen Werte falls welche vorhanden oder
|
|
; deaktiviert die verbesserte Kollisionsueberwachung indem
|
|
; $COLLMON_TOL_PRO[] auf Null gesetzt wird.
|
|
GLOBAL DEF SET_CM_PRO_INACTIVE()
|
|
INT I
|
|
|
|
IF $IMPROVED_COLLMON== TRUE THEN
|
|
FOR I=1 TO 12
|
|
IF (COLLMON_TOL_PRO[I] < 0) THEN ;no global COLLMON_TOL_PRO[I] exists
|
|
$COLLMON_TOL_PRO[I]=0
|
|
$COLLMON_IDX = 0
|
|
ELSE
|
|
$COLLMON_TOL_PRO[I]=COLLMON_TOL_PRO[I] ;sets $COLLMON_TOL_PRO[I] from global
|
|
$COLLMON_IDX=COLLMON_IDX
|
|
ENDIF
|
|
ENDFOR
|
|
ELSE
|
|
FOR I=1 TO 12
|
|
$COLLMON_TOL_PRO[I]=0
|
|
ENDFOR
|
|
ENDIF
|
|
END
|
|
|
|
; Setzt $COLLMON_MAX auf 0 zurueck. Alle folgenden
|
|
; Bewegungen gehen in die Maximumsbildung ein.
|
|
GLOBAL DEF RESET_CM_MAX()
|
|
INT I
|
|
|
|
FOR I=1 TO 12
|
|
$COLLMON_MAX[I]=0
|
|
ENDFOR
|
|
END
|
|
|
|
; Sichert die aktuellen $COLLMON_TOL_PRO[]-Werte in ein
|
|
; Parameter-Feld.
|
|
; Parameter SETTING_IDX: Index des Collmon-Datensatz
|
|
GLOBAL DEF SAVE_CM_PRO_SETTING(SETTING_IDX:IN)
|
|
INT SETTING_IDX,I
|
|
|
|
FOR I=1 to 12
|
|
COLLMON_SETTING[SETTING_IDX,I]=$COLLMON_TOL_PRO[I]
|
|
ENDFOR
|
|
END
|
|
|
|
; Sichert die aktuellen $COLLMON_TOL_COM[]-Werte in ein
|
|
; Parameter-Feld.
|
|
; Parameter SETTING_IDX: Index des Collmon-Datensatz
|
|
GLOBAL DEF SAVE_CM_COM_SETTING(SETTING_IDX:IN)
|
|
INT SETTING_IDX,I
|
|
|
|
FOR I=1 to 12
|
|
COLLMON_SETTING[SETTING_IDX,I]=$COLLMON_TOL_COM[I]
|
|
ENDFOR
|
|
END
|
|
|
|
; Liest einen aktuell gespeicherten Collmon-Datensatz
|
|
; Parameter SETTING_IDX: Index des Collmon-Datensatzes
|
|
; Parameter VALUES: Array der Groesse 12 mit den Werten des Collmon-Datensatzes
|
|
GLOBAL DEF READ_CM_SETTING(SETTING_IDX:IN, VALUES[]:OUT)
|
|
INT SETTING_IDX
|
|
INT VALUES[]
|
|
INT I
|
|
|
|
IF ((SETTING_IDX < 1) OR (SETTING_IDX > NUM_COLLMON_SETTINGS)) THEN
|
|
RETURN
|
|
ENDIF
|
|
|
|
FOR I = 1 TO 12
|
|
VALUES[I] = COLLMON_SETTING[SETTING_IDX,I]
|
|
ENDFOR
|
|
END
|
|
|
|
; Beschreibt einen Collmon-Datensatz
|
|
; Parameter SETTING_IDX: Index des Collmon-Datensatzes
|
|
; Parameter VALUES: Array der Groesse 12 mit den neuen Werten des Collmon-Datensatzes
|
|
GLOBAL DEF WRITE_CM_SETTING(SETTING_IDX:IN, VALUES[]:OUT)
|
|
INT SETTING_IDX
|
|
INT VALUES[]
|
|
INT I
|
|
|
|
IF ((SETTING_IDX < 1) OR (SETTING_IDX > NUM_COLLMON_SETTINGS)) THEN
|
|
RETURN
|
|
ENDIF
|
|
|
|
FOR I = 1 TO 12
|
|
IF (VALUES[I] < 0) THEN
|
|
COLLMON_SETTING[SETTING_IDX,I] = 0
|
|
ELSE
|
|
IF VALUES[I] > 500 THEN
|
|
COLLMON_SETTING[SETTING_IDX,I] = 500
|
|
ELSE
|
|
COLLMON_SETTING[SETTING_IDX,I] = VALUES[I]
|
|
ENDIF
|
|
ENDIF
|
|
ENDFOR
|
|
END
|
|
|
|
; Sichert die aktuellen $COLLMON_MAX[]-Werte in ein
|
|
; Collmon-Datensatz-Feld. Optional kann ein Skalierungs-Prozentwert
|
|
; angegeben werden.
|
|
; Parameter SETTING_IDX: Index des Collmon-Datensatz
|
|
; Parameter OFFSET: optionaler Offset auf alle Komponenten von $COLLMON_MAX[]. Default-Wert=0.
|
|
; Expert call
|
|
GLOBAL DEF SAVE_CM_MAX_SETTING(SETTING_IDX:IN,OFFSET:IN)
|
|
INT SETTING_IDX,OFFSET,TEST,OFFSET_WERT,I
|
|
DECL INT SET_VALUES[12]
|
|
|
|
; Vorbelegung mit Default-Wert 0
|
|
OFFSET_WERT=0
|
|
|
|
ON_ERROR_PROCEED
|
|
TEST=OFFSET
|
|
|
|
IF $ERR.NUMBER == 0 THEN
|
|
OFFSET_WERT=OFFSET ; Parameter verwenden
|
|
ENDIF
|
|
|
|
GET_COLLMON_SET(SETTING_IDX, SET_VALUES[])
|
|
FOR I=1 to 12
|
|
; durch Lernfahrt will man auskeschaltete Achsen nicht einschalten
|
|
IF COLLMON_SETTING[SETTING_IDX,I] <> 0 THEN
|
|
COLLMON_SETTING[SETTING_IDX,I]=SET_VALUES[I] + OFFSET_WERT
|
|
ENDIF
|
|
ENDFOR
|
|
|
|
ERR_CLEAR($ERR)
|
|
END
|
|
|
|
; Schreibt einen gesicherten Collmon-Datensatz zurueck
|
|
; in das $COLLMON_TOL_PRO[]-Feld.
|
|
; Parameter SETTING_IDX: Index des Collmon-Datensatz
|
|
GLOBAL DEF USE_CM_PRO_SETTING(SETTING_IDX:IN)
|
|
INT SETTING_IDX,I
|
|
|
|
FOR I=1 to 12
|
|
$COLLMON_TOL_PRO[I]=COLLMON_SETTING[SETTING_IDX,I]
|
|
ENDFOR
|
|
$COLLMON_IDX = SETTING_IDX
|
|
END
|
|
; Initialisiert alle Collmon-Datensaetze auf 100.
|
|
GLOBAL DEF INIT_CM_SETTINGS()
|
|
INT I,J
|
|
|
|
FOR I=1 TO NUM_COLLMON_SETTINGS
|
|
FOR J=1 TO 12
|
|
COLLMON_SETTING[I,J]=100
|
|
ENDFOR
|
|
ENDFOR
|
|
END
|
|
|
|
;used in old motion command to set all 12 values
|
|
;Expert call
|
|
GLOBAL DEF SET_CD_PARAMS (Index :IN)
|
|
DECL INT Index
|
|
IF (Index > 0) AND (Index < 31) THEN
|
|
USE_CM_PRO_SETTING (Index)
|
|
ELSE
|
|
SET_CM_PRO_INACTIVE()
|
|
ENDIF
|
|
END
|
|
|
|
;Expert call
|
|
GLOBAL DEF USE_CD_PARAMS (Index :IN)
|
|
DECL INT Index, I
|
|
IF (Index > 0) AND (Index < 31) THEN
|
|
FOR I=1 to 12
|
|
COLLMON_TOL_PRO[I]=COLLMON_SETTING[Index,I] ; saves COLLMON_TOL_PRO for global using
|
|
ENDFOR
|
|
COLLMON_IDX=Index
|
|
USE_CM_PRO_SETTING (Index)
|
|
ELSE
|
|
FOR I=1 to 12
|
|
COLLMON_TOL_PRO[I]=-1 ; saves COLLMON_TOL_PRO for global using
|
|
ENDFOR
|
|
COLLMON_IDX=-1
|
|
SET_CM_PRO_INACTIVE()
|
|
ENDIF
|
|
END
|
|
|
|
;resets COLLMON_TOL_PRO on the begin of KRL module
|
|
;by call BAS(#INITMOV,0)
|
|
GLOBAL DEF RESET_CD_PARAMS ()
|
|
DECL INT I
|
|
FOR I=1 to 12
|
|
COLLMON_TOL_PRO[I]=-1 ; resets COLLMON_TOL_PRO for global using
|
|
ENDFOR
|
|
COLLMON_IDX=-1
|
|
END
|
|
|
|
;used in spline command to set all 12 values by just one call
|
|
;like ...WITH $COLLMON_TOL_PRO[1]=USE_CM_PRO_VALUES(SETTING_IDX)
|
|
GLOBAL DEFFCT INT USE_CM_PRO_VALUES(SETTING_IDX:IN)
|
|
INT SETTING_IDX
|
|
|
|
IF (SETTING_IDX == -1) THEN ;Force deactivation
|
|
$COLLMON_TOL_PRO[2]=0
|
|
$COLLMON_TOL_PRO[3]=0
|
|
$COLLMON_TOL_PRO[4]=0
|
|
$COLLMON_TOL_PRO[5]=0
|
|
$COLLMON_TOL_PRO[6]=0
|
|
$COLLMON_TOL_PRO[7]=0
|
|
$COLLMON_TOL_PRO[8]=0
|
|
$COLLMON_TOL_PRO[9]=0
|
|
$COLLMON_TOL_PRO[10]=0
|
|
$COLLMON_TOL_PRO[11]=0
|
|
$COLLMON_TOL_PRO[12]=0
|
|
$COLLMON_IDX = 0
|
|
RETURN 0
|
|
ENDIF
|
|
|
|
IF (SETTING_IDX > 0) AND (SETTING_IDX < 31) THEN
|
|
$COLLMON_TOL_PRO[2]=COLLMON_SETTING[SETTING_IDX,2]
|
|
$COLLMON_TOL_PRO[3]=COLLMON_SETTING[SETTING_IDX,3]
|
|
$COLLMON_TOL_PRO[4]=COLLMON_SETTING[SETTING_IDX,4]
|
|
$COLLMON_TOL_PRO[5]=COLLMON_SETTING[SETTING_IDX,5]
|
|
$COLLMON_TOL_PRO[6]=COLLMON_SETTING[SETTING_IDX,6]
|
|
$COLLMON_TOL_PRO[7]=COLLMON_SETTING[SETTING_IDX,7]
|
|
$COLLMON_TOL_PRO[8]=COLLMON_SETTING[SETTING_IDX,8]
|
|
$COLLMON_TOL_PRO[9]=COLLMON_SETTING[SETTING_IDX,9]
|
|
$COLLMON_TOL_PRO[10]=COLLMON_SETTING[SETTING_IDX,10]
|
|
$COLLMON_TOL_PRO[11]=COLLMON_SETTING[SETTING_IDX,11]
|
|
$COLLMON_TOL_PRO[12]=COLLMON_SETTING[SETTING_IDX,12]
|
|
$COLLMON_IDX = SETTING_IDX
|
|
RETURN COLLMON_SETTING[SETTING_IDX,1]
|
|
ELSE
|
|
IF (COLLMON_TOL_PRO[1] < 0) THEN ;no global COLLMON_TOL_PRO[I] exists
|
|
$COLLMON_TOL_PRO[2]=0
|
|
$COLLMON_TOL_PRO[3]=0
|
|
$COLLMON_TOL_PRO[4]=0
|
|
$COLLMON_TOL_PRO[5]=0
|
|
$COLLMON_TOL_PRO[6]=0
|
|
$COLLMON_TOL_PRO[7]=0
|
|
$COLLMON_TOL_PRO[8]=0
|
|
$COLLMON_TOL_PRO[9]=0
|
|
$COLLMON_TOL_PRO[10]=0
|
|
$COLLMON_TOL_PRO[11]=0
|
|
$COLLMON_TOL_PRO[12]=0
|
|
$COLLMON_IDX = 0
|
|
RETURN 0
|
|
ELSE
|
|
$COLLMON_TOL_PRO[2]=COLLMON_TOL_PRO[2] ;sets $COLLMON_TOL_PRO[I] from global
|
|
$COLLMON_TOL_PRO[3]=COLLMON_TOL_PRO[3] ;sets $COLLMON_TOL_PRO[I] from global
|
|
$COLLMON_TOL_PRO[4]=COLLMON_TOL_PRO[4] ;sets $COLLMON_TOL_PRO[I] from global
|
|
$COLLMON_TOL_PRO[5]=COLLMON_TOL_PRO[5] ;sets $COLLMON_TOL_PRO[I] from global
|
|
$COLLMON_TOL_PRO[6]=COLLMON_TOL_PRO[6] ;sets $COLLMON_TOL_PRO[I] from global
|
|
IF (COLLMON_TOL_PRO[7]>-1) THEN
|
|
$COLLMON_TOL_PRO[7]=COLLMON_TOL_PRO[7] ;sets $COLLMON_TOL_PRO[I] from global
|
|
ELSE
|
|
$COLLMON_TOL_PRO[7]=0
|
|
ENDIF
|
|
IF (COLLMON_TOL_PRO[8]>-1) THEN
|
|
$COLLMON_TOL_PRO[8]=COLLMON_TOL_PRO[8] ;sets $COLLMON_TOL_PRO[I] from global
|
|
ELSE
|
|
$COLLMON_TOL_PRO[8]=0
|
|
ENDIF
|
|
IF (COLLMON_TOL_PRO[9]>-1) THEN
|
|
$COLLMON_TOL_PRO[9]=COLLMON_TOL_PRO[9] ;sets $COLLMON_TOL_PRO[I] from global
|
|
ELSE
|
|
$COLLMON_TOL_PRO[9]=0
|
|
ENDIF
|
|
IF (COLLMON_TOL_PRO[10]>-1) THEN
|
|
$COLLMON_TOL_PRO[10]=COLLMON_TOL_PRO[10] ;sets $COLLMON_TOL_PRO[I] from global
|
|
ELSE
|
|
$COLLMON_TOL_PRO[10]=0
|
|
ENDIF
|
|
IF (COLLMON_TOL_PRO[11]>-1) THEN
|
|
$COLLMON_TOL_PRO[11]=COLLMON_TOL_PRO[11] ;sets $COLLMON_TOL_PRO[I] from global
|
|
ELSE
|
|
$COLLMON_TOL_PRO[11]=0
|
|
ENDIF
|
|
IF (COLLMON_TOL_PRO[12]>-1) THEN
|
|
$COLLMON_TOL_PRO[12]=COLLMON_TOL_PRO[12] ;sets $COLLMON_TOL_PRO[I] from global
|
|
ELSE
|
|
$COLLMON_TOL_PRO[12]=0
|
|
ENDIF
|
|
$COLLMON_IDX=COLLMON_IDX
|
|
RETURN COLLMON_TOL_PRO[1] ;returns $COLLMON_TOL_PRO[I] from global
|
|
ENDIF
|
|
ENDIF
|
|
ENDFCT
|
|
|
|
;Expert call
|
|
GLOBAL DEF SET_TQ_VALUES ( TQ_AX1:IN, TQ_AX2:IN, TQ_AX3:IN, TQ_AX4:IN, TQ_AX5:IN, TQ_AX6:IN)
|
|
INT I, TQ_AX1, TQ_AX2, TQ_AX3, TQ_AX4, TQ_AX5, TQ_AX6
|
|
|
|
IF $IMPROVED_COLLMON== TRUE THEN
|
|
ON_ERROR_PROCEED
|
|
$COLLMON_TOL_PRO[1]=TQ_AX1
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$COLLMON_TOL_PRO[2]=TQ_AX2
|
|
$COLLMON_TOL_PRO[3]=TQ_AX3
|
|
$COLLMON_TOL_PRO[4]=TQ_AX4
|
|
$COLLMON_TOL_PRO[5]=TQ_AX5
|
|
$COLLMON_TOL_PRO[6]=TQ_AX6
|
|
ELSE
|
|
FOR I=1 TO 12
|
|
$COLLMON_TOL_PRO[I]=$COLLMON_TOL_PRO_DEF[I]
|
|
ENDFOR
|
|
ERR_CLEAR($ERR)
|
|
ENDIF
|
|
FOR I=1 TO 12
|
|
COLLMON_TOL_PRO[I]=$COLLMON_TOL_PRO[I]
|
|
ENDFOR
|
|
COLLMON_IDX=0
|
|
$COLLMON_IDX = 0
|
|
ELSE
|
|
ON_ERROR_PROCEED
|
|
$TORQMON[1]=TQ_AX1
|
|
IF $ERR.NUMBER == 0 THEN
|
|
$TORQMON[2]=TQ_AX2
|
|
$TORQMON[3]=TQ_AX3
|
|
$TORQMON[4]=TQ_AX4
|
|
$TORQMON[5]=TQ_AX5
|
|
$TORQMON[6]=TQ_AX6
|
|
ELSE
|
|
FOR I=1 TO 12
|
|
$TORQMON[I]=$TORQMON_DEF[I]
|
|
ENDFOR
|
|
ERR_CLEAR($ERR)
|
|
ENDIF
|
|
ENDIF
|
|
END
|
