Kin_Scara3_Z_ReadConfig (FB) ¶ FUNCTION_BLOCK Kin_Scara3_Z_ReadConfig This FB reads configuration data of SM3_Trafo_Scara3_Z. InOut: Scope Name Type Comment Input Config ConfigData The configuration data Output xElbowRight BOOL TRUE: looking from joint 0 to joint 2, the elbow (joint 1) is on the right side. The angle of joint 1 is greater than 0° and lesser than 180°. FALSE: looking from joint 0 to joint 2, the elbow (joint 1) is on the left side. The angle of joint 1 is lesser than 0° and greater than -180°. xElbowSingularity BOOL TRUE: looking from joint 0 to joint 2, the elbow (joint 1) is straight (singularity). The angle of joint 1 is very close to either 0°, 180° or 360° nPeriodA1 DINT see nPeriodA2 nPeriodA2 DINT see nPeriodA3 nPeriodA3 DINT Determines the period used for orientation axis A3. If the range of A3 is greater than 360°, the period to choose can be set here. If the value is zero, then the period is determined automatically, based on the last position. A positive value i > 0 means the range [360°*(i-1)-180° .. 360°*i-180°[ is used. A negative value i < 0 means the range [360°*i-180° .. 360°*(i+1)-180°[ is used. Note if nPeriodA3 <> 0 and the chosen period is not feasible due to the axis limits, an error is created during transformation (CartesianToAxes).
Kin_Scara3_Z (FB) ¶ FUNCTION_BLOCK Kin_Scara3_Z EXTENDS Kin_Coupled Transformation FB for Scara3 kinematics with an additional Z-axis. The Selective Compliance Assembly Robot Arm (SCARA) is a special type of industrial robot which resembles to an human arm. A Scara3 system exhibits three axes and three degrees of freedom. However, the motion is still limited to the X-Y-plane. Machine coordinate system (MCS) Origin The intersection of axis 0 and the X-Y-plane. X Defined by the direction the first arm points to when the first rotary axis (a0) is at 0°. Y The Y axis results automatically from the definitions of the X and Z axis by rotating the X axis by +90° around the Z axis. ( Z ) This FB features an additional linear axis (a3) perpendicular to the X-Y-plane. The Z axis corresponds directly to the positive direction of this additional axis. The location of the tool coordinate system (TCS) relative to the MCS in zero position: Tool coordinate system (TCS) Origin Relative to MCS: dX = dArmLength1 + dArmLength2 + dArmLength3 dY = 0 dZ = 0 X Along the X-Axis of the MCS in positive direction Y Along the Y-Axis of the MCS in positive direction Z Along the Z-Axis of the MCS in positive direction The system consists of a rotary axis a0 that turns the robot around the Z axis the first joint with length dArmLength1, a second rotary axis a1 that turns the following parts of the robot around the Z axis, the second joint with length dArmLength2, a third rotary axis a2 that turns the following parts of the robot around the Z axis, the third joint with length dArmLength3 and a linear axis (a3) that is orientated in direction of Z. There are two configurations that can be switched with the input xElbowRight of Kin_Scara3_Z_Config. The single axes values have the following interpretation: a0 position of the first rotary axis around Z in degrees a1 position of the second rotary axis around Z in degrees a2 position of the third rotary axis around Z in degrees a3 position of the linear axis in direction of Z axis The zero position of the kinematics can be adjusted by defining constant offsets for the axes. See inputs dOffsetA1 , dOffsetA2 , dOffsetA3 and dOffsetZ . Changing the offsets affects the location and orientation of the TCS. Attributes: sm_kin_libdoc InOut: Scope Name Type Comment Inherited from Input itfPosKin ISMPositionKinematics Kin_Coupled itfOriKin ISMOrientationKinematics Kin_Coupled dArmLength1 LREAL Arm length of 1st joint dArmLength2 LREAL Arm length of 2nd joint dArmLength3 LREAL Arm length of 3rd joint dOffsetA1 LREAL Additional offset of axis A1. This offset is subtracted before the forward transformation and added after the inverse transformation. dOffsetA2 LREAL Additional offset of axis A2. This offset is subtracted before the forward transformation and added after the inverse transformation. dOffsetA3 LREAL Additional offset of axis A3. This offset is subtracted before the forward transformation and added after the inverse transformation. dOffsetZ LREAL Additional offset of axis Z. This offset is subtracted before the forward transformation and added after the inverse transformation. Properties: NumAxes , inherited from Kin_Coupled Methods: GetAxisMapping2 GetKinematicsName Initialize JoinAxes SplitAxes ActivateAutomaticRotaryPeriods , inherited from Kin_Coupled AxesToCartesian , inherited from Kin_Coupled AxisSettings , inherited from Kin_Coupled CPConnectible , inherited from Kin_Coupled CartesianToAxes , inherited from Kin_Coupled GetAxisMapping , inherited from Kin_Coupled GetAxisProperties , inherited from Kin_Coupled GetConfigurationDataSize , inherited from Kin_Coupled GetDefaultConfigurationData , inherited from Kin_Coupled GetFlangeOrientationImageTotal , inherited from Kin_Coupled GetFlangeOrientationImageWithOri , inherited from Kin_Coupled GetPeriods , inherited from Kin_Coupled GetPosAndToolKinematics , inherited from Kin_Coupled IsConfigSingular , inherited from Kin_Coupled IsInitialized , inherited from Kin_Coupled IsSingularity , inherited from Kin_Coupled JoinConfig , inherited from Kin_Coupled SplitConfig , inherited from Kin_Coupled Structure: GetAxisMapping2 (Method) GetKinematicsName (Method) Initialize (Method) JoinAxes (Method) SplitAxes (Method)
Kin_Scara3_Z.GetAxisMapping2 (METH) ¶ METHOD GetAxisMapping2 For Scara3Z, the axes 0, 1 and 3 belong to position kinematics and axis 2 belongs to orientation kinematics InOut: Scope Name Type Inout aAxisMapping ARRAY [0..(SM3M.SMC_MAX_VEC_DIM - 1)] OF Coupled_Kin_Axis_Map
Kin_Scara3_Z.GetKinematicsName (METH) ¶ METHOD GetKinematicsName : STRING(255) InOut: Scope Name Type Return GetKinematicsName STRING(255)
Kin_Scara3_Z.Initialize (METH) ¶ METHOD Initialize : SMC_ERROR InOut: Scope Name Type Return Initialize SMC_ERROR
Kin_Scara3_Z.JoinAxes (METH) ¶ METHOD JoinAxes : BOOL InOut: Scope Name Type Return JoinAxes BOOL Inout a AXISPOS_REF Inout Const a1 AXISPOS_REF a2 AXISPOS_REF
Kin_Scara3_Z.SplitAxes (METH) ¶ METHOD SplitAxes : BOOL InOut: Scope Name Type Return SplitAxes BOOL Inout a1 AXISPOS_REF a2 AXISPOS_REF Inout Const a AXISPOS_REF
Kin_Tripod_Linear.NumAxes (PROP) ¶ PROPERTY NumAxes : UDINT
Kin_Tripod_Linear.ProjectPosition (METH) ¶ METHOD ProjectPosition InOut: Scope Name Type Inout vOut SM3M.SMC_VECTOR3D Inout Const vIn SM3M.SMC_VECTOR3D
Kin_Tripod_Rotary (FB) ¶ FUNCTION_BLOCK Kin_Tripod_Rotary IMPLEMENTS ISMPositionKinematics_Offset , ISMPositionKinematicsInternal, ISMKinematicWithInfo2 , ISMKinematicWithInitialization Transformation FB for Tripod kinematics with rotary axes In the kinematic design for a tripod with rotary drives, three rotary drives are connected to the tool plate by arms and connecting rods. Machine coordinate system (MCS) Origin Defined as the midpoint of the tool plate when all three upper arms (i.e. the ones directly connected to A0, A1, and A2 respectively) are in horizontal position. X Points from the origin and away from the point P of the first axis (A0). Y Defined by X and Z so that the MCS becomes right-handed. Z Orthogonal to the tool plate. Points from the tool plate towards the motors. The location of the tool coordinate system (TCS) relative to the MCS in zero position: Tool coordinate system (TCS) X Along the X-Axis of the MCS in positive direction Y Along the Y-Axis of the MCS in positive direction Z Along the Z-Axis of the MCS in positive direction The image shows the zero position of all axes (the three upper arms are horizontal). The MCS is displayed in the tool plate. Additionally, the arrows at the axes A0, A1, and A2 show the direction of rotation according to the right-hand-rule. Mechanical requirements: The lengths of all three arms must be equal. The lengths of all connecting rods must be equal. The distance between the two connecting rods in each pair must be equal. The members of AXISPOS_REF have the following interpretation: a0 position of the 1st axis of the machine (A0) a1 position of the 2nd axis of the machine (A1) a2 position of the 3rd axis of the machine (A2) Attributes: sm_kin_libdoc InOut: Scope Name Type Initial Comment Input dArmLength1 LREAL Length of the upper arm connected to the motor. dArmLength2 LREAL Length of the lower arm (from the upper arm to the tool plate). dArm1Radius LREAL The radius of the circle that is established by the three points P of the drives. dStewartRadius LREAL 0 The radius of the circle that is established by the six points where the connecting rods connect to the tool plate. dDistance LREAL Distance between the two connecting rods in one pair. dMaxAngleBallJoint LREAL 45 Maximum angle in +/- for the ball joints. Properties: NumAxes Methods: AxesToCartesian AxesToOrientation CartesianToAxes CartesianToAxes_Offset GetAxisProperties GetKinematicsName GetOrientationImage Initialize IsInitialized IsSingularity ProjectPosition Structure: AxesToCartesian (Method) AxesToOrientation (Method) CartesianToAxes (Method) CartesianToAxes_Offset (Method) GetAxisProperties (Method) GetKinematicsName (Method) GetOrientationImage (Method) Initialize (Method) IsInitialized (Method) IsSingularity (Method) NumAxes (Property) ProjectPosition (Method)