The positions of the axes are transferred at the i_lrPositionA, i_lrPositionB, i_lrPositionC, i_lrPositionD , i_lrPositionE , and i_lrPositionF inputs.
They must be converted to Cartesian coordinates and/or orientations within this method and returned as return values at the q_stCoordinate and q_stOrientation outputs. The orientation must be provided in the convention ET_OrientationConvention.ZYX.
Those calculated values are further processed by the robot.
|
Input |
Data type |
Description |
|---|---|---|
|
i_lrPositionA |
LREAL |
Position of axis A. |
|
i_lrPositionB |
LREAL |
Position of axis B. |
|
i_lrPositionC |
LREAL |
Position of axis C. |
|
i_lrPositionD |
LREAL |
Position of axis D. |
|
i_lrPositionE |
LREAL |
Position of axis E. |
|
i_lrPositionF |
LREAL |
Position of axis F. |
|
Output |
Data type |
Description |
|---|---|---|
|
q_etDiag |
General library-independent statement on the diagnostic. A value not equal to GD.ET_Diag.Ok corresponds to a diagnostic message. |
|
|
q_etDiagExt |
POU-specific output on the diagnostic. q_etDiag = ET_Diag.Ok -> Status message q_etDiag <> ET_Diag.Ok -> Diagnostic message |
|
|
q_sMsg |
STRING[80] |
Event-triggered message that gives additional information on the diagnostic state. |
|
q_stCoordinate |
Cartesian position of the TCP (Tool Center Point) in the coordinate system. |
|
|
q_stOrientation |
PDL.ST_Vector3D |
Orientation of the TCP in the coordinate system. The orientation must be provided in the convention ET_OrientationConvention.ZYX. |
Declaration:
METHOD Direct
VAR_INPUT
i_lrPositionA : LREAL;
i_lrPositionB : LREAL;
i_lrPositionC : LREAL;
i_lrPositionD : LREAL;
i_lrPositionE : LREAL;
i_lrPositionF : LREAL;
END_VAR
VAR_OUTPUT
q_etDiag : GD.ET_Diag := GD.ET_Diag.Ok;
q_etDiagExt : ROB.ET_DiagExt := ROB.ET_DiagExt.Ok;
q_sMsg : STRING[80];
q_stCoordinate : PDL.ST_Vector3D;
q_stOrientation : PDL.ST_Vector3D;
END_VAR
VAR
END_VARImplementation:
// Implementation of the forward transformation of the kinematics.
// Calculate the coordinate and orientation of the Tool Center Point.
//********************************************************************
// Coordinate values of the Tool Center Point
//********************************************************************
// q_stCoordinate.lrX := ...;
// q_stCoordinate.lrY := ...;
// q_stCoordinate.lrZ := ...;
//********************************************************************
// Orientation values of the Tool Center Point
//********************************************************************
// q_stOrientation.lrX := ...;
// q_stOrientation.lrY := ...;
// q_stOrientation.lrZ := ...;