&ACCESS RVE
&REL 23
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF Anfahrt_Ablage(AblageNr:IN,Aufgabe:IN)
;FOLD Deklarationen
DECL E6POS e6Vorpos
DECL REAL rLinGeschwLangsam,rLinGeschwSchnell,rTansportGeschw,rTransportGeschRed
DECL ENUM_AUFGABE Aufgabe
DECL INT StationBase
DECL INT AblageNr
;ENDFOLD
;FOLD Interrups Deklarieren
;ENDFOLD
;*****************************************************************************
; Ein-und Ausfahren in/aus Station
; auf Grundlage der Geteachten Endposition
; Nach ablegen wird in Home gefahren
; Bei holen wir von Vorposition weitergefahren
;*****************************************************************************
; Mögliche Aufgaben:
; TR_HOLEN --> Träger holen
; TR_BRINGEN --> Träger bringen
; PP_HOLEN --> Polpaket holen
; PP_BRINGEN --> Polpaket bringen
;*****************************************************************************
rLinGeschwLangsam = 0.02
rLinGeschwSchnell = 0.5
rTransportGeschw = 100
rTansportGeschRed = 30
;FOLD Prozess Vorbereiten
;------------------------------------------------------------------------
;Base setzen, nicht Aendern!
StationBase = 5
;Auf bereitschaft der Station warten
;Nicht notwenig, mechanische Ablage
;Ablage Nr Merken
;Bei uebergabe von 0 ist Nr von SPS vorgegeben
IF AblageNr<>0 THEN
AblageNrSaved = STAT_ABL_POS_NR
ELSE
AblageNrSaved = AblageNr
ENDIF
;Interrupts aktivieren
;Keine verfuegbar
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Positionen Vorladen
;------------------------------------------------------------------------
; Bei Polpaketd holen oder bringen POLPOAKET Endpos Vorladen
IF (Aufgabe==#PP_BRINGEN) OR (Aufgabe==#PP_HOLEN) OR (Aufgabe==#TR_BRINGEN) OR (Aufgabe==#TR_HOLEN) THEN
IF (Aufgabe==#PP_BRINGEN) OR (Aufgabe==#PP_HOLEN) THEN
SWITCH AblageNrSaved
CASE 1
e6Vorpos = e6EndPos_Ablage_Rechts
CASE 2
e6Vorpos = e6EndPos_Ablage_Links
CASE 3
e6Vorpos = e6EndPos_Ablage_Vorne
ENDSWITCH
ENDIF
IF (Aufgabe==#TR_BRINGEN) OR (Aufgabe==#TR_HOLEN) THEN
SWITCH AblageNrSaved
CASE 1
e6Vorpos = e6EndPos_Abl_Rechts_Tr
CASE 2
e6Vorpos = e6EndPos_Abl_Links_Tr
CASE 3
e6Vorpos = e6EndPos_Abl_Vorne_Tr
ENDSWITCH
ENDIF
ELSE
HALT
ENDIF
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Vorposition Berechnen
;------------------------------------------------------------------------
;Vorpositionen je nach Aufgabe Berechnen
SWITCH Aufgabe
CASE #PP_HOLEN
e6Vorpos.Y = e6Vorpos.Y - 17
e6Vorpos.Z = e6Vorpos.Z - 200
CASE #PP_BRINGEN
e6Vorpos.X = e6Vorpos.X - 5
e6Vorpos.Y = e6Vorpos.Y + 70
e6Vorpos.Z = e6Vorpos.Z - 200
CASE #TR_HOLEN
e6Vorpos.Y = e6Vorpos.Y - 17
e6Vorpos.Z = e6Vorpos.Z - 200
CASE #TR_BRINGEN
e6Vorpos.Z = e6Vorpos.Z - 200
ENDSWITCH
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Kollisionserkennung aktivieren
;------------------------------------------------------------------------
;Zulässiges Drehmoment in % pro Achse(default 200)
$TORQMON_COM_DEF[1] = 100
$TORQMON_COM_DEF[2] = 100
$TORQMON_COM_DEF[3] = 100
$TORQMON_COM_DEF[4] = 100
$TORQMON_COM_DEF[5] = 100
$TORQMON_COM_DEF[6] = 100
;------------------------------------------------------------------------
;ENDFOLD
;Einfaedeln und Greifen bzw Ablegen
;Jede Aufgabe hat Ihren eigenen Ablauf
;FOLD Traeger Holen
SWITCH Aufgabe
;------------------------------------------------------------------------
CASE #TR_HOLEN ;---------------------------------------------------------
;------------------------------------------------------------------------
SetSpeedToolBase(14,StationBase,rLinGeschwSchnell,rTransportGeschw)
IN_LOCAL_HOME=FALSE
;FOLD Lokale Home anfahren
PTP LOCAL_HOME_POS C_PTP
IN_LOCAL_HOME=TRUE
;ENDFOLD
PTP e6Vorpos C_PTP
GRIPPER(#DO_OPEN,FALSE,TRUE)
LIN_REL {Z 203} C_DIS #BASE
SetSpeedToolBase(14,StationBase,rLinGeschwLangsam,rTransportGeschw)
LIN_REL {Y 17} #BASE
GRIPPER(#DO_LOW,FALSE,TRUE)
GRIPPER(#DO_CLOSE_CARRY,FALSE,TRUE)
WAIT FOR $IN[35] ; Was ist das ???? ToDo
LIN_REL {Z -10} C_DIS #BASE
GRIPPER(#DO_FULL,FALSE,TRUE)
SetSpeedToolBase(16,StationBase,rLinGeschwSchnell,rTransportGeschw)
LIN_REL {Z -200} C_DIS #BASE
;FOLD Kolliserkennung reset default
$TORQMON_COM_DEF[1] = 200
$TORQMON_COM_DEF[2] = 200
$TORQMON_COM_DEF[3] = 200
$TORQMON_COM_DEF[4] = 200
$TORQMON_COM_DEF[5] = 200
$TORQMON_COM_DEF[6] = 200
PTP LOCAL_HOME_POS C_PTP
;ENDFOLD
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Traeger Bringen
;------------------------------------------------------------------------
CASE #TR_BRINGEN ;-------------------------------------------------------
;------------------------------------------------------------------------
SetSpeedToolBase(16,StationBase,rLinGeschwSchnell,rTransportGeschw)
IN_LOCAL_HOME=FALSE
;FOLD Lokale Home anfahren
PTP LOCAL_HOME_POS C_PTP
IN_LOCAL_HOME=TRUE
;ENDFOLD
PTP e6Vorpos C_PTP
CHECK_ABL_LEER()
LIN_REL {Z 180} C_DIS #BASE
SetSpeedToolBase(16,StationBase,rLinGeschwLangsam,rTransportGeschw)
LIN_REL {Z 23} C_DIS #BASE
GRIPPER(#DO_OPEN,FALSE,TRUE)
LIN_REL {Y -17} C_DIS #BASE
SetSpeedToolBase(16,StationBase,rLinGeschwSchnell,rTransportGeschw)
LIN_REL {Z -200} C_DIS #BASE
;FOLD Kolliserkennung reset default
$TORQMON_COM_DEF[1] = 200
$TORQMON_COM_DEF[2] = 200
$TORQMON_COM_DEF[3] = 200
$TORQMON_COM_DEF[4] = 200
$TORQMON_COM_DEF[5] = 200
$TORQMON_COM_DEF[6] = 200
;ENDFOLD
PTP LOCAL_HOME_POS C_PTP
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Polpaket Holen
;------------------------------------------------------------------------
CASE #PP_HOLEN ;---------------------------------------------------------
;------------------------------------------------------------------------
SetSpeedToolBase(14,StationBase,rLinGeschwSchnell,rTransportGeschw)
IN_LOCAL_HOME=FALSE
;FOLD Lokale Home anfahren
PTP LOCAL_HOME_POS C_PTP
IN_LOCAL_HOME=TRUE
;ENDFOLD
PTP e6Vorpos C_PTP
CHECK_ABL_N_GESPANNT()
WAIT FOR inTraegerNichtGespannt
GRIPPER(#DO_OPEN,FALSE,TRUE)
LIN_REL {Z 205} C_DIS #BASE
SetSpeedToolBase(14,StationBase,rLinGeschwLangsam,rTransportGeschw)
LIN_REL {Y 17} #BASE
GRIPPER(#DO_LOW,FALSE,TRUE)
GRIPPER(#DO_CLOSE_CARRY,FALSE,TRUE)
LIN_REL {Z -10} C_DIS #BASE
GRIPPER(#DO_FULL,FALSE,TRUE)
SetSpeedToolBase(16,StationBase,rLinGeschwSchnell,rTransportGeschw)
LIN_REL {Z -200} C_DIS #BASE
;FOLD Kolliserkennung reset default
$TORQMON_COM_DEF[1] = 200
$TORQMON_COM_DEF[2] = 200
$TORQMON_COM_DEF[3] = 200
$TORQMON_COM_DEF[4] = 200
$TORQMON_COM_DEF[5] = 200
$TORQMON_COM_DEF[6] = 200
;ENDFOLD
PTP LOCAL_HOME_POS C_PTP
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Polpaket bringen
;------------------------------------------------------------------------
CASE #PP_BRINGEN ;-------------------------------------------------------
;------------------------------------------------------------------------
SetSpeedToolBase(15,StationBase,rEinfahrGeschw,rTransportGeschw)
IN_LOCAL_HOME=FALSE
;FOLD Lokale Home anfahren
PTP LOCAL_HOME_POS C_PTP
IN_LOCAL_HOME=TRUE
;ENDFOLD
PTP e6Vorpos C_PTP
WAIT FOR inTraegerNichtGespannt
CHECK_ABL_NUR_TRAEGER()
LIN_REL {Z 175} C_DIS #BASE
SetSpeedToolBase(15,StationBase,rLinGeschwLangsam,rTransportGeschw)
LIN_REL {Y -60} C_DIS #BASE
LIN_REL {X 5, Y -10, Z 25} C_DIS #BASE
GRIPPER(#DO_OPEN,SIMULATE,TRUE)
SetSpeedToolBase(14,StationBase,rLinGeschwLangsam,rTransportGeschw)
LIN_REL {Y -17} C_DIS #BASE
SetSpeedToolBase(14,StationBase,rLinGeschwLangsam,rTransportGeschw)
LIN_REL {Z -200} C_DIS #BASE
;FOLD Kolliserkennung reset default
$TORQMON_COM_DEF[1] = 200
$TORQMON_COM_DEF[2] = 200
$TORQMON_COM_DEF[3] = 200
$TORQMON_COM_DEF[4] = 200
$TORQMON_COM_DEF[5] = 200
$TORQMON_COM_DEF[6] = 200
;ENDFOLD
PTP LOCAL_HOME_POS C_PTP
DEFAULT
;Nicht Definierte Aufgabe = Stopp !
HALT
ENDSWITCH
;------------------------------------------------------------------------
;ENDFOLD
;FOLD Zuruecksetzen
IN_LOCAL_HOME=TRUE
;ENDFOLD
END
;FOLD Unterprogramm CHECK_ABL_LEER
DEF CHECK_ABL_LEER()
SWITCH AblageNrSaved
CASE 1
WAIT FOR (STAT_ABL1_LEER) AND NOT (STAT_ABL1_GESPANNT) AND NOT (STAT_ABL1_NICHT_GESPANNT) AND NOT (STAT_ABL1_PP_VORHANDEN)
CASE 2
WAIT FOR (STAT_ABL2_LEER) AND NOT (STAT_ABL2_GESPANNT) AND NOT (STAT_ABL2_NICHT_GESPANNT) AND NOT (STAT_ABL2_PP_VORHANDEN)
CASE 3
WAIT FOR (STAT_ABL3_LEER) AND NOT (STAT_ABL3_GESPANNT) AND NOT (STAT_ABL3_NICHT_GESPANNT) AND NOT (STAT_ABL3_PP_VORHANDEN)
ENDSWITCH
END
;ENDFOLD
;FOLD Unterprogramm CHECK_ABL_GESPANNT
DEF CHECK_ABL_GESPANNT()
SWITCH AblageNrSaved
CASE 1
WAIT FOR NOT (STAT_ABL1_LEER) AND (STAT_ABL1_GESPANNT) AND NOT (STAT_ABL1_NICHT_GESPANNT) AND (STAT_ABL1_PP_VORHANDEN)
CASE 2
WAIT FOR NOT (STAT_ABL2_LEER) AND (STAT_ABL2_GESPANNT) AND NOT (STAT_ABL2_NICHT_GESPANNT) AND (STAT_ABL2_PP_VORHANDEN)
CASE 3
WAIT FOR NOT (STAT_ABL3_LEER) AND (STAT_ABL3_GESPANNT) AND NOT (STAT_ABL3_NICHT_GESPANNT) AND (STAT_ABL3_PP_VORHANDEN)
ENDSWITCH
END
;ENDFOLD
;FOLD Unterprogramm CHECK_ABL_N_GESPANNT
DEF CHECK_ABL_N_GESPANNT()
SWITCH AblageNrSaved
CASE 1
WAIT FOR NOT (STAT_ABL1_LEER) AND NOT (STAT_ABL1_GESPANNT) AND (STAT_ABL1_NICHT_GESPANNT) AND (STAT_ABL1_PP_VORHANDEN)
CASE 2
WAIT FOR NOT (STAT_ABL2_LEER) AND NOT (STAT_ABL2_GESPANNT) AND (STAT_ABL2_NICHT_GESPANNT) AND (STAT_ABL2_PP_VORHANDEN)
CASE 3
WAIT FOR NOT (STAT_ABL3_LEER) AND NOT (STAT_ABL3_GESPANNT) AND (STAT_ABL3_NICHT_GESPANNT) AND (STAT_ABL3_PP_VORHANDEN)
ENDSWITCH
END
;ENDFOLD
;FOLD Unterprogramm CHECK_ABL_NUR_TRAEGER
DEF CHECK_ABL_NUR_TRAEGER()
SWITCH AblageNrSaved
CASE 1
WAIT FOR NOT (STAT_ABL1_LEER) AND NOT (STAT_ABL1_GESPANNT) AND (STAT_ABL1_NICHT_GESPANNT) AND NOT (STAT_ABL1_PP_VORHANDEN)
CASE 2
WAIT FOR NOT (STAT_ABL2_LEER) AND NOT (STAT_ABL2_GESPANNT) AND (STAT_ABL2_NICHT_GESPANNT) AND NOT (STAT_ABL2_PP_VORHANDEN)
CASE 3
WAIT FOR NOT (STAT_ABL3_LEER) AND NOT (STAT_ABL3_GESPANNT) AND (STAT_ABL3_NICHT_GESPANNT) AND NOT (STAT_ABL3_PP_VORHANDEN)
ENDSWITCH
END
;ENDFOLD
Alles anzeigen