FB_KinCalcMultiTrafo

Mit dem Funktionsbaustein FB_KinCalcMultiTrafo werden für mehrere Positionen die Vorwärts- oder Rückwärtstransformation berechnet, selbst dann, wenn keine Kinematikgruppe mit FB_KinConfigGroup erstellt wurde.

Alternativ kann der Funktionsbaustein FB_KinCalcTrafo verwendet werden, um die kinematischen Transformationen einzeln zu berechnen.

Eingänge

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

Name	Typ	Beschreibung
bExecute	BOOL	Der Befehl wird durch eine steigende Flanke an diesem Eingang ausgelöst.
bForward	BOOL	Bestimmt, ob die Vorwärts- oder Rückwärtstransformation berechnet wird.
oidTrafo	UDINT	Objekt-ID des zu berechnenden kinematischen Transformationsobjekts.
pDataIn	POINTER TO BYTE	Zeiger auf die Eingangsdaten, bestehend aus einer Instanz von ST_KinMultiTrafoHeader und einem Array mit Eingangspositionen. Bei der Berechnung einer Vorwärtstransformation stellen diese die Gelenkpositionen dar. Bei der Berechnung einer Rückwärtstransformation stellen diese die kartesischen Achspositionen dar.
nSizeIn	UDINT	Größe der Eingangsdaten auf die pDataIn zeigt
pDataOut	POINTER TO BYTE	Zeiger auf die Ausgangsdaten.
nSizeOut	UDINT	Größe der Ausgangsdaten auf die pDataOut zeigt.

Ausgänge

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

Name	Typ	Beschreibung
bBusy	BOOL	Der Ausgang wird TRUE, wenn der Befehl mit bExecute gestartet ist und bleibt es dann so lange, wie der Funktionsbaustein den Befehl ausführt. Während bBusy gleich TRUE ist, wird an den Eingängen kein neuer Befehl angenommen. Wenn bBusy wieder FALSE wird, ist der Funktionsbaustein bereit für einen neuen Befehl. Gleichzeitig wird einer der Ausgänge bDone oder bError gesetzt.
bDone	BOOL	Der Ausgang wird TRUE, wenn der Befehl erfolgreich ausgeführt wurde.
bError	BOOL	Der Ausgang bError wird auf TRUE gesetzt, wenn bei der Ausführung des Befehls ein Fehler aufgetreten ist.
nErrorId	UDINT	Enthält den befehlsspezifischen Fehlercode des zuletzt ausgeführten Befehls. Der Fehlercode kann in der ADS-Fehlerdokumentation oder in der NC-Fehlerdokumentation (Fehlercodes ab 0x4000) nachgeschlagen werden.

Beispiel

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

Systemvoraussetzungen

Entwicklungsumgebung	Zielsystem	Einzubindende SPS-Bibliotheken
Advanced Motion Pack V3.1.10.51	PC or CX (x64)	Tc2_NcKinematicTransformation