Hallo zusammen,
ich möchte gerne die Roboterposition bei einem Neustart der Anlage oder zurücksetzen auf PZ-Main abfragen und anschließend in einer IF Bedingung vergleichen ob diese Position einer der Safe Positionen entspricht.
Folgend bin ich hierzu vorgegangen:
Var robtarget p1;
VAR num merkerFreigabeSafepos:=0;
CONST robtarget pSafepos:=[[1028.919,1002.525,1243.223],[0,0,0,1],[0,-1,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pSafepos2:=[[1173.2,-489.549,1452.46],[0,-0.707107,0.707107,0],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pSafepos3:=[[-678.919,-852.525,1543.223],[1,0,0,0],[-2,-1,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PROC CheckSafepos()
p1:=CRobT();
if p1 = pSafepos or p1 = pSafepos2 or p1 = pSafepos3 Then
merkerFreigabeSafepos:=1;
Else
merkerFreigabeSafepos:=0;
Endif
Endproc
Wenn ich das nun Robstudio schritt für schritt durchgehe und der Roboter auf einer Safepos steht, springt dieser nie in die Ja Bedingung.
Unterschiedliche Werte habe ich in den Quaternion Q1 und Q4.
Wenn ich jedoch im bewegen Fenster bin und die Quaternion anschaue sind diese wie im Robtarget, bzw. bei der Zuweisung p1:=CRobT() stimmt Q1 und Q4 nicht.
Habt ihr mir hierfür einen Lösungsansatz, ggf. auch alternative Lösung?