FB_KinCalcTrafo
The function block FB_KinCalcTrafo calculates the forward or backward transformation, even if no kinematic group was created with FB_KinConfigGroup.
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;
uMetaInfoIn : U_KinMetaInfo;
uMetaInfoOut : U_KinMetaInfo;
END_VAR
stAxesPosIn: Array containing the input positions of the transformation. 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.
stAxesPosOut: Array containing the result positions of the transformation. For the calculation of a forward transformation they represent the cartesian axis positions. For the calculation of a backward transformation they represent the joint positions.
uMetaInfoIn: In cases where different robot configurations lead to a solution, the preferred solution can be selected (see sample). For kinematics in which this parameter is not required, a dummy variable can be assigned to this input.
uMetaInfoOut: If different solutions are possible for a transformation, the solution that was found is specified. For kinematics in which this parameter is not required, a dummy variable can be assigned to this input.
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 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: The output becomes TRUE when the command was executed successfully.
bError: The output bError is set to TRUE, if an error occurred during the command execution.
nErrorId: 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
The object ID of the transformation is shown in the transformation object under the kinematic channel.
SCARA transformation - sample object ID
VAR
fbKinCalcTrafo : FB_KinCalcTrafo;
stAxesPosIn : ARRAY[1..8] OF LREAL;
stAxesPosOut : ARRAY[1..8] OF LREAL;
bUserExecute : BOOL;
bUserCalcFwdTrafo : BOOL;
uScaraMetaInfoIn : U_KinMetaInfo;
uScaraMetaInfoOut : U_KinMetaInfo;
END_VAR
uScaraMetaInfoIn.eScara := E_KinMetaInfoScara.scaraLeftArm;
fbKinCalcTrafo(
bExecute := bUserExecute,
bForward := bUserCalcFwdTrafo,
oidTrafo := 16#01010070,
stAxesPosIn := stAxesPosIn,
stAxesPosOut := stAxesPosOut,
uMetaInfoIn:= uScaraMetaInfoIn ,
uMetaInfoOut:= uScaraMetaInfoOut,
bBusy=> ,
bDone=> ,
bError=> ,
nErrorId=> );