DUTs

DUT_AxisStatus_v0_01

TYPE DUT_AxisStatus_v0_01 :
STRUCT
    bEnable: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    nCommand: UINT;
    nCmdData: UINT;
    fVelocity: LREAL;
    fPosition: LREAL;
    fAcceleration: LREAL;
    fDeceleration: LREAL;
    bJogFwd: BOOL;
    bJogBwd: BOOL;
    bLimitFwd: BOOL;
    bLimitBwd: BOOL;
    fOverride: LREAL := 100;
    bHomeSensor: BOOL;
    bEnabled: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
    fActVelocity: LREAL;
    fActPosition: LREAL;
    fActDiff: LREAL;
    bHomed:BOOL;
    bBusy:BOOL;
END_STRUCT
END_TYPE

Related:

• DUT_AxisStatus_v0_01

GVLs

POUs

FB_DriveVirtual

///#########################################################
///Function block to run a virtual drive with Nc
/// Library:
/// Tc2_MC2.lib
///
/// Global Variables:
///
/// Data types:
///
/// External functions:
///
///###########################################################
FUNCTION_BLOCK FB_DriveVirtual
VAR
    sVersion: STRING:='1.0.3';
END_VAR
VAR_INPUT
    En: BOOL;
    bEnable: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    /////   nCommandLocal...
    /////   0 = Jog
    /////   1 = MoveVelocity
    /////   2 = MoveRelative
    /////   3 = MoveAbsolut
    /////   4 = MoveModulo
    /////   10 = Homing
    /////   20 = SuperInp >>>ToBe
    /////   30 = Gear
    nCommand: UINT;
    nCmdData: UINT;
    fVelocity: LREAL;
    fPosition: LREAL;
    fAcceleration: LREAL;
    fDeceleration: LREAL;
    bJogFwd: BOOL;
    bJogBwd: BOOL;
    bLimitFwd: BOOL;
    bLimitBwd: BOOL;
    fOverride: LREAL := 100;
    bHomeSensor: BOOL;
    fHomePosition:LREAL;
    nHomeRevOffset: UINT;
    MasterAxis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bEnabled: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    bHomed: BOOL;
    nErrorId: UDINT;
    nMotionAxisID:UDINT:=0;  //Axis id in Motion (NC)
    Status: ST_AxisStatus;
    fActVelocity: LREAL;
    fActPosition: LREAL;
    fActDiff: LREAL;
    sErrorMessage:STRING;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR
    nCommandLocal: UINT;
    nCmdDataLocal: UINT;
    bFirstScan: BOOL := TRUE;
    fbReset: MC_Reset;
    fbPower: MC_Power;
    fbHalt: MC_Halt;
    fbJog: MC_Jog;
    fbMoveVelocity: MC_MoveVelocity;
    fbMoveRelative: MC_MoveRelative;
    fbMoveAbsolute: MC_MoveAbsolute;
    fbMoveModulo: MC_MoveModulo;
    fbHomeVirtual:FB_HomeVirtual;
    fbGearInDyn: MC_GearInDyn;
    fbGearOut: MC_GearOut;
    fbExecuteRiseEdge: R_TRIG;
    stAxisStatus: DUT_AxisStatus_v0_01;
    stAxisStatusV2: DUT_AxisStatus_v0_01;
END_VAR
EnO:=En;

// Transfer nCommand and nCmdData to local copies at rising edge of bExecute (avoid issues if nCommand or nCmdData are changed during a command)
fbExecuteRiseEdge(CLK:=bExecute);
IF fbExecuteRiseEdge.Q THEN
  nCmdDataLocal:=nCmdData;
  nCommandLocal:=nCommand;
END_IF

// Hack, we can't home these
bHomed:=TRUE;//fbHomeVirtual.bHomed; //Add in DUT_AxisStatus later
bDone:=FALSE;

(*Reset*)
fbReset(
    Execute:=bReset AND Axis.Status.Error,
    Axis:=Axis,
    Done=> ,
    Busy=> ,
    Error=> ,
    ErrorID=> );

(*Power*)
fbPower(
    Axis:=Axis,
    Enable:=bEnable,
    Enable_Positive:=bEnable AND bLimitFwd,
    Enable_Negative:=bEnable AND bLimitBwd,
    Override:=fOverride,
    BufferMode:= ,
    Status=> ,
    Busy=> ,
    Active=> ,
    Error=> ,
    ErrorID=> );

(*Halt*)
fbHalt(
    Execute:=NOT bExecute  AND (((fbMoveVelocity.Busy OR fbPower.Busy) AND (nCommandLocal=1)) OR (fbMoveRelative.Busy AND (nCommandLocal=2)) OR (fbMoveAbsolute.Busy AND (nCommandLocal=3)) OR (fbMoveModulo.Busy AND (nCommandLocal=4)) OR (fbHomeVirtual.bBusy AND (nCommandLocal=10))),
    Deceleration:=fDeceleration,
    Jerk:=0,
    BufferMode:= ,
    Options:= ,
    Axis:=Axis ,
    Done=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

(*Jog (Command = 0)*)
fbJog(
    JogForward:=bJogFwd AND (nCommandLocal=0) ,
    JogBackwards:=bJogBwd AND (nCommandLocal=0) ,
    Mode:=nCmdDataLocal,
    Position:= ,
    Velocity:=fVelocity,
    Acceleration:=fAcceleration,
    Deceleration:=fDeceleration,
    Jerk:=0,
    Axis:=Axis,
    Done=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

(*MoveVelocity (Command = 1)*)
fbMoveVelocity(
    Execute:=bExecute AND (nCommandLocal=1),
    Velocity:=ABS(fVelocity),
    Acceleration:=fAcceleration,
    Deceleration:=fDeceleration,
    Jerk:=0,
    Direction:=SEL(fVelocity<0, MC_Positive_Direction, MC_Negative_Direction),
    BufferMode:= ,
    Options:= ,
    Axis:=Axis,
    InVelocity=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

(*MoveRelative (Command = 2)*)
fbMoveRelative(
    Execute:=bExecute AND (nCommandLocal=2),
    Distance:=fPosition,
    Velocity:=ABS(fVelocity),
    Acceleration:=fAcceleration,
    Deceleration:=fDeceleration,
    Jerk:=0,
    BufferMode:= ,
    Options:= ,
    Axis:=Axis,
    Done=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

IF nCommandLocal=2 THEN
    bDone:=fbMoveRelative.Done;
END_IF

(*MoveAbsolute (Command = 3)*)
fbMoveAbsolute(
    Execute:=bExecute AND (nCommandLocal=3),
    Position:=fPosition,
    Velocity:=ABS(fVelocity),
    Acceleration:=fAcceleration,
    Deceleration:=fDeceleration,
    Jerk:=0,
    BufferMode:= ,
    Options:= ,
    Axis:=Axis,
    Done=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

IF nCommandLocal=3 THEN
    bDone:=fbMoveAbsolute.Done;
END_IF

(*MoveModulo (Command = 4)*)
fbMoveModulo(
    Execute:=bExecute AND (nCommandLocal=4),
    Position:=fPosition,
    Velocity:=ABS(fVelocity),
    Acceleration:=fAcceleration,
    Deceleration:=fDeceleration,
    Jerk:=0,
    Direction:=nCmdDataLocal,
    BufferMode:= ,
    Options:= ,
    Axis:=Axis,
    Done=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

IF nCommandLocal=4 THEN
    bDone:=fbMoveModulo.Done;
END_IF

(*Home (Command = 10)*)
fbHomeVirtual(
    bExecute:= nCommandLocal=10 AND bExecute,
    fHomePosition:=fHomePosition,
    bHomeSensor:=bHomeSensor,
    bLimitBwd:=bLimitBwd,
    bLimitFwd:=bLimitFwd,
    nCmdData:=nCmdDataLocal,
    bReset:=bReset,
    nHomeRevOffset:=nHomeRevOffset,
    Axis:=Axis
    );

IF nCommandLocal=10 THEN
    bDone:=fbHomeVirtual.bDone;
END_IF

(*Gear (Command = 30)*)
fbGearInDyn(
    Enable:=bExecute  AND (nCommandLocal=30),
    GearRatio:=SEL(nCmdDataLocal>0, 1,fVelocity),
    Acceleration:=fAcceleration,
    Deceleration:=fDeceleration,
    Jerk:=0.0,
    BufferMode:= ,
    Options:= ,
    Master:=MasterAxis,
    Slave:=Axis,
    InGear=> ,
    Busy=> ,
    Active=> ,
    CommandAborted=> ,
    Error=> ,
    ErrorID=> );

fbGearOut(
    Execute:=NOT bExecute AND Axis.Status.NotMoving AND (nCommandLocal=30),
    Slave:=Axis,
    Error=>,
    Done=>,
    ErrorID=>);


IF nCommandLocal=30 THEN
    bDone:=Axis.Status.Coupled;
END_IF

(*Busy*)
bBusy:=Axis.Status.HasJob OR Axis.Status.HomingBusy OR fbHomeVirtual.bBusy;

(*Enabled*)
bEnabled:=fbPower.Status;

(*Error from functions and Nc*)
IF fbPower.Error AND fbPower.Active THEN
    bError:=fbPower.Enable;
    nErrorId:=fbPower.ErrorID;
ELSIF fbHalt.Error AND fbHalt.Active THEN
    bError:=fbHalt.Execute;
    nErrorId:=fbHalt.ErrorID;
ELSIF fbJog.Error AND nCommandLocal=0 (*fbJog.Active*) THEN
    bError:=fbJog.JogForward OR fbJog.JogBackwards;
    nErrorId:=fbJog.ErrorID;
ELSIF fbMoveVelocity.Error AND nCommandLocal=1(*fbMoveVelocity.Active*) THEN
    bError:=fbMoveVelocity.Execute;
    nErrorId:=fbMoveVelocity.ErrorID;
ELSIF fbMoveRelative.Error AND nCommandLocal=2 (*fbMoveRelative.Active*) THEN
    bError:=fbMoveRelative.Execute;
    nErrorId:=fbMoveRelative.ErrorID;
ELSIF fbMoveAbsolute.Error AND nCommandLocal=3 (*fbMoveAbsolute.Active*) THEN
    bError:=fbMoveAbsolute.Execute;
    nErrorId:=fbMoveAbsolute.ErrorID;
ELSIF fbMoveModulo.Error AND nCommandLocal=4 (*fbMoveModulo.Active*) THEN
    bError:=fbMoveModulo.Execute;
    nErrorId:=fbMoveModulo.ErrorID;
ELSIF fbHomeVirtual.bError AND nCommandLocal=10 (*fbHome.Active*) THEN
    bError:=fbHomeVirtual.bError;
    nErrorId:=fbHomeVirtual.nErrorID;
ELSIF fbGearInDyn.Error AND nCommandLocal=30 (*fbGearInDyn.Active*) THEN
    bError:=fbGearInDyn.Enable;
    nErrorId:=fbGearInDyn.ErrorID;
ELSIF fbGearOut.Error AND nCommandLocal=30 AND Axis.Status.Coupled THEN
    bError:=fbGearOut.Execute;
    nErrorId:=fbGearOut.ErrorID;
ELSIF Axis.Status.Error  THEN
    bError:=TRUE;
    nErrorId:=Axis.Status.ErrorID;
ELSIF fbHomeVirtual.bError THEN
    bError:=TRUE;
    nErrorId:=fbHomeVirtual.nErrorId;
ELSE
    bError:=FALSE;
    nErrorId:=0;
END_IF;

(*Converese nErrorID to string*)
sErrorMessage:=WORD_TO_HEXSTR(in:=TO_WORD(nErrorID) , iPrecision:= 4, bLoCase:=0 );

(*Status from Nc*)
Status:=Axis.Status;

(*Axis id in motion "motor"*)
nMotionAxisID:=axis.NcToPlc.AxisId;

(*Actual Velocity*)
fActVelocity:=Axis.NcToPlc.ActVelo;

(*Actual Position*)
IF Axis.Status.OpMode.Modulo THEN
    fActPosition:=Axis.NcToPlc.ModuloActPos;
ELSE
    fActPosition:=Axis.NcToPlc.ActPos;
END_IF

(*Actual Position*)
fActDiff:=Axis.NcToPlc.PosDiff;


//Status struct for EPICS communication
stAxisStatus.bEnable:=bEnable;
stAxisStatus.bEnabled:=bEnabled;
stAxisStatus.bError:=bError;
stAxisStatus.bExecute:=bExecute;
stAxisStatus.bHomeSensor:=bHomeSensor;
stAxisStatus.bJogBwd:=bJogBwd;
stAxisStatus.bJogFwd:=bJogFwd;
stAxisStatus.bLimitBwd:=bLimitBwd;
stAxisStatus.bLimitFwd:=bLimitFwd;
stAxisStatus.bReset:=bReset;
stAxisStatus.fAcceleration:=fAcceleration;
stAxisStatus.fActDiff:=fActDiff;
stAxisStatus.fActPosition:=fActPosition;
stAxisStatus.fActVelocity:=fActVelocity;
stAxisStatus.fDeceleration:=fDeceleration;
stAxisStatus.fOverride:=fOverride;
stAxisStatus.fPosition:=fPosition;
stAxisStatus.fVelocity:=fVelocity;
stAxisStatus.nCmdData:=nCmdData;  //Or nCmdDataLocal
stAxisStatus.nCommand:=nCommand;  //Or nCommandLocal
stAxisStatus.nErrorId:=nErrorId;
stAxisStatus.bBusy:=bBusy;
stAxisStatus.bHomed:=bHomed;
stAxisStatusV2 := stAxisStatus;

IF bDone THEN
    bExecute := FALSE;
END_IF
IF bFirstScan THEN
    bFirstScan:=FALSE;
END_IF

END_FUNCTION_BLOCK

Related:

• DUT_AxisStatus_v0_01

• FB_HomeVirtual

FB_HomeDirect

FUNCTION_BLOCK FB_HomeDirect
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    fHomePosition:LREAL;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bHomed:BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR
    fbHome: MC_Home;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

fbHome(
    Execute:=bExecute,
    Position:=fHomePosition,
    HomingMode:=MC_Direct,
    bCalibrationCam:=FALSE,
    Axis:=Axis
    );

bBusy:=fbHome.Busy;
bDone:=fbHome.Done;
bHomed:=Axis.Status.Homed;

bError:=fbHome.Error;

IF fbHome.Error THEN
  nErrorId:=fbHome.ErrorID;
END_IF

END_FUNCTION_BLOCK

FB_HomeFinish

FUNCTION_BLOCK FB_HomeFinish
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    nCmdData: UINT;
    bSofLimEnableLow: BOOL:=TRUE;
    bSofLimEnableHigh: BOOL:=TRUE;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR
    fbHomewriteSoftLimEnable:FB_HomeWriteSoftLimEnable;
    fbExecuteRiseEdge: R_TRIG;
    bExecuteWriteNC:BOOL:=FALSE;
    nState:INT:=0;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

IF NOT bExecute THEN
  bExecuteWriteNC:=FALSE;
  fbHomewriteSoftLimEnable.bSofLimEnableLow:=bSofLimEnableLow;
  fbHomewriteSoftLimEnable.bSofLimEnableHigh:=bSofLimEnableHigh;
  nState:=0;
END_IF

fbExecuteRiseEdge(CLK:=bExecute);
IF fbExecuteRiseEdge.Q THEN
  bExecuteWriteNC:=TRUE;
  fbHomewriteSoftLimEnable.bSofLimEnableLow:=bSofLimEnableLow;
  fbHomewriteSoftLimEnable.bSofLimEnableHigh:=bSofLimEnableHigh;
END_IF

// Write to NC (disable soft limits)
fbHomewriteSoftLimEnable(
    En:=En,
    bExecute:=bExecuteWriteNC AND bExecute,
    Axis:=Axis,
    bReset:=bReset,
);

bBusy:=fbHomewriteSoftLimEnable.bBusy;
bDone:=fbHomewriteSoftLimEnable.bDone;

bError:=fbHomewriteSoftLimEnable.bError;
IF fbHomewriteSoftLimEnable.bError THEN
  nErrorId:=fbHomewriteSoftLimEnable.nErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_HomeWriteSoftLimEnable

FB_HomePrepare

FUNCTION_BLOCK FB_HomePrepare
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    nCmdData: UINT;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
    bSofLimEnableLowOriginal: BOOL:=TRUE;
    bSofLimEnableHighOriginal: BOOL:=TRUE;
    fVelocityToCam: LREAL:=0;
    fVelocityFromCam: LREAL:=0;
END_VAR
VAR
    fbHomeReadSoftLimEnable:FB_HomeReadSoftLimEnable;
    fbHomeDisableSoftLimEnable:FB_HomeWriteSoftLimEnable;
    fbHomeReadNCVelocities: FB_HomeReadNcVelocities;
    fbHomeResetCalibrationFlag:MC_Home;   //Only used for reset of calibration flag
    fbExecuteRiseEdge: R_TRIG;
    bExecuteReadNC:BOOL:=FALSE;
    bExecuteWriteNC:BOOL:=FALSE;
    nState:INT:=0;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

IF NOT bExecute THEN
  bExecuteReadNC:=FALSE;
  bExecuteWriteNC:=FALSE;
  nState:=0;
END_IF

fbExecuteRiseEdge(CLK:=bExecute);
IF fbExecuteRiseEdge.Q THEN
  bExecuteReadNC:=TRUE;
END_IF

// Read from NC
fbHomeReadNCVelocities(
  En:=En,
  bExecute:=bExecuteReadNC, // Actualy not needed for sequence 15 (set position only, no movement))
  bReset:=bReset,
  Axis:=Axis,
);

fbHomeReadSoftLimEnable(
    En:=En,
    bExecute:=bExecuteReadNC AND bExecute,
    Axis:=Axis,
    bReset:=bReset,
);

// Reset calibration flag
fbHomeResetCalibrationFlag(
  Execute:=bExecuteReadNC,
  HomingMode:=MC_ResetCalibration,
  Axis:=Axis
);

bSofLimEnableLowOriginal:=fbHomeReadSoftLimEnable.bSofLimEnableLow;
bSofLimEnableHighOriginal:=fbHomeReadSoftLimEnable.bSofLimEnableHigh;

fVelocityToCam:=fbHomeReadNCVelocities.fVelocityToCam;
fVelocityFromCam:=fbHomeReadNCVelocities.fVelocityFromCam;

IF bExecuteReadNC AND bExecute AND fbHomeReadSoftLimEnable.bDone THEN
    fbHomeDisableSoftLimEnable.bSofLimEnableHigh:=FALSE;
    fbHomeDisableSoftLimEnable.bSofLimEnableLow:=FALSE;
    bExecuteWriteNC:=TRUE; //Always write (only needed if enabled actually)
END_IF

// Write to NC (disable soft limits)
fbHomeDisableSoftLimEnable(
    En:=En,
    bExecute:=bExecuteWriteNC AND bExecute,
    Axis:=Axis,
    bReset:=bReset,
);

bBusy:=fbHomeReadSoftLimEnable.bBusy OR fbHomeDisableSoftLimEnable.bBusy OR fbHomeReadNCVelocities.bBusy OR fbHomeResetCalibrationFlag.Busy;
bDone:=fbHomeReadSoftLimEnable.bDone AND fbHomeDisableSoftLimEnable.bDone AND fbHomeReadNCVelocities.bDone AND fbHomeResetCalibrationFlag.Done AND bExecute;

bError:=fbHomeReadSoftLimEnable.bError OR fbHomeDisableSoftLimEnable.bError OR fbHomeReadNCVelocities.bError OR fbHomeResetCalibrationFlag.Error;
IF fbHomeReadSoftLimEnable.bError THEN
  nErrorId:=fbHomeReadSoftLimEnable.nErrorId;
ELSIF fbHomeDisableSoftLimEnable.bError THEN
  nErrorId:=fbHomeDisableSoftLimEnable.nErrorId;
ELSIF fbHomeResetCalibrationFlag.Error THEN
  nErrorId:=fbHomeResetCalibrationFlag.ErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_HomeReadNcVelocities

• FB_HomeReadSoftLimEnable

• FB_HomeWriteSoftLimEnable

FB_HomeReadNcVelocities

FUNCTION_BLOCK FB_HomeReadNcVelocities
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
    fVelocityToCam: LREAL;
    fVelocityFromCam: LREAL;
END_VAR
VAR
    fbReadVelocityToCam:FB_ReadFloatParameter;
    fbReadVelocityFromCam:FB_ReadFloatParameter;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

fbReadVelocityToCam(
    bExecute:=bExecute,
    nDeviceGroup:= 16#4000,
    nIndexOffset:= 16#6,
    Axis:= Axis);

fbReadVelocityFromCam(
    bExecute:=bExecute,
    nDeviceGroup:= 16#4000,
    nIndexOffset:= 16#7,
    Axis:= Axis);

fVelocityToCam:=fbReadVelocityToCam.nData;
fVelocityFromCam:=fbReadVelocityFromCam.nData;

bBusy:=fbReadVelocityFromCam.bBusy OR fbReadVelocityToCam.bBusy;
bDone:=fbReadVelocityFromCam.bDone AND fbReadVelocityToCam.bDone AND bExecute;

bError:=fbReadVelocityToCam.bError OR fbReadVelocityFromCam.bError;
IF fbReadVelocityToCam.bError THEN
  nErrorId:=fbReadVelocityToCam.nErrorId;
ELSIF fbReadVelocityFromCam.bError THEN
  nErrorId:=fbReadVelocityFromCam.nErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_ReadFloatParameter

FB_HomeReadSoftLimEnable

FUNCTION_BLOCK FB_HomeReadSoftLimEnable
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
    bSofLimEnableLow: BOOL:=TRUE;
    bSofLimEnableHigh: BOOL:=TRUE;
END_VAR
VAR
    fbReadSoftLimEnableLow:FB_ReadParameterInNc_v1_00;
    fbReadSoftLimEnableHigh:FB_ReadParameterInNc_v1_00;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

fbReadSoftLimEnableLow(
    bExecute:=bExecute,
    nDeviceGroup:= 16#5000,
    nIndexOffset:= 16#B,
    Axis:= Axis);

fbReadSoftLimEnableHigh(
    bExecute:=bExecute,
    nDeviceGroup:= 16#5000,
    nIndexOffset:= 16#C,
    Axis:= Axis);

bSofLimEnableLow:=DWORD_TO_BOOL(fbReadSoftLimEnableLow.nData);
bSofLimEnableHigh:=DWORD_TO_BOOL(fbReadSoftLimEnableHigh.nData);

bBusy:=fbReadSoftLimEnableLow.bBusy OR fbReadSoftLimEnableHigh.bBusy;
bDone:=fbReadSoftLimEnableLow.bDone AND fbReadSoftLimEnableHigh.bDone AND bExecute;

bError:=fbReadSoftLimEnableLow.bError OR fbReadSoftLimEnableHigh.bError;
IF fbReadSoftLimEnableLow.bError THEN
  nErrorId:=fbReadSoftLimEnableLow.nErrorId;
ELSIF fbReadSoftLimEnableHigh.bError THEN
  nErrorId:=fbReadSoftLimEnableHigh.nErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_ReadParameterInNc_v1_00

FB_HomeToSwitch

FUNCTION_BLOCK FB_HomeToSwitch
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    bCamSensor:BOOL;
    nSearchDirTwoardsCam: MC_Direction;
    nSearchDirOffCam: MC_Direction;
    fHomePosition:LREAL;
    fVelocityToCamNC: LREAL; //Velcoity when searching for cam
    fVelocityFromCamNC: LREAL; // Velocity after found cam (searching for next signal transition)
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bHomed:BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR
    fbHome: MC_Home;
    fbWriteHomeDirCamToNC:FB_WriteParameterInNc_v1_00;
    fbWriteHomeDirSyncToNC:FB_WriteParameterInNc_v1_00;
    fbWriteHomeModeToNC:FB_WriteParameterInNc_v1_00;
    fbWriteHomeVelocitiesToNC: FB_HomeWriteNcVelocities;
    bConfigNCDone:BOOL:=FALSE;
    fbRTrigg: R_TRIG;
END_VAR
En:=EnO;
IF bReset THEN
    bConfigNCDone:=FALSE;
    bError:=FALSE;
    nErrorId:=0;
END_IF

//Start preparation of NC if rising edge on bExecute
fbRTrigg(CLK:=bExecute);
IF fbRTrigg.Q THEN
    bConfigNCDone:=FALSE;
END_IF


fbWriteHomeDirCamToNC(
    bExecute:=bExecute AND NOT bConfigNCDone,
    nDeviceGroup:=16#5000,
    nIndexOffset:=16#101,   //Direction for Calibration Cam Search
    nData:=BOOL_TO_DWORD(nSearchDirTwoardsCam=MC_Negative_Direction),//BOOL_TO_DWORD(NOT bSearchDirTwoardsCam),
    Axis:=Axis
);

fbWriteHomeDirSyncToNC(
    bExecute:= bExecute AND NOT bConfigNCDone,
    nDeviceGroup:=16#5000 ,
    nIndexOffset:=16#102 , //Direction for Sync Impuls Search
    nData:=BOOL_TO_DWORD(nSearchDirOffCam=MC_Negative_Direction),//BOOL_TO_DWORD(NOT bSearchDirOffCam),
    Axis:= Axis
);

fbWriteHomeModeToNC(
    bExecute:=bExecute AND NOT bConfigNCDone,
    nDeviceGroup:=16#5000,
    nIndexOffset:=16#107,  //Reference Mode
    nData:=1,
    Axis:=axis);

fbWriteHomeVelocitiesToNC(
  En:=En,
  bExecute:=bExecute AND NOT bConfigNCDone,
  bReset:=bReset,
  fVelocityFromCam:=fVelocityFromCamNC,
  fVelocityToCam:=fVelocityToCamNC,
  Axis:=Axis);

fbHome.bCalibrationCam:=bCamSensor;

fbHome(
    Execute:=bExecute AND bConfigNCDone(* AND NOT bError*),
    Position:=fHomePosition,
    HomingMode:=0,
    Axis:=Axis
    );
bBusy:=(fbHome.Busy OR (NOT bConfigNCDone AND bExecute));
bDone:=fbHome.Done AND bConfigNCDone;
bHomed:=Axis.Status.Homed;

IF (NOT bConfigNCDone) AND fbWriteHomeDirCamToNC.bDone AND fbWriteHomeDirSyncToNC.bDone AND fbWriteHomeModeToNC.bDone AND fbWriteHomeVelocitiesToNC.bDone THEN
  bConfigNCDone:=TRUE;
END_IF

//For some reason MC_HOME gives an Error for one cycle of NC-Task 1 SVB (10ms default..) so filter with bExecute
bError:=(fbHome.Error AND bExecute) OR fbWriteHomeDirCamToNC.bError OR fbWriteHomeDirSyncToNC.bError OR fbWriteHomeModeToNC.bError OR fbWriteHomeVelocitiesToNC.bError;

IF (fbHome.Error AND bExecute) THEN
  nErrorId:=fbHome.ErrorID;
ELSIF fbWriteHomeDirCamToNC.bError THEN
  nErrorId:=fbWriteHomeDirCamToNC.nErrorId;
ELSIF fbWriteHomeDirSyncToNC.bError THEN
  nErrorId:=fbWriteHomeDirSyncToNC.nErrorId;
ELSIF fbWriteHomeModeToNC.bError THEN
  nErrorId:=fbWriteHomeModeToNC.nErrorId;
ELSIF fbWriteHomeVelocitiesToNC.bError THEN
  nErrorId:=fbWriteHomeVelocitiesToNC.nErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_HomeWriteNcVelocities

• FB_WriteParameterInNc_v1_00

FB_HomeVirtual

FUNCTION_BLOCK FB_HomeVirtual
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    nCmdData: UINT;
    bLimitFwd: BOOL;
    bLimitBwd: BOOL;
    bHomeSensor: BOOL;
    fHomePosition:LREAL;
    nHomeRevOffset: UINT;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bHomed:BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR
    fbHomeToSwitch: FB_HomeToSwitch;
    fbHomeDirect: FB_HomeDirect; //Only used for direct homing (set of position)
    fbMoveVelocity:MC_MoveVelocity;
    fbHomePrepare:FB_HomePrepare;
    fbHomeFinish:FB_HomeFinish;
    fbExecuteRiseEdge: R_TRIG;
    nHomingState:INT:=0;
    bExecuteHomeToSwitch:BOOL:=FALSE;
    bExecuteMoveVelocity:BOOL:=FALSE;
    bExecutePrepare: BOOL:=FALSE;
    bExecuteFinish: BOOL:=FALSE;
    bExecuteHomeDirect: BOOL;
    nCmdDataLocal: UINT;  //Ensure that nCmdData is not changed during sequence
    bSequenceReady:BOOL:=TRUE;
    bRestoreNCDataNeeded: BOOL:=FALSE;
END_VAR
EnO:=En;

IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

// Reset when bExecute is low
IF NOT bExecute THEN
  nHomingState:=0;
  bSequenceReady:=TRUE;
  bExecuteHomeToSwitch:=FALSE;
  bExecuteHomeDirect:=FALSE;
  bExecuteMoveVelocity:=FALSE;
  bExecutePrepare:=FALSE;
  bExecuteFinish:=FALSE;
END_IF

//Reset at rinsing edge of bExecute
fbExecuteRiseEdge(CLK:=bExecute);
IF fbExecuteRiseEdge.Q THEN
  nCmdDataLocal:=nCmdData; //Ensure that nCmdData is not changed during sequence (nCmdData will only be read at a rising edge of bExecute)
  bSequenceReady:=FALSE;
  bExecutePrepare:=TRUE;
  bRestoreNCDataNeeded:=FALSE;
  //Check if valid nCmdDataLocal
  CASE nCmdDataLocal OF
    1:
    2:
    3:
    4:
    15:
    ELSE //nCmdData not valid
      bError:=TRUE;
      nErrorId:=16#4FFF;
  END_CASE
END_IF

//############# Prepare for homing (Read from NC and reset homed flag)
fbHomePrepare(
  En:=En,
  bExecute:=bExecutePrepare AND NOT bError, // Not needed for sequence 15 (set position only, no movement))
  bReset:=bReset,
  Axis:=Axis,
);

//############# Homing Sequences:
CASE nCmdDataLocal OF

   1: // Home to low limit switch
       CASE nHomingState OF
              0:
                bHomed:=Axis.Status.Homed;
            // Wait for read of velocities from NC and reset of calibration flag
            IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
                      bRestoreNCDataNeeded:=TRUE;
                      IF bLimitBwd THEN
                nHomingState:=1;
              ELSE
                            nHomingState:=2;  //Standing on limit switch go direct to state 2
              END_IF
            END_IF
              1: // wait for reach low limit then trigger fbHomeToSwitch
                bHomed:=FALSE;
                    bSequenceReady:=FALSE;
                    fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
            fbMoveVelocity.Direction:=MC_Negative_Direction;
            bExecuteMoveVelocity:=bExecute;  // Execute MC_MoveVelocity
                IF NOT bLimitBwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
                      nHomingState:=2;
                END_IF
              2: // Wait for fbHomeToSwitch
                bHomed:=FALSE;
                bSequenceReady:=FALSE;
            bExecuteMoveVelocity:=FALSE;
            bExecuteHomeToSwitch:=TRUE;
            fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Positive_Direction;
            fbHomeToSwitch.nSearchDirOffCam:=MC_Positive_Direction;
            fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
            fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam;  // Low speed
                fbHomeToSwitch.bCamSensor:=NOT bLimitBwd;
            IF fbHomeToSwitch.bDone THEN
                      nHomingState:=3;
                      bExecuteFinish:=TRUE;
                      fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
                      fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
            END_IF;
          3: // restore softlimit enable
            bHomed:=FALSE;
                bSequenceReady:=FALSE;
            IF fbHomeFinish.bDone THEN
                      bRestoreNCDataNeeded:=FALSE;
              bSequenceReady:=TRUE;
                      nHomingState:=0;
                      bHomed:=Axis.Status.Homed;
            END_IF;
       END_CASE;

   2: // Home to high limit switch
       CASE nHomingState OF
              0:
                bHomed:=Axis.Status.Homed;
            // Wait for read of velocities from NC and reset of calibration flag
            IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
                      bRestoreNCDataNeeded:=TRUE;
                      IF bLimitFwd THEN
                nHomingState:=1;
              ELSE
                            nHomingState:=2;  //Standing on limit switch go direct to state 2
              END_IF
            END_IF
              1: // wait for reach low limit then trigger fbHomeToSwitch
                bHomed:=FALSE;
                    bSequenceReady:=FALSE;
                    fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
            fbMoveVelocity.Direction:=MC_Positive_Direction;
            bExecuteMoveVelocity:=bExecute;  // Execute MC_MoveVelocity
                IF NOT bLimitFwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
                      nHomingState:=2;
                END_IF
              2: // Wait for fbHomeToSwitch
                bHomed:=FALSE;
                bSequenceReady:=FALSE;
            bExecuteMoveVelocity:=FALSE;
            bExecuteHomeToSwitch:=TRUE;
            fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Negative_Direction;
            fbHomeToSwitch.nSearchDirOffCam:=MC_Negative_Direction;
            fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
            fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam;  // Low speed
                fbHomeToSwitch.bCamSensor:=NOT bLimitFwd;
            IF fbHomeToSwitch.bDone THEN
                      nHomingState:=3;
                      bExecuteFinish:=TRUE;
                      fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
                      fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
            END_IF;
          3: // restore softlimit enable
            bHomed:=FALSE;
                bSequenceReady:=FALSE;
            IF fbHomeFinish.bDone THEN
                      bRestoreNCDataNeeded:=FALSE;
              bSequenceReady:=TRUE;
                      nHomingState:=0;
                      bHomed:=Axis.Status.Homed;
            END_IF;
       END_CASE;

   3: // Home on bHomeSensor via bLimitBwd
       CASE nHomingState OF
              0:
                bHomed:=Axis.Status.Homed;
            // Wait for read of velocities from NC and reset of calibration flag
            IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
                      bRestoreNCDataNeeded:=TRUE;
                      IF bLimitBwd THEN
                nHomingState:=1;
              ELSE
                            nHomingState:=2;  //Standing on limit switch go direct to state 2
              END_IF
            END_IF
              1: // wait for reach low limit then trigger fbHomeToSwitch
                bHomed:=FALSE;
                    bSequenceReady:=FALSE;
                    fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
            fbMoveVelocity.Direction:=MC_Negative_Direction;
            bExecuteMoveVelocity:=bExecute;  // Execute MC_MoveVelocity
                IF NOT bLimitBwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
                      nHomingState:=2;
                END_IF
              2: // Wait for fbHomeToSwitch
                bHomed:=FALSE;
                bSequenceReady:=FALSE;
            bExecuteMoveVelocity:=FALSE;
            bExecuteHomeToSwitch:=TRUE;
            fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Positive_Direction;
            fbHomeToSwitch.nSearchDirOffCam:=MC_Positive_Direction;
            fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
            fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam;  // Low speed
                fbHomeToSwitch.bCamSensor:=bHomeSensor;
            IF fbHomeToSwitch.bDone THEN
                      nHomingState:=3;
                      bExecuteFinish:=TRUE;
                      fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
                      fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
            END_IF;
          3: // restore softlimit enable
            bHomed:=FALSE;
                bSequenceReady:=FALSE;
            IF fbHomeFinish.bDone THEN
                  bRestoreNCDataNeeded:=FALSE;
              bSequenceReady:=TRUE;
                      nHomingState:=0;
                      bHomed:=Axis.Status.Homed;
            END_IF;
       END_CASE;
    4:  // Home on bHomeSensor via bLimitFwd
       CASE nHomingState OF
              0:
                bHomed:=Axis.Status.Homed;
            // Wait for read of velocities from NC and reset of calibration flag
            IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
                      bRestoreNCDataNeeded:=TRUE;
                      IF bLimitFwd THEN
                nHomingState:=1;
              ELSE
                            nHomingState:=2;  //Standing on limit switch go direct to state 2
              END_IF
            END_IF
              1: // wait for reach low limit then trigger fbHomeToSwitch
                bHomed:=FALSE;
                    bSequenceReady:=FALSE;
                    fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
            fbMoveVelocity.Direction:=MC_Positive_Direction;
            bExecuteMoveVelocity:=bExecute;  // Execute MC_MoveVelocity
                IF NOT bLimitFwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
                      nHomingState:=2;
                END_IF
              2: // Wait for fbHomeToSwitch
                bHomed:=FALSE;
                bSequenceReady:=FALSE;
            bExecuteMoveVelocity:=FALSE;
            bExecuteHomeToSwitch:=TRUE;
            fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Negative_Direction;
            fbHomeToSwitch.nSearchDirOffCam:=MC_Negative_Direction;
            fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
            fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam;  // Low speed
                fbHomeToSwitch.bCamSensor:=bHomeSensor;
            IF fbHomeToSwitch.bDone THEN
                      nHomingState:=3;
                      bExecuteFinish:=TRUE;
                      fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
                      fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
            END_IF;
          3: // Restore softlimit enable
            bHomed:=FALSE;
                bSequenceReady:=FALSE;
            IF fbHomeFinish.bDone THEN
                      bRestoreNCDataNeeded:=FALSE;
              bSequenceReady:=TRUE;
                      nHomingState:=0;
                      bHomed:=Axis.Status.Homed;
            END_IF;
       END_CASE;

   15: //Set current position (simplest homing sequence)
     bExecuteHomeDirect:=bExecute;
     bHomed:=Axis.Status.Homed;
     IF fbHomeDirect.bDone THEN  //Homing ready
       bExecuteHomeDirect:=FALSE;
       bSequenceReady:=TRUE;
     END_IF

ELSE
  fbHomeToSwitch.bCamSensor:=FALSE;
  bHomed:=Axis.Status.Homed;
END_CASE;

// Main homing block
fbHomeToSwitch(
    bExecute:=bExecuteHomeToSwitch AND bExecute AND NOT bError AND NOT bExecuteHomeDirect AND NOT bExecuteMoveVelocity,
    bReset:=bReset,
    fHomePosition:=fHomePosition,
    Axis:=Axis
);

// Approach limit switch (error if MC_Home is used)
fbMoveVelocity(
    Execute:= bExecuteMoveVelocity AND bExecute AND NOT bError AND NOT bExecuteHomeToSwitch AND NOT bExecuteHomeDirect,
    Axis:=Axis
);

// No sequence, just set position value (nCmdDataLocal=15). Can not run if fbHomeToSwitch is executed
fbHomeDirect(
    bExecute:=bExecuteHomeDirect AND bExecute AND NOT bError  AND NOT bExecuteHomeToSwitch AND NOT bExecuteMoveVelocity,
    bReset:=bReset,
    fHomePosition:=fHomePosition,
    Axis:=Axis
);


//############# Finish homing

IF NOT bexecute AND bRestoreNCDataNeeded THEN  //If homing is aborted restore is needed
    bExecuteFinish:=TRUE;
    IF fbHomeFinish.bDone THEN
      bExecuteFinish:=FALSE;
      bRestoreNCDataNeeded:=FALSE;
    END_IF
END_IF

fbHomeFinish(
  En:=En,
  bExecute:=bExecuteFinish,
  bReset:=bReset,
  Axis:=Axis,
);

// Error handling
IF NOT bError THEN
  IF fbHomeToSwitch.bError THEN
    bError:=fbHomeToSwitch.bError;
    nErrorId:=fbHomeToSwitch.nErrorId;
  ELSIF fbHomeDirect.bError THEN
    bError:=fbHomeDirect.bError;
    nErrorId:=fbHomeDirect.nErrorId;
  ELSIF fbMoveVelocity.Error THEN
    bError:=fbMoveVelocity.Error;
    nErrorId:=fbMoveVelocity.ErrorId;
  END_IF;
END_IF

// Done and busy bit
bDone:=bSequenceReady AND bExecute;
bBusy:=NOT bSequenceReady;

RETURN;

END_FUNCTION_BLOCK

Related:

• FB_HomeDirect

• FB_HomeFinish

• FB_HomePrepare

• FB_HomeToSwitch

• Main

FB_HomeWriteNcVelocities

FUNCTION_BLOCK FB_HomeWriteNcVelocities
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    fVelocityToCam: LREAL;
    fVelocityFromCam: LREAL;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR
    fbExecuteRiseEdge: R_TRIG;
    fbWriteVelocityToCam:FB_WriteFloatParameter;
    fbWriteVelocityFromCam:FB_WriteFloatParameter;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

fbExecuteRiseEdge(CLK:=bExecute);

fbWriteVelocityToCam(
    bExecute:=bExecute,
    nDeviceGroup:= 16#4000,
    nIndexOffset:= 16#6,
    nData:=fVelocityToCam,
    Axis:= Axis);

fbWriteVelocityFromCam(
    bExecute:=bExecute,
    nDeviceGroup:= 16#4000,
    nIndexOffset:= 16#7,
    nData:=fVelocityFromCam,
    Axis:= Axis);

bBusy:=fbWriteVelocityFromCam.bBusy OR fbWriteVelocityToCam.bBusy;
bDone:=fbWriteVelocityFromCam.bDone AND fbWriteVelocityToCam.bDone AND bExecute;

bError:=fbWriteVelocityToCam.bError OR fbWriteVelocityFromCam.bError;
IF fbWriteVelocityToCam.bError THEN
  nErrorId:=fbWriteVelocityToCam.nErrorId;
ELSIF fbWriteVelocityFromCam.bError THEN
  nErrorId:=fbWriteVelocityFromCam.nErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_WriteFloatParameter

FB_HomeWriteSoftLimEnable

FUNCTION_BLOCK FB_HomeWriteSoftLimEnable
VAR_INPUT
    En: BOOL;
    bReset: BOOL;
    bExecute: BOOL;
    bSofLimEnableLow: BOOL:=TRUE;
    bSofLimEnableHigh: BOOL:=TRUE;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR
    fbExecuteRiseEdge: R_TRIG;
    fbWriteSoftLimEnableLow:FB_WriteParameterInNc_v1_00;
    fbWriteSoftLimEnableHigh:FB_WriteParameterInNc_v1_00;
END_VAR
En:=EnO;
IF bReset THEN
    bError:=FALSE;
    nErrorId:=0;
END_IF

fbExecuteRiseEdge(CLK:=bExecute);

fbWriteSoftLimEnableLow(
    bExecute:=bExecute,
    nDeviceGroup:= 16#5000,
    nIndexOffset:= 16#B,
    nData:=BOOL_TO_DWORD(bSofLimEnableLow),
    Axis:= Axis);

fbWriteSoftLimEnableHigh(
    bExecute:=bExecute,
    nDeviceGroup:= 16#5000,
    nIndexOffset:= 16#C,
    nData:=BOOL_TO_DWORD(bSofLimEnableHigh),
    Axis:= Axis);

bBusy:=fbWriteSoftLimEnableLow.bBusy OR fbWriteSoftLimEnableHigh.bBusy;
bDone:=fbWriteSoftLimEnableLow.bDone AND fbWriteSoftLimEnableHigh.bDone AND bExecute;

bError:=fbWriteSoftLimEnableHigh.bError OR fbWriteSoftLimEnableLow.bError;
IF fbWriteSoftLimEnableHigh.bError THEN
  nErrorId:=fbWriteSoftLimEnableHigh.nErrorId;
ELSIF fbWriteSoftLimEnableLow.bError THEN
  nErrorId:=fbWriteSoftLimEnableLow.nErrorId;
END_IF

END_FUNCTION_BLOCK

Related:

• FB_WriteParameterInNc_v1_00

FB_NcAxis

///#########################################################
///Function block to communicate between Nc and Plc.
///
/// Library:
/// Tc2_MC2.lib
///
/// Global Variables:
///
/// Data types:
///
/// External functions:
///
///###########################################################
FUNCTION_BLOCK FB_NcAxis
VAR
    sVersion: STRING:='1.0.0';
END_VAR
VAR_INPUT
    En: BOOL;
END_VAR
VAR_OUTPUT
    EnO: BOOL;
    bError: BOOL;
    Status: ST_AxisStatus;
END_VAR
VAR
    Axis: AXIS_REF;
    InfoData_State AT %I*: UINT ;
END_VAR
EnO:=En;


IF En AND InfoData_State<>16#8 THEN
    bError:=TRUE;
ELSE
    bError:=FALSE;
END_IF

IF En THEN
    Axis.ReadStatus();
    Status:=Axis.Status;
END_IF

END_FUNCTION_BLOCK

FB_ReadFloatParameter

FUNCTION_BLOCK FB_ReadFloatParameter
VAR_INPUT
    bExecute: BOOL;
    ///16#4000=Axisdata, 16#5000=Encoderdata, 16#6000=Controldata, 16#7000=Drivedata
    nDeviceGroup: UDINT;
    nIndexOffset: UDINT;
END_VAR
VAR_OUTPUT
    nData: LREAL;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR
    nState: UINT;
    fbADSREAD: ADSREAD;
END_VAR
(*Sequence to read parameter in Nc*)
CASE nState OF
0:  (*Start sequence. Wait until bExecute is TRUE*)
    IF bExecute THEN
            bBusy:=TRUE;
            bError:=FALSE;
            nErrorId:=0;
            nState:=10;
    END_IF

10: (*Read parameter in Nc*)
    fbADSREAD(
            PORT:=500,
            IDXGRP:=nDeviceGroup+Axis.NcToPlc.AxisId,
            IDXOFFS:=nIndexOffset,
            LEN:=SIZEOF(nData),
            DESTADDR:=ADR(nData),
            READ:=TRUE);

    (*Wait until it's done or if an error occurs*)
    IF NOT fbADSREAD.ERR THEN
            IF NOT fbADSREAD.BUSY THEN
                    fbADSREAD(READ:=FALSE);
                    nState:=20;
            END_IF
    ELSE
            nErrorId:=fbADSREAD.ERRID;
            nState:=999;
    END_IF

20: (*Sequense is done. Waits until bExecute is FALSE*)
    bBusy:=FALSE;
    bDone:=TRUE;
    IF NOT bExecute THEN
            bDone:=FALSE;
            nState:=0;
    END_IF

999: (*Error in sequence*)
    bError:=TRUE;
    bBusy:=FALSE;
    bDone:=FALSE;
    fbADSREAD(READ:=FALSE);
    IF NOT bExecute THEN
            nState:=0;
    END_IF

END_CASE

END_FUNCTION_BLOCK

FB_ReadParameterInNc_v1_00

///#########################################################
///Function block to read parameter in Nc.
///
/// Library:
/// Tc2_MC2.lib
/// Tc2_System.lib
///
/// Global Variables:
///
/// Data types:
///
/// External functions:
///
/// History:
/// 2014-02-05      v1.00   NB      Release code.
///
/// Known bugs:
///
///
///
///###########################################################
FUNCTION_BLOCK FB_ReadParameterInNc_v1_00
VAR_INPUT
    bExecute: BOOL;
    ///16#4000=Axisdata, 16#5000=Encoderdata, 16#6000=Controldata, 16#7000=Drivedata
    nDeviceGroup: UDINT;
    nIndexOffset: UDINT;
END_VAR
VAR_OUTPUT
    nData: DWORD;
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR
    nState: UINT;
    fbADSREAD: ADSREAD;
END_VAR
(*Sequence to read parameter in Nc*)
CASE nState OF
0:  (*Start sequence. Wait until bExecute is TRUE*)
    IF bExecute THEN
            bBusy:=TRUE;
            bError:=FALSE;
            nErrorId:=0;
            nState:=10;
    END_IF

10: (*Read parameter in Nc*)
    fbADSREAD(
            PORT:=500,
            IDXGRP:=nDeviceGroup+Axis.NcToPlc.AxisId,
            IDXOFFS:=nIndexOffset,
            LEN:=SIZEOF(nData),
            DESTADDR:=ADR(nData),
            READ:=TRUE);

    (*Wait until it's done or if an error occurs*)
    IF NOT fbADSREAD.ERR THEN
            IF NOT fbADSREAD.BUSY THEN
                    fbADSREAD(READ:=FALSE);
                    nState:=20;
            END_IF
    ELSE
            nErrorId:=fbADSREAD.ERRID;
            nState:=999;
    END_IF

20: (*Sequense is done. Waits until bExecute is FALSE*)
    bBusy:=FALSE;
    bDone:=TRUE;
    IF NOT bExecute THEN
            bDone:=FALSE;
            nState:=0;
    END_IF

999: (*Error in sequence*)
    bError:=TRUE;
    bBusy:=FALSE;
    bDone:=FALSE;
    fbADSREAD(READ:=FALSE);
    IF NOT bExecute THEN
            nState:=0;
    END_IF

END_CASE

END_FUNCTION_BLOCK

FB_WriteFloatParameter

///#########################################################
///Function block to write parameter in Nc.
///
/// Library:
/// Tc2_MC2.lib
/// Tc2_System.lib
///
/// Global Variables:
///
/// Data types:
///
/// External functions:
///
/// History:
/// 2014-02-05      v1.00   NB      Release code.
///
/// Known bugs:
///
///
///
///###########################################################
FUNCTION_BLOCK FB_WriteFloatParameter
VAR_INPUT
    bExecute: BOOL;
    ///16#4000=Axisdata, 16#5000=Encoderdata, 16#6000=Controldata, 16#7000=Drivedata
    nDeviceGroup: UDINT;
    nIndexOffset: UDINT;
    nData: LREAL;
END_VAR
VAR_OUTPUT
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR
    nState: UINT;
    fbADSWRITE: ADSWRITE;
END_VAR
(*Sequence to write parameter in Nc*)
CASE nState OF
0:  (*Start sequence. Wait until bExecute is TRUE*)
    IF bExecute THEN
            bBusy:=TRUE;
            bError:=FALSE;
            nErrorId:=0;
            nState:=10;
    END_IF

10: (*Write parameter in Nc*)
    fbADSWRITE(
            PORT:=500,
            IDXGRP:=nDeviceGroup+Axis.NcToPlc.AxisId,
            IDXOFFS:=nIndexOffset,
            LEN:=SIZEOF(nData),
            SRCADDR:=ADR(nData),
            WRITE:=TRUE);

    (*Wait until it's done or if an error occurs*)
    IF NOT fbADSWRITE.ERR THEN
            IF NOT fbADSWRITE.BUSY THEN
                    fbADSWRITE(WRITE:=FALSE);
                    nState:=20;
            END_IF
    ELSE
            nErrorId:=fbADSWRITE.ERRID;
            nState:=999;
    END_IF

20: (*Sequense is done. Waits until bExecute is FALSE*)
    bBusy:=FALSE;
    bDone:=TRUE;
    IF NOT bExecute THEN
            bDone:=FALSE;
            nState:=0;
    END_IF

999: (*Error in sequence*)
    bError:=TRUE;
    bBusy:=FALSE;
    bDone:=FALSE;
    fbADSWRITE(WRITE:=FALSE);
    IF NOT bExecute THEN
            nState:=0;
    END_IF

END_CASE

END_FUNCTION_BLOCK

FB_WriteParameterInNc_v1_00

///#########################################################
///Function block to write parameter in Nc.
///
/// Library:
/// Tc2_MC2.lib
/// Tc2_System.lib
///
/// Global Variables:
///
/// Data types:
///
/// External functions:
///
/// History:
/// 2014-02-05      v1.00   NB      Release code.
///
/// Known bugs:
///
///
///
///###########################################################
FUNCTION_BLOCK FB_WriteParameterInNc_v1_00
VAR_INPUT
    bExecute: BOOL;
    ///16#4000=Axisdata, 16#5000=Encoderdata, 16#6000=Controldata, 16#7000=Drivedata
    nDeviceGroup: UDINT;
    nIndexOffset: UDINT;
    nData: DWORD;
END_VAR
VAR_OUTPUT
    bBusy: BOOL;
    bDone: BOOL;
    bError: BOOL;
    nErrorId: UDINT;
END_VAR
VAR_IN_OUT
    Axis: AXIS_REF;
END_VAR
VAR
    nState: UINT;
    fbADSWRITE: ADSWRITE;
END_VAR
(*Sequence to write parameter in Nc*)
CASE nState OF
0:  (*Start sequence. Wait until bExecute is TRUE*)
    IF bExecute THEN
            bBusy:=TRUE;
            bError:=FALSE;
            nErrorId:=0;
            nState:=10;
    END_IF

10: (*Write parameter in Nc*)
    fbADSWRITE(
            PORT:=500,
            IDXGRP:=nDeviceGroup+Axis.NcToPlc.AxisId,
            IDXOFFS:=nIndexOffset,
            LEN:=SIZEOF(nData),
            SRCADDR:=ADR(nData),
            WRITE:=TRUE);

    (*Wait until it's done or if an error occurs*)
    IF NOT fbADSWRITE.ERR THEN
            IF NOT fbADSWRITE.BUSY THEN
                    fbADSWRITE(WRITE:=FALSE);
                    nState:=20;
            END_IF
    ELSE
            nErrorId:=fbADSWRITE.ERRID;
            nState:=999;
    END_IF

20: (*Sequense is done. Waits until bExecute is FALSE*)
    bBusy:=FALSE;
    bDone:=TRUE;
    IF NOT bExecute THEN
            bDone:=FALSE;
            nState:=0;
    END_IF

999: (*Error in sequence*)
    bError:=TRUE;
    bBusy:=FALSE;
    bDone:=FALSE;
    fbADSWRITE(WRITE:=FALSE);
    IF NOT bExecute THEN
            nState:=0;
    END_IF

END_CASE

END_FUNCTION_BLOCK

Main

PROGRAM Main
VAR
    nSanityCount: UINT;

    M1: FB_DriveVirtual;
    M1Link: FB_NcAxis;
    bLimitFwdM1 AT %I*: BOOL;
    bLimitBwdM1 AT %I*: BOOL;

    M2: FB_DriveVirtual;
    M2Link: FB_NcAxis;
    bLimitFwdM2 AT %I*: BOOL;
    bLimitBwdM2 AT %I*: BOOL;

    M3: FB_DriveVirtual;
    M3Link: FB_NcAxis;
    bLimitFwdM3 AT %I*: BOOL;
    bLimitBwdM3 AT %I*: BOOL;

    M4: FB_DriveVirtual;
    M4Link: FB_NcAxis;
    bLimitFwdM4 AT %I*: BOOL;
    bLimitBwdM4 AT %I*: BOOL;

    M5: FB_DriveVirtual;
    M5Link: FB_NcAxis;
    bLimitFwdM5 AT %I*: BOOL;
    bLimitBwdM5 AT %I*: BOOL;

    M6: FB_DriveVirtual;
    M6Link: FB_NcAxis;
    bLimitFwdM6 AT %I*: BOOL;
    bLimitBwdM6 AT %I*: BOOL;

    M7: FB_DriveVirtual;
    M7Link: FB_NcAxis;
    bLimitFwdM7 AT %I*: BOOL;
    bLimitBwdM7 AT %I*: BOOL;

    M8: FB_DriveVirtual;
    M8Link: FB_NcAxis;
    bLimitFwdM8 AT %I*: BOOL;
    bLimitBwdM8 AT %I*: BOOL;

    M9: FB_DriveVirtual;
    M9Link: FB_NcAxis;
    bLimitFwdM9 AT %I*: BOOL;
    bLimitBwdM9 AT %I*: BOOL;

    M10: FB_DriveVirtual;
    M10Link: FB_NcAxis;
    bLimitFwdM10 AT %I*: BOOL;
    bLimitBwdM10 AT %I*: BOOL;

    M11: FB_DriveVirtual;
    M11Link: FB_NcAxis;
    bLimitFwdM11 AT %I*: BOOL;
    bLimitBwdM11 AT %I*: BOOL;

    M12: FB_DriveVirtual;
    M12Link: FB_NcAxis;
    bLimitFwdM12 AT %I*: BOOL;
    bLimitBwdM12 AT %I*: BOOL;

    M13: FB_DriveVirtual;
    M13Link: FB_NcAxis;
    bLimitFwdM13 AT %I*: BOOL;
    bLimitBwdM13 AT %I*: BOOL;
END_VAR
nSanityCount := nSanityCount + 1;

M1Link(En := TRUE);
M1(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := bLimitFwdM1,
   bLimitBwd := bLimitBwdM1,
   Axis := M1Link.axis);

M2Link(En := TRUE);
M2(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM2,
   bLimitBwd := TRUE, // bLimitBwdM2,
   Axis := M2Link.axis);

M3Link(En := TRUE);
M3(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM3,
   bLimitBwd := TRUE, // bLimitBwdM3,
   Axis := M3Link.axis);

M4Link(En := TRUE);
M4(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM4,
   bLimitBwd := TRUE, // bLimitBwdM4,
   Axis := M4Link.axis);

M5Link(En := TRUE);
M5(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM5,
   bLimitBwd := TRUE, // bLimitBwdM5,
   Axis := M5Link.axis);

M6Link(En := TRUE);
M6(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM6,
   bLimitBwd := TRUE, // bLimitBwdM6,
   Axis := M6Link.axis);

M7Link(En := TRUE);
M7(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM7,
   bLimitBwd := TRUE, // bLimitBwdM7,
   Axis := M7Link.axis);

M8Link(En := TRUE);
M8(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM8,
   bLimitBwd := TRUE, // bLimitBwdM8,
   Axis := M8Link.axis);

M9Link(En := TRUE);
M9(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM9,
   bLimitBwd := TRUE, // bLimitBwdM9,
   Axis := M9Link.axis);

M10Link(En := TRUE);
M10(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM10,
   bLimitBwd := TRUE, // bLimitBwdM10,
   Axis := M10Link.axis);

M11Link(En := TRUE);
M11(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM11,
   bLimitBwd := TRUE, // bLimitBwdM11,
   Axis := M11Link.axis);

M12Link(En := TRUE);
M12(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM12,
   bLimitBwd := TRUE, // bLimitBwdM12,
   Axis := M12Link.axis);

M13Link(En := TRUE);
M13(En := TRUE,
   bEnable := TRUE,
   bLimitFwd := TRUE, // bLimitFwdM13,
   bLimitBwd := TRUE, // bLimitBwdM13,
   Axis := M13Link.axis);

END_PROGRAM

Related:

• FB_DriveVirtual

• FB_NcAxis