FB_CalcTrafo

FB_CalcTrafo 1:

The Function Block FB_CalcTrafo calculates the forward or backward transformation even if no kinematic group is built with FB_ConfigKinGroup.

VAR_INPUT

VAR_INPUT
    bExecute              : BOOL;
    bForward              : BOOL;
    oidTrafo              : UDINT;
END_VAR

bExecute: The command is triggered by a rising edge at this input.

bForward: Determines whether the forward or backward transformation is calculated.

oidTrafo: Object-ID of the kinematic transformation object to be calculated. See example below.

VAR_IN_OUT

VAR_IN_OUT
    stAxesPosIn           : ARRAY[1..8] OF LREAL;
    stAxesPosOut          : ARRAY[1..8] OF LREAL;
END_VAR

stAxesPosIn: Array which contains the input positions of the transformation. In order to calculate a forward transformation this would be the joint positions. In order to calculate a backward transformation this would be the Cartesian axes positions.

stAxesPosOut: Array which contains the resulting positions of the transformation. If a forward transformation is calculated this would be the Cartesian axes positions. If a backward transformation is calculated this would be the joint positions.

VAR_OUTPUT

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

bBusy: The output becomes TRUE when the command is started with bExecute as long as the function block is executing the command. While bBusy is TRUE, no new instructions will be accepted at the inputs. When 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: The output becomes true if the command succeeded.

bError: The output bError becomes true if an error occurs as the command is executed.

nErrorId: Contains the command-specific error code of the most recently executed command. The error code can be found in the ADS error documentation or in the NC error documentation (error codes above 0x4000).

Example

The Scara Transformation has 4 ACS (joint) axes and 4 MCS (Cartesian) axes

Scara Transformation Example Configuration

FB_CalcTrafo 2:

Scara Transformation Example Object Id

FB_CalcTrafo 3:

VAR
    fbFB_CalcTrafo     : FB_CalcTrafo;
    stAxesPosIn        : ARRAY[1..8] OF LREAL;
    stAxesPosOut       : ARRAY[1..8] OF LREAL;
    bUserExecute       : BOOL;
    bUserCalcFwdTrafo  : BOOL;
END_VAR

fbFB_CalcTrafo(
    bExecute := bUserExecute, 
    bForward := bUserCalcFwdTrafo, 
    oidTrafo := 16#01010072, 
    stAxesPosIn := stAxesPosIn, 
    stAxesPosOut := stAxesPosOut, 
    bBusy=> , 
    bDone=> , 
    bError=> , 
    nErrorId=> );