FB_KinCalcMultiTrafo

The function block FB_KinCalcMultiTrafo is used to calculate the forward or backward transformation for several positions, even if no kinematic group has been created with FB_KinConfigGroup.

Alternatively, the function block FB_KinCalcTrafo can be used to calculate the kinematic transformations individually.

Inputs

VAR_INPUT
    bExecute              : BOOL;
    bForward              : BOOL;
    oidTrafo              : UDINT;
    pDataIn               : Pointer to BYTE;
    nSizeIn               : UDINT;
    pDataOut              : Pointer to BYTE;
    nSizeOut              : UDINT;
END_VAR

Name	Type	Description
bExecute	BOOL	The command is triggered by a rising edge at this input.
bForward	BOOL	Determines whether the forward or backward transformation is calculated.
oidTrafo	UDINT	Object ID of the kinematic transformation object to be calculated.
pDataIn	POINTER TO BYTE	Pointer to the input data, consisting of an instance of ST_KinMultiTrafoHeader and an array of input positions. For the calculation of a forward transformation they represent the joint positions. For the calculation of a backward transformation they represent the Cartesian axis positions.
nSizeIn	UDINT	Size of the input data to which pDataIn points
pDataOut	POINTER TO BYTE	Pointer to the output data.
nSizeOut	UDINT	Size of the output data to which pDataOut points.

Outputs

VAR_OUTPUT
    bBusy                 : BOOL;
    bDone                 : BOOL;
    bError                : BOOL;
    nErrorId              : UDINT;
END_VAR

Name	Type	Description
bBusy	BOOL	The output becomes TRUE when the command is started with bExecute and remains TRUE as long as the function block executes the command. While bBusy is TRUE, no new command is accepted at the inputs. If bBusy becomes FALSE again, the function block is ready for a new command. At the same time one of the outputs bDone or bError is set.
bDone	BOOL	The output becomes TRUE when the command was executed successfully.
bError	BOOL	The output bError is set to TRUE, if an error occurred during the command execution.
nErrorId	UDINT	Contains the command-specific error code of the most recently executed command. Details of the error code can be found in the ADS error documentation or in the NC error documentation (error codes from 0x4000).

Sample

ST_KinCalcMultiTrafoIn

TYPE ST_KinCalcMultiTrafoIn :
STRUCT
    hdr  : ST_KinMultiTrafoHeader;
    fPos : ARRAY[1..2] OF ARRAY[1..4] OF LREAL;
END_STRUCT
END_TYPE

ST_KinCalcMultiTrafoOut

TYPE ST_KinCalcMultiTrafoOut :
STRUCT
    fPos      : ARRAY[1..2] OF ARRAY[1..4] OF LREAL;
    fMetaInfo : ARRAY[1..2] OF U_KinMetaInfo;
END_STRUCT
END_TYPE

MAIN

PROGRAM MAIN
VAR
    {attribute 'TcInitSymbol'} oidKinematic: OTCID;
    nState: UDINT := 0;

    fbKinCalcMultiTrafo   : FB_KinCalcMultiTrafo;
    stKinCalcMultiIn      : ST_KinCalcMultiTrafoIn; 
    stKinCalcMultiOut     : ST_KinCalcMultiTrafoOut; 
END_VAR

CASE nState OF
0:
    // Header for Multi Trafo
    stKinCalcMultiIn.hdr.nColumnsIn := 4;
    stKinCalcMultiIn.hdr.nColumnsOut := 4;
    stKinCalcMultiIn.hdr.nLines := 2;
    stKinCalcMultiIn.hdr.uMetaInfo.eScara := E_KinMetaInfoScara.scaraLeftArm;
    stKinCalcMultiIn.hdr.bGetMetaInfo := TRUE;

    // Positions
    stKinCalcMultiIn[1][1]:=0;
    stKinCalcMultiIn[1][2]:=90;
    stKinCalcMultiIn[1][3]:=0;
    stKinCalcMultiIn[1][4]:=0;

    stKinCalcMultiIn[2][1]:=0;
    stKinCalcMultiIn[2][2]:=-90;
    stKinCalcMultiIn[2][3]:=0;
    stKinCalcMultiIn[2][4]:=0;

    nState := nState + 10;

10:
    fbKinCalcMultiTrafo( bExecute := TRUE, 
                         bForward := TRUE,
                         oidTrafo := oidKinematic,
                         pDataIn  := ADR(stKinCalcMultiIn), 
                         nSizeIn  := SIZEOF(stKinCalcMultiIn), 
                         pDataOut := ADR(stKinCalcMultiOut), 
                         nSizeOut := SIZEOF(stKinCalcMultiOut) );

    IF NOT fbKinCalcMultiTrafo.bBusy THEN
        fbKinCalcMultiTrafo(bExecute:= FALSE, bForward:= TRUE, oidTrafo:= oidKinematic,
                            pDataIn:=ADR(stKinCalcMultiIn), nSizeIn:= SIZEOF(stKinCalcMultiIn),
                            pDataOut:=ADR(stKinCalcMultiOut), nSizeOut:= SIZEOF(stKinCalcMultiOut) );

        nState := nState + 10;
    END_IF
END_CASE

System requirements

Development environment	Target system	PLC libraries to include
Advanced Motion Pack V3.1.10.51	PC or CX (x64)	Tc2_NcKinematicTransformation