SMC_Trafo_4AxisPalletizer (FB)

FUNCTION_BLOCK SMC_Trafo_4AxisPalletizer

Inverse transformation for a 4-axis palletizer.

Assumptions:

All a_i in the config are non-negative, a_2 and a_3 are not less than g_fSMC_CNC_EPS

The valid range for axis 0 (dQ0) is contained in ]-180°, 180°[

The valid range for axis 1 (dQ1) is contained in [-90°, 90°]

The valid range for axis 2 (dQ2) is contained in ]-180°, 90°[

The valid range for axis 3 (dQ3) is not constrained and may be greater than 360°

InOut:

Scope	Name	Type	Initial	Comment
Inout	config	SMC_TrafoConfig_4AxisPalletizer		The configuration settings of the palletizer
Input	pi	SMC_PosInfo		The current position and orientation in world coordinates: dX, dY, dZ, and C
	bElbowLow	BOOL	FALSE	Whether axis 2 is in the range ]-180°, -90°[ (TRUE) or [-90°,90°] (FALSE)
	bBackwards	BOOL	FALSE	Whether axis 0 is set so that the palletizer faces the target (FALSE) or that it faces the opposite direction (TRUE)
Output	dQ0	LREAL		The value of axis 0 in degrees
	dQ1	LREAL		The value of axis 1 in degrees
	dQ2	LREAL		The value of axis 2 in degrees
	dQ3	LREAL		The value of axis 3 in degrees
	bError	BOOL		Whether an error occured during the computation
	result	SMC_TrafoResult_4AxisPalletizer		The detailed result of the computation