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 DUT_ErrorState ^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE DUT_ErrorState : ( None, Active, Inactive, Acknowledged ); END_TYPE DUT_MotionPneumaticActuator ^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE DUT_MotionPneumaticActuator : // Defines the EPICS interface to actuating a pneumatic stage STRUCT (* Hardware *) //Readbacks //Limit Switch {attribute 'pytmc' := ' pv: PLC:bInLimitSwitch io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if IN limit is reached '} i_bInLimitSwitch : BOOL; {attribute 'pytmc' := ' pv: PLC:bOutLimitSwitch io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if OUT limit is reached '} i_bOutLimitSwitch : BOOL; //Controls //Digital outputs {attribute 'pytmc' := ' pv: bRetractDigitalOutput; io: i; field: ONAM FALSE field: ZNAM TRUE field: DESC TRUE if Retract digital output is active '} q_bRetract : BOOL; {attribute 'pytmc' := ' pv: bInsetDigitalOutput; io: i; field: ONAM FALSE field: ZNAM TRUE field: DESC TRUE if Insert digital output is active '} q_bInsert : BOOL; //Logic and supervisory {attribute 'pytmc' := ' pv: bInterlockOK; io: i; field: ZNAM FALSE field: ONAM TRUE field: DESC True if the actuator has permission to move in either direction '} bILK_OK: BOOL; {attribute 'pytmc' := ' pv: bInsertEnable; io: i; field: ZNAM FALSE field: ONAM TRUE field: DESC True if the actuator had permission to be retracted '} bInsertOK : BOOL; {attribute 'pytmc' := ' pv: bRetractEnable; io: i; field: ZNAM FALSE field: ONAM TRUE field: DESC True if the actuator had permission to be inserted '} bRetractOK : BOOL; (* Commands *) // Used from Epics to comand the actuator to move {attribute 'pytmc' := ' pv: CMD:IN; io: io; field: DESC Used by EPICS and internally to request Insert motion '} bInsert_SW : BOOL; {attribute 'pytmc' := ' pv: CMD:OUT; io: io; field: DESC Used by EPICS and internally to request retract motion '} bRetract_SW : BOOL; (*Returns*) // TRUE if in the middle of a command {attribute 'pytmc' := ' pv: bBusy io: i field: ONAM FALSE field: ZNAM TRUE field: DESC TRUE if in the middle of a command '} bBusy: BOOL; // TRUE if we've done a command and it has finished {attribute 'pytmc' := ' pv: bDone io: i field: ONAM FALSE field: ZNAM TRUE field: DESC TRUE if command finished successfully '} bDone: BOOL; {attribute 'pytmc' := ' pv: bReset io: io field: ZNAM FALSE field: ONAM TRUE field: DESC Used internally to reset errors '} bReset: BOOL; // TRUE if we're in an error state {attribute 'pytmc' := ' pv: PLC:bError io: i field: ONAM FALSE field: ZNAM TRUE field: DESC TRUE if we're in an error state '} bError: BOOL; // Error code if nonzero {attribute 'pytmc' := ' pv: PLC:nErrorId io: i field: DESC Error code if nonzero '} nErrorId: UDINT; // Message to identify the error state {attribute 'pytmc' := ' pv: PLC:sErrorMessage io: i field: DESC Message to identify the error state '} sErrorMessage: STRING; {attribute 'pytmc' := ' pv: nPositionState ; type: mbbi ; field: ZRST RETRACTED ; field: ONST INSERTED ; field: TWST MOVING ; field: THST INVALID ; io: i field: DESC Pneumatic actuator position '} eState : ENUM_PnuematicActuatorPositionState := ENUM_PnuematicActuatorPositionState.INVALID; END_STRUCT END_TYPE DUT_MotionStage ^^^^^^^^^^^^^^^ :: TYPE DUT_MotionStage : // Defines the EPICS interface to moving a motor in TwinCAT STRUCT (* Hardware *) // PLC Axis Reference Axis: AXIS_REF; // NC Forward Limit Switch: TRUE if ok to move {attribute 'pytmc' := ' pv: PLC:bLimitForwardEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC FALSE if forward limit hit '} bLimitForwardEnable AT %I*: BOOL; // NC Backward Limit Switch: TRUE if ok to move {attribute 'pytmc' := ' pv: PLC:bLimitBackwardEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC FALSE if reverse limit hit '} bLimitBackwardEnable AT %I*: BOOL; // NO Home Switch: TRUE if at home {attribute 'pytmc' := ' pv: PLC:bHome io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if at homing switch '} bHome AT %I*: BOOL; // NC Brake Output: TRUE to release brake {attribute 'pytmc' := ' pv: PLC:bBrakeRelease io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if brake released '} bBrakeRelease AT %Q*: BOOL; // NC STO Input: TRUE if ok to move {attribute 'pytmc' := ' pv: PLC:bHardwareEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if STO not hit '} bHardwareEnable AT %I*: BOOL; // Raw encoder IO for ULINT (Biss-C) nRawEncoderULINT AT %I*: ULINT; // Raw encoder IO for UINT (Relative Encoders) nRawEncoderUINT AT %I*: UINT; // Raw encoder IO for INT (LVDT) nRawEncoderINT AT %I*: INT; (* Psuedo-hardware *) // Forward enable EPS summary {attribute 'pytmc' := ' pv: PLC:bAllForwardEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC Summary of axis permission to move forward '} bAllForwardEnable: BOOL:=FALSE; // Backward enable EPS summary {attribute 'pytmc' := ' pv: PLC:bAllBackwardEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC Summary of axis permission to move backward '} bAllBackwardEnable: BOOL:=FALSE; // Enable EPS summary encapsulating emergency stop button and any additional motion preventive hardware {attribute 'pytmc' := ' pv: PLC:bAllEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC Summary of axis permission to have power '} bAllEnable: BOOL:=FALSE; // Forward virtual gantry limit switch {attribute 'pytmc' := ' pv: PLC:bGantryForwardEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if gantry ok to move forward '} bGantryForwardEnable: BOOL:=FALSE; // Backward virtual gantry limit switch {attribute 'pytmc' := ' pv: PLC:bGantryBackwardEnable io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if gantry ok to move backward '} bGantryBackwardEnable: BOOL:=FALSE; // Encoder count summary, if linked above {attribute 'pytmc' := ' pv: PLC:nEncoderCount io: i field: DESC Count from encoder hardware '} nEncoderCount: UDINT; (* Settings *) // Name to use for log messages, fast faults, etc. {attribute 'pytmc' := ' pv: PLC:sName io: i field: DESC PLC program name '} sName: STRING; // If TRUE, we want to enable the motor independently of PMPS or other safety systems. {attribute 'pytmc' := ' pv: PLC:bPowerSelf io: i field: ZNAM FALSE field: ONAM TRUE field: DESC FALSE if axis is in PMPS '} bPowerSelf: BOOL:=FALSE; // Determines when we automatically enable the motor {attribute 'pytmc' := ' pv: PLC:nEnableMode io: i field: DESC Describes when the axis will automatically get power '} nEnableMode: ENUM_StageEnableMode:=ENUM_StageEnableMode.DURING_MOTION; // Determines when we automatically disengage the brake {attribute 'pytmc' := ' pv: PLC:nBrakeMode io: i field: DESC Describes when the brake will be released '} nBrakeMode: ENUM_StageBrakeMode:=ENUM_StageBrakeMode.IF_ENABLED; // Determines our encoder homing strategy {attribute 'pytmc' := ' pv: PLC:nHomingMode io: i field: DESC Describes our homing strategy '} nHomingMode: ENUM_EpicsHomeCmd:=ENUM_EpicsHomeCmd.NONE; // Set true to activate gantry EPS {attribute 'pytmc' := ' pv: PLC:bGantryAxis io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if gantry EPS active '} bGantryAxis: BOOL:=FALSE; // Set to gantry difference tolerance nGantryTol: LINT:=0; // Encoder count at which this axis is aligned with other axis nEncRef: ULINT:=0; (* Commands *) // Used internally to request enables {attribute 'pytmc' := ' pv: PLC:bEnable io: io field: ZNAM FALSE field: ONAM TRUE field: DESC Used internally to request enables '} bEnable: BOOL; // Used internally to reset errors and other state {attribute 'pytmc' := ' pv: PLC:bReset io: io field: ZNAM FALSE field: ONAM TRUE field: DESC Used internally to reset errors '} bReset: BOOL; // Used internally and by the IOC to start or stop a move {attribute 'pytmc' := ' pv: PLC:bExecute io: io field: ZNAM FALSE field: ONAM TRUE field: DESC Used internally and by the IOC to start or stop '} bExecute: BOOL; // Used by the IOC to disable an axis {attribute 'pytmc' := ' pv: PLC:bUserEnable io: io field: ZNAM DISABLE field: ONAM ENABLE field: DESC Used to disable power entirely for an axis '} bUserEnable: BOOL := 1; (* Shortcut Commands *) // Start a move to fPosition with fVelocity {attribute 'pytmc' := ' pv: PLC:bMoveCmd io: io field: DESC Start a move '} bMoveCmd: BOOL; // Start the homing routine {attribute 'pytmc' := ' pv: PLC:bHomeCmd io: io field: DESC Start the homing routine '} bHomeCmd: BOOL; (* Command Args *) // Used internally and by the IOC to pick what kind of move to do {attribute 'pytmc' := ' pv: PLC:nCommand io: io field: DESC Used internally and by the IOC to pick move type '} nCommand: INT; // Used internally and by the IOC to pass additional data to some commands {attribute 'pytmc' := ' pv: PLC:nCmdData io: io field: DESC Used internally and by the IOC to pass extra args '} nCmdData: INT; // Used internally and by the IOC to pick a destination for the move {attribute 'pytmc' := ' pv: PLC:fPosition io: io field: DESC Used internally and by the IOC as the set position '} fPosition: LREAL; // Used internally and by the IOC to pick a move velocity {attribute 'pytmc' := ' pv: PLC:fVelocity io: io field: DESC Used internally and by the IOC to set velocity '} fVelocity: LREAL; // Used internally and by the IOC to pick a move acceleration {attribute 'pytmc' := ' pv: PLC:fAcceleration io: io field: DESC Used internally and by the IOC to set acceleration '} fAcceleration: LREAL; // Used internally and by the IOC to pick a move deceleration {attribute 'pytmc' := ' pv: PLC:fDeceleration io: io field: DESC Used internally and by the IOC to set deceleration '} fDeceleration: LREAL; // Used internally and by the IOC to pick a home position {attribute 'pytmc' := ' pv: PLC:fHomePosition io: io field: DESC Used internally and by the IOC to pick home position '} fHomePosition: LREAL; (* Info *) // Unique ID assigned to each axis in the NC {attribute 'pytmc' := ' pv: PLC:nMotionAxisID io: i field: DESC Unique ID assigned to each axis in the NC '} nMotionAxisID: UDINT:=0; (* Returns *) // TRUE if done enabling {attribute 'pytmc' := ' pv: PLC:bEnableDone io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if done enabling '} bEnableDone: BOOL; // TRUE if in the middle of a command {attribute 'pytmc' := ' pv: PLC:bBusy io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if in the middle of a command '} bBusy: BOOL; // TRUE if we've done a command and it has finished {attribute 'pytmc' := ' pv: PLC:bDone io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if command finished successfully '} bDone: BOOL; // TRUE if the motor has been homed, or does not need to be homed {attribute 'pytmc' := ' pv: PLC:bHomed io: i field: DESC TRUE if the motor has been homed '} bHomed: BOOL; // TRUE if we have safety permission to move {attribute 'pytmc' := ' pv: PLC:bSafetyReady io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if safe to start a move '} bSafetyReady: BOOL; // TRUE if we're in an error state {attribute 'pytmc' := ' pv: PLC:bError io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if we're in an error state '} bError: BOOL; // Error code if nonzero {attribute 'pytmc' := ' pv: PLC:nErrorId io: i field: DESC Error code if nonzero '} nErrorId: UDINT; // Message to identify the error state {attribute 'pytmc' := ' pv: PLC:sErrorMessage io: i field: DESC Message to identify the error state '} sErrorMessage: STRING; // Internal hook for custom error messages sCustomErrorMessage: STRING; // MC_ReadParameterSet Output stAxisParameters: ST_AxisParameterSet; // True if we've updated stAxisParameters at least once bAxisParamsInit: BOOL; // Misc axis status information for the IOC stAxisStatus: DUT_AxisStatus_v0_01; (* Other status information for users of the IOC *) // Position lag difference {attribute 'pytmc' := ' pv: PLC:fPosDiff io: i field: DESC Position lag difference '} fPosDiff: LREAL; END_STRUCT END_TYPE DUT_PositionState ^^^^^^^^^^^^^^^^^ :: TYPE DUT_PositionState : // Defines settings and current safety status for moves to specific positions for an axis STRUCT // Name as queried via the NAME PV in EPICS {attribute 'pytmc' := ' pv: NAME io: input field: DESC Name of this position state '} sName: STRING := 'Invalid'; // Position associated with this state {attribute 'pytmc' := ' pv: SETPOINT io: io field: DESC Axis position associated with this state '} fPosition: LREAL; {attribute 'pytmc' := ' pv: ENCODER io: i field: DESC Encoder count associated with this state '} nEncoderCount: UDINT; // Maximum allowable deviation from fPosition while at the state {attribute 'pytmc' := ' pv: DELTA io: io field: DRVL 0.0 field: DESC Max deviation from position at this state '} fDelta: LREAL; // Speed at which to move to this state {attribute 'pytmc' := ' pv: VELO io: io field: DESC Speed at which to move to this state '} fVelocity: LREAL; // (optional) Acceleration to use for moves to this state {attribute 'pytmc' := ' pv: ACCL io: io field: DESC Acceleration to use for moves to this state '} fAccel: LREAL; // (optional) Deceleration to use for moves to this state {attribute 'pytmc' := ' pv: DCCL io: io field: DESC Deceleration to use for moves to this state '} fDecel: LREAL; // Safety parameter. This must be set to TRUE by the PLC program to allow moves to this state. This is expected to change as conditions change. {attribute 'pytmc' := ' pv: MOVE_OK io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if the move would be safe '} bMoveOk: BOOL; // Signifies to FB_PositionStateLock that this state should be immutable {attribute 'pytmc' := ' pv: LOCKED io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if state is immutable '} bLocked: BOOL; // Set this to TRUE when you make your state. This defaults to FALSE so that uninitialized states can never be moved to {attribute 'pytmc' := ' pv: VALID io: i field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if this is a real state '} bValid: BOOL; // Set this to TRUE when you want to use the raw encoder counts to define the state bUseRawCounts: BOOL; // Is set to TRUE by FB_PositionStateInternal when called bUpdated: BOOL; // Beam parameters associated with this state stBeamParams: ST_BeamParams := PMPS_GVL.cst0RateBeam; // Transition ID associated with this state nRequestAssertionID: UDINT; END_STRUCT END_TYPE DUT_TerminalError ^^^^^^^^^^^^^^^^^ :: TYPE DUT_TerminalError : STRUCT //Error system related iTerminalID : INT; //ID of the terminal Error_ID : ULINT := 0; //ID for the Error entry ErrorState : DUT_ErrorState; //State of the error //Error related nDateTimeOn : ULINT; //Date and time when the error occured. Raw data sDateTimeOn : STRING(24); //Date and time when the error occured. Readable format nDateTimeOff : ULINT; //Date and time when the error disapeared. Raw data sDateTimeOff : STRING(24); //Date and time when the error disapeared. Readable format bWcState : BOOL; //WcState variable of the terminal uiInfoDataState : UINT; //InfoData.State variable of the terminal sErrorMessage : STRING (128); //Error message corresponding to WcState and InfoData.State ErrorType : INT; //Error types (priorities) need to be developed END_STRUCT END_TYPE dutEL2521_Ctrl ^^^^^^^^^^^^^^ :: TYPE dutEL2521_Ctrl : STRUCT ///The control word (CW) is located in the output process image, and is transmitted from the controller to the terminal. ///CW.0 FREQ_SEL 0bin / 1bin Rapid change of the base frequency (only if the ramp function is inactive): ///0bin = Base frequency 1 (object 8001:02) ///1bin = Base frequency 2 (object 8001:03) FREQ_SEL: BOOL; ///CW.1 RAMP_DIS 1bin Operation of the ramp function is cancelled, in spite of object 8000:06 being active; if travel distance control is active, it is interrupted by this bit RAMP_DIS: BOOL; ///CW.2 GO_COUNTER 1bin If travel distance control is active (object 8000:0A), then a pre-set counter value is approached when the bit is set GO_COUNTER: BOOL; ///CW.5 CNT_CLR 1bin The contents of the counter is cleared or set (object 8000:0B) by this bit. ///Any overflow or underflow bits that might be set are also cleared by this bit. ///The process can be edge triggered or level triggered (object 8000:05). CNT_CLR: BOOL; END_STRUCT END_TYPE dutEL2521_Status ^^^^^^^^^^^^^^^^ :: TYPE dutEL2521_Status : STRUCT ///The status word (SW) is located in the input process image, and is transmitted from the terminal to the controller. ///SW.0 SEL_ACK/END_COUNTER 1bin Confirms the change of base frequency. At activated travel distance control: target counter value reached SEL_ACK: BOOL; ///SW.1 RAMP_ACTIVE 1bin Ramp is currently being followed RAMP_ACTIVE: BOOL; ///SW.2 UNDERFLOW 1bin This bit is set if the 16-bit counter underflows (0 -> 65535). It is reset when the counter drops below two thirds of its measuring range (43690 -> 43689) or immediately an overflow occurs. UNDERFLOW: BOOL; ///SW.3 OVERFLOW 1bin This bit is set if the 16-bit counter overflows (65535 -> 0). It is reset when the counter exceeds one third of its measuring range (21845 -> 21846) or immediately an underflow occurs OVERFLOW: BOOL; ///SW.4 INPUT_T 1bin Status of INPUT_T INPUT_T: BOOL; ///SW.5 INPUT_Z 1bin Status of INPUT_Z INPUT_Z: BOOL; ///SW.6 ERROR 1bin General error bit, included with overflow/underflow ERROR: BOOL; END_STRUCT END_TYPE EL5042_Status ^^^^^^^^^^^^^ :: TYPE EL5042_Status : STRUCT END_STRUCT END_TYPE ENUM_EpicsHomeCmd ^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE ENUM_EpicsHomeCmd : // Defines the valid options for homing in FB_MotionStage ( LOW_LIMIT := 1, // Low limit switch HIGH_LIMIT := 2, // High limit switch HOME_VIA_LOW := 3, // Home switch via low switch HOME_VIA_HIGH := 4, // Home switch via high switch ABSOLUTE_SET := 15, // Set here to be fHomePosition NONE := -1 // Do not home, ever ); END_TYPE ENUM_EpicsInOut ^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} // Example EPICS states enum for use in FB_PositionStateManager // Remove strict attribute for easier handling TYPE ENUM_EpicsInOut : ( UNKNOWN := 0, // UNKNOWN must be in slot 0 or the FB breaks OUT := 1, // OUT at slot 1 is a convention IN := 2 ); END_TYPE ENUM_EpicsMotorCmd ^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE ENUM_EpicsMotorCmd : // Defines valid EPICS commands for nCommand ( JOG := 0, MOVE_VELOCITY := 1, MOVE_RELATIVE := 2, MOVE_ABSOLUTE := 3, MOVE_MODULO := 4, HOME := 10, GEAR := 30 ); END_TYPE ENUM_MotionRequest ^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE ENUM_MotionRequest : // Defines behavior options for when FB_MotionRequest is run during an active move from another source ( WAIT := 0, INTERRUPT := 1, ABORT := 2 ); END_TYPE ENUM_PnuematicActuatorPositionState ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} // Defines the positions for a pnuematic actuator TYPE ENUM_PnuematicActuatorPositionState : ( RETRACTED := 0, // Out limit switch is active INSERTED := 1, // In limit switch is active MOVING := 2, // Neither limit switches is Active INVALID :=3 // Invalid state ); END_TYPE ENUM_StageBrakeMode ^^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE ENUM_StageBrakeMode : // Defines when to send the brake disengage signal in FB_MotionStage ( IF_ENABLED, // Disengage brake when the motor is enabled IF_MOVING, // Disengage brake when the motor is moving NO_BRAKE // Do not change the brake state in FB_MotionStage ); END_TYPE ENUM_StageEnableMode ^^^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE ENUM_StageEnableMode : // Define conditions when FB_MotionStage automatically sets bEnable ( ALWAYS, // Always set bEnable to TRUE NEVER, // Only change bEnable on errors DURING_MOTION // Enable before motion, disable after motion ); END_TYPE ST_ErrorSystem ^^^^^^^^^^^^^^ :: TYPE ST_ErrorSystem : STRUCT //Array of error data. Size = cSizeOfErrorData in the GVL aErrorData : ARRAY [0..GVL_ErrorSystem.cSizeOfErrorData - 1] OF DUT_TerminalError; lNextErrorID : ULINT := 1; //ErrorID for the next error entry nNoErrors : UINT; //Number of errors in the list nNoOverflows : INT := 0; //Number of overflows. How many error entries have been lost END_STRUCT END_TYPE ST_RenishawAbsEnc ^^^^^^^^^^^^^^^^^ :: // Renishaw BiSS-C absolute encoder used with an EL5042 TYPE ST_RenishawAbsEnc : STRUCT Count AT %I*: ULINT; // Connect to encoder "Position" input Status: EL5042_Status; // Status struct placeholder Ref: ULINT; // Encoder zero position (useful for aligned position with gantries) END_STRUCT END_TYPE GVLs ---- Global_Version ^^^^^^^^^^^^^^ :: {attribute 'TcGenerated'} // This function has been automatically generated from the project information. VAR_GLOBAL CONSTANT {attribute 'const_non_replaced'} {attribute 'linkalways'} stLibVersion_lcls_twincat_motion : ST_LibVersion := (iMajor := 1, iMinor := 5, iBuild := 0, iRevision := 0, sVersion := '1.5.0'); END_VAR GVL ^^^ :: VAR_GLOBAL nHomingError:UDINT:=16#14D00; END_VAR GVL_ErrorSystem ^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} VAR_GLOBAL CONSTANT cSizeOfErrorData : UINT := 128; END_VAR MOTION_GVL ^^^^^^^^^^ :: {attribute 'qualified_only'} VAR_GLOBAL stUnknownState: DUT_PositionState := (sName := 'Unknown'); stInvalidState: DUT_PositionState; END_VAR VAR_GLOBAL CONSTANT // Allocated space for 9 states besides Unknown (16 including Unknown is the max for an EPICS MBBI) MAX_STATES: INT := 9; END_VAR POUs ---- BasicTests ^^^^^^^^^^ :: PROGRAM BasicTests VAR Motor: DUT_MotionStage; fbMotion: FB_MotionStageSim; fbRequest: FB_MotionRequest; bError: BOOL; errState: MC_AxisStates; END_VAR fbRequest(stMotionStage := Motor); fbMotion(stMotionStage := Motor); CASE Motor.Axis.Status.MotionState OF MC_AXISSTATE_ERRORSTOP, MC_AXISSTATE_STOPPING, MC_AXISSTATE_HOMING, MC_AXISSTATE_DISCRETEMOTION, MC_AXISSTATE_CONTINOUSMOTION, MC_AXISSTATE_SYNCHRONIZEDMOTION: IF NOT Motor.bBrakeRelease THEN bError := TRUE; errState := Motor.Axis.Status.MotionState; END_IF END_CASE END_PROGRAM CheckBounds ^^^^^^^^^^^ :: // Implicitly generated code : DO NOT EDIT FUNCTION CheckBounds : DINT VAR_INPUT index, lower, upper: DINT; END_VAR // Implicitly generated code : Only an Implementation suggestion {noflow} IF index < lower THEN CheckBounds := lower; ELSIF index > upper THEN CheckBounds := upper; ELSE CheckBounds := index; END_IF {flow} END_FUNCTION CheckDivDInt ^^^^^^^^^^^^ :: // Implicitly generated code : DO NOT EDIT FUNCTION CheckDivDInt : DINT VAR_INPUT divisor:DINT; END_VAR // Implicitly generated code : Only an Implementation suggestion {noflow} IF divisor = 0 THEN CheckDivDInt:=1; ELSE CheckDivDInt:=divisor; END_IF; {flow} END_FUNCTION CheckDivLInt ^^^^^^^^^^^^ :: // Implicitly generated code : DO NOT EDIT FUNCTION CheckDivLInt : LINT VAR_INPUT divisor:LINT; END_VAR // Implicitly generated code : Only an Implementation suggestion {noflow} IF divisor = 0 THEN CheckDivLInt:=1; ELSE CheckDivLInt:=divisor; END_IF; {flow} END_FUNCTION CheckDivLReal ^^^^^^^^^^^^^ :: // Implicitly generated code : DO NOT EDIT FUNCTION CheckDivLReal : LREAL VAR_INPUT divisor:LREAL; END_VAR // Implicitly generated code : Only an Implementation suggestion {noflow} IF divisor = 0 THEN CheckDivLReal:=1; ELSE CheckDivLReal:=divisor; END_IF; {flow} END_FUNCTION CheckDivReal ^^^^^^^^^^^^ :: // Implicitly generated code : DO NOT EDIT FUNCTION CheckDivReal : REAL VAR_INPUT divisor:REAL; END_VAR // Implicitly generated code : Only an Implementation suggestion {noflow} IF divisor = 0 THEN CheckDivReal:=1; ELSE CheckDivReal:=divisor; END_IF; {flow} END_FUNCTION EK1200 ^^^^^^ :: FUNCTION_BLOCK EK1200 VAR_INPUT En: BOOL; END_VAR VAR_OUTPUT EnO: BOOL; END_VAR EnO:=En; END_FUNCTION_BLOCK EL1008 ^^^^^^ :: ///EL1008 | 8-channel digital input terminal 24 V DC, 3 ms FUNCTION_BLOCK EL1008 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bDi_1: BOOL; bDi_2: BOOL; bDi_3: BOOL; bDi_4: BOOL; bDi_5: BOOL; bDi_6: BOOL; bDi_7: BOOL; bDi_8: BOOL; bError: BOOL; END_VAR VAR Channel_1_Input AT %I*: BOOL; Channel_2_Input AT %I*: BOOL; Channel_3_Input AT %I*: BOOL; Channel_4_Input AT %I*: BOOL; Channel_5_Input AT %I*: BOOL; Channel_6_Input AT %I*: BOOL; Channel_7_Input AT %I*: BOOL; Channel_8_Input AT %I*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL1008_Error : FB_TerminalError; END_VAR EnO:=En; //Error FB instance EL1008_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN bDi_1:=Channel_1_Input; bDi_2:=Channel_2_Input; bDi_3:=Channel_3_Input; bDi_4:=Channel_4_Input; bDi_5:=Channel_5_Input; bDi_6:=Channel_6_Input; bDi_7:=Channel_7_Input; bDi_8:=Channel_8_Input; ELSE bDi_1:=FALSE; bDi_2:=FALSE; bDi_3:=FALSE; bDi_4:=FALSE; bDi_5:=FALSE; bDi_6:=FALSE; bDi_7:=FALSE; bDi_8:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL1018 ^^^^^^ :: ///EL1018 | 8-channel digital input terminal 24 V DC, 10 µs FUNCTION_BLOCK EL1018 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bDi_1: BOOL; bDi_2: BOOL; bDi_3: BOOL; bDi_4: BOOL; bDi_5: BOOL; bDi_6: BOOL; bDi_7: BOOL; bDi_8: BOOL; bError : BOOL; END_VAR VAR Channel_1_Input AT %I*: BOOL; Channel_2_Input AT %I*: BOOL; Channel_3_Input AT %I*: BOOL; Channel_4_Input AT %I*: BOOL; Channel_5_Input AT %I*: BOOL; Channel_6_Input AT %I*: BOOL; Channel_7_Input AT %I*: BOOL; Channel_8_Input AT %I*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL1018_Error : FB_TerminalError; END_VAR EnO:=En; //Error FB instance EL1018_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN bDi_1:=Channel_1_Input; bDi_2:=Channel_2_Input; bDi_3:=Channel_3_Input; bDi_4:=Channel_4_Input; bDi_5:=Channel_5_Input; bDi_6:=Channel_6_Input; bDi_7:=Channel_7_Input; bDi_8:=Channel_8_Input; ELSE bDi_1:=FALSE; bDi_2:=FALSE; bDi_3:=FALSE; bDi_4:=FALSE; bDi_5:=FALSE; bDi_6:=FALSE; bDi_7:=FALSE; bDi_8:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL1808 ^^^^^^ :: ///EL1808 | HD EtherCAT Terminals, 8-channel digital input 24 V DC, 3 ms, 2-wire connection FUNCTION_BLOCK EL1808 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bDi_1: BOOL; bDi_2: BOOL; bDi_3: BOOL; bDi_4: BOOL; bDi_5: BOOL; bDi_6: BOOL; bDi_7: BOOL; bDi_8: BOOL; bError: BOOL; END_VAR VAR Channel_1_Input AT %I*: BOOL; Channel_2_Input AT %I*: BOOL; Channel_3_Input AT %I*: BOOL; Channel_4_Input AT %I*: BOOL; Channel_5_Input AT %I*: BOOL; Channel_6_Input AT %I*: BOOL; Channel_7_Input AT %I*: BOOL; Channel_8_Input AT %I*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL1808_Error : FB_TerminalError; END_VAR EnO:=En; //Error FB instance EL1808_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN bDi_1:=Channel_1_Input; bDi_2:=Channel_2_Input; bDi_3:=Channel_3_Input; bDi_4:=Channel_4_Input; bDi_5:=Channel_5_Input; bDi_6:=Channel_6_Input; bDi_7:=Channel_7_Input; bDi_8:=Channel_8_Input; ELSE bDi_1:=FALSE; bDi_2:=FALSE; bDi_3:=FALSE; bDi_4:=FALSE; bDi_5:=FALSE; bDi_6:=FALSE; bDi_7:=FALSE; bDi_8:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL1809 ^^^^^^ :: ///EL1809 | HD EtherCAT Terminals, 16-channel digital input 24 V DC, 3 ms FUNCTION_BLOCK EL1809 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bDi_1: BOOL; bDi_2: BOOL; bDi_3: BOOL; bDi_4: BOOL; bDi_5: BOOL; bDi_6: BOOL; bDi_7: BOOL; bDi_8: BOOL; bDi_9: BOOL; bDi_10: BOOL; bDi_11: BOOL; bDi_12: BOOL; bDi_13: BOOL; bDi_14: BOOL; bDi_15: BOOL; bDi_16: BOOL; bError: BOOL; END_VAR VAR Channel_1_Input AT %I*: BOOL; Channel_2_Input AT %I*: BOOL; Channel_3_Input AT %I*: BOOL; Channel_4_Input AT %I*: BOOL; Channel_5_Input AT %I*: BOOL; Channel_6_Input AT %I*: BOOL; Channel_7_Input AT %I*: BOOL; Channel_8_Input AT %I*: BOOL; Channel_9_Input AT %I*: BOOL; Channel_10_Input AT %I*: BOOL; Channel_11_Input AT %I*: BOOL; Channel_12_Input AT %I*: BOOL; Channel_13_Input AT %I*: BOOL; Channel_14_Input AT %I*: BOOL; Channel_15_Input AT %I*: BOOL; Channel_16_Input AT %I*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL1809_Error : FB_TerminalError; END_VAR EnO:=En; //Error FB instance EL1809_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN bDi_1:=Channel_1_Input; bDi_2:=Channel_2_Input; bDi_3:=Channel_3_Input; bDi_4:=Channel_4_Input; bDi_5:=Channel_5_Input; bDi_6:=Channel_6_Input; bDi_7:=Channel_7_Input; bDi_8:=Channel_8_Input; bDi_9:=Channel_9_Input; bDi_10:=Channel_10_Input; bDi_11:=Channel_11_Input; bDi_12:=Channel_12_Input; bDi_13:=Channel_13_Input; bDi_14:=Channel_14_Input; bDi_15:=Channel_15_Input; bDi_16:=Channel_16_Input; ELSE bDi_1:=FALSE; bDi_2:=FALSE; bDi_3:=FALSE; bDi_4:=FALSE; bDi_5:=FALSE; bDi_6:=FALSE; bDi_7:=FALSE; bDi_8:=FALSE; bDi_9:=FALSE; bDi_10:=FALSE; bDi_11:=FALSE; bDi_12:=FALSE; bDi_13:=FALSE; bDi_14:=FALSE; bDi_15:=FALSE; bDi_16:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL1819 ^^^^^^ :: ///EL1819 | HD EtherCAT Terminals, 16-channel digital input 24 V DC, 10 µs FUNCTION_BLOCK EL1819 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bDi_1: BOOL; bDi_2: BOOL; bDi_3: BOOL; bDi_4: BOOL; bDi_5: BOOL; bDi_6: BOOL; bDi_7: BOOL; bDi_8: BOOL; bDi_9: BOOL; bDi_10: BOOL; bDi_11: BOOL; bDi_12: BOOL; bDi_13: BOOL; bDi_14: BOOL; bDi_15: BOOL; bDi_16: BOOL; bError: BOOL; END_VAR VAR Channel_1_Input AT %I*: BOOL; Channel_2_Input AT %I*: BOOL; Channel_3_Input AT %I*: BOOL; Channel_4_Input AT %I*: BOOL; Channel_5_Input AT %I*: BOOL; Channel_6_Input AT %I*: BOOL; Channel_7_Input AT %I*: BOOL; Channel_8_Input AT %I*: BOOL; Channel_9_Input AT %I*: BOOL; Channel_10_Input AT %I*: BOOL; Channel_11_Input AT %I*: BOOL; Channel_12_Input AT %I*: BOOL; Channel_13_Input AT %I*: BOOL; Channel_14_Input AT %I*: BOOL; Channel_15_Input AT %I*: BOOL; Channel_16_Input AT %I*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL1819_Error : FB_TerminalError; END_VAR EnO:=En; //Error FB instance EL1819_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN bDi_1:=Channel_1_Input; bDi_2:=Channel_2_Input; bDi_3:=Channel_3_Input; bDi_4:=Channel_4_Input; bDi_5:=Channel_5_Input; bDi_6:=Channel_6_Input; bDi_7:=Channel_7_Input; bDi_8:=Channel_8_Input; bDi_9:=Channel_9_Input; bDi_10:=Channel_10_Input; bDi_11:=Channel_11_Input; bDi_12:=Channel_12_Input; bDi_13:=Channel_13_Input; bDi_14:=Channel_14_Input; bDi_15:=Channel_15_Input; bDi_16:=Channel_16_Input; ELSE bDi_1:=FALSE; bDi_2:=FALSE; bDi_3:=FALSE; bDi_4:=FALSE; bDi_5:=FALSE; bDi_6:=FALSE; bDi_7:=FALSE; bDi_8:=FALSE; bDi_9:=FALSE; bDi_10:=FALSE; bDi_11:=FALSE; bDi_12:=FALSE; bDi_13:=FALSE; bDi_14:=FALSE; bDi_15:=FALSE; bDi_16:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL2014 ^^^^^^ :: FUNCTION_BLOCK EL2014 VAR_INPUT En: BOOL; iTerminal_ID : INT; bDo_1: BOOL; bDo_2: BOOL; bDo_3: BOOL; bDo_4: BOOL; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO : BOOL; bError : BOOL; END_VAR VAR Channel_1_Output AT %Q*: BOOL; Channel_2_Output AT %Q*: BOOL; Channel_3_Output AT %Q*: BOOL; Channel_4_Output AT %Q*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL2014_Error : FB_TerminalError; END_VAR (* * TODO: * Channel diagnostic variables and device diag variables *) EnO:=En; EL2014_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN Channel_1_Output:=bDo_1; Channel_2_Output:=bDo_2; Channel_3_Output:=bDo_3; Channel_4_Output:=bDo_4; ELSE Channel_1_Output:=FALSE; Channel_2_Output:=FALSE; Channel_3_Output:=FALSE; Channel_4_Output:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL2252 ^^^^^^ :: ///EL2252 | XFC, 2-channel digital output terminal with time stamp, tri-state FUNCTION_BLOCK EL2252 VAR_INPUT En: BOOL; iTerminal_ID : INT; bDo_1: BOOL; bDo_2: BOOL; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO : BOOL; bError : BOOL; END_VAR VAR Channel_1_Output AT %Q*: BOOL; Channel_2_Output AT %Q*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL2252_Error : FB_TerminalError; END_VAR (* * TODO: * Add the DC sync variables *) EL2252_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN Channel_1_Output:=bDo_1; Channel_2_Output:=bDo_2; ELSE Channel_1_Output:=FALSE; Channel_2_Output:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL2808 ^^^^^^ :: FUNCTION_BLOCK EL2808 VAR_INPUT En: BOOL; iTerminal_ID : INT; bDo_1: BOOL; bDo_2: BOOL; bDo_3: BOOL; bDo_4: BOOL; bDo_5: BOOL; bDo_6: BOOL; bDo_7: BOOL; bDo_8: BOOL; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO : BOOL; bError : BOOL; END_VAR VAR Channel_1_Output AT %Q*: BOOL; Channel_2_Output AT %Q*: BOOL; Channel_3_Output AT %Q*: BOOL; Channel_4_Output AT %Q*: BOOL; Channel_5_Output AT %Q*: BOOL; Channel_6_Output AT %Q*: BOOL; Channel_7_Output AT %Q*: BOOL; Channel_8_Output AT %Q*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL2808_Error : FB_TerminalError; END_VAR EnO:=En; EL2808_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN Channel_1_Output:=bDo_1; Channel_2_Output:=bDo_2; Channel_3_Output:=bDo_3; Channel_4_Output:=bDo_4; Channel_5_Output:=bDo_5; Channel_6_Output:=bDo_6; Channel_7_Output:=bDo_7; Channel_8_Output:=bDo_8; ELSE Channel_1_Output:=FALSE; Channel_2_Output:=FALSE; Channel_3_Output:=FALSE; Channel_4_Output:=FALSE; Channel_5_Output:=FALSE; Channel_6_Output:=FALSE; Channel_7_Output:=FALSE; Channel_8_Output:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL2819 ^^^^^^ :: FUNCTION_BLOCK EL2819 VAR_INPUT En: BOOL; iTerminal_ID : INT; bDo_1: BOOL; bDo_2: BOOL; bDo_3: BOOL; bDo_4: BOOL; bDo_5: BOOL; bDo_6: BOOL; bDo_7: BOOL; bDo_8: BOOL; bDo_9: BOOL; bDo_10: BOOL; bDo_11: BOOL; bDo_12: BOOL; bDo_13: BOOL; bDo_14: BOOL; bDo_15: BOOL; bDo_16: BOOL; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO : BOOL; bError : BOOL; END_VAR VAR Channel_1_Output AT %Q*: BOOL; Channel_2_Output AT %Q*: BOOL; Channel_3_Output AT %Q*: BOOL; Channel_4_Output AT %Q*: BOOL; Channel_5_Output AT %Q*: BOOL; Channel_6_Output AT %Q*: BOOL; Channel_7_Output AT %Q*: BOOL; Channel_8_Output AT %Q*: BOOL; Channel_9_Output AT %Q*: BOOL; Channel_10_Output AT %Q*: BOOL; Channel_11_Output AT %Q*: BOOL; Channel_12_Output AT %Q*: BOOL; Channel_13_Output AT %Q*: BOOL; Channel_14_Output AT %Q*: BOOL; Channel_15_Output AT %Q*: BOOL; Channel_16_Output AT %Q*: BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL2819_Error : FB_TerminalError; END_VAR (* * TODO: * Channel diagnostic variables and device diag variables *) EnO:=En; EL2819_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError=FALSE THEN Channel_1_Output:=bDo_1; Channel_2_Output:=bDo_2; Channel_3_Output:=bDo_3; Channel_4_Output:=bDo_4; Channel_5_Output:=bDo_5; Channel_6_Output:=bDo_6; Channel_7_Output:=bDo_7; Channel_8_Output:=bDo_8; Channel_9_Output:=bDo_9; Channel_10_Output:=bDo_10; Channel_11_Output:=bDo_11; Channel_12_Output:=bDo_12; Channel_13_Output:=bDo_13; Channel_14_Output:=bDo_14; Channel_15_Output:=bDo_15; Channel_16_Output:=bDo_16; ELSE Channel_1_Output:=FALSE; Channel_2_Output:=FALSE; Channel_3_Output:=FALSE; Channel_4_Output:=FALSE; Channel_5_Output:=FALSE; Channel_6_Output:=FALSE; Channel_7_Output:=FALSE; Channel_8_Output:=FALSE; Channel_9_Output:=FALSE; Channel_10_Output:=FALSE; Channel_11_Output:=FALSE; Channel_12_Output:=FALSE; Channel_13_Output:=FALSE; Channel_14_Output:=FALSE; Channel_15_Output:=FALSE; Channel_16_Output:=FALSE; END_IF END_IF END_FUNCTION_BLOCK EL3174_0002 ^^^^^^^^^^^ :: //EL3174-0002 | 4-channel analog input, -10/0...+10V, -20/0/+4...20mA, 16 bit, differential FUNCTION_BLOCK EL3174_0002 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; iAi_Ch1_Value : INT; iAi_Ch2_Value : INT; iAi_Ch3_Value : INT; iAi_Ch4_Value : INT; bError: BOOL; END_VAR VAR //Channels AI_Std_Ch_1_Status AT %I* : WORD; AI_Std_Ch_1_Value AT %I* : INT; AI_Std_Ch_2_Status AT %I* : WORD; AI_Std_Ch_2_Value AT %I* : INT; AI_Std_Ch_3_Status AT %I* : WORD; AI_Std_Ch_3_Value AT %I* : INT; AI_Std_Ch_4_Status AT %I* : WORD; AI_Std_Ch_4_Value AT %I* : INT; //Terminal status WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL3174_0002_Error : FB_TerminalError; END_VAR (* * TODO: * Status words of the channels *) EnO := En; //Error FB instance EL3174_0002_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError = FALSE THEN iAi_Ch1_Value := AI_Std_Ch_1_Value; iAi_Ch2_Value := AI_Std_Ch_2_Value; iAi_Ch3_Value := AI_Std_Ch_3_Value; iAi_Ch4_Value := AI_Std_Ch_4_Value; ELSE iAi_Ch1_Value := 0; iAi_Ch2_Value := 0; iAi_Ch3_Value := 0; iAi_Ch4_Value := 0; END_IF END_IF END_FUNCTION_BLOCK EL3214 ^^^^^^ :: //EL3214 | 4-channel analog input terminal, PT100 (RTD) FUNCTION_BLOCK EL3214 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; iAi_Ch1_Value : INT; iAi_Ch2_Value : INT; iAi_Ch3_Value : INT; iAi_Ch4_Value : INT; bError: BOOL; END_VAR VAR //Channels AI_RTD_Ch_1_Status AT %I* : WORD; AI_RTD_Ch_1_Value AT %I* : INT; AI_RTD_Ch_2_Status AT %I* : WORD; AI_RTD_Ch_2_Value AT %I* : INT; AI_RTD_Ch_3_Status AT %I* : WORD; AI_RTD_Ch_3_Value AT %I* : INT; AI_RTD_Ch_4_Status AT %I* : WORD; AI_RTD_Ch_4_Value AT %I* : INT; //Terminal status WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL3214_Error : FB_TerminalError; END_VAR (* * TODO: * Status words of the channels *) EnO := En; //Error FB instance EL3214_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF En THEN IF bError = FALSE THEN iAi_Ch1_Value := AI_RTD_Ch_1_Value; iAi_Ch2_Value := AI_RTD_Ch_2_Value; iAi_Ch3_Value := AI_RTD_Ch_3_Value; iAi_Ch4_Value := AI_RTD_Ch_4_Value; ELSE iAi_Ch1_Value := 0; iAi_Ch2_Value := 0; iAi_Ch3_Value := 0; iAi_Ch4_Value := 0; END_IF END_IF END_FUNCTION_BLOCK EL3255 ^^^^^^ :: //EL3255 | 5-channel potentiometer measurement with sensor supply FUNCTION_BLOCK EL3255 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO : BOOL; Ch1_Value : INT; Ch2_Value : INT; Ch3_Value : INT; Ch4_Value : INT; Ch5_Value : INT; bError : BOOL; END_VAR VAR AI_Std_Ch1_Value AT %I* : INT; AI_Std_Ch2_Value AT %I* : INT; AI_Std_Ch3_Value AT %I* : INT; AI_Std_Ch4_Value AT %I* : INT; AI_Std_Ch5_Value AT %I* : INT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL3255_Error : FB_TerminalError; END_VAR (* * TODO: * Channel Status words *) EnO:=En; //Error FB instance EL3255_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF EN THEN IF NOT bError THEN Ch1_Value := AI_Std_Ch1_Value; Ch2_Value := AI_Std_Ch2_Value; Ch3_Value := AI_Std_Ch3_Value; Ch4_Value := AI_Std_Ch4_Value; Ch5_Value := AI_Std_Ch5_Value; ELSE Ch1_Value := 0; Ch2_Value := 0; Ch3_Value := 0; Ch4_Value := 0; Ch5_Value := 0; END_IF END_IF END_FUNCTION_BLOCK EL5002 ^^^^^^ :: //EL5002 | 2-chennel SSI absolute encoder terminal FUNCTION_BLOCK EL5002 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; Ch1_Counter_Value : UDINT; Ch2_Counter_Value : UDINT; bError: BOOL; END_VAR VAR udi_Ch1_Cnt_Value AT %I* : UDINT; udi_Ch2_Cnt_Value AT %I* : UDINT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL5002_Error : FB_TerminalError; END_VAR (* * TODO: * Channel Status words *) EnO:=En; //Error FB instance EL5002_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF EN THEN IF NOT bError THEN Ch1_Counter_Value := udi_Ch1_Cnt_Value; Ch2_Counter_Value := udi_Ch2_Cnt_Value; ELSE Ch1_Counter_Value := 0; Ch2_Counter_Value := 0; END_IF END_IF END_FUNCTION_BLOCK EL5021 ^^^^^^ :: //EL5021 | 1-channel Sin/Cos encoder FUNCTION_BLOCK EL5021 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; Counter_Value : UDINT; Latch_Value : UDINT; bError: BOOL; END_VAR VAR udiCounter_Value AT %I* : UDINT; udiLatch_Value AT %I* : UDINT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL5021_Error : FB_TerminalError; END_VAR (* * TODO: * Channel Status words, control words *) EnO:=En; //Error FB instance EL5021_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF EN THEN IF NOT bError THEN Counter_Value := udiCounter_Value; Latch_Value := udiLatch_Value; ELSE Counter_Value := 0; Latch_Value := 0; END_IF END_IF END_FUNCTION_BLOCK EL5042 ^^^^^^ :: //EL5042 | 2-channel BiSS-C absolute encoder terminal FUNCTION_BLOCK EL5042 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; Ch1_Position : ULINT; Ch2_Position : ULINT; bError: BOOL; END_VAR VAR FB_Inputs_ch1_Position AT %I* : ULINT; FB_Inputs_ch2_Position AT %I* : ULINT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL5042_Error : FB_TerminalError; END_VAR (* * TODO: * Channel Status words *) EnO:=En; //Error FB instance EL5042_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF EN THEN IF NOT bError THEN Ch1_Position := FB_Inputs_ch1_Position; Ch2_Position := FB_Inputs_ch2_Position; ELSE Ch1_Position := 0; Ch2_Position := 0; END_IF END_IF END_FUNCTION_BLOCK EL5101 ^^^^^^ :: // EL5101 | 1-channel incremental encoder terminal, 5V, RS422 FUNCTION_BLOCK EL5101 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; Counter_Value : UINT; Latch_Value : UINT; bError: BOOL; END_VAR VAR uiCounter_Value AT %I* : UINT; uiLatch_Value AT %I* : UINT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL5101_Error : FB_TerminalError; END_VAR (* * TODO: * Channel Status word, control words *) EnO:=En; //Error FB instance EL5101_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); IF EN THEN IF NOT bError THEN Counter_Value := uiCounter_Value; Latch_Value := uiLatch_Value; ELSE Counter_Value := 0; Latch_Value := 0; END_IF END_IF END_FUNCTION_BLOCK EL7211_v1_00 ^^^^^^^^^^^^ :: ///EL7211 | Servo motor termional 5 A FUNCTION_BLOCK EL7211_v1_00 VAR_INPUT En: BOOL; END_VAR VAR_OUTPUT EnO: BOOL; bError: BOOL; END_VAR VAR WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; END_VAR EnO:=En; IF En AND (WcState_WcState OR InfoData_State<>16#8) THEN bError:=TRUE; ELSE bError:=FALSE; END_IF END_FUNCTION_BLOCK EL9410 ^^^^^^ :: //EL9410 | E-Bus power supplier and refresher, diagnostics FUNCTION_BLOCK EL9410 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bError: BOOL; END_VAR VAR bStatus_Us_UV AT %I* : BOOL; bStatus_Up_UV AT %I* : BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL9410_Error : FB_TerminalError; END_VAR (* * TODO: * Status bits *) EnO:=En; //Error FB instance EL9410_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); END_FUNCTION_BLOCK EL9505 ^^^^^^ :: //EL9505 | Power supply terminal 5V FUNCTION_BLOCK EL9505 VAR_INPUT En: BOOL; iTerminal_ID : INT; ErrorSystem : POINTER TO ST_ErrorSystem; END_VAR VAR_OUTPUT EnO: BOOL; bError: BOOL; END_VAR VAR bStatus_Uo_Power_OK AT %I* : BOOL; bStatus_Uo_Overload AT %I* : BOOL; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; //FB-s EL9505_Error : FB_TerminalError; END_VAR (* * TODO: * Status bits *) EnO:=En; //Error FB instance EL9505_Error( En := TRUE, iTerminal_ID := iTerminal_ID, bWcState := WcState_WcState, uiInfoData_State := InfoData_State, pErrorSystem := ErrorSystem, bError => bError, ); END_FUNCTION_BLOCK EL9576_v1_00 ^^^^^^^^^^^^ :: ///EL9576 | Brake terminal (vap and resistor) FUNCTION_BLOCK EL9576_v1_00 VAR_INPUT En: BOOL; END_VAR VAR_OUTPUT EnO: BOOL; bError: BOOL; OverTemperature: BOOL; I2TError : BOOL; I2TWarning : BOOL; OverVoltage : BOOL; UnderVoltage : BOOL; ChopperOn : BOOL; DCLinkVoltage : LREAL; DutyCycle : LREAL; ResistorCurrent : LREAL; END_VAR VAR BCTOverTemperature AT %I*: BOOL; BCTI2TError AT %I*: BOOL; BCTI2TWarning AT %I*: BOOL; BCTOverVoltage AT %I*: BOOL; BCTUnderVoltage AT %I*: BOOL; BCTChopperOn AT %I*: BOOL; BCTDCLinkVoltage AT %I*: UDINT; BCTDutyCycle AT %I*: USINT; BCTResistorCurrent AT %I*: UDINT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; END_VAR EnO:=En; IF En AND (WcState_WcState OR InfoData_State<>16#8 OR BCTI2TError OR BCTI2TWarning OR BCTOverTemperature OR BCTOverVoltage OR BCTUnderVoltage) THEN bError:=TRUE; ELSE bError:=FALSE; END_IF OverTemperature:=BCTOverTemperature; I2TError:=BCTI2TError; I2TWarning:=BCTI2TWarning; OverVoltage:=BCTOverVoltage; UnderVoltage:=bctUnderVoltage; ChopperOn:=bctChopperOn; DCLinkVoltage:=UDINT_TO_LREAL(bctDCLinkVoltage); DutyCycle:=USINT_TO_LREAL(BCTDutyCycle); ResistorCurrent:=UDINT_TO_LREAL(BCTResistorCurrent); END_FUNCTION_BLOCK F_AtPositionState ^^^^^^^^^^^^^^^^^ :: FUNCTION F_AtPositionState : BOOL // Check if the motor is within the state bounds VAR_INPUT stMotionStage: DUT_MotionStage; stPositionState: DUT_PositionState; END_VAR // If state is defined, we are within the delta, and we are either not moving or our destination is within the delta, we are at the state F_AtPositionState := stPositionState.bValid AND stPositionState.bUpdated AND F_PosWithinDelta(stMotionStage.stAxisStatus.fActPosition, stPositionState) AND ((NOT stMotionStage.bExecute) OR F_PosWithinDelta(stMotionStage.fPosition, stPositionState)); END_FUNCTION F_MotionErrorCodeLookup ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION F_MotionErrorCodeLookup : STRING VAR_INPUT nErrorId: UDINT; END_VAR VAR msg: STRING; END_VAR CASE nErrorId OF // Common NC errors 16#4221: msg:='Requested set velocity is not allowed'; 16#4222: msg:='Requested set position is not allowed'; 16#4223: msg:='No enable for controller and/or feed'; 16#4225: msg:='Drive not ready during axis start'; 16#4260: msg:='Drive disabled'; 16#4357: msg:='Negative limit hit'; 16#4358: msg:='Positive limit hit'; 16#4550: msg:='Stall: position lag monitoring error'; 16#4650: msg:='Drive hardware not ready to operate'; 16#4655: msg:='Invalid IO data'; 16#4467: msg:='Encoder error: invalid actual position data'; 16#4B07: msg:='Timeout axis function block after 6 seconds'; 16#4FFF: msg:='Unknown NC error (not in manual)'; // Custom error definitions 16#7900: msg:='Aborted move request with active move in progress'; 16#7901: msg:='Position state unsafe'; 16#7902: msg:='Position state invalid'; // Fallbacks 0: msg:=''; ELSE msg:='Contact PCDS to add new message'; END_CASE F_MotionErrorCodeLookup := msg; END_FUNCTION F_PosOverLowerBound ^^^^^^^^^^^^^^^^^^^ :: FUNCTION F_PosOverLowerBound : BOOL VAR_INPUT fPosition: LREAL; stPositionState: DUT_PositionState; END_VAR F_PosOverLowerBound := fPosition > (stPositionState.fPosition - ABS(stPositionState.fDelta)); END_FUNCTION F_PosUnderUpperBound ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION F_PosUnderUpperBound : BOOL VAR_INPUT fPosition: LREAL; stPositionState: DUT_PositionState; END_VAR F_PosUnderUpperBound := fPosition < (stPositionState.fPosition + ABS(stPositionState.fDelta)); END_FUNCTION F_PosWithinDelta ^^^^^^^^^^^^^^^^ :: FUNCTION F_PosWithinDelta : BOOL VAR_INPUT fPosition: LREAL; stPositionState: DUT_PositionState; END_VAR F_PosWithinDelta := F_PosOverLowerBound(fPosition, stPositionState) AND F_PosUnderUpperBound(fPosition, stPositionState); END_FUNCTION FB_CalculateFrequency_3702_v0_01 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_CalculateFrequency_3702_v0_01 VAR CONSTANT ///200 samples/period cBufferSize: INT := 1000; END_VAR VAR_INPUT En: BOOL; bCalculate: BOOL; aBufferValue: ARRAY[0..(cBuffersize - 1)] OF INT; aBufferDcTime: ARRAY[0..(cBuffersize - 1)] OF UDINT; ///If curve has a DC offset nDCOffset: INT := 0; END_VAR VAR_OUTPUT EnO: BOOL; bError: BOOL; fActFrequency: LREAL; END_VAR VAR nIndex: INT; nFirstZeroCrossing: INT; nLastZeroCrossing: INT; rTimeFirst: REAL := 0; rTimeLast: REAL := 0; rTimeRes: REAL := 0; nCrossings: INT := 0; END_VAR VAR_TEMP rRange: REAL := 0; rTimeSpan: REAL := 0; END_VAR EnO:=En; bError:=FALSE; nCrossings:=0; IF En AND bCalculate THEN //Serach for crossings of nDCOffset FOR nIndex:=1 TO cBuffersize-1 DO IF((aBufferValue[nIndex]>nDCOffset AND aBufferValue[nIndex-1]<=nDCOffset) OR (aBufferValue[nIndex]=nDCOffset)) THEN IF(nCrossings=0) THEN nFirstZeroCrossing:=nIndex; END_IF nLastZeroCrossing:=nIndex; nCrossings:=nCrossings+1; END_IF END_FOR IF nFirstZeroCrossing < nLastZeroCrossing AND nCrossings>2 THEN //interpolate zero crossings for higher accuracy rTimeRes:=UDINT_TO_REAL(aBufferDcTime[1]-aBufferDcTime[0]); //Buffer must contain more than 2 values rRange:=INT_TO_REAL(ABS(aBufferValue[nFirstZeroCrossing-1]-aBufferValue[nFirstZeroCrossing])); rTimeFirst:=UDINT_TO_REAL( aBufferDcTime[nFirstZeroCrossing-1])+ABS(INT_TO_REAL(aBufferValue[nFirstZeroCrossing-1])/rRange*rTimeRes); rRange:=INT_TO_REAL(ABS(aBufferValue[nLastZeroCrossing-1]-aBufferValue[nLastZeroCrossing])); rTimeLast:=UDINT_TO_REAL( aBufferDcTime[nLastZeroCrossing-1])+ABS(INT_TO_REAL(aBufferValue[nLastZeroCrossing-1])/rRange*rTimeRes); //Time span first to last (considering that max one time counter overflow have occured in the total time range of the time buffer) IF rTimeFirst Average frequency over buffer. ELSE fActFrequency:=0; END_IF END_IF END_FUNCTION_BLOCK 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; bPowerSelf: BOOL; 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; 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 bHomed:=fbHomeVirtual.bHomed; //Add in DUT_AxisStatus later bDone:=FALSE; (*Reset*) fbReset( Execute:=bReset AND Axis.Status.Error, Axis:=Axis, Done=> , Busy=> , Error=> , ErrorID=> ); (*Power*) IF bPowerSelf THEN fbPower( Axis:=Axis, Enable:=bEnable, Enable_Positive:=bEnable AND bLimitFwd, Enable_Negative:=bEnable AND bLimitBwd, Override:=fOverride, BufferMode:= , Status=> , Busy=> , Active=> , Error=> , ErrorID=> ); END_IF (*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:=UINT_TO_INT(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:=UINT_TO_INT(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; IF bFirstScan THEN bFirstScan:=FALSE; END_IF END_FUNCTION_BLOCK FB_EL1252ASM_v1_00 ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EL1252ASM_v1_00 VAR_INPUT En: BOOL; END_VAR VAR_OUTPUT EnO: BOOL; Di_1: BOOL; Di_2: BOOL; Di_1_LatchTimePos: ULINT; Di_2_LatchTimePos: ULINT; Di_1_LatchTimeNeg: ULINT; Di_2_LatchTimeNeg: ULINT; ///Below bits can be used but then they must be enabled in the COE of the card. Nicklas suggested to not use these (since something wss written that you then only were allowed to read the latch time onece (some kkind of auto reset??)) ///Di_1_LatchNeg:BOOL; /// Di_1_LatchPos:BOOL; /// Di_2_LatchNeg:BOOL; /// Di_2_LatchPos:BOOL; Error: BOOL; END_VAR VAR Channel_1_Input AT %I*: BOOL; Channel_2_Input AT %I*: BOOL; ///Latch_Status1 AT %I*: USINT; /// Latch_Status2 AT %I*: USINT; Latch_LatchPos1 AT %I*: ULINT; Latch_LatchNeg1 AT %I*: ULINT; Latch_LatchPos2 AT %I*: ULINT; Latch_LatchNeg2 AT %I*: ULINT; WcState_WcState AT %I*: BOOL; InfoData_State AT %I*: UINT; END_VAR EnO:=En; IF En AND (WcState_WcState OR InfoData_State<>16#8) THEN //InfoData_State==0 => in OP mode Error:=TRUE; ELSE Error:=FALSE; END_IF IF En THEN IF Error=FALSE THEN Di_1:=Channel_1_Input; Di_2:=Channel_2_Input; (* Di_1_LatchPos:=Latch_Status1.0; Di_1_LatchNeg:=Latch_Status1.1; Di_2_LatchPos:=Latch_Status2.0; Di_2_LatchNeg:=Latch_Status2.1;*) Di_1_LatchTimePos:=Latch_LatchPos1; Di_2_LatchTimePos:=Latch_LatchPos2; Di_1_LatchTimeNeg:=Latch_LatchNeg1; Di_2_LatchTimeNeg:=Latch_LatchNeg2; ELSE Di_1:=FALSE; Di_2:=FALSE; END_IF END_IF END_FUNCTION_BLOCK FB_EncErrorFFO ^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EncErrorFFO (* Example usage of FB_NCErrorFFO that only counts encoder errors as faults. *) VAR_IN_OUT // Motion stage to monitor stMotionStage: DUT_MotionStage; // FFO to trip fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // Reset the fault bReset: BOOL; // Auto reset the fault bAutoReset: BOOL; END_VAR VAR_OUTPUT // Quick way for nearby code to check if this block has tripped the FFO. bTripped: BOOL; END_VAR VAR fbNCErrorFFO: FB_NCErrorFFO := ( nLowErrorId := 16#4400, nHighErrorId := 16#44FF, sDesc := 'Encoder error'); END_VAR fbNCErrorFFO( stMotionStage := stMotionStage, fbFFHWO := fbFFHWO, bReset := bReset, bAutoReset := bAutoReset, bTripped => bTripped); END_FUNCTION_BLOCK FB_EncoderValue ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EncoderValue (* Process the encoder value for DUT_MotionStage A few different problems this is trying to solve: 1. Different encoders show as different types in the IO, but we want a consistent type for checks and for PVs 2. Some inverted encoders display as very high numbers as they count down from max int instead of up from 0 3. Some encoders have raw signed values, others unsigned. To this end, we figure out which encoder is linked and process the value appropriately. *) VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR IF stMotionStage.nRawEncoderULINT <> 0 THEN IF stMotionStage.nRawEncoderULINT < 4294967296 THEN stMotionStage.nEncoderCount := ULINT_TO_UDINT(stMotionStage.nRawEncoderULINT); ELSE stMotionStage.nEncoderCount := ULINT_TO_UDINT(18446744073709551615 - stMotionStage.nRawEncoderULINT); END_IF ELSIF stMotionStage.nRawEncoderUINT <> 0 THEN stMotionStage.nEncoderCount := UINT_TO_UDINT(stMotionStage.nRawEncoderUINT); ELSIF stMotionStage.nRawEncoderINT <> 0 THEN stMotionStage.nEncoderCount := INT_TO_UDINT(stMotionStage.nRawEncoderINT); ELSE stMotionStage.nEncoderCount := 0; END_IF END_FUNCTION_BLOCK FB_EncSaveRestore ^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EncSaveRestore VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR VAR_INPUT bEnable: BOOL; END_VAR VAR_OUTPUT END_VAR VAR fbSetPos: MC_SetPosition; timer: TON; tSaveDelay: TIME := T#10s; bInit: BOOL; bLoad: BOOL; nLatchError: UDINT; bEncError: BOOL; END_VAR VAR PERSISTENT bSaved: BOOL; fPosition: LREAL; END_VAR IF bEnable THEN // Trigger a load if anything was saved at all IF NOT bInit THEN bInit := TRUE; bLoad S= bSaved; fbSetPos.Options.ClearPositionLag := TRUE; END_IF // Set our position if bLoad is true fbSetPos( Axis:=stMotionStage.Axis, Execute:=bLoad, Position:=fPosition); // Only load once, at startup bLoad R= fbSetPos.Done OR fbSetPos.Error; IF fbSetPos.Error THEN // Keep the error latched, it can disappear if Execute is set to FALSE nLatchError := fbSetPos.ErrorID; // Alert the user that something has gone wrong stMotionStage.bError := TRUE; stMotionStage.nErrorId := fbSetPos.ErrorID; stMotionStage.sCustomErrorMessage := 'Error loading previously saved position.'; END_IF // Check DUT_MotionStage for an encoder error (range 0x44nn) bEncError := stMotionStage.bError AND stMotionStage.nErrorId >= 16#4400 AND stMotionStage.nErrorId <= 16#44FF; // Do not save if we're currently loading or if there is an encoder error IF NOT bLoad AND NOT bEncError THEN fPosition := stMotionStage.stAxisStatus.fActPosition; // This persistent variable lets us check if anything was saved // It will be TRUE at startup if we have saved values bSaved := TRUE; END_IF END_IF END_FUNCTION_BLOCK FB_EpicsInOut ^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EpicsInOut // Example usage of FB_PositionStateManager for a simple IN/OUT axis. See NOTE: comments. // Also usable as a drop-in for these cases (no need to roll your own in/out) VAR_IN_OUT // Motor to apply states to stMotionStage: DUT_MotionStage; // Information about the OUT position stOut: DUT_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN parameter stIn: DUT_PositionState; END_VAR VAR_INPUT // If TRUE, the motor will be moved when enumSet is changed bEnable: BOOL; // When changed, sets the destination and starts a move {attribute 'pytmc' := ' pv: SET io: io '} enumSet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumSet END_VAR VAR_OUTPUT // If TRUE, we are in an error state bError: BOOL; // NOTE: do not pragma these, already has pragma in manager // Error code nErrorId: UDINT; // Message associated with bError = TRUE sErrorMessage: STRING; // If TRUE, we are currently moving between states bBusy: BOOL; // If TRUE, we asked for a move between states, it completed successfully, and there is no ongoing move bDone: BOOL; // The current state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumGet END_VAR VAR bInit: BOOL; arrStates: ARRAY[1..15] OF DUT_PositionState; {attribute 'pytmc' := ' pv: io: io '} fbStateManager: FB_PositionStateManager; // NOTE: Please copy this pragma exactly to pick up the standard PVs END_VAR // Fist cycle setup IF NOT bInit THEN stOut.sName := 'OUT'; stIn.sName := 'IN'; bInit := TRUE; END_IF // Stuff first two values of the 15 element array for the manager arrStates[1] := stOut; arrStates[2] := stIn; // Call the function block every cycle fbStateManager( stMotionStage := stMotionStage, arrStates := arrStates, setState := enumSet, bEnable := bEnable, bError => bError, nErrorId => nErrorId, sErrorMessage => sErrorMessage, bBusy => bBusy, bDone => bDone, getState => enumGet); // Cannot do this assignment if enumGet has attribute strict END_FUNCTION_BLOCK FB_ErrorList ^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_ErrorList VAR_INPUT En : BOOL; //Enable input bReset : BOOL; //Delete all Error entry lErrorID : ULINT; //ErrorID to be acknoledged bACK : BOOL; //Acknoledge the given error and delete it from the list END_VAR VAR_OUTPUT EnO : BOOL; //Enable output nNoError: UINT; //Number of Errors nNoOverflow : INT; //Number of Overflows pErrorSystem : POINTER TO ST_ErrorSystem; //Pointer to ErrorSystem END_VAR VAR nFreePos : UINT; //Number of free position in the list nListCnt1 : UINT; //work variable ErrorSystem : ST_ErrorSystem; //Data structure of the Error list END_VAR (* * ================================================================================ * DESCRIPTION * ================================================================================ * This Function Block implements the core structure of the Error/Warning Handling * system. It realizes the datastructure containing every Error Entry, collect * statistics about the usage and manage the Error Entries. * Note: The system is under development, most of the functionalities are not * implemented or existing functionalities may change with time. * ================================================================================ *) EnO := En; //Ponter to ErrorSystem pErrorSystem := ADR(ErrorSystem); //Number of overflows nNoOverflow := ErrorSystem.nNoOverflows; IF bReset THEN MEMSET ( ADR(ErrorSystem.aErrorData[0]), 0, GVL_ErrorSystem.cSizeOfErrorData * SIZEOF(DUT_TerminalError)); ErrorSystem.lNextErrorID := 1; END_IF //Number of errors in the system nNoError := 0; FOR nListCnt1 := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO IF ErrorSystem.aErrorData[nListCnt1].Error_ID <> 0 THEN nNoError := nNoError+1; END_IF END_FOR ErrorSystem.nNoErrors := nNoError; //Number of free position in the list nFreePos := GVL_ErrorSystem.cSizeOfErrorData - nNoError; //Acknoledge specified Error entry IF bACK THEN FOR nListCnt1 := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO IF ErrorSystem.aErrorData[nListCnt1].Error_ID = lErrorID THEN ErrorSystem.aErrorData[nListCnt1].ErrorState := DUT_ErrorState.Acknowledged; END_IF END_FOR END_IF //Deleting acknoledged errors FOR nListCnt1 := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO IF ErrorSystem.aErrorData[nListCnt1].ErrorState = DUT_ErrorState.Acknowledged THEN MEMMOVE (ADR(ErrorSystem.aErrorData[nListCnt1]), ADR(ErrorSystem.aErrorData[nListCnt1+1]), (GVL_ErrorSystem.cSizeOfErrorData - 1 - nListCnt1) * SIZEOF(DUT_TerminalError)); MEMSET(ADR(ErrorSystem.aErrorData[GVL_ErrorSystem.cSizeOfErrorData - 1]), 0, SIZEOF(DUT_TerminalError)); END_IF END_FOR END_FUNCTION_BLOCK FB_GantryAutoCoupling ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_GantryAutoCoupling VAR_INPUT nGantryTol : LINT; END_VAR VAR_OUTPUT bGantryAlreadyCoupled : BOOL; END_VAR VAR_IN_OUT Master : DUT_MotionStage; MasterEnc : ST_RenishawAbsEnc; Slave : DUT_MotionStage; SlaveEnc : ST_RenishawAbsEnc; bExecuteCouple : BOOL; bExecuteDecouple : BOOL; END_VAR VAR gantry_diff_limit : FB_GantryDiffVirtualLimitSwitch; couple : MC_GEARIN; decouple : MC_GEAROUT; bInitComplete : BOOL; fbSetEnables : FB_SetEnables; END_VAR // Designate Master and SLave Axes Master.bGantryAxis := TRUE; Slave.bGantryAxis := TRUE; Master.nGantryTol := nGantryTol; Slave.nGantryTol := Master.nGantryTol; // Activate Gantry Virtual Limit Switch gantry_diff_limit(Penc:=MasterEnc, SEnc:=SlaveEnc, GantDiffTol:=Master.nGantryTol, PLimFwd=>Master.bGantryForwardEnable, PLimBwd=>Master.bGantryBackwardEnable, SLimFwd=>Slave.bGantryForwardEnable, SLimBwd=>Slave.bGantryBackwardEnable); // Coupling Status Bit bGantryAlreadyCoupled := Master.Axis.NcToPlc.CoupleState=1 AND Slave.Axis.NcToPlc.CoupleState=3; fbSetEnables(stMotionStage:=Master); fbSetEnables(stMotionStage:=Slave); IF bGantryAlreadyCoupled THEN Master.bGantryForwardEnable := Master.bGantryForwardEnable AND Slave.bAllForwardEnable; Slave.bGantryForwardEnable := Master.bAllForwardEnable AND Slave.bGantryForwardEnable; Master.bGantryBackwardEnable := Master.bGantryBackwardEnable AND Slave.bAllBackwardEnable; Slave.bGantryBackwardEnable := Master.bAllBackwardEnable AND Slave.bGantryBackwardEnable; END_IF // Coupling states // Auto-coupling at init and auto-reset of coupling boolean bExecuteCouple S= NOT bInitComplete; bExecuteCouple R= couple.Busy OR bGantryAlreadyCoupled; couple(Master:=Master.Axis, Slave:=Slave.Axis, Execute:=bExecuteCouple); bInitComplete S= bGantryAlreadyCoupled; // Decoupling with auto-reset of coupling boolean bExecuteDecouple R= decouple.Busy OR NOT bGantryAlreadyCoupled; decouple(Slave:=Slave.Axis, Execute:=bExecuteDecouple); END_FUNCTION_BLOCK FB_GantryDiffVirtualLimitSwitch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_GantryDiffVirtualLimitSwitch VAR_INPUT PEnc: ST_RenishawAbsEnc; // Primary axis encoder (usually the upstream one) SEnc: ST_RenishawAbsEnc; // Secondary axis encoder (couples to the primary) GantDiffTol: LINT; // Gantry differenace tolerance in encoder counts END_VAR VAR_OUTPUT PLimFwd: BOOL; // Primary axis forward direction enable PLimBwd: BOOL; // Primary axis reverse direction enable SLimFwd: BOOL; // Secondary axis forward direction enable SLimBwd: BOOL; // Secondary axis reverse direction enable END_VAR VAR GantryDiff: LINT; END_VAR (* Gantry Difference Virtual Limit Switch A. Wallace 2017-2-15 Determines which direction is disabled due to it increasing the gantry difference. Refer to the ESD for actual conventions. A positive gantry error refers to a CCW clocked assembly: eg. for X X1 upstream, X2 downstream. Primary axis is always upstream. Gantry difference > 0 when X2>X1 Therefore X2 positive direction disabled X1 negative direction disabled Call before FB_MotionStage fb calls for the gantry axes. *) GantryDiff := ( ULINT_TO_LINT(PEnc.Count) - ULINT_TO_LINT(PEnc.Ref) ) - ( ULINT_TO_LINT(SEnc.Count) - ULINT_TO_LINT(SEnc.Ref) ); IF ABS(GantryDiff) > GantDiffTol THEN IF GantryDiff < 0 THEN PLimBwd := FALSE; SLimFwd := FALSE; ELSE PLimBwd := TRUE; SLimFwd := TRUE; END_IF IF GantryDiff > 0 THEN PLimFwd := FALSE; SLimBwd := FALSE; ELSE PLimFwd := TRUE; SLimBwd := TRUE; END_IF ELSE //If there is no fault, all directions are enabled PLimFwd := TRUE; PLimBwd := TRUE; SLimFwd := TRUE; SLimBwd := TRUE; END_IF END_FUNCTION_BLOCK 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 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 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 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 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 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 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 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 FB_MicroStepCountTest ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MicroStepCountTest VAR_INPUT bExecute: BOOL; fStepSize: LREAL; nSteps: UINT; fMicroStep: LREAL; fVelocity: LREAL; tSettleTime: TIME; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT nStepsCounted: UINT; nTheorySteps: UINT; fPercent: LREAL; fEstMicroSize: LREAL; END_VAR VAR fbMoveRel: MC_MoveRelative; fbSettleTimer: TON; bDoMove: BOOL; nStepCounter: UINT; arrPosBuffer: ARRAY [0..99] OF LREAL; fAvgPos: LREAL; nArrIndex: UINT; nLoopIndex: UINT; fStartPos: LREAL; fPrevPos: LREAL; fStepChange: LREAL; fStepSum: LREAL; END_VAR // Motion FB fbMoveRel(Axis:=Axis, Execute:=bDoMove, Distance:=fStepSize, Velocity:=fVelocity); // Settle time fbSettleTimer(IN:=fbMoveRel.Done, PT:=tSettleTime); // Re-enable the move for next cycle bDoMove := bExecute AND nStepCounter < nSteps; // Calculate rolling average arrPosBuffer[nArrIndex] := Axis.NcToPlc.ActPos; fAvgPos := 0; FOR nLoopIndex := 0 TO 99 DO fAvgPos := fAvgPos + arrPosBuffer[nLoopIndex]; END_FOR; fAvgPos := fAvgPos / 100; nArrIndex := (nArrIndex + 1) MOD 100; // Initialize starting variables IF NOT bExecute THEN fStartPos := fAvgPos; fPrevPos := fAvgPos; END_IF // Check results IF fbSettleTimer.Q THEN fStepChange := fAvgPos - fPrevPos; // Invert fStepChange if we were doing negative steps IF fStepSize < 0 THEN fStepChange := fStepChange * -1; END_IF IF fStepChange > fMicroStep * 0.5 THEN nStepsCounted := nStepsCounted + 1; fStepSum := fStepSum + fStepChange; fEstMicroSize := fStepSum / nStepsCounted; END_IF nTheorySteps := DINT_TO_UINT(TRUNC(ABS((fStartPos - fAvgPos) / fMicroStep))); IF nTheorySteps > 0 THEN fPercent := 100 * nStepsCounted / nTheorySteps; END_IF fPrevPos := fAvgPos; nStepCounter := nStepCounter + 1; // Reset the move block bDoMove := FALSE; END_IF END_FUNCTION_BLOCK FB_MotionHoming ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionHoming VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR VAR_INPUT bExecute: BOOL; END_VAR VAR_OUTPUT bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorID: UDINT; END_VAR VAR fbSetPos: MC_SetPosition; fbJog: MC_Jog; rtExec: R_TRIG; ftExec: F_TRIG; nHomeStateMachine: INT := IDLE; nStateAfterStop: INT; nMoves: INT; bFirstDirection: BOOL; bAtHome: BOOL; bMove: BOOL; nErrCount: INT; bInterrupted: BOOL; END_VAR VAR CONSTANT IDLE: INT := 0; NEXT_MOVE: INT := 1; CHECK_FWD: INT := 2; CHECK_BWD: INT := 3; FINAL_MOVE: INT := 4; FINAL_SETPOS: INT := 5; ERROR: INT := 6; WAIT_STOP: INT := 7; (* This is a simpler way of disabling the soft limits that ends up being really obvious if something has gone wrong. If you turn the limits off/on, not only do you need to keep track of if you had soft limits set, but you need to always restore this properly or risk some issue. Instead, I set position to a ridiculous value that can always move forward or backward. If this gets stuck for any reason it's very clear that something has gone wrong, rather than a silent failure of the soft limit marks. *) FWD_START: LREAL := -99999999; BWD_START: LREAL := 99999999; END_VAR fbSetPos.Options.ClearPositionLag := TRUE; rtExec(CLK:=bExecute); ftExec(CLK:=bExecute); bError R= NOT bExecute; IF NOT bError THEN nErrorID := 0; END_IF CASE stMotionStage.nHomingMode OF ENUM_EpicsHomeCmd.LOW_LIMIT: bFirstDirection := FALSE; bAtHome := NOT stMotionStage.bLimitBackwardEnable; bMove := TRUE; ENUM_EpicsHomeCmd.HIGH_LIMIT: bFirstDirection := TRUE; bAtHome := NOT stMotionStage.bLimitForwardEnable; bMove := TRUE; ENUM_EpicsHomeCmd.HOME_VIA_LOW: bFirstDirection := FALSE; bAtHome := stMotionStage.bHome; bMove := TRUE; ENUM_EpicsHomeCmd.HOME_VIA_HIGH: bFirstDirection := TRUE; bAtHome := stMotionStage.bHome; bMove := TRUE; ENUM_EpicsHomeCmd.ABSOLUTE_SET: fbSetPos( Axis:=stMotionStage.Axis, Execute:=bExecute, Position:=stMotionStage.fHomePosition); bBusy := rtExec.Q; bDone := NOT rtExec.Q; bMove := FALSE; ENUM_EpicsHomeCmd.NONE: bMove := FALSE; bBusy := rtExec.Q; bDone := NOT rtExec.Q; ELSE bMove := FALSE; END_CASE IF bMove THEN IF bBusy AND ftExec.Q THEN nHomeStateMachine := ERROR; bInterrupted := TRUE; END_IF CASE nHomeStateMachine OF // Wait for a rising edge IDLE: bBusy := FALSE; nErrCount := 0; fbSetPos( Axis:=stMotionStage.Axis, Execute:=FALSE); fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=FALSE); IF rtExec.Q THEN nHomeStateMachine := NEXT_MOVE; nMoves := 0; bDone := FALSE; bBusy := TRUE; bError := FALSE; nErrorID := 0; bInterrupted := FALSE; END_IF // Figure out whether to move forward, move backward, or give up NEXT_MOVE: fbSetPos( Axis:=stMotionStage.Axis, Execute:=FALSE); fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=FALSE); CASE nMoves OF 0: IF bFirstDirection THEN nStateAfterStop := CHECK_FWD; ELSE nStateAfterStop := CHECK_BWD; END_IF 1: IF NOT bFirstDirection THEN nStateAfterStop := CHECK_FWD; ELSE nStateAfterStop := CHECK_BWD; END_IF ELSE nStateAfterStop := ERROR; END_CASE nMoves := nMoves + 1; IF bAtHome THEN nStateAfterStop := FINAL_MOVE; END_IF nHomeStateMachine := WAIT_STOP; // Move forward until we find the home signal or reach end of travel CHECK_FWD: fbSetPos( Axis:=stMotionStage.Axis, Execute:=TRUE, Position:=FWD_START); fbJog( Axis:=stMotionStage.Axis, JogForward:=stMotionStage.bLimitForwardEnable AND NOT bATHome, JogBackwards:=FALSE, Mode:=E_JogMode.MC_JOGMODE_CONTINOUS, Velocity:=stMotionStage.stAxisParameters.fRefVeloSearch); IF NOT fbJog.JogForward THEN nHomeStateMachine := NEXT_MOVE; ELSIF fbJog.Error THEN fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=FALSE); nErrCount := nErrCount + 1; IF nErrCount >= 3 THEN nHomeStateMachine := ERROR; END_IF END_IF // Move backward until we find the home signal or reach end of travel CHECK_BWD: fbSetPos( Axis:=stMotionStage.Axis, Execute:=TRUE, Position:=BWD_START); fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=stMotionStage.bLimitBackwardEnable AND NOT bATHome, Mode:=E_JogMode.MC_JOGMODE_CONTINOUS, Velocity:=stMotionStage.stAxisParameters.fRefVeloSearch); IF NOT fbJog.JogBackwards THEN nHomeStateMachine := NEXT_MOVE; ELSIF fbJog.Error THEN fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=FALSE); nErrCount := nErrCount + 1; IF nErrCount >= 3 THEN nHomeStateMachine := ERROR; END_IF END_IF // Set position to get within soft lims, move slowly off signal FINAL_MOVE: fbSetPos( Axis:=stMotionStage.Axis, Execute:=TRUE, Position:=stMotionStage.fHomePosition); IF bAtHome THEN fbJog( Axis:=stMotionStage.Axis, JogForward:=NOT bFirstDirection, JogBackwards:=bFirstDirection, Mode:=E_JogMode.MC_JOGMODE_CONTINOUS, Velocity:=stMotionStage.stAxisParameters.fRefVeloSync); ELSIF fbJog.Error THEN fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=FALSE); nErrCount := nErrCount + 1; IF nErrCount >= 3 THEN nHomeStateMachine := ERROR; END_IF ELSE fbJog( Axis:=stMotionStage.Axis, JogForward:=FALSE, JogBackwards:=FALSE); fbSetPos( Axis:=stMotionStage.Axis, Execute:=FALSE); nHomeStateMachine := WAIT_STOP; nStateAfterStop := FINAL_SETPOS; END_IF FINAL_SETPOS: fbSetPos( Axis:=stMotionStage.Axis, Execute:=TRUE, Position:=stMotionStage.fHomePosition); nHomeStateMachine := IDLE; bBusy := FALSE; bDone := TRUE; ERROR: bError := TRUE; nErrorID := fbJog.ErrorID; nHomeStateMachine := FINAL_SETPOS; fbSetPos( Axis:=stMotionStage.Axis, Execute:=FALSE); IF bInterrupted THEN stMotionStage.sCustomErrorMessage := 'Homing interrupted'; ELSE stMotionStage.sCustomErrorMessage := 'Homing failure'; END_IF WAIT_STOP: IF stMotionStage.Axis.Status.NotMoving THEN nHomeStateMachine := nStateAfterStop; END_IF END_CASE END_IF END_FUNCTION_BLOCK FB_MotionPneumaticActuator ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: (*This function blcok implements a pnuematic actuator. That can be signle or double acting by setting the ibSingleCntrl accordingly*) (* with double acting ibCntrlHold signal should be false, while with single acting the signal should be true*) FUNCTION_BLOCK FB_MotionPneumaticActuator VAR_INPUT (*EPS Interlock Bits*) ibInsertOK: BOOL; (*Actuator can be Inserted*) ibRetractOK: BOOL; (*ACtuator can be retracted*) ibPMPS_OK:BOOL; (*to be linked the Arbiter bit*) ibSingleCntrl:BOOL;(* TRUE if Actuator requires one Output signal to be activated, FALSE if its double acting i.e two outputs are required*) ibCntrlHold:BOOL; (* Control Signal must retain its value, must be TRUE in the case of single acting*) ibOverrideInterlock:BOOL; (*if true interlocks are ignored*) // Reset fault {attribute 'pytmc' := ' pv: FF_Reset '} i_xReset: BOOL; END_VAR VAR_OUTPUT {attribute 'pytmc' := ' pv: '} stPneumaticActuator : DUT_MotionPneumaticActuator; {attribute 'pytmc' := ' pv: MPS_OK field: ZNAM FALSE field: ONAM TRUE field: DESC TRUE if MPS signal is OK '} xMPS_OK:BOOL; END_VAR VAR_IN_OUT io_fbFFHWO : FB_HardwareFFOutput; END_VAR VAR // PMPS fbFF : FB_FastFault :=( i_DevName := 'MPA', i_Desc := 'Fault occurs when the device is moving', i_TypeCode := 10#1010); (*Init*) xFirstPass : BOOL; fbFSInit : R_TRIG; (* Timeouts*) tTimeOutDuration: TIME:= T#10S; tInserttimeout: TON; tRetracttimeout:TON; (*Limit switch latch timer*) tLimitSwitchLatchDuration: TIME:=T#1S; tInsertLimitSwitch:TON; tRetractLimitSwitch:TON; (*Logging*) fbLogger : FB_LogMessage := (eSubsystem:=E_SubSystem.MOTION); ePrevState : ENUM_PnuematicActuatorPositionState; tAction : R_TRIG; // Primary action of this device (Insert_DO, Retract_DO, etc.) tOverrideActivated : R_TRIG; (*IO*) i_xInsertedLS AT%I*: BOOL; i_xRetractedLS AT%I*: BOOL; q_xInsert_DO AT%Q*: BOOL; q_xRetract_DO AT%Q*: BOOL; END_VAR (*Initialize*) fbFSInit( CLK := TRUE, Q => xFirstPass); IF xFirstPass THEN stPneumaticActuator.eState := ENUM_PnuematicActuatorPositionState.INVALID; stPneumaticActuator.bRetract_SW := FALSE; stPneumaticActuator.bInsert_SW := FALSE; END_IF (*Soft IO Mapping to EPICS PVs*) ACT_IO(); (* Manage States*) IF stPneumaticActuator.i_bInLimitSwitch AND stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=ENUM_PnuematicActuatorPositionState.INVALID; ELSIF NOT stPneumaticActuator.i_bInLimitSwitch AND stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=ENUM_PnuematicActuatorPositionState.RETRACTED; ELSIF stPneumaticActuator.i_bInLimitSwitch AND NOT stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=ENUM_PnuematicActuatorPositionState.INSERTED; ELSIF NOT stPneumaticActuator.i_bInLimitSwitch AND NOT stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=ENUM_PnuematicActuatorPositionState.MOVING; ELSE stPneumaticActuator.eState:=ENUM_PnuematicActuatorPositionState.INVALID ; END_IF (*Set the Done/Busy signal*) stPneumaticActuator.bDone := (stPneumaticActuator.bRetract_SW AND stPneumaticActuator.eState=ENUM_PnuematicActuatorPositionState.RETRACTED) OR (stPneumaticActuator.bInsert_SW AND stPneumaticActuator.eState=ENUM_PnuematicActuatorPositionState.INSERTED); stPneumaticActuator.bBusy := (stPneumaticActuator.bRetract_SW AND stPneumaticActuator.eState<>ENUM_PnuematicActuatorPositionState.RETRACTED) OR (stPneumaticActuator.bInsert_SW AND stPneumaticActuator.eState<>ENUM_PnuematicActuatorPositionState.INSERTED); (*MPS FAULT*) (* MPS Faults when the actuator is in motion*) xMPS_OK := (stPneumaticActuator.eState=ENUM_PnuematicActuatorPositionState.RETRACTED) OR (stPneumaticActuator.eState=ENUM_PnuematicActuatorPositionState.INSERTED); (*PMPS PERMISSION*) // yet to be implemented (* Can't have bRetract_SW and bInsert_SW both be true*) If (stPneumaticActuator.bRetract_SW) AND (stPneumaticActuator.bInsert_SW) THEN stPneumaticActuator.bRetract_SW := FALSE; stPneumaticActuator.bInsert_SW := FALSE; END_IF //Redundant?? (*Check if both digital outputs active at the same time, and clear all*) IF stPneumaticActuator.q_bInsert THEN stPneumaticActuator.q_bRetract := FALSE; stPneumaticActuator.bRetract_SW:= FALSE; END_IF; IF stPneumaticActuator.q_bRetract THEN stPneumaticActuator.q_bInsert := FALSE; stPneumaticActuator.bInsert_SW:= FALSE; END_IF; (*Actuate the device*) stPneumaticActuator.q_bRetract := stPneumaticActuator.bRetractOK AND stPneumaticActuator.bRetract_SW AND NOT stPneumaticActuator.bInsert_SW ; stPneumaticActuator.q_bInsert := stPneumaticActuator.bInsertOK AND stPneumaticActuator.bInsert_SW AND NOT stPneumaticActuator.bRetract_SW ; (*Reset the Control signal when command has been executed and give time to ensure the actuator is fully seated in either direction*) IF (NOT ibSingleCntrl AND NOT ibCntrlHold) THEN IF (stPneumaticActuator.bRetract_SW AND stPneumaticActuator.i_bOutLimitSwitch AND tRetractLimitSwitch.Q ) THEN stPneumaticActuator.q_bRetract := FALSE; END_IF IF (stPneumaticActuator.bInsert_SW AND stPneumaticActuator.i_bInLimitSwitch AND tInsertLimitSwitch.Q) THEN stPneumaticActuator.q_bInsert := FALSE; END_IF END_IF (*Timers*) tInserttimeout(IN:= stPneumaticActuator.q_bInsert, PT := tTimeOutDuration ); tRetracttimeout(IN:= stPneumaticActuator.q_bRetract, PT := tTimeOutDuration); tInsertLimitSwitch(IN:= stPneumaticActuator.i_bInLimitSwitch, PT := tLimitSwitchLatchDuration); tRetractLimitSwitch(IN:= stPneumaticActuator.i_bOutLimitSwitch, PT := tLimitSwitchLatchDuration); ///Check moving postion timout IF NOT stPneumaticActuator.i_bInLimitSwitch AND tInserttimeout.Q THEN stPneumaticActuator.bError := TRUE; stPneumaticActuator.sErrorMessage:= 'Actuator insert timeout'; ELSIF NOT stPneumaticActuator.i_bOutLimitSwitch AND tRetracttimeout.Q THEN stPneumaticActuator.bError := TRUE; stPneumaticActuator.sErrorMessage:= 'Actuator retract timeout'; END_IF // Reset error stPneumaticActuator.bError R= stPneumaticActuator.bReset; (*FAST FAULT*) fbFF(i_xOK := xMPS_OK, i_xReset := i_xReset, io_fbFFHWO := io_fbFFHWO); (*Soft IO Mapping to Epics pvs*) ACT_IO(); END_FUNCTION_BLOCK ACTION ACT_IO: (*Inputs*) stPneumaticActuator.i_bInLimitSwitch := i_xInsertedLS; stPneumaticActuator.i_bOutLimitSwitch := i_xRetractedLS; (*outputs*) q_xInsert_DO:=stPneumaticActuator.q_bInsert; q_xRetract_DO:=stPneumaticActuator.q_bRetract; (*EPICS*) stPneumaticActuator.bRetractOK := ibRetractOK; stPneumaticActuator.bInsertOK := ibInsertOK; END_ACTION ACTION ACT_Logger: // Log Valve timeouts IF (tInserttimeout.Q OR tRetracttimeout.Q) THEN fbLogger(sMsg:=stPneumaticActuator.sErrorMessage, eSevr:=TcEventSeverity.Warning); END_IF // Log Actuator commands // Log valve open tAction(CLK:= (stPneumaticActuator.q_bRetract OR stPneumaticActuator.q_bInsert)); IF tAction.Q THEN IF(stPneumaticActuator.q_bRetract) THEN fbLogger(sMsg:='Actuator commanded retract', eSevr:=TcEventSeverity.Info); END_IF IF(stPneumaticActuator.q_bInsert) THEN fbLogger(sMsg:='Actuator commanded insert', eSevr:=TcEventSeverity.Info); END_IF END_IF // State Logging IF ePrevState <> stPneumaticActuator.eState THEN CASE stPneumaticActuator.eState OF ENUM_PnuematicActuatorPositionState.INVALID: fbLogger(sMsg:='Actuator invalid position.', eSevr:=TcEventSeverity.Critical); ENUM_PnuematicActuatorPositionState.MOVING: fbLogger(sMsg:='Actuator moving', eSevr:=TcEventSeverity.Warning); ENUM_PnuematicActuatorPositionState.RETRACTED: fbLogger(sMsg:='Actuator Retracted.', eSevr:=TcEventSeverity.Info); ENUM_PnuematicActuatorPositionState.INSERTED: fbLogger(sMsg:='Actuator Inserted.', eSevr:=TcEventSeverity.Info); END_CASE ePrevState := stPneumaticActuator.eState; END_IF END_ACTION FB_MotionRequest ^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionRequest (* Request a move from an axis controlled via EPICS using FB_MotionStage This exists to manage situations where different bits of code may need to move the same motor. With just the DUT_MotionStage/FB_MotionStage setup it is possible for two function blocks to fight with and interfere with each other and with the EPICS commands. *) VAR_IN_OUT // Motor to move stMotionStage: DUT_MotionStage; END_VAR VAR_INPUT // Start move on rising edge, stop move on falling edge bExecute: BOOL; // Reset errors on rising edge bReset: BOOL; // Define behavior for when the motor is already moving enumMotionRequest: ENUM_MotionRequest := ENUM_MotionRequest.WAIT; // Goal position fPos: LREAL; // Move velocity fVel: LREAL; // Optional acceleration fAcc: LREAL; // Optional deceleration fDec: LREAL; END_VAR VAR_OUTPUT // True if in error state bError: BOOL; // Error code nErrorId: UDINT; // What the error code means sErrorMessage: STRING; // If TRUE, we are moving the motor bBusy: BOOL; // If TRUE, we are not moving the motor and our most recent move was successful bDone: BOOL; END_VAR VAR rtExec: R_TRIG; ftExec: F_TRIG; rtReset: R_TRIG; ftBusy: F_TRIG; nState: UINT := 0; bMyMove: BOOL; bCausedError: BOOL; END_VAR // Define local constants for our state machine states VAR CONSTANT INIT: UINT := 0; WAIT_EXEC: UINT := 1; PICK_REQUEST: UINT := 2; WAIT_OTHER_MOVE: UINT := 3; STOP_OTHER_MOVE: UINT := 4; START_MOVE: UINT := 5; WAIT_MY_MOVE: UINT := 6; STOP_MY_MOVE: UINT := 7; DONE_MOVING: UINT := 8; ERROR: UINT := 9; END_VAR rtExec(CLK:=bExecute); ftExec(CLK:=bExecute); rtReset(CLK:=bReset); // Go back to INIT state on reset IF rtReset.Q THEN nState := INIT; stMotionStage.bReset := TRUE; END_IF IF rtExec.Q OR ftExec.Q THEN bDone := FALSE; END_IF CASE nState OF // Start by setting everything to a known value INIT: nState := WAIT_EXEC; bError := FALSE; sErrorMessage := ''; bDone := FALSE; bCausedError := FALSE; // Normal "waiting for move command" state WAIT_EXEC: bMyMove := FALSE; // Looking for a rising edge on bExecute IF rtExec.Q THEN bDone := FALSE; nState := PICK_REQUEST; END_IF // Decide how to handle the request PICK_REQUEST: IF stMotionStage.bBusy THEN CASE enumMotionRequest OF ENUM_MotionRequest.WAIT: nState := WAIT_OTHER_MOVE; ENUM_MotionRequest.INTERRUPT: nState := STOP_OTHER_MOVE; ENUM_MotionRequest.ABORT: nState := ERROR; bError := TRUE; nErrorId := 16#7900; END_CASE ELSE nState := START_MOVE; END_IF // Watch the other move, then see if it's our turn WAIT_OTHER_MOVE: IF NOT stMotionStage.bBusy THEN // Try to pick request again next cycle once the move is over nState := PICK_REQUEST; END_IF // Interrupt the other move, then go to start ours STOP_OTHER_MOVE: stMotionStage.bExecute := FALSE; IF NOT stMotionStage.bBusy THEN nState := START_MOVE; END_IF // Set the correct values on DUT_MotionStage to start a new absolute move START_MOVE: bMyMove := TRUE; stMotionStage.bExecute := TRUE; stMotionStage.nCommand := ENUM_EpicsMotorCmd.MOVE_ABSOLUTE; stMotionStage.fPosition := fPos; stMotionStage.fVelocity := fVel; stMotionStage.fAcceleration := fAcc; stMotionStage.fDeceleration := fDec; nState := WAIT_MY_MOVE; // Watch our ongoing move, look for the move to end or requests to stop the move from this FB WAIT_MY_MOVE: ftBusy(CLK:=stMotionStage.bBusy); IF ftBusy.Q THEN nState := DONE_MOVING; END_IF // Implement stop on falling trigger IF ftExec.Q THEN nState := STOP_MY_MOVE; END_IF // Request a stop and wait for it to happen STOP_MY_MOVE: stMotionStage.bExecute := FALSE; IF NOT stMotionStage.bBusy THEN nState := DONE_MOVING; END_IF // Pick out the bDone state and return to waiting DONE_MOVING: bDone := stMotionStage.bDone; nState := WAIT_EXEC; // Lock us into the error state until the FB is reset ERROR: bMyMove := FALSE; END_CASE // Transition to the ERROR state if applicable IF bMyMove AND stMotionStage.bError THEN nState := ERROR; bError := TRUE; nErrorId := stMotionStage.nErrorId; bCausedError := TRUE; END_IF sErrorMessage := F_MotionErrorCodeLookup(nErrorId := nErrorId); CASE nState OF INIT, WAIT_EXEC, ERROR: bBusy := FALSE; ELSE bBusy := TRUE; END_CASE END_FUNCTION_BLOCK FB_MotionStage ^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionStage (* Default implementation for PLC behavior when motor IOC asks for a move This can be extended or replaced in your PLC project if you want non-default behavior to arise from the motor record processing *) VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR VAR fbDriveVirtual: FB_DriveVirtual; fbMotionHome: FB_MotionHoming; fbSaveRestore: FB_EncSaveRestore; bExecute: BOOL; bExecMove: BOOL; bExecHome: BOOL; bFwdHit: BOOL; bBwdHit: BOOL; ftExec: F_TRIG; rtExec: R_TRIG; rtUserExec: R_TRIG; rtTarget: R_TRIG; rtHomed: R_TRIG; fbSetEnables: FB_SetEnables; bPosGoal: BOOL; bNegGoal: BOOL; fbEncoderValue: FB_EncoderValue; fbNCParams: FB_MotionStageNCParams; bNewMoveReq: BOOL; bPrepareDisable: BOOL; bMoveCmd: BOOL; rtMoveCmdShortcut: R_TRIG; rtHomeCmdShortcut: R_TRIG; END_VAR // Start with an accurate status stMotionStage.Axis.ReadStatus(); // Check for the plc shortcut commands // Used for testing or to circumvent motor record issues rtMoveCmdShortcut(CLK:=stMotionStage.bMoveCmd); rtHomeCmdShortcut(CLK:=stMotionStage.bHomeCmd); // Execute on rising edge IF rtMoveCmdShortcut.Q AND NOT stMotionStage.bExecute THEN stMotionStage.bExecute := TRUE; stMotionStage.nCommand := ENUM_EpicsMotorCmd.MOVE_ABSOLUTE; ELSIF rtHomeCmdShortcut.Q AND NOT stMotionStage.bExecute THEN stMotionStage.bExecute := TRUE; stMotionStage.nCommand := ENUM_EpicsMotorCmd.HOME; END_IF // Always reset, even if not rising edge, so command can be issued again IF stMotionStage.bMoveCmd OR stMotionStage.bHomeCmd THEN stMotionStage.bMoveCmd := FALSE; stMotionStage.bHomeCmd := FALSE; END_IF // Automatically fill the correct nCmdData for homing IF stMotionStage.nCommand = ENUM_EpicsMotorCmd.HOME THEN stMotionStage.nCmdData := stMotionStage.nHomingMode; END_IF // Check if the command wants to cause a move bMoveCmd R= stMotionStage.nCmdData = ENUM_EpicsHomeCmd.ABSOLUTE_SET; bMoveCmd R= stMotionStage.nCmdData = ENUM_EpicsHomeCmd.NONE; bMoveCmd S= stMotionStage.nCommand <> ENUM_EpicsMotorCmd.HOME; // Handle main execs rtUserExec(CLK := stMotionStage.bExecute); bNewMoveReq S= rtUserExec.Q AND bMoveCmd; bNewMoveReq R= NOT stMotionStage.bExecute; bPrepareDisable R= bNewMoveReq; bPosGoal := stMotionStage.stAxisStatus.fActPosition < stMotionStage.fPosition; bNegGoal := stMotionStage.stAxisStatus.fActPosition > stMotionStage.fPosition; // Moves are automatically allowed if no safety hooks. Otherwise, some other code will set this. stMotionStage.bSafetyReady S= stMotionStage.bPowerSelf; // Handle auto-enable timing CASE stMotionStage.nEnableMode OF ENUM_StageEnableMode.ALWAYS: stMotionStage.bEnable := TRUE; ENUM_StageEnableMode.DURING_MOTION: IF bNewMoveReq THEN IF stMotionStage.nCommand = ENUM_EpicsMotorCmd.HOME THEN stMotionStage.bEnable := stMotionStage.bSafetyReady; ELSIF bPosGoal THEN IF stMotionStage.bAllForwardEnable THEN stMotionStage.bEnable S= stMotionStage.bSafetyReady; ELSIF NOT stMotionStage.bError THEN // Not an error, just a warning stMotionStage.sErrorMessage := 'Cannot move past positive limit.'; stMotionStage.bExecute := FALSE; END_IF ELSIF bNegGoal THEN IF stMotionStage.bAllBackwardEnable THEN stMotionStage.bEnable S= stMotionStage.bSafetyReady; ELSIF NOT stMotionStage.bError THEN // Not an error, just a warning stMotionStage.sErrorMessage := 'Cannot move past negative limit.'; stMotionStage.bExecute := FALSE; END_IF ELSE // Super rare condition where we asked for a move to exactly the same floating point we're already at stMotionStage.bEnable S= stMotionStage.bSafetyReady; END_IF IF stMotionStage.bEnable OR stMotionStage.bError THEN bNewMoveReq := FALSE; END_IF END_IF END_CASE // Update all enable booleans fbSetEnables(stMotionStage:=stMotionStage); IF stMotionStage.stAxisStatus.bBusy AND NOT bExecute THEN // Wait for the previous move to end bExecute := FALSE; ELSIF bMoveCmd THEN // Do not start the move until we have power and the safety system says it is OK bExecute := stMotionStage.bExecute AND stMotionStage.bAllEnable AND stMotionStage.bEnableDone AND stMotionStage.bSafetyReady; ELSE bExecute := stMotionStage.bExecute; END_IF IF bExecute AND NOT stMotionStage.bError THEN // Reset local warnings if things are going well stMotionStage.sErrorMessage := ''; END_IF // No moves allowed in error states IF stMotionStage.bError THEN bExecute := FALSE; END_IF bExecHome := bExecute AND stMotionStage.nCommand = 10; bExecMove := bExecute AND NOT bExecHome; // Handle standard commands using ESS's FB fbDriveVirtual(En:=TRUE, bEnable:=stMotionStage.bAllEnable, bReset:=stMotionStage.bReset, bExecute:=bExecMove, nCommand:=INT_TO_UINT(stMotionStage.nCommand), nCmdData:=INT_TO_UINT(stMotionStage.nCmdData), fVelocity:=stMotionStage.fVelocity, fPosition:=stMotionStage.fPosition, fAcceleration:=stMotionStage.fAcceleration, fDeceleration:=stMotionStage.fDeceleration, bLimitFwd:=stMotionStage.bAllForwardEnable, bLimitBwd:=stMotionStage.bAllBackwardEnable, bHomeSensor:=stMotionStage.bHome, fHomePosition:=stMotionStage.fHomePosition, bPowerSelf:=stMotionStage.bPowerSelf, nMotionAxisID=>stMotionStage.nMotionAxisID, Axis:=stMotionStage.Axis); // Some custom home handling fbMotionHome( stMotionStage:=stMotionStage, bExecute:=bExecHome); // Update status again after the move starts or stops stMotionStage.Axis.ReadStatus(); // Check for a new error IF NOT stMotionStage.bError THEN stMotionStage.bError := fbDriveVirtual.bError; stMotionStage.nErrorId := fbDriveVirtual.nErrorId; END_IF IF NOT stMotionStage.bError THEN stMotionStage.bError := fbMotionHome.bError; stMotionStage.nErrorId := fbMotionHome.nErrorId; END_IF IF NOT stMotionStage.bError AND stMotionStage.bExecute AND NOT stMotionStage.bUserEnable THEN stMotionStage.bError := TRUE; stMotionStage.nErrorId := 1; stMotionStage.sCustomErrorMessage := 'Move requested, but user enable is disabled!'; END_IF // Set the error message if we have one IF stMotionStage.bError THEN // Hook if other code wants to inject a non-NC error IF stMotionStage.sCustomErrorMessage <> '' THEN stMotionStage.sErrorMessage := stMotionSTage.sCustomErrorMessage; ELSE stMotionStage.sErrorMessage := F_MotionErrorCodeLookup(nErrorId := stMotionStage.nErrorId); END_IF END_IF // When we start, set the busy/done appropriately rtExec(CLK:=bExecute); IF rtExec.Q THEN stMotionStage.bBusy := TRUE; stMotionStage.bDone := FALSE; END_IF // Force everything off in case of error IF stMotionStage.bError THEN stMotionStage.bBusy := FALSE; stMotionStage.bDone := FALSE; stMotionStage.bEnable := FALSE; END_IF // Check the limits and cancel execution if appropriate. Without this block we have infinite error spam bFwdHit := stMotionStage.Axis.Status.PositiveDirection AND NOT stMotionStage.bAllForwardEnable; bBwdHit := stMotionStage.Axis.Status.NegativeDirection AND NOT stMotionStage.bAllBackwardEnable; IF (bFwdHit OR bBwdHit) AND NOT fbMotionHome.bBusy THEN stMotionStage.bExecute := FALSE; END_IF // Check done moving via user stop, fbDriveVirtual and Target Position Monitoring, or from homing. ftExec(CLK:=stMotionStage.bExecute); rtTarget(CLK:=(stMotionStage.Axis.Status.InTargetPosition AND fbDriveVirtual.bDone AND bExecMove)); rtHomed(CLK:=fbMotionHome.bDone AND bExecHome); IF ftExec.Q OR rtTarget.Q OR rtHomed.Q THEN IF NOT stMotionStage.bDone THEN stMotionStage.bDone := TRUE; stMotionStage.bBusy := FALSE; IF NOT stMotionStage.Axis.Status.Error THEN bExecute := FALSE; stMotionStage.bExecute := FALSE; END_IF END_IF END_IF // Handle auto-disable timing bPrepareDisable S= stMotionStage.nEnableMode = ENUM_StageEnableMode.DURING_MOTION AND ftExec.Q; // Delay the disable until we reach standstill, else brake issues or other race conditions IF bPrepareDisable AND stMotionStage.Axis.Status.MotionState = MC_AXISSTATE_STANDSTILL THEN bPrepareDisable := FALSE; stMotionStage.bEnable := FALSE; END_IF // Get a definitive bEnabled reading CASE stMotionStage.Axis.Status.MotionState OF // We are not enabled if there is an issue MC_AXISSTATE_UNDEFINED, MC_AXISSTATE_DISABLED, MC_AXISSTATE_ERRORSTOP: stMotionStage.bEnableDone := FALSE; ELSE stMotionStage.bEnableDone := TRUE; END_CASE // Handle the brake. TRUE means brake disabled/released IF stMotionStage.nBrakeMode <> ENUM_StageBrakeMode.NO_BRAKE THEN CASE stMotionStage.Axis.Status.MotionState OF MC_AXISSTATE_UNDEFINED, MC_AXISSTATE_DISABLED, MC_AXISSTATE_ERRORSTOP: stMotionStage.bBrakeRelease := FALSE; MC_AXISSTATE_STANDSTILL: IF stMotionStage.nBrakeMode = ENUM_StageBrakeMode.IF_MOVING THEN stMotionStage.bBrakeRelease := FALSE; ELSE stMotionStage.bBrakeRelease := TRUE; END_IF ELSE stMotionStage.bBrakeRelease := TRUE; END_CASE END_IF // Sync the epics status struct stMotionStage.stAxisStatus := fbDriveVirtual.stAxisStatus; stMotionStage.stAxisStatus.bEnabled := stMotionStage.bEnableDone; // Override homing status, dmov as appropriate stMotionStage.bHomed := fbMotionHome.bDone AND NOT fbMotionHome.bError; stMotionStage.stAxisStatus.bHomed := stMotionStage.bHomed; stMotionStage.stAxisStatus.bExecute := bExecute; stMotionStage.stAxisStatus.nCommand := 3; // If this is not 3, the IOC stops updating positions during homing // Fill in auxiliary status info stMotionStage.fPosDiff := stMotionStage.Axis.NcToPlc.PosDiff; // Reset everything when bReset is flagged IF stMotionStage.bReset THEN stMotionStage.bEnable := FALSE; stMotionStage.bReset := FALSE; stMotionStage.bExecute := FALSE; stMotionStage.bError := FALSE; stMotionStage.nErrorId := 0; stMotionStage.sErrorMessage := ''; stMotionStage.sCustomErrorMessage := ''; bExecute := FALSE; END_IF fbEncoderValue(stMotionStage:=stMotionStage); fbNCParams( stMotionStage:=stMotionStage, bEnable:=TRUE, tRefreshDelay:=T#1s); // Save and restore as long as not an absolute encoder fbSaveRestore( stMotionStage:=stMotionStage, bEnable:=stMotionStage.nHomingMode <> ENUM_EpicsHomeCmd.NONE); END_FUNCTION_BLOCK FB_MotionStageNCParams ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionStageNCParams (* Read and refresh axis parameters struct on DUT_MotionStage *) VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR VAR_INPUT bEnable: BOOL; tRefreshDelay: TIME; END_VAR VAR_OUTPUT bError: BOOL; END_VAR VAR mcReadParams: MC_ReadParameterSet; timer: TON; bExecute: BOOL := TRUE; nLatchErrId: UDINT; END_VAR timer( IN:=bEnable AND NOT bExecute, PT:=tRefreshDelay); bExecute S= timer.Q; mcReadParams( Parameter:=stMotionStage.stAxisParameters, Axis:=stMotionStage.Axis, Execute:=bEnable AND bExecute, Error=>bError); IF mcReadParams.ErrorID <> 0 THEN nLatchErrId := 0; END_IF bExecute R= mcReadParams.Done OR mcReadParams.Error; stMotionStage.bAxisParamsInit S= mcReadParams.Done; END_FUNCTION_BLOCK FB_MotionStageSim ^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionStageSim (* Set all the values needed for a fake motor to be movable via the IOC, then call FB_MotionStage *) VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR VAR_INPUT nEnableMode: ENUM_StageEnableMode := ENUM_StageEnableMode.ALWAYS; END_VAR VAR fbMotionStage: FB_MotionStage; bInit: BOOL; END_VAR IF NOT bInit THEN bInit := TRUE; // Stand-in for no forward limit stMotionStage.bLimitForwardEnable := TRUE; // Stand-in for no reverse limit stMotionStage.bLimitBackwardEnable := TRUE; // Stand-in for no STO button stMotionStage.bHardwareEnable := TRUE; // Stand-in for no PMPS governer stMotionStage.bPowerSelf := TRUE; // Always keep it enabled for testing ease stMotionStage.nEnableMode := nEnableMode; END_IF // Call FB_MotionStage to do the thing fbMotionStage(stMotionStage := stMotionStage); END_FUNCTION_BLOCK 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_NCErrorFFO ^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_NCErrorFFO (* Configure a DUT_MotionStage to trigger an FFO when we have an error. This can be configured to only apply to specific error ranges, though the default is the normal 16#4XXX NC error range. The error ranges are: 16#40XX General Errors 16#41XX Channel Errors 16#42XX Group Errors 16#43XX Axis Errors 16#44XX Encoder Errors 16#45XX Controller Errors 16#46XX Drive Errors 16#4AXX Table Errors 16#4BXX NC PLC Errors 16#4CXX Kinematic Transformation There is also a new extended NC error range, but it is sparsely populated. This range is 16#8XXX: 16#8100 - 16#811F: Bode plot (diagnosis) 16#8120 - 16#8FFF: Further errors To configure multiple ranges, simply use multiple instances of this function block. *) VAR_IN_OUT // Motion stage to monitor stMotionStage: DUT_MotionStage; // FFO to trip fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // Reset the fault bReset: BOOL; // Auto-reset the fault bAutoReset: BOOL; // The lowest error code that will trip the FFO nLowErrorId: UDINT := 16#4000; // The highest error code that will trip the FFO nHighErrorId: UDINT := 16#4FFF; // A description of the fault sDesc: STRING := 'Motor error'; END_VAR VAR_OUTPUT // Quick way for nearby code to check if this block has tripped the FFO. bTripped: BOOL; // Error code sent to PMPS. Is always 16#20XX, where XX is the first two hex in the NC error. nErrorTypeCode: UINT; END_VAR VAR bInit: BOOL := TRUE; bNCError: BOOL; stBeamParams: ST_BeamParams; fbFF: FB_FastFault; rtTrip: R_TRIG; END_VAR IF bInit THEN fbFF.i_DevName := stMotionStage.sName; fbFF.i_Desc := sDesc; IF LEN(stMotionStage.sName) > 0 THEN bInit := FALSE; END_IF ELSE bNCError := stMotionStage.bError AND stMotionStage.nErrorId >= nLowErrorId AND stMotionStage.nErrorId <= nHighErrorId; bTripped := bNCError AND PMPS_GVL.stCurrentBeamParameters.nRate > 0; rtTrip(CLK:=bTripped); IF rtTrip.Q THEN nErrorTypeCode := 16#2000 + UDINT_TO_UINT(SHR(stMotionStage.nErrorId, 8)); END_IF fbFF(i_xOK := NOT bTripped, i_xReset := bReset, i_xAutoReset := bAutoReset, i_TypeCode:= nErrorTypeCode, io_fbFFHWO := fbFFHWO); END_IF END_FUNCTION_BLOCK FB_PositionStateBase ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateBase (* Handles EPICS moves between multiple states for a single axis Should be subclassed for a specific enumSet and enumGet. See body comment or FB_PositionStateInOut for an implementation example. *) VAR_IN_OUT // Motor to move stMotionStage: DUT_MotionStage; END_VAR VAR_INPUT // If TRUE, start a move when setState transitions to a nonzero number bEnable: BOOL; // On rising edge, reset this FB {attribute 'pytmc' := ' pv: RESET io: io field: ZNAM False field: ONAM True '} bReset: BOOL; END_VAR VAR_OUTPUT // If TRUE, there is an error {attribute 'pytmc' := ' pv: ERR io: i field: ZNAM False field: ONAM True '} bError: BOOL; // Error ID {attribute 'pytmc' := ' pv: ERRID io: i '} nErrorId: UDINT; // The error that caused bError to flip TRUE {attribute 'pytmc' := ' pv: ERRMSG io: i '} sErrorMessage: STRING; // If TRUE, we are moving the motor {attribute 'pytmc' := ' pv: BUSY io: i field: ZNAM False field: ONAM True '} bBusy: BOOL; // If TRUE, we are not moving the motor and the last move completed successfully {attribute 'pytmc' := ' pv: DONE io: i field: ZNAM False field: ONAM True '} bDone: BOOL; END_VAR VAR // Pre-allocated array of states {attribute 'pytmc' := ' pv: io: io expand: %.2d '} arrStates: ARRAY[1..MOTION_GVL.MAX_STATES] OF DUT_PositionState; // Corresponding arrStates index to move to, or 0 if no move selected setState: INT; // The current position we are trying to reach, or 0 goalState: INT; // The readback position getState: INT; bInit: BOOL; stUnknown: DUT_PositionState; stGoal: DUT_PositionState; fbStateMove: FB_PositionStateMove; fbStateInternal: ARRAY[1..MOTION_GVL.MAX_STATES] OF FB_PositionStateInternal; nIndex: INT; bNewGoal: BOOL; bInnerExec: BOOL; bInnerReset: BOOL; rtReset: R_TRIG; bMoveRequested: BOOL; END_VAR (* Subclass me, define enums to translate setState and getState, call Exec Example: setState := enumSet; Exec(); enumGet := getState; enumSet := setState; *) END_FUNCTION_BLOCK ACTION Exec: StateHandler(); END_ACTION ACTION StateHandler: // Reset just goes through the first-cycle init again rtReset(CLK:=bReset); IF rtReset.Q THEN bInit := FALSE; bReset := FALSE; END_IF // First-cycle init IF NOT bInit THEN bError := FALSE; nErrorID := 0; sErrorMessage := ''; bBusy := FALSE; bDone := FALSE; bNewGoal := FALSE; bInnerExec := FALSE; bInnerReset := TRUE; setState := 0; goalState := 0; END_IF // All state internal handlers FOR nIndex := 1 TO MOTION_GVL.MAX_STATES DO IF arrStates[nIndex].bValid THEN fbStateInternal[nIndex]( stMotionStage:=stMotionStage, stPositionState:=arrStates[nIndex]); END_IF END_FOR // Check where we are by going through all possible states. // Note this favors the highest-number state that we're at, it's up to you to make your states mutually exclusive. getState := 0; FOR nIndex := 1 TO MOTION_GVL.MAX_STATES DO IF F_AtPositionState(stMotionStage:=stMotionStage, stPositionState:=arrStates[nIndex]) THEN getState := nIndex; END_IF END_FOR // Use the changing set pv as a rising-edge trigger IF setState <> goalState THEN goalState := setState; bNewGoal := TRUE; END_IF // Special move handling for error/enable state IF bError OR NOT bEnable THEN bInnerExec := FALSE; ELSIF bNewGoal THEN IF fbStateMove.bBusy THEN // Stop previous move if we were already moving but want a new move bInnerExec := FALSE; ELSE // If we hit this branch, we're ready to start a new move bInnerExec := TRUE; bInnerReset := FALSE; bNewGoal := FALSE; END_IF END_IF // Pick the correct goal structure or Unknown IF goalState = 0 THEN stGoal := stUnknown; ELSE stGoal := arrStates[goalState]; END_IF // Do the move fbStateMove( stMotionStage := stMotionStage, stPositionState := stGoal, bExecute := bInnerExec, bReset := bInnerReset, enumMotionRequest := ENUM_MotionRequest.INTERRUPT, bBusy => bBusy); // Only pass up bDone information if this FB is active IF bInnerExec THEN bDone := fbStateMove.bDone; END_IF // Pick up any new errors, but don't override uncleared existing errors IF NOT bError THEN bError := fbStateMove.bError; IF bError THEN nErrorId := fbStateMove.nErrorId; sErrorMessage := fbStateMove.sErrorMessage; END_IF END_IF // Reset the setpoint and goal to 0 if we're not doing anything // because FB is waiting for a change from 0 to "something" bMoveRequested := bInnerExec AND NOT bDone; IF NOT bError AND NOT bNewGoal AND NOT bMoveRequested THEN setState := 0; goalState := 0; bInnerExec := FALSE; END_IF // Bring inner reset back low at the end of the init cycle so that it can be triggered again later IF NOT bInit THEN bInit := TRUE; bInnerReset := FALSE; END_IF END_ACTION FB_PositionStateBase_WithPMPS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateBase_WithPMPS EXTENDS FB_PositionStateBase (* Handles EPICS moves between multiple states for a single axis with PMPS. Should be subclassed for a specific enumSet and enumGet. See body comment or FB_PositionStateInOut_WithPMPS for an implementation example. *) VAR_IN_OUT fbArbiter: FB_Arbiter; fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT stTransitionBeam: ST_BeamParams := PMPS_GVL.cst0RateBeam; nTransitionAssertionID: UDINT; nUnknownAssertionID: UDINT; {attribute 'pytmc' := ' pv: PMPS:ARB:ENABLE io: io '} bArbiterEnabled: BOOL := TRUE; tArbiterTimeout: TIME := T#1s; bMoveOnArbiterTimeout: BOOL := TRUE; fStateBoundaryDeadband: LREAL := 0; END_VAR VAR {attribute 'pytmc' := 'pv: PMPS'} fbStatePMPS: FB_PositionStatePMPS; fbEncErrFFO: FB_EncErrorFFO; END_VAR (* Subclass me, define enums to translate setState and getState, call Exec Example: setState := enumSet; Exec(); enumGet := getState; enumSet := setState; *) END_FUNCTION_BLOCK ACTION Exec: StateHandler(); PMPSHandler(); END_ACTION ACTION PMPSHandler: fbStatePMPS( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stMotionStage:=stMotionStage, arrStates:=arrStates, bRequestTransition:=setState <> 0, setState:=setState, getState:=getState, stTransitionBeam:=stTransitionBeam, nTransitionAssertionID:=nTransitionAssertionID, nUnknownAssertionID:=nUnknownAssertionID, bArbiterEnabled:=bArbiterEnabled, tArbiterTimeout:=tArbiterTimeout, bMoveOnArbiterTimeout:=bMoveOnArbiterTimeout, fStateBoundaryDeadband:=fStateBoundaryDeadband); fbEncErrFFO( stMotionStage:=stMotionStage, fbFFHWO:=fbFFHWO, bAutoReset:=TRUE); END_ACTION FB_PositionStateBase_WithPMPS_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateBase_WithPMPS_Test EXTENDS FB_PositionStateBase (* Handles EPICS moves between multiple states for a single axis with fake PMPS. Should be subclassed for a specific enumSet and enumGet. See body comment or FB_PositionStateInOut_WithPMPS_Test for an implementation example. *) VAR_INPUT stTransitionBeam: ST_BeamParams := PMPS_GVL.cst0RateBeam; nTransitionAssertionID: UDINT; END_VAR VAR fbStatePMPS: FB_PositionStatePMPS_Test; END_VAR (* Subclass me, define enums to translate setState and getState, call Exec Example: setState := enumSet; Exec(); enumGet := getState; enumSet := setState; *) END_FUNCTION_BLOCK ACTION Exec: StateHandler(); PMPSHandler(); END_ACTION ACTION PMPSHandler: fbStatePMPS( stMotionStage:=stMotionStage, arrStates:=arrStates, bRequestTransition:=setState <> 0, setState:=setState, getState:=getState, stTransitionBeam:=stTransitionBeam, nTransitionAssertionID:=nTransitionAssertionID); END_ACTION FB_PositionStateInOut ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateInOut EXTENDS FB_PositionStateBase (* Example usage of FB_PositionStateBase for a simple IN/OUT axis. See NOTE: comments. Also usable as a drop-in for these cases (no need to roll your own in/out) *) VAR_INPUT // The enum position to move to {attribute 'pytmc' := ' pv: SET io: io '} enumSet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumSet // Information about the OUT position stOut: DUT_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN position stIn: DUT_PositionState; END_VAR VAR_OUTPUT // The enum state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumGet END_VAR VAR bInOutInit: BOOL; END_VAR IF NOT bInOutInit THEN bInOutInit := TRUE; arrStates[1] := stOut; arrStates[2] := stIn; END_IF setState := enumSet; Exec(); enumGet := getState; enumSet := setState; END_FUNCTION_BLOCK FB_PositionStateInOut_WithPMPS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateInOut_WithPMPS EXTENDS FB_PositionStateBase_WithPMPS (* Example usage of FB_PositionStateBase_WithPMPS for a simple IN/OUT axis. See NOTE: comments. Also usable as a drop-in for these cases (no need to roll your own in/out) This is the PMPS version. Note that the only difference is that we extend the _WithPMPS FB. *) VAR_INPUT // The enum position to move to {attribute 'pytmc' := ' pv: SET io: io '} enumSet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumSet // Information about the OUT position stOut: DUT_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN position stIn: DUT_PositionState; END_VAR VAR_OUTPUT // The enum state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumGet END_VAR VAR bInOutInit: BOOL; END_VAR IF NOT bInOutInit THEN bInOutInit := TRUE; arrStates[1] := stOut; arrStates[2] := stIn; END_IF setState := enumSet; Exec(); enumGet := getState; enumSet := setState; END_FUNCTION_BLOCK FB_PositionStateInOut_WithPMPS_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateInOut_WithPMPS_Test EXTENDS FB_PositionStateBase_WithPMPS_Test (* Example usage of FB_PositionStateBase_WithPMPS_Test for a simple IN/OUT axis. See NOTE: comments. Also usable as a drop-in for these cases (no need to roll your own in/out) This is the PMPS version. Note that the only difference is that we extend the _WithPMPS_Test FB. *) VAR_INPUT // The enum position to move to {attribute 'pytmc' := ' pv: SET io: io '} enumSet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumSet // Information about the OUT position stOut: DUT_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN position stIn: DUT_PositionState; END_VAR VAR_OUTPUT // The enum state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut; // NOTE: Please copy this pragma exactly on your enumGet END_VAR VAR bInOutInit: BOOL; END_VAR IF NOT bInOutInit THEN bInOutInit := TRUE; arrStates[1] := stOut; arrStates[2] := stIn; END_IF setState := enumSet; Exec(); enumGet := getState; enumSet := setState; END_FUNCTION_BLOCK FB_PositionStateInternal ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateInternal (* Routines that must be called on all DUT_PositionState *) VAR_IN_OUT stMotionStage: DUT_MotionStage; stPositionState: DUT_PositionState; END_VAR VAR_OUTPUT END_VAR VAR fbEncConverter: FB_RawCountConverter; fbLock: FB_PositionStateLock; END_VAR // Mark that we've been here stPositionState.bUpdated := TRUE; // Update pos state's count or egu position as appropriate IF stMotionStage.bAxisParamsInit THEN fbEncConverter( stParameters:=stMotionStage.stAxisParameters, nCountCheck:=stPositionState.nEncoderCount, fPosCheck:=stPositionState.fPosition); IF stPositionState.bUseRawCounts THEN stPositionState.fPosition := fbEncConverter.fPosGet; ELSE stPositionState.nEncoderCount := fbEncConverter.nCountGet; END_IF END_IF // Handle state parameter locking fbLock( stPositionState:=stPositionState, bEnable:=stMotionStage.bAxisParamsInit); END_FUNCTION_BLOCK FB_PositionStateLock ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateLock (* Implements immutability for a locked DUT_PositionState Once this is called the first time, the parameters at the time of the call will be restored on all subsequent calls. *) VAR_IN_OUT stPositionState: DUT_PositionState; END_VAR VAR_INPUT bEnable: BOOL; END_VAR VAR stCachedPositionState: DUT_PositionState; bInit: BOOL := FALSE; END_VAR IF bEnable THEN // Force values to be cached if we've cached something IF bInit AND stPositionState.bLocked THEN stPositionState.sName := stCachedPositionState.sName; stPositionState.fPosition := stCachedPositionState.fPosition; stPositionState.fDelta := stCachedPositionState.fDelta; stPositionState.fVelocity := stCachedPositionState.fVelocity; stPositionState.fAccel := stCachedPositionState.fAccel; stPositionState.fDecel := stCachedPositionState.fDecel; // If we haven't cached and we should, make the cache. Note that we skip bLocked, bValid, and bMoveOk ELSIF NOT bInit AND stPositionState.bLocked THEN stCachedPositionState.sName := stPositionState.sName; stCachedPositionState.fPosition := stPositionState.fPosition; stCachedPositionState.fDelta := stPositionState.fDelta; stCachedPositionState.fVelocity := stPositionState.fVelocity; stCachedPositionState.fAccel := stPositionState.fAccel; stCachedPositionState.fDecel := stPositionState.fDecel; bInit := TRUE; // Do nothing, or unlock the state if bLocked goes FALSE ELSIF NOT stPositionState.bLocked THEN bInit := FALSE; END_IF END_IF END_FUNCTION_BLOCK FB_PositionStateManager ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateManager (* Handles EPICS moves between multiple states for a single axis Should be wrapped inside a block with enums for states, see FB_EpicsInOut for an example. *) VAR_IN_OUT // Motor to move stMotionStage: DUT_MotionStage; // Allocated space for 15 states besides Unknown (16 is the max for an EPICS MBBI) {attribute 'pytmc' := ' pv: io: io expand: %.2d '} arrStates: ARRAY[1..15] OF DUT_PositionState; // Corresponding arrStates index to move to, or 0 if no move selected setState: INT; END_VAR VAR_INPUT // If TRUE, start a move when setState transitions to a nonzero number bEnable: BOOL; // On rising edge, reset this FB {attribute 'pytmc' := ' pv: RESET io: io field: ZNAM False field: ONAM True '} bReset: BOOL; END_VAR VAR_OUTPUT // If TRUE, there is an error {attribute 'pytmc' := ' pv: ERR io: i field: ZNAM False field: ONAM True '} bError: BOOL; // Error ID {attribute 'pytmc' := ' pv: ERRID io: i '} nErrorId: UDINT; // The error that caused bError to flip TRUE {attribute 'pytmc' := ' pv: ERRMSG io: i '} sErrorMessage: STRING; // If TRUE, we are moving the motor {attribute 'pytmc' := ' pv: BUSY io: i field: ZNAM False field: ONAM True '} bBusy: BOOL; // If TRUE, we are not moving the motor and the last move completed successfully {attribute 'pytmc' := ' pv: DONE io: i field: ZNAM False field: ONAM True '} bDone: BOOL; // The current position we are trying to reach, or 0 goalState: INT; // The readback position getState: INT; END_VAR VAR bInit: BOOL; stUnknown: DUT_PositionState; stGoal: DUT_PositionState; fbStateMove: FB_PositionStateMove; fbStateInternal: ARRAY[1..15] OF FB_PositionStateInternal; nIndex: INT; bNewGoal: BOOL; bInnerExec: BOOL; bInnerReset: BOOL; rtReset: R_TRIG; bMoveRequested: BOOL; END_VAR // Reset just goes through the first-cycle init again rtReset(CLK:=bReset); IF rtReset.Q THEN bInit := FALSE; END_IF // First-cycle init IF NOT bInit THEN bError := FALSE; sErrorMessage := ''; bBusy := FALSE; bDone := FALSE; bNewGoal := FALSE; bInnerExec := FALSE; bInnerReset := TRUE; setState := 0; goalState := 0; END_IF // All state internal handlers FOR nIndex := 1 TO 15 DO IF arrStates[nIndex].bValid THEN fbStateInternal[nIndex]( stMotionStage:=stMotionStage, stPositionState:=arrStates[nIndex]); END_IF END_FOR // Check where we are by going through all possible states. // Note this favors the highest-number state that we're at, it's up to you to make your states mutually exclusive. getState := 0; FOR nIndex := 1 TO 15 DO IF F_AtPositionState(stMotionStage:=stMotionStage, stPositionState:=arrStates[nIndex]) THEN getState := nIndex; END_IF END_FOR // Use the changing set pv as a rising-edge trigger IF setState <> goalState THEN goalState := setState; bNewGoal := TRUE; END_IF // Special move handling for error/enable state IF bError OR NOT bEnable THEN bInnerExec := FALSE; // Reset inner block this cycle if we were already moving but want a new move ELSIF bInnerExec AND bNewGoal THEN bInnerExec := FALSE; bInnerReset := TRUE; // If we hit this branch, we're starting a new move ELSIF bNewGoal THEN bInnerExec := TRUE; bInnerReset := FALSE; bNewGoal := FALSE; END_IF // Pick the correct goal structure or Unknown IF goalState = 0 THEN stGoal := stUnknown; ELSE stGoal := arrStates[goalState]; END_IF // Do the move fbStateMove( stMotionStage := stMotionStage, stPositionState := stGoal, bExecute := bInnerExec, bReset := bInnerReset, enumMotionRequest := ENUM_MotionRequest.INTERRUPT, bBusy => bBusy); // Only pass up bDone information if this FB is active IF bInnerExec THEN bDone := fbStateMove.bDone; END_IF // Pick up any new errors, but don't override uncleared existing errors IF NOT bError THEN bError := fbStateMove.bError; nErrorId := fbStateMove.nErrorId; sErrorMessage := fbStateMove.sErrorMessage; END_IF // Reset the setpoint and goal to 0 if we're not doing anything // because FB is waiting for a change from 0 to "something" bMoveRequested := bInnerExec AND NOT bDone; IF NOT bError AND NOT bNewGoal AND NOT bMoveRequested THEN setState := 0; goalState := 0; bInnerExec := FALSE; END_IF // Bring inner reset back low at the end of the init cycle so that it can be triggered again later IF NOT bInit THEN bInit := TRUE; bInnerReset := FALSE; END_IF END_FUNCTION_BLOCK FB_PositionStateMove ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateMove // Request a move to a particular state from an axis controlled via EPICS // pytmc PVs here only exposed if using directly and not through FB_PositionStateManager VAR_IN_OUT // Motor to move stMotionStage: DUT_MotionStage; // State to move to {attribute 'pytmc' := ' pv: '} stPositionState: DUT_PositionState; END_VAR VAR_INPUT // Start move on rising edge, stop move on falling edge {attribute 'pytmc' := ' pv: GO io: io field: ZNAM False field: ONAM True '} bExecute: BOOL; // Rising edge error reset {attribute 'pytmc' := ' pv: RESET io: io field: ZNAM False field: ONAM True '} bReset: BOOL; // Define behavior for when a move is already active enumMotionRequest: ENUM_MotionRequest := ENUM_MotionRequest.WAIT; END_VAR VAR_OUTPUT // TRUE if the motor is at this state {attribute 'pytmc' := ' pv: AT_STATE io: input field: ZNAM False field: ONAM True '} bAtState: BOOL; // TRUE if we have an error {attribute 'pytmc' := ' pv: ERR io: input field: ZNAM False field: ONAM True '} bError: BOOL; // Error code {attribute 'pytmc' := ' pv: ERRID io: input '} nErrorID: UDINT; // Error description {attribute 'pytmc' := ' pv: ERRMSG io: input '} sErrorMessage: STRING; // TRUE if we are moving to a state {attribute 'pytmc' := ' pv: BUSY io: input field: ZNAM False field: ONAM True '} bBusy: BOOL; // TRUE if we are note moving and we reached a state successfully on our last move {attribute 'pytmc' := ' pv: DONE io: input field: ZNAM False field: ONAM True '} bDone: BOOL; END_VAR VAR fbMotionRequest: FB_MotionRequest; bAllowMove: BOOL; END_VAR // Veto the move for uninitialized and unsafe states bAllowMove := stPositionState.bMoveOk AND stPositionState.bValid AND stPositionState.bUpdated; // Do the move fbMotionRequest( stMotionStage := stMotionStage, bExecute := bExecute AND bAllowMove, bReset := bReset, enumMotionRequest := enumMotionRequest, fPos := stPositionState.fPosition, fVel := stPositionState.fVelocity, fAcc := stPositionState.fAccel, fDec := stPositionState.fDecel, bError => bError, nErrorId => nErrorId, sErrorMessage => sErrorMessage, bBusy => bBusy, bDone => bDone); // Inject custom error if we can't move because of bMoveOk or bValid IF bExecute AND NOT bAllowMove THEN bError := TRUE; IF stPositionState.bValid THEN nErrorId := 16#7901; ELSE nErrorId := 16#7902; END_IF sErrorMessage := F_MotionErrorCodeLookup(nErrorId := nErrorID); END_IF // This can be useful if we're running this FB standalone for some reason bAtState := F_AtPositionState(stMotionStage:=stMotionStage, stPositionState:=stPositionState); END_FUNCTION_BLOCK FB_PositionStatePMPS ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS EXTENDS FB_PositionStatePMPS_Base (* Hooks up a position state to an arbiter and an FFO Use BeamParameterTransitionManager to manage transition requests between states Hook up to the inputs/outputs of the state function block Raises FFO if beam parameters are worse than required for current state *) VAR_IN_OUT fbArbiter: FB_Arbiter; fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT stHighBeamThreshold: ST_BeamParams := PMPS_GVL.cstFullBeam; END_VAR VAR bptm: BeamParameterTransitionManager; ffBeamParamsOk: FB_FastFault; ffZeroRate: FB_FastFault; ffBPTMTimeoutAndMove: FB_FastFault; ffBPTMError: FB_FastFault; ffMaint: FB_FastFault; bFFOxOk: BOOL; bAtSafeState: BOOL; nIter: INT; END_VAR Exec(); END_FUNCTION_BLOCK ACTION AssertHere: fbArbiter.AddRequest( nReqID := stStateReq.nRequestAssertionID, stReqBP := stStateReq.stBeamParams); END_ACTION ACTION ClearAsserts: fbArbiter.RemoveRequest(nTransitionAssertionID); fbArbiter.RemoveRequest(nUnknownAssertionID); FOR nIter := 1 TO MOTION_GVL.MAX_STATES DO fbArbiter.RemoveRequest(arrStates[nIter].nRequestAssertionID); END_FOR END_ACTION ACTION HandleBPTM: (* Handle finishing the bptm late if timed out bptm needs a rising edge of bTransDone after authorizing transition If we fall into this block, bTransDone would otherwise be stuck at TRUE forever so the BPTM would never see a rising edge and therefore stay stuck We set to FALSE here to reset the BPTM, then gets set to TRUE again if really done. *) rtDoLateFinish(CLK:=bLateFinish AND bInternalAuth); IF rtDoLateFinish.Q THEN bTransDone := FALSE; bLateFinish := FALSE; END_IF // Just normal bptm call bptm(fbArbiter:=fbArbiter, i_TransitionAssertionID:=nTransitionAssertionID, i_stTransitionAssertion:=stTransitionBeam, i_nRequestedAssertionID:=stStateReq.nRequestAssertionID, i_stRequestedAssertion:=stStateReq.stBeamParams, i_xDoneMoving:=bTransDone, stCurrentBeamParameters:=PMPS_GVL.stCurrentBeamParameters, q_xTransitionAuthorized=>bInternalAuth, bDone=>bBPTMDone); END_ACTION ACTION HandleFFO: // stBeamNeeded will point to Unknown/No beam if we are out of state bounds or in motion // Otherwise we'll have the current state's beam parameters // Check for bad conditions bFFOxOk := F_SafeBPCompare(PMPS_GVL.stCurrentBeamParameters, stBeamNeeded); // It is safe to reset automatically if our current state can take full beam. // Otherwise we'll have to ask for a user acknowledgement to clear. // This avoids rapidly cycling the FFOs on/off // You can pass in a different stHighBeamThreshold as an input parameter to customize this behavior bAtSafeState := F_SafeBPCompare(stHighBeamThreshold, stBeamNeeded); // If the beam parameters are wrong, it is a fault! This encompasses all unknown arbiter-related errors. ffBeamParamsOk.i_xOK := bFFOxOk; ffBeamParamsOk.i_xReset S= bFFOxOk AND bAtSafeState; ffBeamParamsOk.i_xReset R= NOT ffBeamParamsOk.i_xOK; ffBeamParamsOk( i_DevName:=stMotionStage.sName, i_Desc:='Beam parameter mismatch', i_TypeCode:=16#1000, io_fbFFHWO:=fbFFHWO); // Trip the beam for zero-rate states. This is a PMPS training wheel and should ultimately be removed. // Note: I think this is already redundant ffZeroRate( i_xOk := stBeamNeeded.nRate > 0, i_xAutoReset := TRUE, i_DevName := stMotionStage.sName, i_Desc := 'Device requesting zero rate', i_TypeCode := 16#1001, io_fbFFHWO := fbFFHWO); // Trip the beam for BPTM timeouts if we want to move // Only reset at safe beam OR at no bptm errors (some other FF should catch additional issues) ffBPTMTimeoutAndMove.i_xOK := NOT (bArbiterTimeout AND bMoveOnArbiterTimeout); ffBPTMTimeoutAndMove.i_xReset S= bAtSafeState OR (bptm.bDone AND NOT bptm.bError); ffBPTMTimeoutAndMove.i_xReset R= NOT ffBPTMTimeoutAndMove.i_xOK; ffBPTMTimeoutAndMove( i_DevName := stMotionStage.sName, i_Desc := 'BPTM Timeout', i_TypeCode := 16#1002, io_fbFFHWO := fbFFHWO); // Trip the beam for BPTM Errors ffBPTMError.i_xOK := NOT bptm.bError; ffBPTMError.i_xReset S= bptm.bDone AND NOT bptm.bError; ffBPTMError.i_xReset R= NOT ffBPTMError.i_xOK; ffBPTMError( i_DevName := stMotionStage.sName, i_Desc := 'BPTM error, state transition failed', i_TypeCode := 16#1003, io_fbFFHWO := fbFFHWO); // Trip the beam for maintenance mode ffMaint( i_xOK := NOT bMaintMode, i_xAutoReset := TRUE, i_DevName := stMotionStage.sName, i_Desc := 'Device is in maintenance mode', i_TypeCode := 16#1004, io_fbFFHWO := fbFFHWO); END_ACTION FB_PositionStatePMPS_Base ^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS_Base (* FB_PositionStatePMPS without Arbiter, BPTM, FFO This allows me to test most of the code without an arbiter plc setup *) VAR_IN_OUT stMotionStage: DUT_MotionStage; arrStates: ARRAY[1..MOTION_GVL.MAX_STATES] OF DUT_PositionState; END_VAR VAR_INPUT bArbiterEnabled: BOOL := TRUE; {attribute 'pytmc' := ' pv: MAINT io: io '} bMaintMode: BOOL; bRequestTransition: BOOL; setState: INT; getState: INT; fStateBoundaryDeadband: LREAL := 0; stTransitionBeam: ST_BeamParams := PMPS_GVL.cst0RateBeam; nTransitionAssertionID: UDINT; nUnknownAssertionID: UDINT; tArbiterTimeout: TIME := T#1s; bMoveOnArbiterTimeout: BOOL := TRUE; stInvalidState: DUT_PositionState := MOTION_GVL.stInvalidState; stUnknownState: DUT_PositionState := MOTION_GVL.stUnknownState; END_VAR VAR_OUTPUT bTransitionAuthorized: BOOL; bForwardAuthorized: BOOL; bBackwardAuthorized: BOOL; bArbiterTimeout: BOOL; END_VAR VAR bInit: BOOL := TRUE; bTransDone: BOOL; rtTransReq: R_TRIG; bBPTMDone: BOOL; rtBPTMDone: R_TRIG; ftMotorExec: F_TRIG; rtTransDone: R_TRIG; rtDoLateFinish: R_TRIG; tonDone: TON; stStateReq: DUT_PositionState; mcPower: MC_POWER; fUpperBound: LREAL; fLowerBound: LREAL; nGoalState: INT; stGoalState: DUT_PositionState; fActPos: LREAL; fReqPos: LREAL; bInTransition: BOOL; stBeamNeeded: ST_BeamParams; bFwdOk: BOOL; bBwdOk: BOOL; tonArbiter: TON; bLateFinish: BOOL; bInternalAuth: BOOL; END_VAR // This is meant to be subclassed. The parent class body is in the Exec action. END_FUNCTION_BLOCK ACTION AssertHere: END_ACTION ACTION ClearAsserts: END_ACTION ACTION Exec: // Initialize or reinitialize to the current state value rtBPTMDone(CLK:=bBPTMDone); ftMotorExec(CLK:=stMotionStage.bExecute); tonDone( IN:=bTransDone, PT:=T#100ms ); IF rtBPTMDone.Q OR ftMotorExec.Q OR tonDone.Q THEN bInit := TRUE; END_IF IF bInit OR nGoalState = 0 OR stMotionStage.bError THEN bInit R= stMotionStage.bAxisParamsInit; nGoalState := getState; stStateReq := GetStateStruct(getState); bInTransition := FALSE; stInvalidState.nRequestAssertionID := nUnknownAssertionID; stUnknownState.nRequestAssertionID := nUnknownAssertionID; rtTransReq(CLK:=FALSE); bTransitionAuthorized := FALSE; bArbiterTimeout := FALSE; END_IF // Request transition on rising edge rtTransReq(CLK:=bRequestTransition); IF rtTransReq.Q THEN nGoalState := setState; stStateReq := GetStateStruct(setState); bInTransition := TRUE; bTransDone := FALSE; ELSE bTransDone := F_AtPositionState(stMotionStage:=stMotionStage, stPositionState:=stStateReq) AND NOT stMotionStage.bBusy; END_IF // Mark late finish if bTransDone -> true before the bptm is done // This means that we finished the move so fast that the bptm needs to be unstuck via toggling bTransDone rtTransDone(CLK:=bTransDone); bLateFinish S= rtTransDone.Q AND NOT bBPTMDone; IF bArbiterEnabled THEN // Handles getting the request to the arbiter and back HandleBPTM(); // Handle arbiter timeouts IF tArbiterTimeout > T#0s THEN tonArbiter( IN:=bInTransition AND NOT bInternalAuth, PT:=tArbiterTimeout, Q=>bArbiterTimeout); ELSE bArbiterTimeout := FALSE; END_IF bTransitionAuthorized S= bInternalAuth OR (bArbiterTimeout AND bMoveOnArbiterTimeout); ELSE // Clear all of our assertions if we decide to disable the arbiter ClearAsserts(); // Do some dummy request handling bTransitionAuthorized := stMotionStage.bExecute; bArbiterTimeout := stMotionStage.bExecute; END_IF // Set up MPS virtual limit for moves at and between states stGoalState := GetStateStruct(nGoalState); fActPos := stMotionStage.stAxisStatus.fActPosition; IF stMotionStage.bExecute THEN fReqPos := stMotionStage.fPosition; ELSE fReqPos := fActPos; END_IF // Start with no move authority bForwardAuthorized := FALSE; bBackwardAuthorized := FALSE; // Check if it would be OK to move without granting auth bFwdOk := F_PosUnderUpperBound(MAX(fActPos, fReqPos) + ABS(fStateBoundaryDeadband), stGoalState); bBwdOk := F_PosOverLowerBound(MIN(fActPos, fReqPos) - ABS(fStateBoundaryDeadband), stGoalState); // Grant auth during moves based on goal state, current position, and goal position IF stMotionStage.bExecute AND stGoalState.bValid THEN bForwardAuthorized := bFwdOk; bBackwardAuthorized := bBwdOk; END_IF IF bInTransition THEN // Deny auth during a transition request until transition is authorized bForwardAuthorized R= NOT bTransitionAuthorized; bBackwardAuthorized R= NOT bTransitionAuthorized; // Have the motor wait for permission to start move instead of immediately erroring stMotionStage.bSafetyReady := bTransitionAuthorized; ELSE // If not transitioning, no need to wait for safety: immediately try to move and error if no auth stMotionStage.bSafetyReady := stMotionStage.bExecute; // Set an error message override in case this causes an error IF stMotionStage.bError AND bArbiterEnabled AND NOT bMaintMode THEN IF fReqPos > fActPos AND NOT bFwdOk THEN stMotionStage.sCustomErrorMessage := 'Unsafe tweak forward blocked by PMPS'; ELSIF fReqPos < fActPos AND NOT bBwdOk THEN stMotionStage.sCustomErrorMessage := 'Unsafe tweak backward blocked by PMPS'; END_IF END_IF END_IF IF bArbiterEnabled AND NOT bMaintMode THEN // Only let us move if the transition is allowed, or if we are moving within a state's bounds mcPower(Axis:=stMotionStage.Axis, Enable:=stMotionStage.bAllEnable, Enable_Positive:=stMotionStage.bAllForwardEnable AND bForwardAuthorized, Enable_Negative:=stMotionStage.bAllBackwardEnable AND bBackwardAuthorized); ELSE mcPower(Axis:=stMotionStage.Axis, Enable:=stMotionStage.bAllEnable, Enable_Positive:=stMotionStage.bAllForwardEnable, Enable_Negative:=stMotionStage.bAllBackwardEnable); stMotionStage.bSafetyReady := TRUE; END_IF // Raise fast faults as needed stBeamNeeded := GetBeamFromState(getState); HandleFFO(); END_ACTION ACTION HandleBPTM: END_ACTION ACTION HandleFFO: END_ACTION FB_PositionStatePMPS_Test ^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS_Test EXTENDS FB_PositionStatePMPS_Base (* Implement position state pmps with no FFO and auto-accept transition after 3s Use for offline testing of everything except the explicit interface *) VAR tonReq: TON; END_VAR Exec(); END_FUNCTION_BLOCK ACTION HandleBPTM: // Send the fake BPTM our assertion request by changing stStateReq.stBeamParams // We expect to recieve bTransitionAuthorized TRUE after some delta T // We expect bTransitionAuthorized to go FALSE after stMotionStage.bBusy goes FALSE tonReq( IN:=bInTransition, PT:=T#3s); bTransitionAuthorized := tonReq.Q AND stMotionStage.bExecute; END_ACTION ACTION HandleFFO: // Skip implementing this for offline testing // We won't be able to tell if it worked anyway END_ACTION FB_RawCountConverter ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_RawCountConverter (* Utility function block for converting raw counts to EGU and back *) VAR_INPUT // Parameters to check against stParameters: ST_AxisParameterSet; // Optional count to convert to a real position nCountCheck: UDINT; // Optional position to convert to encoder counts fPosCheck: LREAL; END_VAR VAR_OUTPUT // If converting position, the number of counts nCountGet: UDINT; // If converting counts, the position fPosGet: LREAL; // True during a parameter get/calc bBusy: BOOL; // True after a successful get/calc bDone: BOOL; // True if the calculation errored bError: BOOL; END_VAR IF stParameters.fEncScaleFactorInternal <> 0 THEN nCountGet := LREAL_TO_UDINT((fPosCheck - stParameters.fEncOffset) / stParameters.fEncScaleFactorInternal); fPosGet := nCountCheck * stParameters.fEncScaleFactorInternal + stParameters.fEncOffset; 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_SetEnables ^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_SetEnables // Update the all enable booleans based on the booleans that make them up VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR stMotionStage.bAllForwardEnable := stMotionStage.bLimitForwardEnable AND (stMotionStage.bGantryForwardEnable OR NOT stMotionStage.bGantryAxis); stMotionStage.bAllBackwardEnable := stMotionStage.bLimitBackwardEnable AND (stMotionStage.bGantryBackwardEnable OR NOT stMotionStage.bGantryAxis); stMotionStage.bAllEnable := stMotionStage.bEnable AND stMotionStage.bHardwareEnable; stMotionStage.bAllEnable R= NOT stMotionStage.bUserEnable; END_FUNCTION_BLOCK FB_StatePTPMove ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StatePTPMove // Do not use, this is deprecated VAR_INPUT {attribute 'pytmc' := ' pv: '} stPositionState: DUT_PositionState; {attribute 'pytmc' := ' pv: GO io: io field: ZNAM False field: ONAM True '} bExecute: BOOL; bMoveOk: BOOL; END_VAR VAR_IN_OUT stMotionStage: DUT_MotionStage; END_VAR VAR_OUTPUT {attribute 'pytmc' := ' pv: AT_STATE io: input field: ZNAM False field: ONAM True '} bAtState: BOOL; {attribute 'pytmc' := ' pv: DMOV io: input field: ZNAM False field: ONAM True '} bDone: BOOL; {attribute 'pytmc' := ' pv: BUSY io: input field: ZNAM False field: ONAM True '} bBusy: BOOL; {attribute 'pytmc' := ' pv: ERR io: input field: ZNAM False field: ONAM True '} bError: BOOL; {attribute 'pytmc' := ' pv: ERRMSG io: input '} sError: STRING; END_VAR VAR bExecTrig: R_TRIG; bExecEnd: F_TRIG; fActPosition: LREAL; fLowPos: LREAL; fHighPos: LREAL; END_VAR bExecTrig(CLK:=bExecute); IF bExecTrig.Q AND bMoveOk THEN IF NOT stMotionStage.bBusy AND NOT stMotionStage.bError THEN stMotionStage.bExecute := TRUE; stMotionStage.nCommand := ENUM_EpicsMotorCmd.MOVE_ABSOLUTE; stMotionStage.fPosition := stPositionState.fPosition; stMotionStage.fVelocity := stPositionState.fVelocity; stMotionStage.fAcceleration := stPositionState.fAccel; stMotionStage.fDeceleration := stPositionState.fDecel; bDone := FALSE; bBusy := TRUE; END_IF END_IF bError := stMotionStage.bError; sError := stMotionStage.sErrorMessage; fActPosition := stMotionStage.stAxisStatus.fActPosition; fLowPos := stPositionState.fPosition - stPositionState.fDelta; fHighPos := stPositionState.fPosition + stPositionState.fDelta; IF (fLowPos < fActPosition) AND (fHighPos > fActPosition) THEN bAtState := TRUE; IF NOT stMotionStage.bBusy THEN bDone := TRUE; bBusy := FALSE; bExecute := FALSE; END_IF ELSE bAtState := FALSE; END_IF bExecEnd(CLK:=bExecute); IF bExecEnd.Q AND bBusy THEN stMotionStage.bExecute := FALSE; END_IF IF NOT stMotionStage.bExecute OR NOT bExecute THEN bDone := TRUE; bBusy := FALSE; bExecute := FALSE; END_IF END_FUNCTION_BLOCK FB_TerminalError ^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_TerminalError VAR_INPUT En : BOOL; iTerminal_ID : INT; bWcState : BOOL; uiInfoData_State : UINT; pErrorSystem : POINTER TO ST_ErrorSystem; //Pointer to the error system END_VAR VAR_OUTPUT EnO : BOOL; bError : BOOL := FALSE; END_VAR VAR iStateError : UINT; iOtherError : UINT; ErrorData : DUT_TerminalError; nErrSysCNT : UINT; //testing bStateChanged : BOOL; //Indicate if state change happened uiInfoData_State_Prev : UINT := 16#8; //Previous value of Infodata.State bWcState_Prev : BOOL := FALSE; //Previous state of WcState //FB-s END_VAR (* Currently: Problem: TODO: *) //Connect EN to EnO EnO:=En; //Check if pointer is OK IF pErrorSystem=0 THEN RETURN; END_IF //Any difference from normal state creates an error IF En AND (bWcState OR uiInfoData_State<>16#8) THEN bError:=TRUE; ELSE bError:=FALSE; END_IF //Change detection IF uiInfoData_State <> uiInfoData_State_Prev OR bWcState <> bWcState_Prev THEN bStateChanged := TRUE; ELSE bStateChanged := FALSE; END_IF //Update previous values uiInfoData_State_Prev := uiInfoData_State; bWcState_Prev := bWcState; //Decision tree IF bStateChanged THEN IF bError THEN IF ErrorData.ErrorState = DUT_ErrorState.Active THEN //Close active error //Read system time ErrorData.nDateTimeOff := Tc2_EtherCAT.F_GetActualDcTime64(); ErrorData.sDateTimeOff := Tc2_EtherCat.DCTIME64_TO_STRING(ErrorData.nDateTimeOff); ErrorData.ErrorState := DUT_ErrorState.Inactive; //Write Off-time to Error System FOR nErrSysCNT := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO IF pErrorSystem^.aErrorData[nErrSysCNT].Error_ID = ErrorData.Error_ID THEN pErrorSystem^.aErrorData[nErrSysCNT].nDateTimeOff := ErrorData.nDateTimeOff; pErrorSystem^.aErrorData[nErrSysCNT].sDateTimeOff := ErrorData.sDateTimeOff; pErrorSystem^.aErrorData[nErrSysCNT].ErrorState := DUT_ErrorState.Inactive; EXIT; END_IF END_FOR //Clear ErrorData MEMSET(ADR(ErrorData), 0, SIZEOF(ErrorData)); END_IF //Open a new error ErrorData.ErrorState := DUT_ErrorState.Active; //Set Error State ErrorData.nDateTimeOn := Tc2_EtherCAT.F_GetActualDcTime64(); //Get system time ErrorData.sDateTimeOn := Tc2_EtherCat.DCTIME64_TO_STRING(ErrorData.nDateTimeOn); //Convert to string ErrorData.iTerminalID := iTerminal_ID; //Terminal_ID ErrorData.bWcState := bWcState; //WcState bit ErrorData.uiInfoDataState := uiInfoData_State; //uiInfoData_State //Error message according to uiInfoData_State and WcState iStateError := (uiInfoData_State AND 16#000F); //Mask for operation state iOtherError := (uiInfoData_State AND 16#00F0); //Mask for the other 3 kind of errors //Error messages according to the least significant digit CASE iStateError OF 16#0001 : ErrorData.sErrorMessage := 'Slave in INIT state; '; 16#0002 : ErrorData.sErrorMessage := 'Slave in PREOP state; '; 16#0003 : ErrorData.sErrorMessage := 'Slave in BOOT state; '; 16#0004 : ErrorData.sErrorMessage := 'Slave in SAFEOP state; '; 16#0008 : ; //Normal operation state ELSE ErrorData.sErrorMessage := 'Undefined State of operation; '; //I hope we will never see this message ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, UINT_TO_STRING(iStateError)); ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, ' '); END_CASE //Error messages according to the second least significant digit CASE iOtherError OF 16#0000 : ; //No error case 16#0010 : ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Slave signals error; '); 16#0020 : ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Invalid vendorID/productCode read; '); 16#0040 : ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Initialisation error occured; '); ELSE ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Undefined Error ID: '); ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, UINT_TO_STRING(iOtherError)); ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, ' '); END_CASE //Errormessage according to WcState bit IF bWcState THEN ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Invalid Data;'); END_IF //Check for overflow IF pErrorSystem^.nNoErrors = GVL_ErrorSystem.cSizeOfErrorData THEN pErrorSystem^.nNoOverflows := pErrorSystem^.nNoOverflows+1; END_IF //Write Error Data into Error System ErrorData.Error_ID := pErrorSystem^.lNextErrorID ; MEMMOVE( ADR(pErrorSystem^.aErrorData[1]), ADR(pErrorSystem^.aErrorData[0]), (GVL_ErrorSystem.cSizeOfErrorData-1) * SIZEOF(DUT_TerminalError)); pErrorSystem^.aErrorData[0] := ErrorData; pErrorSystem^.lNextErrorID := pErrorSystem^.lNextErrorID+1; ELSE //Close Active Error //Read system time ErrorData.nDateTimeOff := Tc2_EtherCAT.F_GetActualDcTime64(); ErrorData.sDateTimeOff := Tc2_EtherCat.DCTIME64_TO_STRING(ErrorData.nDateTimeOff); ErrorData.ErrorState := DUT_ErrorState.Inactive; //Write Off time to Error System FOR nErrSysCNT := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO IF pErrorSystem^.aErrorData[nErrSysCNT].Error_ID = ErrorData.Error_ID THEN pErrorSystem^.aErrorData[nErrSysCNT].nDateTimeOff := ErrorData.nDateTimeOff; pErrorSystem^.aErrorData[nErrSysCNT].sDateTimeOff := ErrorData.sDateTimeOff; pErrorSystem^.aErrorData[nErrSysCNT].ErrorState := DUT_ErrorState.Inactive; EXIT; END_IF END_FOR //Clear ErrorData MEMSET(ADR(ErrorData), 0, SIZEOF(ErrorData)); END_IF END_IF 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 Interactive ^^^^^^^^^^^ :: PROGRAM Interactive VAR bInit: BOOL := FALSE; M1: DUT_MotionStage; fbMotionStageSim: FB_MotionStageSim; nCounter: UINT; stOut: DUT_PositionState; fbGoOut: FB_PositionStateMove; bOut: BOOL; bGoOut: BOOL; stIn: DUT_PositionState; fbGoIn: FB_PositionStateMove; bIn: BOOL; bGoIn: BOOL; stUnsafe: DUT_PositionState; fbGoBad: FB_PositionStateMove; bHCF: BOOL; bGoHCF: BOOL; END_VAR BasicTests(); fbMotionStageSim(stMotionStage:=M1, nEnableMode:=ENUM_StageEnableMode.DURING_MOTION); nCounter := nCounter + 1; IF NOT bInit THEN bInit := TRUE; stOut.sName := 'Out'; stOut.fPosition := 100; stOut.fDelta := 20; stOut.fVelocity := 10; stOut.bValid := TRUE; stOut.bMoveOk := TRUE; stIn.sName := 'In'; stIn.fPosition := 0; stIn.fDelta := 0.1; stIn.fVelocity := 5; stIn.bValid := TRUE; stIn.bMoveOk := TRUE; stUnsafe.sName := 'HCF'; stUnsafe.fPosition := -999; stUnsafe.fDelta := 6; stUnsafe.fVelocity := 42; stUnsafe.bValid := TRUE; stUnsafe.bMoveOk := FALSE; END_IF fbGoOut( bExecute:=bGoOut, stMotionStage:=M1, stPositionState:=stOut, bAtState=>bOut); fbGoIn( bExecute:=bGoIn, stMotionStage:=M1, stPositionState:=stIn, bAtState=>bIn); fbGoBad( bExecute:=bGoHCF, stMotionStage:=M1, stPositionState:=stUnsafe, bAtState=>bHCF); END_PROGRAM