Guten Morgen,
also ich muss doch etwas Code zeigen, ist teilweise etwas unschön, weil ich gedacht hab ob vielleicht der Vorlaufzeiger was damit zu tun hat.
Ich starte den Submit im Teachprogramm und bleib dort drin bis er wieder abgewählt wurde und danach wird erst wieder eine SAK Fahrt gemacht, darum glaub ich weniger an an zeitliches Problem.
Ich mach das ganze über den Submit weil ich ein Dialog anzeige (wenn Zustimmtaster gedrückt wieder lösche) in dem ich die Abweichung zur Soll Position Anzeige und wenn die Abweichung zu groß ist gar nicht zulasse das geteacht wird.
Und fürs Base brauche ich drei Punkte, im Hauptprogramm würde immer wieder eine SAK Fahrt gemacht werden.
Unterprogramm mit An/Abfahrt und Aufruf zum Teachprogramm:
PTP startAxTeachBase
BAS(#VEL_CP,velSchnell)
TEACH_BASE_ERNEUT:
LIN {X 0,Y 0,Z 70,A 0,B 0,C 0}:ptRef
WAIT SEC 0.2
Base_Teachen(p1TeachBase,p2TeachBase,p3TeachBase,baseKorr,teachBaseMaxDist)
WAIT FOR NOT BaseTeachenAktiv
IF BaseTeachenErneut THEN
GOTO TEACH_BASE_ERNEUT
ENDIF
IF BaseTeachenSpeichern THEN
BaseTeachenSpeichern = FALSE
baseKorr = BaseTeachenKorrNeu
WAIT FOR TRUE
BaseTeachenKorrNeu = $NULLFRAME
WAIT FOR TRUE
Base_Aktiv(baseData:baseKorr)
LIN {X 0,Y 0,Z 30,A 0,B 0,C 0}:ptRef
LIN {X 0,Y 0,Z 2,A 0,B 0,C 0}:ptRef
HALT
ENDIF
LIN {X 0,Y 0,Z 70,A 0,B 0,C 0}:ptRef
PTP startAxTeachBase
Display More
Unterprogramm .dat
CONST FRAME baseData={X 877.819,Y -2524.93896,Z 924.000,A -70.0000,B 0.0,C 0.0}
FRAME baseKorr={X 4.01829338,Y -68.3535,Z -22.9292736,A -1.41691315,B 0.0170311499,C -0.214197621}
CONST E6AXIS startAxTeachBase={A1 75.0000,A2 -110.000,A3 118.000,A4 -15.0000,A5 20.0000,A6 15.0000,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
CONST FRAME p1TeachBase={X 0.0,Y 0.0,Z 0.0,A 180.000,B 0.0,C 180.000}
CONST FRAME p2TeachBase={X 100.000}
CONST FRAME p3TeachBase={X 100.000,Y 100.000}
Base_Teachen.src
BaseTeachenAkt = $BASE
BaseTeachenP1 = p1
BaseTeachenP2 = p1
BaseTeachenP3 = p1
BaseTeachenP2.X = p2.X
BaseTeachenP3.X = p3.X
BaseTeachenP3.Y = p3.Y
BaseTeachenKorrAkt = baseKorr
tmpBaseTeachenKorrNeu = $NULLFRAME
CWRITE ($CMD, STAT, MODE, "RUN /R1/Base_Teach_Sub() > 9")
WAIT FOR $PROG_INFO[9].P_STATE == #P_ACTIVE
WAIT FOR $PROG_INFO[9].P_STATE <> #P_ACTIVE
CWRITE ($CMD, STAT, MODE, "STOP 9")
CWRITE ($CMD, STAT, MODE, "CANCEL 9")
WAIT FOR $PROG_INFO[9].P_STATE == #P_FREE
IF BaseTeachenAktiv THEN
BaseTeachenAktiv = FALSE
IF BaseTeachenSpeichern THEN
BaseTeachenNeu = Base_Berechnen(BaseTeachenP1,BaseTeachenP2,BaseTeachenP3)
BaseTeachenKorrNeu = BaseTeachenKorrAkt:BaseTeachenNeu
pErg = BaseTeachenNeu
ofst = 0
SWRITE(erg[], st, ofst, "Korrektur: X%.3f Y%.3f Z%.3f A%.3f B%.3f C%.3f", pErg.x,pErg.y,pErg.z,pErg.a,pErg.b,pErg.c)
MsgDialog(nAntw,erg[],"Base_Korrektur",,,,,,"Abbrechen","Erneut","Speichern")
SWITCH nAntw
CASE 1
tmpBaseTeachenKorrNeu = BaseTeachenKorrNeu
BaseTeachenErneut = FALSE
RETURN
CASE 2
BaseTeachenSpeichern = FALSE
BaseTeachenErneut = TRUE
RETURN
ENDSWITCH
ENDIF
ENDIF
BaseTeachenKorrNeu = baseKorr
BaseTeachenSpeichern = FALSE
BaseTeachenErneut = FALSE
Display More
Base_Teachen.dat
GLOBAL FRAME BaseTeachenAkt={X 9316.05469,Y -1835.74524,Z 529.635864,A -0.0274604075,B 0.0739889294,C 0.209390864}
FRAME BaseTeachenNeu={X -0.968746185,Y -1.02284896,Z 39.4307098,A -0.0412013605,B 0.00451369165,C -0.0691743195}
FRAME BaseTeachenKorrAkt={X -10.0145922,Y 8.75479317,Z -19.3641109,A -0.0274604075,B 0.0739889294,C 0.209390864}
FRAME tmpBaseTeachenKorrNeu={X 0.0,Y 0.0,Z 0.0,A 0.0,B 0.0,C 0.0}
GLOBAL FRAME BaseTeachenKorrNeu={X 0.0,Y 0.0,Z 0.0,A 0.0,B 0.0,C 0.0}
GLOBAL FRAME BaseTeachenP1={X -0.968746185,Y -1.02284896,Z 39.4307098,A 134.999969,B 0.00190862385,C -179.999817}
GLOBAL FRAME BaseTeachenP2={X 1075.27844,Y -1.79677784,Z 39.3459244,A 134.999939,B 0.000123272883,C -180.000}
GLOBAL FRAME BaseTeachenP3={X 533.203613,Y 716.069275,Z 38.5224457,A 134.999969,B -0.000376011682,C -179.999985}
GLOBAL INT BaseTeachenMaxDist=150
GLOBAL BOOL BaseTeachenAktiv=FALSE
GLOBAL BOOL BaseTeachenSpeichern=FALSE
GLOBAL BOOL BaseTeachenErneut=FALSE
Display More
Submit
DECL KrlMsg_T msg
DECL KrlMsgPar_T par[3]
DECL KrlMsgOpt_T opt
DECL KrlMsgDlgSK_T SK[7]
DECL State_T state
INT nHandle, keyNr, ofst, pNum
FRAME pAkt,p1,p2,p3,baseOfst,baseNeu
BaseTeachenSpeichern = FALSE
BaseTeachenAktiv = TRUE
msg.msg_txt[] = " "
msg.modul[] = "Base_Teachen"
msg.Nr = 1
SK[1] = {sk_Type #empty, sk_txt[] "TouchUp"}
SK[2] = {sk_Type #value, sk_txt[] "Abbrechen"}
pNum = 1
baseOfst = $NULLFRAME
baseNeu = BaseTeachenAkt
LOOP
WAIT FOR StrClear(msg.msg_txt[])
SWITCH pNum
CASE 1
pAkt = INV_POS(BaseTeachenP1):$POS_ACT_MES
ofst = 0
SWRITE(msg.msg_txt[], state, ofst, "P1(NP): X%.2f Y%.2f Z%.2f", pAkt.x,pAkt.y,pAkt.z)
CASE 2
pAkt = $POS_ACT_MES:INV_POS(BaseTeachenP2):INV_POS(baseNeu):BaseTeachenAkt
ofst = 0
SWRITE(msg.msg_txt[], state, ofst, "P2(+X): X%.2f Y%.2f Z%.2f", pAkt.x,pAkt.y,pAkt.z)
CASE 3
pAkt = $POS_ACT_MES:INV_POS(BaseTeachenP3):INV_POS(baseNeu):BaseTeachenAkt
ofst = 0
SWRITE(msg.msg_txt[], state, ofst, "P3(+Y): X%.2f Y%.2f Z%.2f", pAkt.x,pAkt.y,pAkt.z)
DEFAULT
EXIT
ENDSWITCH
IF (distanz(pAkt) < BaseTeachenMaxDist) THEN
SK[1].sk_type = #value
ELSE
SK[1].sk_type = #empty
ENDIF
IF ($SAFETY_SW == #RELEASED) THEN
nHandle = SET_KRLDLG(msg, par[], SK[], opt)
IF (nHandle > 0) THEN
WHILE (Exists_KrlDlg(nHandle, keyNr))
WAIT SEC 0.2
IF ($SAFETY_SW == #PRESSED) THEN
WAIT FOR CLEAR_KRLMSG(nHandle) OR TRUE
nHandle = -1
ENDIF
ENDWHILE
SWITCH keyNr
CASE 1 ;TouchUp
SWITCH pNum
CASE 1
p1 = $POS_ACT_MES
baseOfst.X = p1.X
baseOfst.Y = p1.Y
baseOfst.Z = p1.Z
baseNeu = BaseTeachenAkt:baseOfst
pNum = 2
nHandle = -1
CASE 2
p2 = $POS_ACT_MES
pNum = 3
nHandle = -1
CASE 3
p3 = $POS_ACT_MES
BaseTeachenP1 = p1
BaseTeachenP2 = p2
BaseTeachenP3 = p3
BaseTeachenSpeichern = TRUE
pNum = 0
nHandle = -1
DEFAULT
nHandle = -1
EXIT
ENDSWITCH
CASE 2 ;Abbrechen
nHandle = -1
EXIT
ENDSWITCH
ENDIF
ENDIF
ENDLOOP
Display More