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`_