Bosch REXROTH MLC 13VRS Robot-Control V2 Referenz-Anleitung Seite 337
Vorschau ausblenden
Andere Handbücher für REXROTH MLC 13VRS Robot-Control V2:
- Funktionsbeschreibung (610 Seiten) ,
- Programmieranleitung (332 Seiten) ,
- Projektierungsbeschreibung (244 Seiten)
Inhaltsverzeichnis
Werbung
DOK-MLC***-ROCO****V13-RE03-DE-P
Rexroth IndraMotion MLC 13VRS Robot-Control V2
RETURN;
END_IF
dPHI11 :=
dPHI1 * GRAD_RAD_FAKTOR;
dPHI12 := ( dPHI2 * GRAD_RAD_FAKTOR ) + dPHI11;
BCS[1] := ( dL1 * COS( dPHI11 ) ) + ( dL2 * COS( dPHI12 ) );
BCS[2] := ( dL1 * SIN( dPHI11 ) ) + ( dL2 * SIN( dPHI12 ) );
Implementieren der Rückwärts-
Transformation
METHOD Inverse : UDINT
VAR_INPUT
BCS
: REFERENCE TO ARRAY [1..16] OF LREAL;
ACS
: REFERENCE TO ARRAY [1..16] OF LREAL;
END_VAR
VAR
dLengthTCP : LREAL;
dGamma : LREAL;
dBeta : LREAL;
dTheta : LREAL;
dTemp : LREAL;
dPHI1 : LREAL;
dPHI2 : LREAL;
dphi11 : LREAL;
dphi21 : LREAL;
END_VAR
(*************************
Get Length of TCP-Origin
*************************)
dLengthTCP := SQRT(BCS[1]*BCS[1] + BCS[2]*BCS[2]);
IF ( (dLengthTCP - FLT_EPSILON) > (dL1 + dL2) ) THEN
Inverse := 16#F22B0022; // too far away
RETURN;
END_IF
IF ( dLengthTCP > (dL1 + dL2) ) THEN
dLengthTCP := dL1 + dL2;
END
(**************
Calc d__Gamma
**************)
dTemp
:=
( (dL1 * dL1) + (dL2 * dL2) - (dLengthTCP * dLengthTCP) )
/ ( 2.0 * dL1 * dL2 );
IF ( ABS(dTemp) > 1.0 ) THEN
Inverse := 16#F12C0479; // singular position: acos is not defined for |x| > 1, catch possible failure
RETURN;
END_IF
dGamma := ACOS(dTemp);
(*************
Calc d__Beta
*************)
IF (dLengthTCP < FLT_EPSILON) THEN
inverse := 16#F12C0479; // to near to base point
RETURN;
END_IF
dTemp :=
( (dL1 * dL1) + (dLengthTCP * dLengthTCP) - (dL2 * dL2) )
/ ( 2.0 * dL1 * dLengthTCP );
IF ( ABS(dTemp) > 1.0 ) THEN
inverse := 16#F12C0479; // singular position: acos is not defined for |x| > 1, catch possible failure
RETURN;
END_IF
dBeta := ACOS(dTemp);
(**************
Calc d__Theta
**************)
dTheta := atan2( BCS[2], BCS[1] );
(**************************************
Die Rückwärts-Transformation berechnet aus Koordinaten im Basis-Koordi‐
natensystem Koordinaten im Achs-Koordinatensystem.
Zu beachten sind die Fehlerfälle!
Rückwärts-Transformation
//length of the distance from the TCP to the origin
//angle of the triangle; leads to phi2
//angle of the triangle; leads to phi1
//pitch angle of the distance from the TCP to the origin; leads to phi1
//temporary variable
//angle of the motor in degrees
//angle of the motor in degrees
//angle of the motor in radian; d_phi12 = d__phi1 + d__phi2
//angle of the motor in radian; d_phi12 = d__phi1 + d__phi2
Bosch Rexroth AG
335/401
Anwendungsbeispiele
Werbung
Inhaltsverzeichnis
