Bosch REXROTH MLC 13VRS Robot-Control V2 Referenz-Anleitung Seite 334

332/401 Bosch Rexroth AG
Anwendungsbeispiele
Implementieren der Rückwärts-
Transformation
METHOD Inverse : INT
VAR_INPUT
OriginalCoordinates
JointCoordinates
END_VAR
VAR
ATCP, AB : REAL;
Beta : REAL; //d__Angle_ATCP_X
Delta : REAL; //d__Angle_AC_ATCP
Alpha : REAL; //d__Angle_AC_X
Ax, Bx : REAL;
TCPx, TCPy : REAL;
GRAD_RAD_FAKTOR : REAL :=
END_VAR
IF (Ymin > OriginalCoordinates[2] OR Ymax <= OriginalCoordinates[2]) THEN
Inverse := 1; // unreachable
RETURN;
END_IF
TCPx := OriginalCoordinates[1];
TCPy := OriginalCoordinates[2];
ATCP := SQRT((L1*L1) + (L2*L2) - (2 * L1 * L2 * COS(Gamma* GRAD_RAD_FAKTOR))); // Law of cosines
Ax := TCPx - SQRT(ATCP*ATCP - TCPy*TCPy);
Beta := ATAN2(-TCPy, (TCPx-Ax));
Delta := ACOS( -((L2*L2) - (L1*L1) - (ATCP * ATCP)) / (2 * L1 * ATCP) ); // Law of cosines
Alpha := Beta + Delta;
AB := 2 * L1 * COS(Alpha);
Bx := Ax + AB;
JointCoordinates[1] := Ax;
JointCoordinates[2] := Bx;
Inverse := 0;
Anmelden der Transformation an
eine Kinematik
PROGRAM PlcProg
VAR
myDeltaSlide
setKinTrafo : MC_SetKinTransform;
initTransformation : BOOL;
END_VAR
IF(initTransformation) THEN
setKinTrafo.KinTransform := myDeltaSlide;
setKinTrafo.Execute
setKinTrafo(AxisGroup := K1);
IF(setKinTrafo.Done) THEN
initTransformation
setKinTrafo.Execute
setKinTrafo(AxisGroup := K1);
END_IF
IF(setKinTrafo.Error) THEN
initTransformation
END_IF
END_IF
Die Rückwärts-Transformation berechnet aus Koordinaten im Basis-Koordi‐
natensystem Koordinaten im Achs-Koordinatensystem.
Zu beachten sind die Fehlerfälle, wenn der Punkt nicht erreicht werden kann.
Rückwärts-Transformation
: REFERENCE TO ARRAY [1..16] OF REAL;
: REFERENCE TO ARRAY [1..16] OF REAL;
0.01745329251994329;
Es wird angenommen, dass eine Kinematik mit dem Namen "K1" existiert.
Für diese soll die Transformation "DeltaSlide" angemeldet werden.
Zu beachten ist, dass die Instanz myDeltaSlide an der Kinematik angemeldet
wird und nicht der Funktionsbaustein-Typ DeltaSlide.
Auch für jede weitere Kinematiken ist immer eine eigene Instanz des Funk‐
tionsbaustein DeltaSlide zu verwenden.
Verwendung
: DeltaSlide;
:= TRUE;
:= FALSE;
:= FALSE;
:= FALSE;
DOK-MLC***-ROCO****V13-RE03-DE-P
Rexroth IndraMotion MLC 13VRS Robot-Control V2
// (2*PI)/360