DUTs ---- DUT_AxisStatus_v0_01 ^^^^^^^^^^^^^^^^^^^^ :: TYPE DUT_AxisStatus_v0_01 : STRUCT bEnable: BOOL; bReset: BOOL; bExecute: BOOL; nCommand: UINT; nCmdData: UINT; fVelocity: LREAL; fPosition: LREAL; fAcceleration: LREAL; fDeceleration: LREAL; bJogFwd: BOOL; bJogBwd: BOOL; bLimitFwd: BOOL; bLimitBwd: BOOL; fOverride: LREAL := 100; bHomeSensor: BOOL; bEnabled: BOOL; bError: BOOL; nErrorId: UDINT; fActVelocity: LREAL; fActPosition: LREAL; fActDiff: LREAL; bHomed:BOOL; bBusy:BOOL; END_STRUCT END_TYPE Related: * `DUT_AxisStatus_v0_01`_ DUT_ErrorState ^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE DUT_ErrorState : ( None, Active, Inactive, Acknowledged ); END_TYPE DUT_MotionPneumaticActuator ^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'DUT_MotionPneumaticActuator has been renamed to ST_MotionPneumaticActuator'} TYPE DUT_MotionPneumaticActuator : ST_MotionPneumaticActuator; END_TYPE Related: * `ST_MotionPneumaticActuator`_ DUT_MotionStage ^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'DUT_MotionStage has been renamed to ST_MotionStage'} TYPE DUT_MotionStage : ST_MotionStage; END_TYPE Related: * `ST_MotionStage`_ DUT_PositionState ^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'DUT_PositionState has been renamed to ST_PositionState'} TYPE DUT_PositionState : ST_PositionState; END_TYPE Related: * `ST_PositionState`_ 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 Related: * `DUT_ErrorState`_ 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 E_EpicsHomeCmd ^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_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 Related: * `FB_MotionStage`_ E_EpicsInOut ^^^^^^^^^^^^ :: {attribute 'qualified_only'} // Example EPICS states enum for use in all versions of the states FBs // Remove strict attribute for easier handling TYPE E_EpicsInOut : ( UNKNOWN := 0, // UNKNOWN must be in slot 0 or the FB breaks OUT := 1, // OUT at slot 1 is a convention IN := 2 ) UINT; END_TYPE E_EpicsMotorCmd ^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_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 E_MotionFFType ^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} TYPE E_MotionFFType : ( STOPPER_FAULT := 16#1000, ZERO_RATE := 16#1001, BPTM_TIMEOUT := 16#1002, BPTM_ERROR := 16#1003, MAINT_MODE := 16#1004, NOT_A_STATE := 16#1005, INVALID_GOAL := 16#1006, TOO_MANY_TRIPS := 16#1007, BP_MISMATCH := 16#1008, INTERNAL_ERROR := 16#1009, PNEUMATIC_MOVE := 16#1010, MOT_GENERIC := 16#1011, LOW_RESERVED_NC := 16#2000, HIGH_RESERVED_NC := 16#20FF, DEVICE_MOVE := 16#2100 ) UINT := MOT_GENERIC; END_TYPE E_MotionRequest ^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_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 Related: * `FB_MotionRequest`_ E_PnuematicActuatorPositionState ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} // Defines the positions for a pnuematic actuator TYPE E_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 E_StageBrakeMode ^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_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 Related: * `FB_MotionStage`_ E_StageEnableMode ^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_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 Related: * `FB_MotionStage`_ E_StatePMPSStatus ^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_StatePMPSStatus : ( // No other enum state describes it UNKNOWN := 0, // Moving toward a known state TRANSITION := 1, // Within a known state, not trying to leave AT_STATE := 2, // PMPS is in some way disabled, either with maint mode or arbiter disable DISABLED := 3 ); END_TYPE E_TestStates ^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE E_TestStates : ( Unknown := 0, OUT := 1, TARGET1 := 2, TARGET2 := 3 ) UINT; END_TYPE EL5042_Status ^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'EL5042_Status has been renamed to ST_EL5042_Status'} TYPE EL5042_Status : ST_EL5042_Status; END_TYPE Related: * `ST_EL5042_Status`_ ENUM_EpicsHomeCmd ^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_EpicsHomeCmd'} TYPE ENUM_EpicsHomeCmd : E_EpicsHomeCmd; END_TYPE Related: * `E_EpicsHomeCmd`_ ENUM_EpicsInOut ^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_EpicsInOut'} TYPE ENUM_EpicsInOut : E_EpicsInOut; END_TYPE Related: * `E_EpicsInOut`_ ENUM_EpicsInOut_INT ^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use ENUM_EpicsInOut'} {attribute 'qualified_only'} // Example EPICS states enum for use in all versions of the states FBs // Remove strict attribute for easier handling TYPE ENUM_EpicsInOut_INT : ( UNKNOWN := 0, // UNKNOWN must be in slot 0 or the FB breaks OUT := 1, // OUT at slot 1 is a convention IN := 2 ) INT; END_TYPE Related: * `ENUM_EpicsInOut`_ ENUM_EpicsMotorCmd ^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_EpicsMotorCmd'} TYPE ENUM_EpicsMotorCmd : E_EpicsMotorCmd; END_TYPE Related: * `E_EpicsMotorCmd`_ ENUM_MotionFFType ^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_MotionFFType'} TYPE ENUM_MotionFFType : E_MotionFFType; END_TYPE Related: * `E_MotionFFType`_ ENUM_MotionRequest ^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_MotionRequest'} TYPE ENUM_MotionRequest : E_MotionRequest; END_TYPE Related: * `E_MotionRequest`_ ENUM_PnuematicActuatorPositionState ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_PnuematicActuatorPositionState'} TYPE ENUM_PnuematicActuatorPositionState : E_PnuematicActuatorPositionState; END_TYPE Related: * `E_PnuematicActuatorPositionState`_ ENUM_StageBrakeMode ^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_StageBrakeMode'} TYPE ENUM_StageBrakeMode : E_StageBrakeMode; END_TYPE Related: * `E_StageBrakeMode`_ ENUM_StageEnableMode ^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_StageEnableMode'} TYPE ENUM_StageEnableMode : E_StageEnableMode; END_TYPE Related: * `E_StageEnableMode`_ ENUM_StatePMPSStatus ^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use E_StatePMPSStatus'} TYPE ENUM_StatePMPSStatus : E_StatePMPSStatus; END_TYPE Related: * `E_StatePMPSStatus`_ ST_EL5042_Status ^^^^^^^^^^^^^^^^ :: TYPE ST_EL5042_Status : STRUCT END_STRUCT 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 Related: * `DUT_TerminalError`_ * `GVL`_ * `GVL_ErrorSystem`_ ST_MotionPneumaticActuator ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} {attribute 'strict'} TYPE ST_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: bInsertDigitalOutput; 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 : E_PnuematicActuatorPositionState := E_PnuematicActuatorPositionState.INVALID; END_STRUCT END_TYPE Related: * `E_PnuematicActuatorPositionState`_ ST_MotionStage ^^^^^^^^^^^^^^ :: TYPE ST_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 bLimitForwardEnable AT %I*: BOOL; // NC Backward Limit Switch: TRUE if ok to move bLimitBackwardEnable AT %I*: BOOL; // NO Home Switch: TRUE if at home bHome AT %I*: BOOL; // NC Brake Output: TRUE to release brake 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 bAllForwardEnable: BOOL:=FALSE; // Backward enable EPS summary bAllBackwardEnable: BOOL:=FALSE; // Enable EPS summary encapsulating emergency stop button and any additional motion preventive hardware bAllEnable: BOOL:=FALSE; // Forward virtual gantry limit switch bGantryForwardEnable: BOOL:=FALSE; // Backward virtual gantry limit switch bGantryBackwardEnable: BOOL:=FALSE; // Encoder count summary, if linked above {attribute 'pytmc' := ' pv: PLC:nEncoderCount io: i field: DESC Count from encoder hardware '} nEncoderCount: UDINT; // Forward Enable EPS struct {attribute 'pytmc' := ' pv: PLC:stEPSF io: i field: DESC Forward Enable Interlocks '} stEPSForwardEnable: DUT_EPS; // Backward Enable EPS struct {attribute 'pytmc' := ' pv: PLC:stEPSB io: i field: DESC Backward Enable Interlocks '} stEPSBackwardEnable: DUT_EPS; // Power Enable EPS struct {attribute 'pytmc' := ' pv: PLC:stEPSP io: i field: DESC Power Interlocks '} stEPSPowerEnable: DUT_EPS; (* Settings *) // Name to use for log messages, fast faults, etc. sName: STRING; // If TRUE, we want to enable the motor independently of PMPS or other safety systems. bPowerSelf: BOOL:=FALSE; // Determines when we automatically enable the motor nEnableMode: E_StageEnableMode:=E_StageEnableMode.DURING_MOTION; // Determines when we automatically disengage the brake nBrakeMode: E_StageBrakeMode:=E_StageBrakeMode.IF_ENABLED; // Determines our encoder homing strategy nHomingMode: E_EpicsHomeCmd:=E_EpicsHomeCmd.NONE; // Set true to activate gantry EPS 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 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 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 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 nCommand: INT; // Used internally and by the IOC to pass additional data to some commands nCmdData: INT; // Used internally and by the IOC to pick a destination for the move fPosition: LREAL; // Used internally and by the IOC to pick a move velocity fVelocity: LREAL; // Used internally and by the IOC to pick a move acceleration fAcceleration: LREAL; // Used internally and by the IOC to pick a move 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 nMotionAxisID: UDINT:=0; (* Returns *) // TRUE if done enabling bEnableDone: BOOL; // TRUE if in the middle of a command bBusy: BOOL; // TRUE if we've done a command and it has finished bDone: BOOL; // TRUE if the motor has been homed, or does not need to be homed bHomed: BOOL; // TRUE if we have safety permission to 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 are in an error state update: 100Hz notify '} bError: BOOL; // Error code if nonzero {attribute 'pytmc' := ' pv: PLC:nErrorId io: i field: DESC Error code if nonzero update: 100Hz notify '} nErrorId: UDINT; // Message to identify the error state {attribute 'pytmc' := ' pv: PLC:sErrorMessage io: i field: DESC Message to identify the error state update: 100Hz notify '} 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 Related: * `DUT_AxisStatus_v0_01`_ * `E_EpicsHomeCmd`_ * `E_StageBrakeMode`_ * `E_StageEnableMode`_ ST_PositionState ^^^^^^^^^^^^^^^^ :: TYPE ST_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 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 fAccel: LREAL; // (optional) 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 bLocked: BOOL; // Set this to TRUE when you make your state. This defaults to FALSE so that uninitialized states can never be moved to 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; // We give this a state name and it is used to load parameters from the pmps database. stPMPS: ST_DbStateParams; END_STRUCT END_TYPE Related: * `FB_PositionStateInternal`_ * `FB_PositionStateLock`_ 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: ST_EL5042_Status; // Status struct placeholder Ref: ULINT; // Encoder zero position (useful for aligned position with gantries) END_STRUCT END_TYPE Related: * `EL5042`_ * `ST_EL5042_Status`_ ST_StateEpicsToPlc ^^^^^^^^^^^^^^^^^^ :: TYPE ST_StateEpicsToPlc : (* This data structure contains the standard EPICS input connection points for the state movers. The data in this struct flows from EPICS to the PLC. It includes everything except the SET PV, which cannot be included here as it is sourced from enum values unique to the application, and the PMPS PVs, which are gathered in their own data type, ST_StatePMPSEpicsToPlc. That actually means that this only holds the RESET PV, for now. nSetValue here is actively used by state blocks even though it is not exposed directly to EPICS. You should avoid manually modifying it or else you may interfere with normal operations of the state function blocks. For including your own ENUM input, you should pytmc pragma the PV name to end in "SET", and match the prefix of this FB's "RESET" PV. Then, you can include your enum as the eEnumSet IN_OUT var and let the EPICS IOC handle it. *) STRUCT // For internal use only. This holds new goal positions as an integer, else it is 0 if there is no new state move request. It is written to from the user's input enum. nSetValue: UINT; // Set this to TRUE to acknowledge and clear an error. {attribute 'pytmc' := ' pv: RESET io: io field: ZNAM False field: ONAM True '} bReset: BOOL; END_STRUCT END_TYPE Related: * `ST_StatePMPSEpicsToPlc`_ ST_StatePlcToEpics ^^^^^^^^^^^^^^^^^^ :: TYPE ST_StatePlcToEpics : (* This data structure contains the standard EPICS connection points for the state movers. The data in this struct flows from the PLC to EPICS. It includes everything except the GET PV, which cannot be included here as it is sourced from enum values unique to the application, and the PMPS PVs, which are gathered in their own data type, ST_StatePMPSPlcToEpics. nGetValue here is actively used by state blocks even though it is not exposed directly to EPICS. It is safe to read this value, but you should avoid modifying it, which may interfere with normal operations of the state function blocks. For including your own ENUM input, you should pytmc pragma the PV name to end in "GET", and match the prefix of this FB's "DONE" PV. Then, you can include your enum as the eEnumGet IN_OUT var and let the EPICS IOC handle it. *) STRUCT // For internal use only. This holds the current position index as an integer, else it is 0 if we are changing states or not at any particular state. nGetValue: UINT; // This will be TRUE when we are in an active state move and FALSE otherwise. {attribute 'pytmc' := ' pv: BUSY io: i field: ZNAM False field: ONAM True '} bBusy: BOOL; // This will be TRUE after a move completes and FALSE otherwise. {attribute 'pytmc' := ' pv: DONE io: i field: ZNAM False field: ONAM True '} bDone: BOOL; // This will be TRUE if the most recent move had an error and FALSE otherwise. {attribute 'pytmc' := ' pv: ERR io: i field: ZNAM False field: ONAM True '} bError: BOOL; // This will be set to an NC error code during an error if one exists or left at 0 otherwise. {attribute 'pytmc' := ' pv: ERRID io: i '} nErrorID: UDINT; // This will be set to an appropriate error message during an error if one exists or left as an empty string otherwise. {attribute 'pytmc' := ' pv: ERRMSG io: i '} sErrorMsg: STRING; END_STRUCT END_TYPE Related: * `ST_StatePMPSPlcToEpics`_ ST_StatePMPSEpicsToPlc ^^^^^^^^^^^^^^^^^^^^^^ :: TYPE ST_StatePMPSEpicsToPlc : (* This data structure contains the standard PMPS EPICS connection points for the state movers. The data in this struct flows from EPICS to the PLC. *) STRUCT // User setting: TRUE to enable the arbiter, FALSE to disable it. {attribute 'pytmc' := ' pv: PMPS:ARB:ENABLE io: io '} bArbiterEnabled: BOOL := TRUE; // User setting: TRUE to enable maintenance mode (Fast fault, free motion), FALSE to disable it. {attribute 'pytmc' := ' pv: PMPS:MAINT io: io '} bMaintMode: BOOL; END_STRUCT END_TYPE ST_StatePMPSPlcToEpics ^^^^^^^^^^^^^^^^^^^^^^ :: TYPE ST_StatePMPSPlcToEpics : (* This data structure contains the standard PMPS EPICS connection points for the state movers. The data in this struct flows from the PLC to EPICS. *) STRUCT // The database entry for the transition state. This should always be present. {attribute 'pytmc' := ' pv: PMPS:TRANS io: i '} stTransitionDb: ST_DbStateParams; 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 := 4, iMinor := 0, iBuild := 4, iRevision := 0, sVersion := '4.0.4'); 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 // Global file reader instance, used in fbStandardPMPSDB fbPmpsFileReader: FB_JsonFileToJsonDoc; {attribute 'pytmc' := ' pv: @(PREFIX)DB io: io '} // Global DB handler, Must be called in PLC project to use the PMPS DB for a motion project fbStandardPMPSDB: FB_Standard_PMPSDB; // Debug, records the highest number of motors used in an ND states block in the PLC. Can be used to limit MotionConstants.MAX_STATE_MOTORS to save on memory usage and PV count. nMaxStateMotorCount: UINT; // Debug, records the highest state count in the PLC. Can be used to limit GeneralConstants.MAX_STATES to save on memory usage and PV count. nMaxStates: UINT; END_VAR Related: * `FB_Standard_PMPSDB`_ * `MotionConstants`_ MotionConstants ^^^^^^^^^^^^^^^ :: {attribute 'qualified_only'} (* Global Configurable Motion Constants These are reconfigurable at the project level. When reconfigured they are set prior to compilation and cannot be changed at runtime. *) VAR_GLOBAL CONSTANT (* Arbitary cap on multidimensional states to simplify statements for the compiler. This is reconfigurable at the project level and should be set to the highest number of motors used in a states block. If you are not sure how many motors are used per state block, check MOTION_GVL.nMaxStateMotorCount *) MAX_STATE_MOTORS: UINT := 3; END_VAR Related: * `MOTION_GVL`_ POUs ---- 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 Related: * `FB_TerminalError`_ * `ST_ErrorSystem`_ 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 This will only run properly if FB_PositionStateInternal has been called on the position state to initialize it. *) VAR_INPUT stMotionStage: ST_MotionStage; stPositionState: ST_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 Related: * `FB_PositionStateInternal`_ * `F_PosWithinDelta`_ * `ST_MotionStage`_ * `ST_PositionState`_ 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#42A0: msg:='Coupled axis error'; 16#4357: msg:='Negative limit hit'; 16#4358: msg:='Positive limit hit'; 16#4395: msg:='Set velocity not allowed'; 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'; 16#FFFF: msg:='Fake testing error'; // 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: ST_PositionState; END_VAR F_PosOverLowerBound := fPosition > (stPositionState.fPosition - ABS(stPositionState.fDelta)); END_FUNCTION Related: * `ST_PositionState`_ F_PosUnderUpperBound ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION F_PosUnderUpperBound : BOOL VAR_INPUT fPosition: LREAL; stPositionState: ST_PositionState; END_VAR F_PosUnderUpperBound := fPosition < (stPositionState.fPosition + ABS(stPositionState.fDelta)); END_FUNCTION Related: * `ST_PositionState`_ F_PosWithinDelta ^^^^^^^^^^^^^^^^ :: FUNCTION F_PosWithinDelta : BOOL VAR_INPUT fPosition: LREAL; stPositionState: ST_PositionState; END_VAR F_PosWithinDelta := F_PosOverLowerBound(fPosition, stPositionState) AND F_PosUnderUpperBound(fPosition, stPositionState); END_FUNCTION Related: * `F_PosOverLowerBound`_ * `F_PosUnderUpperBound`_ * `ST_PositionState`_ FB_AtPositionState_Test ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_AtPositionState_Test EXTENDS TcUnit.FB_TestSuite (* Test the following related helper functions: - F_PosOverLowerBound - F_PosUnderUpperBound - F_PosWithinDelta - F_AtPositionState *) VAR // For the multi-cycle tests stMotionStage: ST_MotionStage; fbMotionStage: FB_MotionStage; fbTestMove: FB_TestHelperSetAndMove; stPositionStateInactive: ST_PositionState; stPositionStateInvalid: ST_PositionState; stPositionStateGood: ST_PositionState; tonInactive: TON; fbInternalGood: FB_PositionStateInternal; fbInternalInvalid: FB_PositionStateInternal; nTestCounter: UINT; bOneTestDone: BOOL; END_VAR // Single cycle tests TestPosOverLowerBoundYes(); TestPosOverLowerBoundNo(); TestPosUnderUpperBoundYes(); TestPosUnderUpperBoundNo(); TestPosWithinDeltaTooLow(); TestPosWithinDeltaTooHigh(); TestPosWithinDeltaJustRight(); // Multi cycle tests fbMotionStage(stMotionStage:=stMotionStage); stPositionStateInactive.sName := 'Inactive'; stPositionStateInactive.fPosition := 30.0; stPositionStateInactive.fDelta := 1.0; stPositionStateInactive.bValid := TRUE; tonInactive( IN:=TRUE, PT:=T#1s, ); TestAtPositionStateWithoutInternal(); stPositionStateInvalid.sName := 'Invalid'; stPositionStateInvalid.fPosition := 40.0; stPositionStateInvalid.fDelta := 1.0; stPositionStateInvalid.bValid := FALSE; fbInternalInvalid( stMotionStage:=stMotionStage, stPositionState:=stPositionStateInvalid, ); TestAtPositionStateInvalid(); stPositionStateGood.sName := 'Good'; stPositionStateGood.fPosition := 50.0; stPositionStateGood.fDelta := 1.0; stPositionStateGood.bValid := TRUE; fbInternalGood( stMotionStage:=stMotionStage, stPositionState:=stPositionStateGood, ); TestAtPositionStateTooLow(); TestAtPositionStateTooHigh(); TestAtPositionStateJustRight(); TestAtPositionStateTweak(); TestAtPositionStateLeave(); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; fbTestMove( stMotionStage:=stMotionStage, bExecute:=FALSE, ); END_IF END_FUNCTION_BLOCK METHOD PRIVATE TestAtPositionStateInvalid (* If a position state is invalid, it is never the right state. *) VAR END_VAR TEST('TestAtPositionStateInvalid'); IF nTestCounter <> 1 THEN RETURN; END_IF fbTestMove( stMotionStage:=stMotionstage, bExecute:=TRUE, fStartPosition:=stPositionStateInvalid.fPosition, fGoalPosition:=stPositionStateInvalid.fPosition, ); IF fbTestMove.tElapsed > T#1s OR (stPositionStateInvalid.bUpdated AND fbTestMove.bSetDone) THEN AssertFalse( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateInvalid, ), Message:='Invalid state was marked OK', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#1s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestAtPositionStateJustRight VAR fLocalGoal: LREAL; END_VAR TEST('TestAtPositionStateJustRight'); IF nTestCounter <> 4 THEN RETURN; END_IF fLocalGoal := stPositionStateGood.fPosition + 0.2*stPositionStateGood.fDelta; fbTestMove( stMotionStage:=stMotionstage, bExecute:=TRUE, fStartPosition:=fLocalGoal, fGoalPosition:=fLocalGoal, ); IF fbTestMove.tElapsed > T#1s OR (stPositionStateGood.bUpdated AND fbTestMove.bSetDone) THEN AssertTrue( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateGood, ), Message:='Within delta counted as outside range', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#1s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestAtPositionStateLeave (* A move away from a state should be not at the state, even before we've left *) VAR END_VAR TEST('TestAtPositionStateLeave'); IF nTestCounter <> 6 THEN RETURN; END_IF fbTestMove( stMotionStage:=stMotionStage, bExecute:=TRUE, fStartPosition:=stPositionStateGood.fPosition, fGoalPosition:=stPositionStateGood.fPosition + 100 * stPositionStateGood.fDelta, fVelocity:=0.001, bHWEnable:=TRUE, ); IF fbTestMove.tElapsed > T#5s OR (stPositionStateGood.bUpdated AND fbTestMove.bMotionStarted) THEN AssertTrue( Condition:=fbTestMove.fActPosition < stPositionStateGood.fPosition + stPositionStateGood.fDelta, Message:='We must be at the state location still to properly run this test.', ); AssertFalse( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateGood, ), Message:='Leaving state is not at state once the move starts', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#5s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestAtPositionStateTooHigh VAR fLocalGoal: LREAL; END_VAR TEST('TestAtPositionStateTooHigh'); IF nTestCounter <> 3 THEN RETURN; END_IF fLocalGoal := stPositionStateGood.fPosition + 2*stPositionStateGood.fDelta; fbTestMove( stMotionStage:=stMotionstage, bExecute:=TRUE, fStartPosition:=fLocalGoal, fGoalPosition:=fLocalGoal, ); IF fbTestMove.tElapsed > T#1s OR (stPositionStateGood.bUpdated AND fbTestMove.bSetDone) THEN AssertFalse( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateGood, ), Message:='Above delta counted as in range', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#1s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestAtPositionStateTooLow VAR fLocalGoal: LREAL; END_VAR TEST('TestAtPositionStateTooLow'); IF nTestCounter <> 2 THEN RETURN; END_IF fLocalGoal := stPositionStateGood.fPosition - 2*stPositionStateGood.fDelta; fbTestMove( stMotionStage:=stMotionstage, bExecute:=TRUE, fStartPosition:=fLocalGoal, fGoalPosition:=fLocalGoal, ); IF fbTestMove.tElapsed > T#1s OR (stPositionStateGood.bUpdated AND fbTestMove.bSetDone) THEN AssertFalse( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateGood, ), Message:='Below delta counted as in range', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#1s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestAtPositionStateTweak (* A small tweak move within the delta of a position state should be OK *) VAR END_VAR TEST('TestAtPositionStateTweak'); IF nTestCounter <> 5 THEN RETURN; END_IF fbTestMove( stMotionStage:=stMotionStage, bExecute:=TRUE, fStartPosition:=stPositionStateGood.fPosition, fGoalPosition:=stPositionStateGood.fPosition + 0.9 * stPositionStateGood.fDelta, fVelocity:=0.001, bHWEnable:=TRUE, ); IF fbTestMove.tElapsed > T#5s OR (stPositionStateGood.bUpdated AND fbTestMove.bMotionStarted) THEN AssertTrue( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateGood, ), Message:='Small tweak in range should count as at state', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#5s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestAtPositionStateWithoutInternal (* If a position state is never updated by the internal FB, it is never the right state. *) VAR END_VAR TEST('TestAtPositionStateWithoutInternal'); IF nTestCounter <> 0 THEN RETURN; END_IF fbTestMove( stMotionStage:=stMotionstage, bExecute:=TRUE, fStartPosition:=stPositionStateInactive.fPosition, fGoalPosition:=stPositionStateInactive.fPosition, ); IF fbTestMove.tElapsed > T#1s OR (stPositionStateInvalid.bUpdated AND fbTestMove.bSetDone) THEN // Check for a different state to be bUpdated for timing purposes, this one never gets bUpdated AssertFalse( Condition:=F_AtPositionState( stMotionStage:=stMotionStage, stPositionState:=stPositionStateInactive, ), Message:='This is the control group, internal was not run so this should not work', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#1s, Message:='Timeout in multi cycle test', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestPosOverLowerBoundNo VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosOverLowerBoundNo'); stPositionState.fPosition := 10; stPositionState.fDelta := 1; AssertFalse( Condition:=F_PosOverLowerBound( fPosition:=8.5, stPositionState:=stPositionState, ), Message:='Decided 8.5 > 9.0', ); TEST_FINISHED(); END_METHOD METHOD PRIVATE TestPosOverLowerBoundYes VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosOverLowerBoundYes'); stPositionState.fPosition := 10; stPositionState.fDelta := 1; AssertTrue( Condition:=F_PosOverLowerBound( fPosition:=9.1, stPositionState:=stPositionState, ), Message:='Decided 9.0 > 9.1', ); TEST_FINISHED(); END_METHOD METHOD PRIVATE TestPosUnderUpperBoundNo VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosUnderUpperBoundNo'); stPositionState.fPosition := 10; stPositionState.fDelta := 1; AssertFalse( Condition:=F_PosUnderUpperBound( fPosition:=12.0, stPositionState:=stPositionState, ), Message:='Decided 11.0 > 12.0', ); TEST_FINISHED(); END_METHOD METHOD PRIVATE TestPosUnderUpperBoundYes VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosUnderUpperBoundYes'); stPositionState.fPosition := 10; stPositionState.fDelta := 1; AssertTrue( Condition:=F_PosUnderUpperBound( fPosition:=10.9, stPositionState:=stPositionState, ), Message:='Decided 10.9 > 11.0', ); TEST_FINISHED(); END_METHOD METHOD PRIVATE TestPosWithinDeltaJustRight VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosWithinDeltaJustRight'); stPositionState.fPosition := 20; stPositionState.fDelta := 1; AssertTrue( Condition:=F_PosWithinDelta( fPosition:=20.2, stPositionState:=stPositionState, ), Message:='Decided 20.2 not within 19 to 21 bounds', ); TEST_FINISHED(); END_METHOD METHOD PRIVATE TestPosWithinDeltaTooHigh VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosWithinDeltaTooHigh'); stPositionState.fPosition := 20; stPositionState.fDelta := 1; AssertFalse( Condition:=F_PosWithinDelta( fPosition:=25.0, stPositionState:=stPositionState, ), Message:='Decided 21.0 > 25.0', ); TEST_FINISHED(); END_METHOD METHOD PRIVATE TestPosWithinDeltaTooLow VAR stPositionState: ST_PositionState; END_VAR TEST('TestPosWithinDeltaTooLow'); stPositionState.fPosition := 20; stPositionState.fDelta := 1; AssertFalse( Condition:=F_PosWithinDelta( fPosition:=12.0, stPositionState:=stPositionState, ), Message:='Decided 12.0 > 19.0', ); TEST_FINISHED(); END_METHOD Related: * `FB_MotionStage`_ * `FB_PositionStateInternal`_ * `FB_TestHelperSetAndMove`_ * `F_AtPositionState`_ * `F_PosOverLowerBound`_ * `F_PosUnderUpperBound`_ * `F_PosWithinDelta`_ * `ST_MotionStage`_ * `ST_PositionState`_ 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_CauseNCError ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_CauseNCError (* Simulate an NC error. This will look like a real NC error for everyone, including TwinCAT itself. *) VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_INPUT bExecute: BOOL; nErrorCode: UDINT; END_VAR VAR_OUTPUT bBusy: BOOL; bDone: BOOL; END_VAR VAR rtExec: R_TRIG; adsWrite: ADSWRITE; mcReadDriveAddress: MC_ReadDriveAddress; END_VAR rtExec(CLK:=bExecute); IF NOT bExecute THEN bDone := FALSE; END_IF IF rtExec.Q THEN bBusy := TRUE; END_IF IF bBusy AND NOT mcReadDriveAddress.Done THEN mcReadDriveAddress( Axis:=Axis, Execute:=TRUE, DriveAddress=>Axis.DriveAddress); END_IF IF bBusy AND mcReadDriveAddress.Done THEN bBusy := TRUE; adsWrite( PORT:=501, IDXGRP:=16#4200 + Axis.DriveAddress.NcDriveId, IDXOFFS:=16#0019, LEN:=SIZEOF(nErrorCode), SRCADDR:=ADR(nErrorCode), WRITE:=TRUE, BUSY=>bBusy); IF NOT bBusy THEN bDone := TRUE; adsWrite(WRITE:=FALSE); 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 Related: * `DUT_AxisStatus_v0_01`_ * `FB_HomeVirtual`_ 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: ST_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 Related: * `FB_NCErrorFFO`_ * `ST_MotionStage`_ FB_EncoderValue ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EncoderValue (* Process the encoder value for ST_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: ST_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 Related: * `ST_MotionStage`_ FB_EncSaveRestore ^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_EncSaveRestore VAR_IN_OUT stMotionStage: ST_MotionStage; END_VAR VAR_INPUT bEnable: BOOL; END_VAR VAR_OUTPUT END_VAR VAR fbSetPos: MC_SetPosition; timer: TON; bInit: BOOL; bLoad: BOOL; nLatchError: UDINT; bEncError: BOOL; tRetryDelay: TIME := T#1s; nMaxRetries: UINT := 10; nCurrTries: UINT := 0; bWaitRetry: BOOL; tonRetry: TON; 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; nCurrTries := nCurrTries + 1; IF nCurrTries >= nMaxRetries THEN // Alert the user that something has gone wrong stMotionStage.bError := TRUE; stMotionStage.nErrorId := nLatchError; stMotionStage.sCustomErrorMessage := 'Error loading previously saved position.'; ELSE // Reset the FB for the next retry fbSetPos( Axis:=stMotionStage.Axis, Execute:=FALSE, Position:=fPosition); // Try again bWaitRetry := TRUE; END_IF END_IF tonRetry( IN := bWaitRetry, PT := tRetryDelay); bLoad S= tonRetry.Q; bWaitRetry R= tonRetry.Q; // Check ST_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 AND NOT bWaitRetry 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 Related: * `ST_MotionStage`_ FB_EpicsInOut ^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionState1D_InOut instead'} 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: ST_MotionStage; // Information about the OUT position stOut: ST_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN parameter stIn: ST_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_INT; // 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_INT; // NOTE: Please copy this pragma exactly on your enumGet END_VAR VAR bInit: BOOL; arrStates: ARRAY[1..15] OF ST_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 Related: * `ENUM_EpicsInOut_INT`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateManager`_ * `ST_MotionStage`_ * `ST_PositionState`_ 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 Related: * `DUT_ErrorState`_ * `DUT_TerminalError`_ * `GVL_ErrorSystem`_ * `ST_ErrorSystem`_ FB_GantryAutoCoupling ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_GantryAutoCoupling VAR_INPUT nGantryTol : LINT; END_VAR VAR_OUTPUT bGantryAlreadyCoupled : BOOL; END_VAR VAR_IN_OUT Master : ST_MotionStage; MasterEnc : ST_RenishawAbsEnc; Slave : ST_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 Related: * `FB_GantryDiffVirtualLimitSwitch`_ * `FB_SetEnables`_ * `ST_MotionStage`_ * `ST_RenishawAbsEnc`_ 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 Related: * `FB_MotionStage`_ * `ST_RenishawAbsEnc`_ FB_HomeDirect ^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeDirect VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; fHomePosition:LREAL; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bHomed:BOOL; bError: BOOL; nErrorId: UDINT; END_VAR VAR fbHome: MC_Home; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF fbHome( Execute:=bExecute, Position:=fHomePosition, HomingMode:=MC_Direct, bCalibrationCam:=FALSE, Axis:=Axis ); bBusy:=fbHome.Busy; bDone:=fbHome.Done; bHomed:=Axis.Status.Homed; bError:=fbHome.Error; IF fbHome.Error THEN nErrorId:=fbHome.ErrorID; END_IF END_FUNCTION_BLOCK FB_HomeFinish ^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeFinish VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; nCmdData: UINT; bSofLimEnableLow: BOOL:=TRUE; bSofLimEnableHigh: BOOL:=TRUE; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorId: UDINT; END_VAR VAR fbHomewriteSoftLimEnable:FB_HomeWriteSoftLimEnable; fbExecuteRiseEdge: R_TRIG; bExecuteWriteNC:BOOL:=FALSE; nState:INT:=0; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF IF NOT bExecute THEN bExecuteWriteNC:=FALSE; fbHomewriteSoftLimEnable.bSofLimEnableLow:=bSofLimEnableLow; fbHomewriteSoftLimEnable.bSofLimEnableHigh:=bSofLimEnableHigh; nState:=0; END_IF fbExecuteRiseEdge(CLK:=bExecute); IF fbExecuteRiseEdge.Q THEN bExecuteWriteNC:=TRUE; fbHomewriteSoftLimEnable.bSofLimEnableLow:=bSofLimEnableLow; fbHomewriteSoftLimEnable.bSofLimEnableHigh:=bSofLimEnableHigh; END_IF // Write to NC (disable soft limits) fbHomewriteSoftLimEnable( En:=En, bExecute:=bExecuteWriteNC AND bExecute, Axis:=Axis, bReset:=bReset, ); bBusy:=fbHomewriteSoftLimEnable.bBusy; bDone:=fbHomewriteSoftLimEnable.bDone; bError:=fbHomewriteSoftLimEnable.bError; IF fbHomewriteSoftLimEnable.bError THEN nErrorId:=fbHomewriteSoftLimEnable.nErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_HomeWriteSoftLimEnable`_ FB_HomePrepare ^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomePrepare VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; nCmdData: UINT; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorId: UDINT; bSofLimEnableLowOriginal: BOOL:=TRUE; bSofLimEnableHighOriginal: BOOL:=TRUE; fVelocityToCam: LREAL:=0; fVelocityFromCam: LREAL:=0; END_VAR VAR fbHomeReadSoftLimEnable:FB_HomeReadSoftLimEnable; fbHomeDisableSoftLimEnable:FB_HomeWriteSoftLimEnable; fbHomeReadNCVelocities: FB_HomeReadNcVelocities; fbHomeResetCalibrationFlag:MC_Home; //Only used for reset of calibration flag fbExecuteRiseEdge: R_TRIG; bExecuteReadNC:BOOL:=FALSE; bExecuteWriteNC:BOOL:=FALSE; nState:INT:=0; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF IF NOT bExecute THEN bExecuteReadNC:=FALSE; bExecuteWriteNC:=FALSE; nState:=0; END_IF fbExecuteRiseEdge(CLK:=bExecute); IF fbExecuteRiseEdge.Q THEN bExecuteReadNC:=TRUE; END_IF // Read from NC fbHomeReadNCVelocities( En:=En, bExecute:=bExecuteReadNC, // Actualy not needed for sequence 15 (set position only, no movement)) bReset:=bReset, Axis:=Axis, ); fbHomeReadSoftLimEnable( En:=En, bExecute:=bExecuteReadNC AND bExecute, Axis:=Axis, bReset:=bReset, ); // Reset calibration flag fbHomeResetCalibrationFlag( Execute:=bExecuteReadNC, HomingMode:=MC_ResetCalibration, Axis:=Axis ); bSofLimEnableLowOriginal:=fbHomeReadSoftLimEnable.bSofLimEnableLow; bSofLimEnableHighOriginal:=fbHomeReadSoftLimEnable.bSofLimEnableHigh; fVelocityToCam:=fbHomeReadNCVelocities.fVelocityToCam; fVelocityFromCam:=fbHomeReadNCVelocities.fVelocityFromCam; IF bExecuteReadNC AND bExecute AND fbHomeReadSoftLimEnable.bDone THEN fbHomeDisableSoftLimEnable.bSofLimEnableHigh:=FALSE; fbHomeDisableSoftLimEnable.bSofLimEnableLow:=FALSE; bExecuteWriteNC:=TRUE; //Always write (only needed if enabled actually) END_IF // Write to NC (disable soft limits) fbHomeDisableSoftLimEnable( En:=En, bExecute:=bExecuteWriteNC AND bExecute, Axis:=Axis, bReset:=bReset, ); bBusy:=fbHomeReadSoftLimEnable.bBusy OR fbHomeDisableSoftLimEnable.bBusy OR fbHomeReadNCVelocities.bBusy OR fbHomeResetCalibrationFlag.Busy; bDone:=fbHomeReadSoftLimEnable.bDone AND fbHomeDisableSoftLimEnable.bDone AND fbHomeReadNCVelocities.bDone AND fbHomeResetCalibrationFlag.Done AND bExecute; bError:=fbHomeReadSoftLimEnable.bError OR fbHomeDisableSoftLimEnable.bError OR fbHomeReadNCVelocities.bError OR fbHomeResetCalibrationFlag.Error; IF fbHomeReadSoftLimEnable.bError THEN nErrorId:=fbHomeReadSoftLimEnable.nErrorId; ELSIF fbHomeDisableSoftLimEnable.bError THEN nErrorId:=fbHomeDisableSoftLimEnable.nErrorId; ELSIF fbHomeResetCalibrationFlag.Error THEN nErrorId:=fbHomeResetCalibrationFlag.ErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_HomeReadNcVelocities`_ * `FB_HomeReadSoftLimEnable`_ * `FB_HomeWriteSoftLimEnable`_ FB_HomeReadNcVelocities ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeReadNcVelocities VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorId: UDINT; fVelocityToCam: LREAL; fVelocityFromCam: LREAL; END_VAR VAR fbReadVelocityToCam:FB_ReadFloatParameter; fbReadVelocityFromCam:FB_ReadFloatParameter; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF fbReadVelocityToCam( bExecute:=bExecute, nDeviceGroup:= 16#4000, nIndexOffset:= 16#6, Axis:= Axis); fbReadVelocityFromCam( bExecute:=bExecute, nDeviceGroup:= 16#4000, nIndexOffset:= 16#7, Axis:= Axis); fVelocityToCam:=fbReadVelocityToCam.nData; fVelocityFromCam:=fbReadVelocityFromCam.nData; bBusy:=fbReadVelocityFromCam.bBusy OR fbReadVelocityToCam.bBusy; bDone:=fbReadVelocityFromCam.bDone AND fbReadVelocityToCam.bDone AND bExecute; bError:=fbReadVelocityToCam.bError OR fbReadVelocityFromCam.bError; IF fbReadVelocityToCam.bError THEN nErrorId:=fbReadVelocityToCam.nErrorId; ELSIF fbReadVelocityFromCam.bError THEN nErrorId:=fbReadVelocityFromCam.nErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_ReadFloatParameter`_ FB_HomeReadSoftLimEnable ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeReadSoftLimEnable VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorId: UDINT; bSofLimEnableLow: BOOL:=TRUE; bSofLimEnableHigh: BOOL:=TRUE; END_VAR VAR fbReadSoftLimEnableLow:FB_ReadParameterInNc_v1_00; fbReadSoftLimEnableHigh:FB_ReadParameterInNc_v1_00; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF fbReadSoftLimEnableLow( bExecute:=bExecute, nDeviceGroup:= 16#5000, nIndexOffset:= 16#B, Axis:= Axis); fbReadSoftLimEnableHigh( bExecute:=bExecute, nDeviceGroup:= 16#5000, nIndexOffset:= 16#C, Axis:= Axis); bSofLimEnableLow:=DWORD_TO_BOOL(fbReadSoftLimEnableLow.nData); bSofLimEnableHigh:=DWORD_TO_BOOL(fbReadSoftLimEnableHigh.nData); bBusy:=fbReadSoftLimEnableLow.bBusy OR fbReadSoftLimEnableHigh.bBusy; bDone:=fbReadSoftLimEnableLow.bDone AND fbReadSoftLimEnableHigh.bDone AND bExecute; bError:=fbReadSoftLimEnableLow.bError OR fbReadSoftLimEnableHigh.bError; IF fbReadSoftLimEnableLow.bError THEN nErrorId:=fbReadSoftLimEnableLow.nErrorId; ELSIF fbReadSoftLimEnableHigh.bError THEN nErrorId:=fbReadSoftLimEnableHigh.nErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_ReadParameterInNc_v1_00`_ FB_HomeToSwitch ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeToSwitch VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; bCamSensor:BOOL; nSearchDirTwoardsCam: MC_Direction; nSearchDirOffCam: MC_Direction; fHomePosition:LREAL; fVelocityToCamNC: LREAL; //Velcoity when searching for cam fVelocityFromCamNC: LREAL; // Velocity after found cam (searching for next signal transition) END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bHomed:BOOL; bError: BOOL; nErrorId: UDINT; END_VAR VAR fbHome: MC_Home; fbWriteHomeDirCamToNC:FB_WriteParameterInNc_v1_00; fbWriteHomeDirSyncToNC:FB_WriteParameterInNc_v1_00; fbWriteHomeModeToNC:FB_WriteParameterInNc_v1_00; fbWriteHomeVelocitiesToNC: FB_HomeWriteNcVelocities; bConfigNCDone:BOOL:=FALSE; fbRTrigg: R_TRIG; END_VAR En:=EnO; IF bReset THEN bConfigNCDone:=FALSE; bError:=FALSE; nErrorId:=0; END_IF //Start preparation of NC if rising edge on bExecute fbRTrigg(CLK:=bExecute); IF fbRTrigg.Q THEN bConfigNCDone:=FALSE; END_IF fbWriteHomeDirCamToNC( bExecute:=bExecute AND NOT bConfigNCDone, nDeviceGroup:=16#5000, nIndexOffset:=16#101, //Direction for Calibration Cam Search nData:=BOOL_TO_DWORD(nSearchDirTwoardsCam=MC_Negative_Direction),//BOOL_TO_DWORD(NOT bSearchDirTwoardsCam), Axis:=Axis ); fbWriteHomeDirSyncToNC( bExecute:= bExecute AND NOT bConfigNCDone, nDeviceGroup:=16#5000 , nIndexOffset:=16#102 , //Direction for Sync Impuls Search nData:=BOOL_TO_DWORD(nSearchDirOffCam=MC_Negative_Direction),//BOOL_TO_DWORD(NOT bSearchDirOffCam), Axis:= Axis ); fbWriteHomeModeToNC( bExecute:=bExecute AND NOT bConfigNCDone, nDeviceGroup:=16#5000, nIndexOffset:=16#107, //Reference Mode nData:=1, Axis:=axis); fbWriteHomeVelocitiesToNC( En:=En, bExecute:=bExecute AND NOT bConfigNCDone, bReset:=bReset, fVelocityFromCam:=fVelocityFromCamNC, fVelocityToCam:=fVelocityToCamNC, Axis:=Axis); fbHome.bCalibrationCam:=bCamSensor; fbHome( Execute:=bExecute AND bConfigNCDone(* AND NOT bError*), Position:=fHomePosition, HomingMode:=0, Axis:=Axis ); bBusy:=(fbHome.Busy OR (NOT bConfigNCDone AND bExecute)); bDone:=fbHome.Done AND bConfigNCDone; bHomed:=Axis.Status.Homed; IF (NOT bConfigNCDone) AND fbWriteHomeDirCamToNC.bDone AND fbWriteHomeDirSyncToNC.bDone AND fbWriteHomeModeToNC.bDone AND fbWriteHomeVelocitiesToNC.bDone THEN bConfigNCDone:=TRUE; END_IF //For some reason MC_HOME gives an Error for one cycle of NC-Task 1 SVB (10ms default..) so filter with bExecute bError:=(fbHome.Error AND bExecute) OR fbWriteHomeDirCamToNC.bError OR fbWriteHomeDirSyncToNC.bError OR fbWriteHomeModeToNC.bError OR fbWriteHomeVelocitiesToNC.bError; IF (fbHome.Error AND bExecute) THEN nErrorId:=fbHome.ErrorID; ELSIF fbWriteHomeDirCamToNC.bError THEN nErrorId:=fbWriteHomeDirCamToNC.nErrorId; ELSIF fbWriteHomeDirSyncToNC.bError THEN nErrorId:=fbWriteHomeDirSyncToNC.nErrorId; ELSIF fbWriteHomeModeToNC.bError THEN nErrorId:=fbWriteHomeModeToNC.nErrorId; ELSIF fbWriteHomeVelocitiesToNC.bError THEN nErrorId:=fbWriteHomeVelocitiesToNC.nErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_HomeWriteNcVelocities`_ * `FB_WriteParameterInNc_v1_00`_ FB_HomeVirtual ^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeVirtual VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; nCmdData: UINT; bLimitFwd: BOOL; bLimitBwd: BOOL; bHomeSensor: BOOL; fHomePosition:LREAL; nHomeRevOffset: UINT; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bHomed:BOOL; bError: BOOL; nErrorId: UDINT; END_VAR VAR fbHomeToSwitch: FB_HomeToSwitch; fbHomeDirect: FB_HomeDirect; //Only used for direct homing (set of position) fbMoveVelocity:MC_MoveVelocity; fbHomePrepare:FB_HomePrepare; fbHomeFinish:FB_HomeFinish; fbExecuteRiseEdge: R_TRIG; nHomingState:INT:=0; bExecuteHomeToSwitch:BOOL:=FALSE; bExecuteMoveVelocity:BOOL:=FALSE; bExecutePrepare: BOOL:=FALSE; bExecuteFinish: BOOL:=FALSE; bExecuteHomeDirect: BOOL; nCmdDataLocal: UINT; //Ensure that nCmdData is not changed during sequence bSequenceReady:BOOL:=TRUE; bRestoreNCDataNeeded: BOOL:=FALSE; END_VAR EnO:=En; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF // Reset when bExecute is low IF NOT bExecute THEN nHomingState:=0; bSequenceReady:=TRUE; bExecuteHomeToSwitch:=FALSE; bExecuteHomeDirect:=FALSE; bExecuteMoveVelocity:=FALSE; bExecutePrepare:=FALSE; bExecuteFinish:=FALSE; END_IF //Reset at rinsing edge of bExecute fbExecuteRiseEdge(CLK:=bExecute); IF fbExecuteRiseEdge.Q THEN nCmdDataLocal:=nCmdData; //Ensure that nCmdData is not changed during sequence (nCmdData will only be read at a rising edge of bExecute) bSequenceReady:=FALSE; bExecutePrepare:=TRUE; bRestoreNCDataNeeded:=FALSE; //Check if valid nCmdDataLocal CASE nCmdDataLocal OF 1: 2: 3: 4: 15: ELSE //nCmdData not valid bError:=TRUE; nErrorId:=16#4FFF; END_CASE END_IF //############# Prepare for homing (Read from NC and reset homed flag) fbHomePrepare( En:=En, bExecute:=bExecutePrepare AND NOT bError, // Not needed for sequence 15 (set position only, no movement)) bReset:=bReset, Axis:=Axis, ); //############# Homing Sequences: CASE nCmdDataLocal OF 1: // Home to low limit switch CASE nHomingState OF 0: bHomed:=Axis.Status.Homed; // Wait for read of velocities from NC and reset of calibration flag IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN bRestoreNCDataNeeded:=TRUE; IF bLimitBwd THEN nHomingState:=1; ELSE nHomingState:=2; //Standing on limit switch go direct to state 2 END_IF END_IF 1: // wait for reach low limit then trigger fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam); fbMoveVelocity.Direction:=MC_Negative_Direction; bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity IF NOT bLimitBwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends). nHomingState:=2; END_IF 2: // Wait for fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; bExecuteMoveVelocity:=FALSE; bExecuteHomeToSwitch:=TRUE; fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Positive_Direction; fbHomeToSwitch.nSearchDirOffCam:=MC_Positive_Direction; fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed fbHomeToSwitch.bCamSensor:=NOT bLimitBwd; IF fbHomeToSwitch.bDone THEN nHomingState:=3; bExecuteFinish:=TRUE; fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal; fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal; END_IF; 3: // restore softlimit enable bHomed:=FALSE; bSequenceReady:=FALSE; IF fbHomeFinish.bDone THEN bRestoreNCDataNeeded:=FALSE; bSequenceReady:=TRUE; nHomingState:=0; bHomed:=Axis.Status.Homed; END_IF; END_CASE; 2: // Home to high limit switch CASE nHomingState OF 0: bHomed:=Axis.Status.Homed; // Wait for read of velocities from NC and reset of calibration flag IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN bRestoreNCDataNeeded:=TRUE; IF bLimitFwd THEN nHomingState:=1; ELSE nHomingState:=2; //Standing on limit switch go direct to state 2 END_IF END_IF 1: // wait for reach low limit then trigger fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam); fbMoveVelocity.Direction:=MC_Positive_Direction; bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity IF NOT bLimitFwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends). nHomingState:=2; END_IF 2: // Wait for fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; bExecuteMoveVelocity:=FALSE; bExecuteHomeToSwitch:=TRUE; fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Negative_Direction; fbHomeToSwitch.nSearchDirOffCam:=MC_Negative_Direction; fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed fbHomeToSwitch.bCamSensor:=NOT bLimitFwd; IF fbHomeToSwitch.bDone THEN nHomingState:=3; bExecuteFinish:=TRUE; fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal; fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal; END_IF; 3: // restore softlimit enable bHomed:=FALSE; bSequenceReady:=FALSE; IF fbHomeFinish.bDone THEN bRestoreNCDataNeeded:=FALSE; bSequenceReady:=TRUE; nHomingState:=0; bHomed:=Axis.Status.Homed; END_IF; END_CASE; 3: // Home on bHomeSensor via bLimitBwd CASE nHomingState OF 0: bHomed:=Axis.Status.Homed; // Wait for read of velocities from NC and reset of calibration flag IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN bRestoreNCDataNeeded:=TRUE; IF bLimitBwd THEN nHomingState:=1; ELSE nHomingState:=2; //Standing on limit switch go direct to state 2 END_IF END_IF 1: // wait for reach low limit then trigger fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam); fbMoveVelocity.Direction:=MC_Negative_Direction; bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity IF NOT bLimitBwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends). nHomingState:=2; END_IF 2: // Wait for fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; bExecuteMoveVelocity:=FALSE; bExecuteHomeToSwitch:=TRUE; fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Positive_Direction; fbHomeToSwitch.nSearchDirOffCam:=MC_Positive_Direction; fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed fbHomeToSwitch.bCamSensor:=bHomeSensor; IF fbHomeToSwitch.bDone THEN nHomingState:=3; bExecuteFinish:=TRUE; fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal; fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal; END_IF; 3: // restore softlimit enable bHomed:=FALSE; bSequenceReady:=FALSE; IF fbHomeFinish.bDone THEN bRestoreNCDataNeeded:=FALSE; bSequenceReady:=TRUE; nHomingState:=0; bHomed:=Axis.Status.Homed; END_IF; END_CASE; 4: // Home on bHomeSensor via bLimitFwd CASE nHomingState OF 0: bHomed:=Axis.Status.Homed; // Wait for read of velocities from NC and reset of calibration flag IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN bRestoreNCDataNeeded:=TRUE; IF bLimitFwd THEN nHomingState:=1; ELSE nHomingState:=2; //Standing on limit switch go direct to state 2 END_IF END_IF 1: // wait for reach low limit then trigger fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam); fbMoveVelocity.Direction:=MC_Positive_Direction; bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity IF NOT bLimitFwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends). nHomingState:=2; END_IF 2: // Wait for fbHomeToSwitch bHomed:=FALSE; bSequenceReady:=FALSE; bExecuteMoveVelocity:=FALSE; bExecuteHomeToSwitch:=TRUE; fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Negative_Direction; fbHomeToSwitch.nSearchDirOffCam:=MC_Negative_Direction; fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed fbHomeToSwitch.bCamSensor:=bHomeSensor; IF fbHomeToSwitch.bDone THEN nHomingState:=3; bExecuteFinish:=TRUE; fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal; fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal; END_IF; 3: // Restore softlimit enable bHomed:=FALSE; bSequenceReady:=FALSE; IF fbHomeFinish.bDone THEN bRestoreNCDataNeeded:=FALSE; bSequenceReady:=TRUE; nHomingState:=0; bHomed:=Axis.Status.Homed; END_IF; END_CASE; 15: //Set current position (simplest homing sequence) bExecuteHomeDirect:=bExecute; bHomed:=Axis.Status.Homed; IF fbHomeDirect.bDone THEN //Homing ready bExecuteHomeDirect:=FALSE; bSequenceReady:=TRUE; END_IF ELSE fbHomeToSwitch.bCamSensor:=FALSE; bHomed:=Axis.Status.Homed; END_CASE; // Main homing block fbHomeToSwitch( bExecute:=bExecuteHomeToSwitch AND bExecute AND NOT bError AND NOT bExecuteHomeDirect AND NOT bExecuteMoveVelocity, bReset:=bReset, fHomePosition:=fHomePosition, Axis:=Axis ); // Approach limit switch (error if MC_Home is used) fbMoveVelocity( Execute:= bExecuteMoveVelocity AND bExecute AND NOT bError AND NOT bExecuteHomeToSwitch AND NOT bExecuteHomeDirect, Axis:=Axis ); // No sequence, just set position value (nCmdDataLocal=15). Can not run if fbHomeToSwitch is executed fbHomeDirect( bExecute:=bExecuteHomeDirect AND bExecute AND NOT bError AND NOT bExecuteHomeToSwitch AND NOT bExecuteMoveVelocity, bReset:=bReset, fHomePosition:=fHomePosition, Axis:=Axis ); //############# Finish homing IF NOT bexecute AND bRestoreNCDataNeeded THEN //If homing is aborted restore is needed bExecuteFinish:=TRUE; IF fbHomeFinish.bDone THEN bExecuteFinish:=FALSE; bRestoreNCDataNeeded:=FALSE; END_IF END_IF fbHomeFinish( En:=En, bExecute:=bExecuteFinish, bReset:=bReset, Axis:=Axis, ); // Error handling IF NOT bError THEN IF fbHomeToSwitch.bError THEN bError:=fbHomeToSwitch.bError; nErrorId:=fbHomeToSwitch.nErrorId; ELSIF fbHomeDirect.bError THEN bError:=fbHomeDirect.bError; nErrorId:=fbHomeDirect.nErrorId; ELSIF fbMoveVelocity.Error THEN bError:=fbMoveVelocity.Error; nErrorId:=fbMoveVelocity.ErrorId; END_IF; END_IF // Done and busy bit bDone:=bSequenceReady AND bExecute; bBusy:=NOT bSequenceReady; RETURN; END_FUNCTION_BLOCK Related: * `FB_HomeDirect`_ * `FB_HomeFinish`_ * `FB_HomePrepare`_ * `FB_HomeToSwitch`_ FB_HomeWriteNcVelocities ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeWriteNcVelocities VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; fVelocityToCam: LREAL; fVelocityFromCam: LREAL; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorId: UDINT; END_VAR VAR fbExecuteRiseEdge: R_TRIG; fbWriteVelocityToCam:FB_WriteFloatParameter; fbWriteVelocityFromCam:FB_WriteFloatParameter; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF fbExecuteRiseEdge(CLK:=bExecute); fbWriteVelocityToCam( bExecute:=bExecute, nDeviceGroup:= 16#4000, nIndexOffset:= 16#6, nData:=fVelocityToCam, Axis:= Axis); fbWriteVelocityFromCam( bExecute:=bExecute, nDeviceGroup:= 16#4000, nIndexOffset:= 16#7, nData:=fVelocityFromCam, Axis:= Axis); bBusy:=fbWriteVelocityFromCam.bBusy OR fbWriteVelocityToCam.bBusy; bDone:=fbWriteVelocityFromCam.bDone AND fbWriteVelocityToCam.bDone AND bExecute; bError:=fbWriteVelocityToCam.bError OR fbWriteVelocityFromCam.bError; IF fbWriteVelocityToCam.bError THEN nErrorId:=fbWriteVelocityToCam.nErrorId; ELSIF fbWriteVelocityFromCam.bError THEN nErrorId:=fbWriteVelocityFromCam.nErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_WriteFloatParameter`_ FB_HomeWriteSoftLimEnable ^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_HomeWriteSoftLimEnable VAR_INPUT En: BOOL; bReset: BOOL; bExecute: BOOL; bSofLimEnableLow: BOOL:=TRUE; bSofLimEnableHigh: BOOL:=TRUE; END_VAR VAR_IN_OUT Axis: AXIS_REF; END_VAR VAR_OUTPUT EnO: BOOL; bBusy: BOOL; bDone: BOOL; bError: BOOL; nErrorId: UDINT; END_VAR VAR fbExecuteRiseEdge: R_TRIG; fbWriteSoftLimEnableLow:FB_WriteParameterInNc_v1_00; fbWriteSoftLimEnableHigh:FB_WriteParameterInNc_v1_00; END_VAR En:=EnO; IF bReset THEN bError:=FALSE; nErrorId:=0; END_IF fbExecuteRiseEdge(CLK:=bExecute); fbWriteSoftLimEnableLow( bExecute:=bExecute, nDeviceGroup:= 16#5000, nIndexOffset:= 16#B, nData:=BOOL_TO_DWORD(bSofLimEnableLow), Axis:= Axis); fbWriteSoftLimEnableHigh( bExecute:=bExecute, nDeviceGroup:= 16#5000, nIndexOffset:= 16#C, nData:=BOOL_TO_DWORD(bSofLimEnableHigh), Axis:= Axis); bBusy:=fbWriteSoftLimEnableLow.bBusy OR fbWriteSoftLimEnableHigh.bBusy; bDone:=fbWriteSoftLimEnableLow.bDone AND fbWriteSoftLimEnableHigh.bDone AND bExecute; bError:=fbWriteSoftLimEnableHigh.bError OR fbWriteSoftLimEnableLow.bError; IF fbWriteSoftLimEnableHigh.bError THEN nErrorId:=fbWriteSoftLimEnableHigh.nErrorId; ELSIF fbWriteSoftLimEnableLow.bError THEN nErrorId:=fbWriteSoftLimEnableLow.nErrorId; END_IF END_FUNCTION_BLOCK Related: * `FB_WriteParameterInNc_v1_00`_ FB_LogMotionError ^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_LogMotionError (* If the motion struct has an error, log it. The log condition is: - When bError goes TRUE (catch transition from no error to error) - When the error message changes while bError is TRUE (catch transition from error a to error b) Includes the motor name and the NC error id in the json blob *) VAR_IN_OUT stMotionStage: ST_MotionStage; END_VAR VAR_INPUT bEnable: BOOL; END_VAR VAR fbLogMessage: FB_LogMessage; rtNewError: R_TRIG; bChangedError: BOOL; sPrevErr: STRING; fbJson: FB_JsonSaxWriter; END_VAR rtNewError(CLK:=stMotionStage.bError); bChangedError := stMotionStage.sErrorMessage <> '' AND stMotionStage.sErrorMessage <> sPrevErr; sPrevErr := stMotionStage.sErrorMessage; IF bEnable AND (rtNewError.Q OR bChangedError) THEN fbJson.StartObject(); fbJson.AddKey('schema'); fbJson.AddString('ST_MotionStage.bError'); fbJson.AddKey('dut_name'); fbJson.AddString(stMotionStage.sName); fbJson.AddKey('axis_name'); fbJson.AddString(stMotionStage.stAxisParameters.sAxisName); fbJson.AddKey('axis_id'); fbJson.AddUdint(stMotionStage.stAxisParameters.AxisId); fbJson.AddKey('err_id'); fbJson.AddUdint(stMotionStage.nErrorId); fbJson.AddKey('position'); fbJson.AddLreal(stMotionStage.stAxisStatus.fActPosition); fbJson.AddKey('position_lag'); fbJson.AddLreal(stMotionStage.stAxisStatus.fActDiff); fbJson.EndObject(); fbLogMessage.sJson := fbJson.GetDocument(); fbLogMessage( sMsg := stMotionStage.sErrorMessage, eSevr := TcEventSeverity.Error, eSubsystem := E_Subsystem.MOTION, ); fbJson.ResetDocument(); END_IF END_FUNCTION_BLOCK Related: * `ST_MotionStage`_ 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_MiscStatesErrorFFO ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MiscStatesErrorFFO (* A catch-all for miscellaneous state FFOs that are not better organized elsewhere. Contains the following fast faults: - ffBeamParamsOK: trip the beam if the beam parameters are not safe enough for our current (known) state - ffZeroRate: trip the beam if we've asked for zero rate (as a shortcut) - ffUnknown: trip the beam if we're at an unknown state and our transition id is not asserted. - ffDebounce: trip the beam (no autoreset) if ffBeamParamsOK fast faults fault on and off multiple times too quickly. This solves an issue where ffBeamParamsOK might solve its own problem and creating a blinking fast fault issue. The inputs are mostly outputs of non-pmps function blocks and heavily-reused info like state details. *) VAR_IN_OUT // The arbiter to request beam states from fbArbiter: FB_Arbiter; // The fast fault output to fault to fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // A name to link to these fast faults sDeviceName: STRING; // Current requested beam details: either a known state or the transition beam stCurrentBeamReq: ST_BeamParams; // TRUE if we're at a known state (doesn't matter which) bKnownState: BOOL; // Lookup ID of the transition beam nTransitionID: DWORD; END_VAR VAR_OUTPUT END_VAR VAR CONSTANT // Number of consecutive trips before we debounce nMaxTrips: UINT := 5; // Decrease trip count by 1 after this much time has passed tTripReset: TIME := T#1s; END_VAR VAR // If the beam parameters are wrong, it is a fault! This encompasses all unknown arbiter-related errors. ffBeamParamsOk: FB_FastFault; // If we asked for zero rate (NC or SC) then we can cut the beam early. This is somewhat redundant. ffZeroRate: FB_FastFault; // Trip the beam for unknown state ffUnknown: FB_FastFault; // Trip the beam (no autoreset) if ffBeamParamsOK faults/resets multiple times too quickly. ffDebounce: FB_FastFault; // Number of consecutive trips so far nTripCount: UINT; // Increase by 1 whenever there is a fault (rising edge) ftTripCount: F_TRIG; // Decrease trip count by 1 each timeout tonTripCount: TON; // TRUE on only the first cycle bFirstCycle: BOOL := TRUE; END_VAR ffBeamParamsOk( i_xOK:=F_SafeBPCompare(PMPS_GVL.stCurrentBeamParameters, stCurrentBeamReq), i_xAutoReset:=TRUE, i_DevName:=sDeviceName, i_Desc:='Beam parameter mismatch', i_TypeCode:=E_MotionFFType.BP_MISMATCH, io_fbFFHWO:=fbFFHWO); CASE PMPS_GVL.stCurrentBeamParameters.nMachineMode OF // NC mode 1: ffZeroRate.i_xOK := stCurrentBeamReq.nRate > 0; // SC mode 2: ffZeroRate.i_xOK := stCurrentBeamReq.nBCRange > 0; ELSE // Ambiguous or new mode ffZeroRate.i_xOK := stCurrentBeamReq.nRate > 0 AND stCurrentBeamReq.nBCRange > 0; END_CASE ffZeroRate( i_xAutoReset := TRUE, i_DevName := sDeviceName, i_Desc := 'Device requesting zero rate', i_TypeCode := E_MotionFFType.ZERO_RATE, io_fbFFHWO := fbFFHWO, ); ffUnknown( i_xOK := bknownState OR fbArbiter.CheckRequestInPool(nReqID:=nTransitionID), i_xAutoReset := TRUE, i_DevName := sDeviceName, i_Desc := 'Unknown position between moves', i_TypeCode := E_MotionFFType.NOT_A_STATE, io_fbFFHWO := fbFFHWO, ); ftTripCount(CLK:=ffBeamParamsOk.i_xOK); IF ftTripCount.Q THEN nTripCount := nTripCount + 1; END_IF tonTripCount( IN:=NOT tonTripCount.Q, PT:=tTripReset, ); IF tonTripCount.Q AND nTripCount > 0 THEN nTripCount := nTripCount - 1; END_IF // This is required for non-autoresetting FBs to come up in a beam permitted state ffDebounce.i_xReset S= bFirstCycle; ffDebounce( i_xOK := nTripCount < 5, i_xAutoReset := FALSE, i_DevName := sDeviceName, i_Desc := 'Tripped beam parameter mismatch off/on too many times, hold off', i_TypeCode := E_MotionFFType.TOO_MANY_TRIPS, io_fbFFHWO := fbFFHWO, ); ffDebounce.i_xReset R= bFirstCycle; bFirstCycle := FALSE; END_FUNCTION_BLOCK Related: * `E_MotionFFType`_ FB_MiscStatesErrorFFO_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MiscStatesErrorFFO_Test EXTENDS TcUnit.FB_TestSuite (* Unit tests for FB_MiscStatesErrorFFO *) VAR END_VAR TestBeamParamsNotOk(); TestZeroRate(); TestUnknownState(); TestTransitionState(); TestDebounce(); END_FUNCTION_BLOCK METHOD TestBeamParamsNotOk VAR_INST fbMiscFFO: FB_MiscStatesErrorFFO; fbArbiter: FB_Arbiter(1); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); stBeamReq: ST_BeamParams; END_VAR TEST('TestBeamParamsNotOk'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault', ); // Trigger only a beam parameter mismatch // Do not trip zero rate, unknown state, or debounce PMPS_GVL.stCurrentBeamParameters := PMPS_GVL.cstFullBeam; stBeamReq := PMPS_GVL.cstFullBeam; stBeamReq.nTran := 0.5; fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=stBeamReq, bKnownState:=TRUE, nTransitionID:=1, ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Did not fault with bad attenuator state', ); TEST_FINISHED(); END_METHOD METHOD TestDebounce VAR_INST fbMiscFFO: FB_MiscStatesErrorFFO; fbArbiter: FB_Arbiter(5); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR VAR nIter: DINT; END_VAR TEST('TestDebounce'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault prior to first FB run-through', ); // Ask for full beam: there should be no faults of any kind fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=PMPS_GVL.cstFullBeam, bKnownState:=TRUE, nTransitionID:=5, ); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault prior to trip checks', ); // Trip and untrip the beam fast fault once, show no fault TripUntrip( fbMiscFFO:=fbMiscFFO, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Control group failed: one trip/untrip should not be enough to debounce', ); // Trip and untrip the beam fast fault like 10 times, show fault FOR nIter := 1 TO 10 DO TripUntrip( fbMiscFFO:=fbMiscFFO, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, ); END_FOR AssertFalse( fbFFHWO.q_xFastFaultOut, 'Debouncer failed to debounce', ); TEST_FINISHED(); END_METHOD METHOD TestTransitionState VAR_INST fbMiscFFO: FB_MiscStatesErrorFFO; fbArbiter: FB_Arbiter(4); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestTransitionState'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault', ); // Trigger no faults, we're at an unknown state but it's a transition state // Do not trip bad beam, zero rate, or debounce fbArbiter.AddRequest( nReqID:=4, stReqBP:=PMPS_GVL.cstFullBeam, sDevName:='UnitTest', ); PMPS_GVL.stCurrentBeamParameters := PMPS_GVL.cstFullBeam; fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=PMPS_GVL.cstFullBeam, bKnownState:=FALSE, nTransitionID:=4, ); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Faulted in unknown states even though we were moving normally', ); TEST_FINISHED(); END_METHOD METHOD TestUnknownState VAR_INST fbMiscFFO: FB_MiscStatesErrorFFO; fbArbiter: FB_Arbiter(3); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestUnknownState'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault', ); // Trigger only an unknown states // Do not trip bad beam, zero rate, or debounce PMPS_GVL.stCurrentBeamParameters := PMPS_GVL.cstFullBeam; fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=PMPS_GVL.cstFullBeam, bKnownState:=FALSE, nTransitionID:=3, ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Did not fault with unknown state', ); TEST_FINISHED(); END_METHOD METHOD TestZeroRate VAR_INST fbMiscFFO: FB_MiscStatesErrorFFO; fbArbiter: FB_Arbiter(2); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); stNoBeam: ST_BeamParams; END_VAR TEST('TestBeamZeroRate'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault', ); // Trigger only a zero rate // Do not trip beam parameter mismatch, unknown state, or debounce PMPS_GVL.stCurrentBeamParameters := stNoBeam; fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=PMPS_GVL.cst0RateBeam, bKnownState:=TRUE, nTransitionID:=2 ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Did not fault with zero rate', ); TEST_FINISHED(); END_METHOD METHOD TripUntrip VAR_IN_OUT fbMiscFFO: FB_MiscStatesErrorFFO; fbArbiter: FB_Arbiter; fbFFHWO: FB_HardwareFFOutput; END_VAR VAR stBeamReq: ST_BeamParams; END_VAR // Use anything other than 0 rate, which is its own check stBeamReq := PMPS_GVL.cstFullBeam; stBeamReq.nTran := 0.5; // Trip: too much beam! PMPS_GVL.stCurrentBeamParameters := PMPS_GVL.cstFullBeam; fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=stBeamReq, bKnownState:=TRUE, nTransitionID:=5, ); fbFFHWO.EvaluateOutput(); // Untrip: correct beam! PMPS_GVL.stCurrentBeamParameters := stBeamReq; fbMiscFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stCurrentBeamReq:=stBeamReq, bKnownState:=TRUE, nTransitionID:=5, ); fbFFHWO.EvaluateOutput(); END_METHOD Related: * `FB_MiscStatesErrorFFO`_ FB_MotionBPTM ^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionBPTM (* This function block handles the beam parameter transition manager for a group of motors moving together to a destination with shared beam state. stGoalParams and stTransParams are required arguments and must not share IDs with other state parameters in the PLC. This is a building block not intended for use outside of lcls-twincat-motion. *) VAR_IN_OUT // Array of motors that will move for this beam transition astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // The arbiter to request beam states from fbArbiter: FB_Arbiter; // The fast fault output to fault to fbFFHWO: FB_HardwareFFOutput; // The parameters we want to transition to stGoalParams: ST_DbStateParams; // The parameters we want to use during the transition stTransParams: ST_DbStateParams; END_VAR VAR_INPUT // The number of motors we're actually using nActiveMotorCount: UINT; // Set to TRUE to use the BPTM, FALSE to stop using the BPTM. bEnable: BOOL; // TRUE if we're at the physical state that matches the goal parameters bAtState: BOOL; // A device name to use in the GUI sDeviceName: STRING; // How long to wait for parameters before timing out tArbiterTimeout: TIME := T#1s; // Whether to fault and move on timeout (TRUE) or to wait (FALSE) bMoveOnArbiterTimeout: BOOL := TRUE; // Set to TRUE when it is safe to reset the BPTM timeout fast fault, to reset it early. bResetBPTMTimeout: BOOL; END_VAR VAR_OUTPUT // This becomes TRUE when the motors are allowed to move to their destinations. bTransitionAuthorized: BOOL; // This becomes TRUE once the full beam transition is done. bDone: BOOL; // TRUE if we're using a bad motor count bMotorCountError: BOOL; END_VAR VAR bptm: BeamParameterTransitionManager; bDoneMoving: BOOL; nPrevID: UDINT; nIndex: DINT; bInternalAuth: BOOL; bDoneResetQueued: BOOL; tonArbiter: TON; bArbiterTimeout: BOOL; ffBPTMTimeoutAndMove: FB_FastFault; ffBPTMError: FB_FastFault; END_VAR CheckCount(); IF NOT bMotorCountError THEN SetDoneMoving(); IF bEnable THEN RunBPTM(); HandleTimeout(); END_IF END_IF END_FUNCTION_BLOCK ACTION CheckCount: // Make sure the motor count is valid (positive, nonzero, less or equal to the max) bMotorCountError S= nActiveMotorCount <= 0; bMotorCountError S= nActiveMotorCount > MotionConstants.MAX_STATE_MOTORS; END_ACTION ACTION HandleTimeout: // Measure the time that the bptm is busy tonArbiter( IN:=bptm.bBusy, PT:=tArbiterTimeout, Q=>bArbiterTimeout, ); bTransitionAuthorized := bInternalAuth OR (bArbiterTimeout AND bMoveOnArbiterTimeout); // 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= bResetBPTMTimeout OR (bptm.bDone AND NOT bptm.bError); ffBPTMTimeoutAndMove.i_xReset R= NOT ffBPTMTimeoutAndMove.i_xOK; ffBPTMTimeoutAndMove( i_DevName := sDeviceName, i_Desc := 'BPTM Timeout', i_TypeCode := E_MotionFFType.BPTM_TIMEOUT, io_fbFFHWO := fbFFHWO, ); END_ACTION ACTION RunBPTM: bptm( fbArbiter:=fbArbiter, i_sDeviceName:=sDeviceName, i_TransitionAssertionID:=stTransParams.nRequestAssertionID, i_stTransitionAssertion:=stTransParams.stBeamParams, i_nRequestedAssertionID:=stGoalParams.nRequestAssertionID, i_stRequestedAssertion:=stGoalParams.stBeamParams, i_xDoneMoving:=bDoneMoving AND bAtState, stCurrentBeamParameters:=PMPS_GVL.stCurrentBeamParameters, q_xTransitionAuthorized=>bInternalAuth, bDone=>bDone, ); // Trip the beam for BPTM Errors ffBPTMError.i_xOK := NOT bptm.bError; ffBPTMError.i_xReset S= bptm.bDone AND ffBPTMError.i_xOK; ffBPTMError.i_xReset R= NOT ffBPTMError.i_xOK; ffBPTMError( i_DevName := sDeviceName, i_Desc := 'BPTM error, state transition failed', i_TypeCode := E_MotionFFType.BPTM_ERROR, io_fbFFHWO := fbFFHWO, ); END_ACTION ACTION SetDoneMoving: // Set bDoneMoving if all the motors are done bDoneMoving := TRUE; FOR nIndex := 1 TO nActiveMotorCount DO bDoneMoving := bDoneMoving AND NOT astMotionStage[nIndex].bBusy AND NOT astMotionStage[nIndex].bExecute; END_FOR // Reset bDoneMoving if the goal has changed to reset bptm's motor done for an in-place transition // This allows us to change to a new beam state without requiring a motor state change // BPTM only checks for this "done" transition after the transition has been authorized, so we may need to wait bDoneResetQueued S= nPrevID <> stGoalParams.nRequestAssertionID; bDoneMoving R= bDoneResetQueued AND bptm.q_xTransitionAuthorized; bDoneResetQueued R= bptm.q_xTransitionAuthorized; nPrevID := stGoalParams.nRequestAssertionID; END_ACTION Related: * `E_MotionFFType`_ * `MotionConstants`_ * `ST_MotionStage`_ FB_MotionBPTM_Test ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionBPTM_Test EXTENDS TcUnit.FB_TestSuite (* Test the functionality of the motion bptm, Which is just a bptm wrapped up with a set of n motors. We're basically just making sure that we push the done button at the appropriate time. Direct tests of BPTM itself are reserved for the pmps library. The BPTM takes care of these executions across multiple cycles, so these tests must implement timeouts. *) VAR astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; stGoal1: ST_DbStateParams; stGoal2: ST_DbStateParams; stTrans: ST_DbStateParams; stNoBeam: ST_BeamParams; END_VAR stGoal1.stBeamParams := PMPS_GVL.cstFullBeam; stGoal1.nRequestAssertionID := 1; stGoal1.sPmpsState := 'stGoal1'; stGoal2.stBeamParams := PMPS_GVL.cstFullBeam; stGoal2.stBeamParams.nTran := 0.5; stGoal2.nRequestAssertionID := 2; stGoal2.sPmpsState := 'stGoal2'; stTrans.stBeamParams := PMPS_GVL.cst0RateBeam; stTrans.nRequestAssertionID := 3; stTrans.sPmpsState := 'stTrans'; // Just put a blanket global no beam so these won't wait for any changes ever PMPS_GVL.stCurrentBeamParameters := stNoBeam; TestInit(); Test3dMove(); TestNoMove(); TestCount(); END_FUNCTION_BLOCK METHOD AssertInPool VAR_IN_OUT fbArbiter: FB_Arbiter; stDbStateParams: ST_DbStateParams; END_VAR VAR_INPUT bInPool: BOOL; sContext: STRING; END_VAR IF bInPool THEN AssertTrue( fbArbiter.CheckRequestInPool(stDbStateParams.nRequestAssertionID), CONCAT(CONCAT(stDbStateParams.sPmpsState, ' Beam parameters were not in the pool '), sContext), ); ELSE AssertFalse( fbArbiter.CheckRequestInPool(stDbStateParams.nRequestAssertionID), CONCAT(CONCAT(stDbStateParams.sPmpsState, ' Beam parameters unexpectedly found in the pool '), sContext), ); END_IF END_METHOD METHOD SetMotorDone VAR_INPUT END_VAR // Force post-move state astMotionStage[1].bBusy := FALSE; astMotionStage[1].bDone := TRUE; astMotionStage[2].bBusy := FALSE; astMotionStage[2].bDone := TRUE; astMotionStage[3].bBusy := FALSE; astMotionStage[3].bDone := TRUE; END_METHOD METHOD SetMotorMoving VAR_INPUT END_VAR // Force a moving but not done state astMotionStage[1].bBusy := TRUE; astMotionStage[1].bDone := FALSE; astMotionStage[2].bBusy := TRUE; astMotionStage[2].bDone := FALSE; astMotionStage[3].bBusy := TRUE; astMotionStage[3].bDone := FALSE; END_METHOD METHOD SetMotorStartup VAR_INPUT END_VAR // Force some sort of default looking state astMotionStage[1].bBusy := FALSE; astMotionStage[1].bDone := FALSE; astMotionStage[2].bBusy := FALSE; astMotionStage[2].bDone := FALSE; astMotionStage[3].bBusy := FALSE; astMotionStage[3].bDone := FALSE; END_METHOD METHOD Test3dMove : BOOL (* Can we safely do a 3d move? *) VAR_INST fbBptm: FB_MotionBPTM; fbArbiter: FB_Arbiter(2); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbSubSysIO : FB_DummyArbIO; nState: UINT; tonTimer: TON; END_VAR TEST('Test3DMove'); tonTimer( IN:=TRUE, PT:=T#5s, ); IF tonTimer.Q THEN nState := 4; END_IF CASE nState OF 0: // Establish baseline at Goal1 SetMotorStartup(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal1, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=TRUE, bAtState:=TRUE, ); IF fbBptm.bDone THEN nState := 1; END_IF 1: // Request a move SetMotorStartup(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal2, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=TRUE, bAtState:=FALSE, ); IF fbBptm.bTransitionAuthorized THEN // We should have transition and goal 2 asserts in AssertInPool(fbArbiter, stGoal1, FALSE, 'with trans auth'); AssertInPool(fbArbiter, stGoal2, TRUE, 'with trans auth'); AssertInPool(fbArbiter, stTrans, TRUE, 'with trans auth'); nState := 2; END_IF 2: // Simulate a move SetMotorMoving(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal2, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=TRUE, bAtState:=FALSE, ); // Same situation as before AssertInPool(fbArbiter, stGoal1, FALSE, 'after move started'); AssertInPool(fbArbiter, stGoal2, TRUE, 'after move started'); AssertInPool(fbArbiter, stTrans, TRUE, 'after move started'); nState := 3; 3: // Move is done SetMotorDone(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal2, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=TRUE, bAtState:=TRUE, ); IF fbBptm.bDone THEN // Dropped the transition assert AssertInPool(fbArbiter, stGoal1, FALSE, 'with move complete'); AssertInPool(fbArbiter, stGoal2, TRUE, 'with move complete'); AssertInPool(fbArbiter, stTrans, FALSE, 'with move complete'); nState := 4; END_IF 4: AssertFalse( tonTimer.Q, 'Timeout in test', ); TEST_FINISHED(); END_CASE fbSubSysIO( LA:=fbArbiter, FFO:=fbFFHWO, ); END_METHOD METHOD TestCount (* FB Should just error out if we forgot to give a count *) VAR_INST fbBptm: FB_MotionBPTM; fbArbiter: FB_Arbiter(1); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); tonWait: TON; END_VAR TEST('TestCount'); SetMotorStartup(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal1, stTransParams:=stTrans, bEnable:=True, bAtState:=True, ); AssertTrue( fbBptm.bMotorCountError, 'Did not properly error on missing motor count', ); tonWait( IN:=TRUE, PT:=T#1s, ); IF tonWait.Q THEN // Should have no arbiter activity at all AssertInPool(fbArbiter, stGoal1, FALSE, 'with bad count'); AssertInPool(fbArbiter, stGoal2, FALSE, 'with bad count'); AssertInPool(fbArbiter, stTrans, FALSE, 'with bad count'); TEST_FINISHED(); END_IF END_METHOD METHOD TestInit : BOOL (* If we initialize with still motors, do we get an arbiter request at the current goal? Hopefully we do. *) VAR_INST fbBptm: FB_MotionBPTM; fbArbiter: FB_Arbiter(1); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbSubSysIO : FB_DummyArbIO; tonTimer: TON; END_VAR TEST('TestInit'); SetMotorStartup(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal1, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=True, bAtState:=True, ); fbSubSysIO( LA:=fbArbiter, FFO:=fbFFHWO, ); tonTimer( IN:=TRUE, PT:=T#5s, ); IF tonTimer.Q OR fbBptm.bDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); // We should have a request in the pool for goal 1 but not for transition // If no request are in the pool, we may come up in a no protection state! // If both requests are in the pool, we may come up in a too much blocking beam state! AssertInPool(fbArbiter, stGoal1, TRUE, 'at startup'); AssertInPool(fbArbiter, stTrans, FALSE, 'at startup'); TEST_FINISHED(); END_IF END_METHOD METHOD TestNoMove (* In place transitions should work at startup and also at done positions. *) VAR_INST fbBptm: FB_MotionBPTM; fbArbiter: FB_Arbiter(1); fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbSubSysIO : FB_DummyArbIO; nState: UINT; tonTimer: TON; END_VAR TEST('TestNoMove'); tonTimer( IN:=TRUE, PT:=T#5s, ); IF tonTimer.Q THEN nState := 3; END_IF CASE nState OF 0: SetMotorStartup(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal1, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=True, bAtState:=True, ); IF fbBptm.bDone THEN nState := 1; END_IF 1: SetMotorStartup(); // NOTE: we kept bAtState TRUE the whole time, so this should be a completed in-place transition fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal2, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=True, bAtState:=True, ); IF fbBptm.bDone THEN // Only Goal2 should be in the pool AssertInPool(fbArbiter, stGoal1, FALSE, 'after switching goals (1)'); AssertInPool(fbArbiter, stGoal2, TRUE, 'after switching goals (1)'); AssertInPool(fbArbiter, stTrans, FALSE, 'after switching goals (1)'); nState := 2; END_IF 2: // Repeat from a done position! SetMotorDone(); fbBptm( astMotionStage:=astMotionStage, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=stGoal1, stTransParams:=stTrans, nActiveMotorCount:=3, bEnable:=True, bAtState:=True, ); IF fbBptm.bDone THEN // Only Goal1 should be in the pool AssertInPool(fbArbiter, stGoal1, TRUE, 'after switching goals (2)'); AssertInPool(fbArbiter, stGoal2, FALSE, 'after switching goals (2)'); AssertInPool(fbArbiter, stTrans, FALSE, 'after switching goals (2)'); nState := 3; END_IF 3: AssertFalse( tonTimer.Q, 'Timeout in test', ); TEST_FINISHED(); END_CASE fbSubSysIO( LA:=fbArbiter, FFO:=fbFFHWO, ); END_METHOD Related: * `FB_MotionBPTM`_ * `MotionConstants`_ * `ST_MotionStage`_ FB_MotionClearAsserts ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionClearAsserts (* Clear all of the PMPS asserts related to a states mover. *) VAR_IN_OUT // All states to deactivate: transition + the static position states astDbStateParams: ARRAY[0..GeneralConstants.MAX_STATES] OF ST_DbStateParams; // The arbiter who made the PMPS assert requests fbArbiter: FB_ARBITER; END_VAR VAR_INPUT // Clear asserts on rising edge bExecute: BOOL; END_VAR VAR_OUTPUT END_VAR VAR rtExec: R_TRIG; nIter: DINT; END_VAR rtExec(CLK:=bExecute); IF rtExec.Q THEN FOR nIter := 0 TO GeneralConstants.MAX_STATES DO fbArbiter.RemoveRequest(astDbStateParams[nIter].nRequestAssertionID); END_FOR END_IF END_FUNCTION_BLOCK FB_MotionClearAsserts_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionClearAsserts_Test EXTENDS TcUnit.FB_TestSuite VAR END_VAR TestBasic(); END_FUNCTION_BLOCK METHOD TestBasic VAR nIter: UINT; END_VAR VAR_INST fbClear: FB_MotionClearAsserts; astDbStateParams: ARRAY[0..GeneralConstants.MAX_STATES] OF ST_DbStateParams; fbArbiter: FB_Arbiter(1); END_VAR TEST('TestBasic'); FOR nIter := 0 TO GeneralConstants.MAX_STATES DO astDbStateParams[nIter].nRequestAssertionID := 100 + nIter; fbArbiter.AddRequest( nReqID:=100 + nIter, stReqBP:=PMPS_GVL.cstFullBeam, sDevName:='UnitTest', ); END_FOR FOR nIter := 0 TO GeneralConstants.MAX_STATES DO AssertTrue( fbArbiter.CheckRequestInPool(100 + nIter), CONCAT(CONCAT('State ', UDINT_TO_STRING(nIter)), ' was not in the pool'), ); END_FOR fbClear( astDbStateParams:=astDbStateParams, fbArbiter:=fbArbiter, bExecute:=TRUE, ); FOR nIter := 0 TO GeneralConstants.MAX_STATES DO AssertFalse( fbArbiter.CheckRequestInPool(100 + nIter), CONCAT(CONCAT('State ', UDINT_TO_STRING(nIter)), ' was not cleared from the pool'), ); END_FOR TEST_FINISHED(); END_METHOD Related: * `FB_MotionClearAsserts`_ FB_MotionHoming ^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionHoming VAR_IN_OUT stMotionStage: ST_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 E_EpicsHomeCmd.LOW_LIMIT: bFirstDirection := FALSE; bAtHome := NOT stMotionStage.bLimitBackwardEnable; bMove := TRUE; E_EpicsHomeCmd.HIGH_LIMIT: bFirstDirection := TRUE; bAtHome := NOT stMotionStage.bLimitForwardEnable; bMove := TRUE; E_EpicsHomeCmd.HOME_VIA_LOW: bFirstDirection := FALSE; bAtHome := stMotionStage.bHome; bMove := TRUE; E_EpicsHomeCmd.HOME_VIA_HIGH: bFirstDirection := TRUE; bAtHome := stMotionStage.bHome; bMove := TRUE; E_EpicsHomeCmd.ABSOLUTE_SET: fbSetPos( Axis:=stMotionStage.Axis, Execute:=bExecute, Position:=stMotionStage.fHomePosition); bBusy := rtExec.Q; bDone := NOT rtExec.Q; bMove := FALSE; E_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 Related: * `E_EpicsHomeCmd`_ * `ST_MotionStage`_ 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: FFO_Reset '} i_xReset: BOOL; {attribute 'pytmc' := ' pv: FFO_AutoReset '} i_xAutoReset: BOOL; END_VAR VAR_OUTPUT {attribute 'pytmc' := ' pv: '} stPneumaticActuator : ST_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 := E_MotionFFType.PNEUMATIC_MOVE); (*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 : E_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 := E_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:=E_PnuematicActuatorPositionState.INVALID; ELSIF NOT stPneumaticActuator.i_bInLimitSwitch AND stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=E_PnuematicActuatorPositionState.RETRACTED; ELSIF stPneumaticActuator.i_bInLimitSwitch AND NOT stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=E_PnuematicActuatorPositionState.INSERTED; ELSIF NOT stPneumaticActuator.i_bInLimitSwitch AND NOT stPneumaticActuator.i_bOutLimitSwitch THEN stPneumaticActuator.eState:=E_PnuematicActuatorPositionState.MOVING; ELSE stPneumaticActuator.eState:=E_PnuematicActuatorPositionState.INVALID ; END_IF (*Set the Done/Busy signal*) stPneumaticActuator.bDone := (stPneumaticActuator.bRetract_SW AND stPneumaticActuator.eState=E_PnuematicActuatorPositionState.RETRACTED) OR (stPneumaticActuator.bInsert_SW AND stPneumaticActuator.eState=E_PnuematicActuatorPositionState.INSERTED); stPneumaticActuator.bBusy := (stPneumaticActuator.bRetract_SW AND stPneumaticActuator.eState<>E_PnuematicActuatorPositionState.RETRACTED) OR (stPneumaticActuator.bInsert_SW AND stPneumaticActuator.eState<>E_PnuematicActuatorPositionState.INSERTED); (*MPS FAULT*) (* MPS Faults when the actuator is in motion*) xMPS_OK := (stPneumaticActuator.eState=E_PnuematicActuatorPositionState.RETRACTED) OR (stPneumaticActuator.eState=E_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, i_xAutoReset := i_xAutoReset, 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 E_PnuematicActuatorPositionState.INVALID: fbLogger(sMsg:='Actuator invalid position.', eSevr:=TcEventSeverity.Critical); E_PnuematicActuatorPositionState.MOVING: fbLogger(sMsg:='Actuator moving', eSevr:=TcEventSeverity.Warning); E_PnuematicActuatorPositionState.RETRACTED: fbLogger(sMsg:='Actuator Retracted.', eSevr:=TcEventSeverity.Info); E_PnuematicActuatorPositionState.INSERTED: fbLogger(sMsg:='Actuator Inserted.', eSevr:=TcEventSeverity.Info); END_CASE ePrevState := stPneumaticActuator.eState; END_IF END_ACTION Related: * `E_MotionFFType`_ * `E_PnuematicActuatorPositionState`_ * `ST_MotionPneumaticActuator`_ FB_MotionReadPMPSDBND ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionReadPMPSDBND (* When we read the JSON PMPS database file, update the lookup parameters for one state mover. It is a building block not meant for use outside of lcls-twincat-motion. This is intended to support one N-dimensional state motion function block. The keys for the database lookup can be set on any of the motor's position states. Each of them have an allocated state.stPMPS.sPmpsState STRING parameter. If there is a conflict and two of the motors disagree on parameter lookups, that will be a fast fault. When the global JSON read function block is no longer busy and has no errors, we will assume that the file has been read and we will update the parameters here. This will also re-read in the event that the input position state keys change in any way, provided that we've read once before. *) VAR_IN_OUT // Each motor's respective position states along its direction. These will not be modified. astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Hardware output to fault to if there is a problem. fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // The database lookup key for the transition state. This has no corresponding ST_PositionState. sTransitionKey: STRING; // A name to use for fast faults, etc. sDeviceName: STRING; // For debug: set this to TRUE in online mode to read the database immediately. bReadNow: BOOL; END_VAR VAR_OUTPUT // The raw lookup results from this FB. Index 0 is the transition beam, the rest of the indices match the state positions. astDbStateParams: ARRAY[0..GeneralConstants.MAX_STATES] OF ST_DbStateParams; // TRUE if we've had at least one successful read. bFirstReadDone: BOOL; // This will be set to TRUE if there was an error reading from the database. bError: BOOL; END_VAR VAR ffError: FB_FastFault; fbReadPmpsDb: FB_JsonDocToSafeBP; ftDbBusy: F_TRIG; ftRead: F_TRIG; bReadPmpsDb: BOOL; nIterMotor: DINT; nIterState: DINT; nIterState2: DINT; sLoopNewKey: STRING; sLoopPrevKey: STRING; abStateError: ARRAY[0..GeneralConstants.MAX_STATES] OF BOOL; asLookupKeys: ARRAY[0..GeneralConstants.MAX_STATES] OF STRING; asPrevLookupKeys: ARRAY[0..GeneralConstants.MAX_STATES] OF STRING; bNewKeys: BOOL; sTempBackfill: STRING; END_VAR SelectLookupKeys(); ReadDatabase(); RunFastFaults(); BackfillInfo(); END_FUNCTION_BLOCK ACTION BackfillInfo: // Put the results of the PMPS lookup back to the motion states // This is purely for debugging purposes, as only the astDbStateParams output is used by the libraries. // Copy everything except for the lookup key: avoid clobbering the user's original keys // Do it this way instead of one element at a time to be forwards-compatible with any future additions to the db struct FOR nIterState := 1 TO GeneralConstants.MAX_STATES DO FOR nIterMotor := 1 TO MotionConstants.MAX_STATE_MOTORS DO sTempBackfill := astPositionState[nIterMotor][nIterState].stPMPS.sPmpsState; astPositionState[nIterMotor][nIterState].stPMPS := astDbStateParams[nIterState]; astPositionState[nIterMotor][nIterState].stPMPS.sPmpsState := sTempBackfill; END_FOR END_FOR END_ACTION ACTION ReadDatabase: // Read the database at the right timing ftDbBusy(CLK:=MOTION_GVL.fbPmpsFileReader.bBusy); IF ftDbBusy.Q THEN bReadPmpsDb S= NOT MOTION_GVL.fbPmpsFileReader.bError; END_IF bReadPmpsDb S= bFirstReadDone AND bNewKeys; bReadPmpsDb S= bReadNow; bReadNow := FALSE; fbReadPmpsDb( bExecute:=bReadPmpsDb, jsonDoc:=PMPS_GVL.BP_jsonDoc, sDeviceName:=sDeviceName, io_fbFFHWO:=fbFFHWO, arrStates:=astDbStateParams, ); bReadPmpsDb R= NOT fbReadPmpsDb.bBusy; ftRead(CLK:=fbReadPmpsDb.bBusy); bFirstReadDone S= ftRead.Q AND NOT fbReadPmpsDb.bError; END_ACTION ACTION RunFastFaults: ffError( i_xOK:=NOT bError, i_xAutoReset:=TRUE, i_DevName:=sDeviceName, i_Desc:='Programmer error selecting state names in ND motion FB', i_TypeCode:=E_MotionFFType.INTERNAL_ERROR, io_fbFFHWO:=fbFFHWO, ); END_ACTION ACTION SelectLookupKeys: // Fill the lookup key information in astDbStateParams based on the strings from astPositionState and sTransitionKey. // Start by emptying any pre-existing values FOR nIterState := 0 TO GeneralConstants.MAX_STATES DO asLookupKeys[nIterState] := ''; abStateError[nIterState] := FALSE; END_FOR // Transition key is simple asLookupKeys[0] := sTransitionKey; // The other keys might be at different points in the astPositionState array. // Try all of the posibilities, set error if we end up overwriting something. // Outer loop: index of each motor at this position state FOR nIterMotor := 1 TO MotionConstants.MAX_STATE_MOTORS DO // Inner loop: index of each position state for this motor FOR nIterState := 1 TO GeneralConstants.MAX_STATES DO sLoopNewKey := astPositionState[nIterMotor][nIterState].stPMPS.sPmpsState; IF sLoopNewKey <> '' THEN // We have a new key, start doing things sLoopPrevKey := asLookupKeys[nIterState]; IF sLoopPrevKey = '' OR sLoopPrevKey = sLoopNewKey THEN // No key yet, or exactly the same key (redudant programmer) asLookupKeys[nIterState] := sLoopNewKey; ELSE // We already had a different key! Don't just override it, have an error! bError := TRUE; abStateError[nIterState] := TRUE; END_IF END_IF END_FOR END_FOR // Check for duplicated sPmpsState strings FOR nIterState := 0 TO GeneralConstants.MAX_STATES DO FOR nIterState2 := 0 TO nIterState DO IF nIterState <> nIterState2 AND asLookupKeys[nIterState] = asLookupKeys[nIterState2] AND asLookupKeys[nIterState] <> '' THEN // Duplicated key, we need an error and a flag in both spots bError := TRUE; abStateError[nIterState] := TRUE; abStateError[nIterState2] := TRUE; END_IF END_FOR END_FOR // Clear the erroneous states so they won't be used as lookups IF bError THEN FOR nIterState := 0 TO GeneralConstants.MAX_STATES DO IF abStateError[nIterState] THEN asLookupKeys[nIterState] := ''; END_IF END_FOR END_IF // Copy the keys into the db state params FOR nIterState := 0 TO GeneralConstants.MAX_STATES DO astDbStateParams[nIterState].sPmpsState := asLookupKeys[nIterState]; END_FOR // Check if the keys changed from prev cycle bNewKeys := FALSE; FOR nIterState := 0 TO GeneralConstants.MAX_STATES DO IF asLookupKeys[nIterState] <> asPrevLookupKeys[nIterState] THEN bNewKeys := TRUE; EXIT; END_IF END_FOR // Save prev keys for next cycle asPrevLookupKeys := asLookupKeys; END_ACTION Related: * `E_MotionFFType`_ * `MOTION_GVL`_ * `MotionConstants`_ * `ST_PositionState`_ FB_MotionReadPMPSDBND_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionReadPMPSDBND_Test EXTENDS TcUnit.FB_TestSuite (* We test the actual db read in the pmps lib Here, we test that the correct keys are ready for the read based on the inputs. The user submits a transition key and all of their ST_PositionState instances from all of their motors. Any subset of these instances can have lookup keys. If only one state at the index has a lookup key, use that key. If more than one state at the index has the same lookup key, use that key. If states at the same index have different keys, that's an error and a fast fault. If states at different indices have the same keys, that's an error and a fast fault. *) VAR astCorrectStates: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astNonsenseStates: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astDuplicatedStates: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astHalfFullStates: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; nIter: UINT; END_VAR // Set up the state arrays that can be used in test methods FOR nIter := 1 TO GeneralConstants.MAX_STATES DO astCorrectStates[nIter].stPMPS.sPmpsState := CONCAT('State', UINT_TO_STRING(nIter)); astNonsenseStates[nIter].stPMPS.sPmpsState := CONCAT('asdf', UINT_TO_STRING(nIter)); IF UINT_TO_BOOL(nIter MOD 2) THEN astDuplicatedStates[nIter].stPMPS.sPmpsState := 'State0'; ELSE astDuplicatedStates[nIter].stPMPS.sPmpsState := 'State1'; END_IF IF nIter <= GeneralConstants.MAX_STATES / 2 THEN astHalfFullStates[nIter].stPMPS.sPmpsState := CONCAT('State', UINT_TO_STRING(nIter)); END_IF END_FOR TestSolo(); TestTrio(); TestNonsense(); TestDupe(); TestBackfill(); TestHalfFull(); END_FUNCTION_BLOCK METHOD TestBackfill VAR_INST fbRead: FB_MotionReadPMPSDBND; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); stDefaultBP: ST_DbStateParams; END_VAR TEST('TestBackfill'); astPositionState[1] := astCorrectStates; // Pick a few parameters to drop in astPositionState[1][3].stPMPS.stBeamParams.nRate := 999; astPositionState[2][1].stPMPS.nRequestAssertionID := 777; astPositionState[3][2].stPMPS.bBeamParamsLoaded := NOT stDefaultBP.bBeamParamsLoaded; fbRead( astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, sTransitionKey:='State0', sDeviceName:='TestBackfill', ); // Everything should be cleared to the defaults: the library should let us know what params got loaded (none) AssertEquals_UDINT( Expected:=stDefaultBP.stBeamParams.nRate, Actual:=astPositionState[1][3].stPMPS.stBeamParams.nRate, Message:='nRate of motor 1 state 3 not backfilled', ); AssertEquals_UDINT( Expected:=stDefaultBP.nRequestAssertionID, Actual:=astPositionState[2][1].stPMPS.nRequestAssertionID, Message:='nRequestAssertionID of motor 2 state 3 not backfilled', ); AssertEquals_BOOL( Expected:=stDefaultBP.bBeamParamsLoaded, Actual:=astPositionState[3][2].stPMPS.bBeamParamsLoaded, Message:='bBeamParamsLoaded of motor 3 state 2 not backfilled', ); TEST_FINISHED(); END_METHOD METHOD TestDupe VAR_INST fbRead: FB_MotionReadPMPSDBND; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestDupe'); astPositionState[1] := astDuplicatedStates; fbRead( astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, sTransitionKey:='State0', sDeviceName:='TestTrio', ); fbFFHWO.EvaluateOutput(); AssertTrue( fbRead.bError, 'Should have had an error', ); FOR nIter := 0 TO GeneralConstants.MAX_STATES DO AssertEquals_STRING( Expected:='', Actual:=fbRead.astDbStateParams[nIter].sPmpsState, Message:=CONCAT('Errored output should have had no state names: ', UINT_TO_STRING(nIter)), ); END_FOR TEST_FINISHED(); END_METHOD METHOD TestHalfFull VAR_INST fbRead: FB_MotionReadPMPSDBND; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestHalfFull'); astPositionState[1] := astHalfFullStates; fbRead( astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, sTransitionKey:='State0', sDeviceName:='TestHalfFull', ); fbFFHWO.EvaluateOutput(); AssertFalse( fbRead.bError, 'Had an error', ); AssertEquals_STRING( Expected:=fbRead.sTransitionKey, Actual:=fbRead.astDbStateParams[0].sPmpsState, Message:='Output did not have the correct transition state', ); FOR nIter := 1 TO GeneralConstants.MAX_STATES / 2 DO AssertEquals_STRING( Expected:=astCorrectStates[nIter].stPMPS.sPmpsState, Actual:=fbRead.astDbStateParams[nIter].sPmpsState, Message:=CONCAT('Output did not have the correct position state: ', UINT_TO_STRING(nIter)), ); END_FOR TEST_FINISHED(); END_METHOD METHOD TestNonsense VAR_INST fbRead: FB_MotionReadPMPSDBND; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestNonsense'); astPositionState[1] := astCorrectStates; astPositionState[2] := astNonsenseStates; fbRead( astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, sTransitionKey:='State0', sDeviceName:='TestTrio', ); fbFFHWO.EvaluateOutput(); AssertTrue( fbRead.bError, 'Should have had an error', ); // Only the transition state should be spared from the nonsense AssertEquals_STRING( Expected:='State0', Actual:=fbRead.astDbStateParams[0].sPmpsState, Message:='Transition state should be OK', ); FOR nIter := 1 TO GeneralConstants.MAX_STATES DO AssertEquals_STRING( Expected:='', Actual:=fbRead.astDbStateParams[nIter].sPmpsState, Message:=CONCAT('Errored output should have had no state names: ', UINT_TO_STRING(nIter)), ); END_FOR TEST_FINISHED(); END_METHOD METHOD TestSolo VAR_INST fbRead: FB_MotionReadPMPSDBND; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestSolo'); astPositionState[1] := astCorrectStates; fbRead( astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, sTransitionKey:='State0', sDeviceName:='TestSolo', ); fbFFHWO.EvaluateOutput(); AssertFalse( fbRead.bError, 'Had an error', ); AssertEquals_STRING( Expected:=fbRead.sTransitionKey, Actual:=fbRead.astDbStateParams[0].sPmpsState, Message:='Output did not have the correct transition state', ); FOR nIter := 1 TO GeneralConstants.MAX_STATES DO AssertEquals_STRING( Expected:=astCorrectStates[nIter].stPMPS.sPmpsState, Actual:=fbRead.astDbStateParams[nIter].sPmpsState, Message:=CONCAT('Output did not have the correct position state: ', UINT_TO_STRING(nIter)), ); END_FOR TEST_FINISHED(); END_METHOD METHOD TestTrio VAR_INST fbRead: FB_MotionReadPMPSDBND; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestTrio'); astPositionState[1] := astCorrectStates; astPositionState[2] := astCorrectStates; astPositionState[3] := astCorrectStates; fbRead( astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, sTransitionKey:='State0', sDeviceName:='TestTrio', ); fbFFHWO.EvaluateOutput(); AssertFalse( fbRead.bError, 'Had an error', ); AssertEquals_STRING( Expected:=fbRead.sTransitionKey, Actual:=fbRead.astDbStateParams[0].sPmpsState, Message:='Output did not have the correct transition state', ); FOR nIter := 1 TO GeneralConstants.MAX_STATES DO AssertEquals_STRING( Expected:=astCorrectStates[nIter].stPMPS.sPmpsState, Actual:=fbRead.astDbStateParams[nIter].sPmpsState, Message:=CONCAT('Output did not have the correct position state: ', UINT_TO_STRING(nIter)), ); END_FOR TEST_FINISHED(); END_METHOD Related: * `FB_MotionReadPMPSDBND`_ * `MotionConstants`_ * `ST_PositionState`_ 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 ST_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: ST_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: E_MotionRequest := E_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 E_MotionRequest.WAIT: nState := WAIT_OTHER_MOVE; E_MotionRequest.INTERRUPT: nState := STOP_OTHER_MOVE; E_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 ST_MotionStage to start a new absolute move START_MOVE: bMyMove := TRUE; stMotionStage.bExecute := TRUE; stMotionStage.nCommand := E_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 Related: * `E_EpicsMotorCmd`_ * `E_MotionRequest`_ * `FB_MotionStage`_ * `F_MotionErrorCodeLookup`_ * `ST_MotionStage`_ 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: ST_MotionStage; END_VAR VAR fbDriveVirtual: FB_DriveVirtual; fbMotionHome: FB_MotionHoming; fbSaveRestore: FB_EncSaveRestore; fbLogError: FB_LogMotionError; 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 := E_EpicsMotorCmd.MOVE_ABSOLUTE; ELSIF rtHomeCmdShortcut.Q AND NOT stMotionStage.bExecute THEN stMotionStage.bExecute := TRUE; stMotionStage.nCommand := E_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 = E_EpicsMotorCmd.HOME THEN stMotionStage.nCmdData := stMotionStage.nHomingMode; END_IF // Check if the command wants to cause a move bMoveCmd R= stMotionStage.nCmdData = E_EpicsHomeCmd.ABSOLUTE_SET; bMoveCmd R= stMotionStage.nCmdData = E_EpicsHomeCmd.NONE; bMoveCmd S= stMotionStage.nCommand <> E_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 E_StageEnableMode.ALWAYS: stMotionStage.bEnable := TRUE; E_StageEnableMode.DURING_MOTION: IF bNewMoveReq THEN IF stMotionStage.nCommand = E_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 fbLogError( stMotionStage:=stMotionStage, bEnable:=TRUE); // 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 = E_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 <> E_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 = E_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 <> E_EpicsHomeCmd.NONE); END_FUNCTION_BLOCK Related: * `E_EpicsHomeCmd`_ * `E_EpicsMotorCmd`_ * `E_StageBrakeMode`_ * `E_StageEnableMode`_ * `FB_DriveVirtual`_ * `FB_EncSaveRestore`_ * `FB_EncoderValue`_ * `FB_LogMotionError`_ * `FB_MotionHoming`_ * `FB_MotionStageNCParams`_ * `FB_SetEnables`_ * `F_MotionErrorCodeLookup`_ * `ST_MotionStage`_ FB_MotionStageNCParams ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotionStageNCParams (* Read and refresh axis parameters struct on ST_MotionStage *) VAR_IN_OUT stMotionStage: ST_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 Related: * `ST_MotionStage`_ 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: ST_MotionStage; END_VAR VAR_INPUT nEnableMode: E_StageEnableMode := E_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 Related: * `E_StageEnableMode`_ * `FB_MotionStage`_ * `ST_MotionStage`_ FB_MotorTestSuite ^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_MotorTestSuite EXTENDS TcUnit.FB_TestSuite (* Base class for motion tests. Contains some helper methods that would otherwise need to be instantiated many times, but in this form can be accessed quickly and succinctly in the test suite. *) END_FUNCTION_BLOCK METHOD SetEnables (* Set a motion stage's enables such that it is fully allowed to move. *) VAR_IN_OUT stMotionStage: ST_MotionStage; END_VAR stMotionStage.bHardwareEnable := TRUE; stMotionStage.bLimitBackwardEnable := TRUE; stMotionStage.bLimitForwardEnable := TRUE; stMotionStage.bPowerSelf := TRUE; END_METHOD METHOD SetEnablesPMPS (* Set a motion stage's enables such that only PMPS would be preventing a move. *) VAR_IN_OUT stMotionStage: ST_MotionStage; END_VAR SetEnables(stMotionStage); stMotionStage.bPowerSelf := FALSE; END_METHOD METHOD SetGoodState (* Mark a state as valid and ready to use. *) VAR_IN_OUT stPositionState: ST_PositionState; END_VAR stPositionState.bMoveOk := TRUE; stPositionState.bValid := TRUE; END_METHOD Related: * `ST_MotionStage`_ * `ST_PositionState`_ 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 ST_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: ST_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; stBeamParams: ST_BeamParams; fbFF: FB_FastFault; rtTrip: R_TRIG; END_VAR IF NOT bInit THEN fbFF.i_Desc := sDesc; IF LEN(stMotionStage.sName) > 0 THEN fbFF.i_DevName := stMotionStage.sName; ELSE fbFF.i_DevName := 'Unnamed Motor'; END_IF bInit := TRUE; END_IF bTripped := stMotionStage.bError AND stMotionStage.nErrorId >= nLowErrorId AND stMotionStage.nErrorId <= nHighErrorId; rtTrip(CLK:=bTripped); IF rtTrip.Q THEN nErrorTypeCode := E_MotionFFType.LOW_RESERVED_NC + UDINT_TO_UINT(SHR(stMotionStage.nErrorId, 8)); nErrorTypeCode := MIN(nErrorTypeCode, E_MotionFFType.HIGH_RESERVED_NC); END_IF fbFF(i_xOK := NOT bTripped, i_xReset := bReset, i_xAutoReset := bAutoReset, i_TypeCode:= nErrorTypeCode, io_fbFFHWO := fbFFHWO); END_FUNCTION_BLOCK Related: * `E_MotionFFType`_ * `ST_MotionStage`_ FB_NCErrorFFO_Test ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_NCErrorFFO_Test EXTENDS TcUnit.FB_TestSuite (* Test the following related FBs: - FB_NCErrorFFO - FB_EncErrorFFO These function blocks are designed to trip the beam when there is an NC error reported by stMotionStage. *) VAR stMotionStage: ST_MotionStage; fbMotionStage: FB_MotionStage; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbCauseNCError: FB_CauseNCError; fbNCErrorFFO: FB_NCErrorFFO; fbEncErrorFFO: FB_EncErrorFFO; nTestCounter: UINT; bOneTestDone: BOOL; tonTimer: TON; END_VAR fbMotionStage(stMotionStage:=stMotionStage); // Limit to drive errors so I can isolate it from the enc error fbNCErrorFFO( stMotionStage:=stMotionStage, fbFFHWO:=fbFFHWO, bAutoReset:=TRUE, nLowErrorId:=16#4500, nHighErrorId:=16#45FF, ); fbEncErrorFFO( stMotionStage:=stMotionStage, fbFFHWO:=fbFFHWO, bAutoReset:=TRUE, ); // Fast fault output is TRUE when there are no issues and FALSE when there is an issue fbFFHWO.EvaluateOutput(); TestNC(0); TestEnc(1); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; tonTimer(IN:=FALSE); fbCauseNCError( Axis:=stMotionStage.Axis, bExecute:=FALSE, ); stMotionStage.bReset := TRUE; fbMotionStage(stMotionStage:=stMotionStage); END_IF // Use this timer to time out any tests that stall tonTimer( IN:=TRUE, PT:=T#5s, ); END_FUNCTION_BLOCK METHOD TestEnc VAR_INPUT nTestID: UINT; END_VAR VAR_INST nState: UINT; END_VAR TEST('TestEncError'); IF nTestCounter <> nTestID THEN RETURN; END_IF CASE nState OF 0: // First cycle: no error, no fault AssertFalse( stMotionStage.bError, 'Should have had no error before running this test', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Should have had no fault with no error', ); nState := 1; 1: // Next time: cause an error fbCauseNCError( Axis:=stMotionStage.Axis, bExecute:=TRUE, nErrorCode:=16#4467, // Invalid encoder position data ); END_CASE // Wait for the fault or the timeout, then check everything IF tonTimer.Q OR (NOT fbFFHWO.q_xFastFaultOut AND stMotionStage.nErrorID = 16#4467) THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertTrue( stMotionStage.bError, 'Did not get motor error in error test', ); AssertEquals_UDINT( Expected:=16#4467, Actual:=stMotionStage.nErrorId, Message:='Error causer is broken', ); AssertFalse( fbFFHWO.q_xFastFaultOut, Message:='Did not cause a fast fault', ); AssertFalse( fbNCErrorFFO.bTripped, Message:='Drive error FB tripped with an enc error', ); AssertTrue( fbEncErrorFFO.bTripped, Message:='Enc error fb did not trip with an enc error', ); bOneTestDone := TRUE; nState := 0; TEST_FINISHED(); END_IF END_METHOD METHOD TestNC VAR_INPUT nTestID: UINT; END_VAR VAR_INST nState: UINT; END_VAR TEST('TestNCError'); IF nTestCounter <> nTestID THEN RETURN; END_IF CASE nState OF 0: // First cycle: no error, no fault AssertFalse( stMotionStage.bError, 'Should have had no error before running this test', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Should have had no fault with no error', ); nState := 1; 1: // Next time: cause an error fbCauseNCError( Axis:=stMotionStage.Axis, bExecute:=TRUE, nErrorCode:=16#4550, // Position lag monitoring error code ); END_CASE // Wait for the fault or the timeout, then check everything IF tonTimer.Q OR (NOT fbFFHWO.q_xFastFaultOut AND stMotionStage.nErrorId = 16#4550) THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertTrue( stMotionStage.bError, 'Did not get motor error in error test', ); AssertEquals_UDINT( Expected:=16#4550, Actual:=stMotionStage.nErrorId, Message:='Error causer is broken', ); AssertFalse( fbFFHWO.q_xFastFaultOut, Message:='Did not cause a fast fault', ); AssertTrue( fbNCErrorFFO.bTripped, Message:='NC error FB did not trip with a drive error', ); AssertFalse( fbEncErrorFFO.bTripped, Message:='Enc error tripped with a drive error', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD Related: * `FB_CauseNCError`_ * `FB_EncErrorFFO`_ * `FB_MotionStage`_ * `FB_NCErrorFFO`_ * `ST_MotionStage`_ FB_PerMotorFFOND ^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PerMotorFFOND (* PMPS fast faults that must be done per motor, rather than per state, based purely on the motor status and not other PMPS considerations. These currently include: - Fault if the encoder has an error. Every other protection is based on the encoder, so we can't trust anything if the encoder is faulting. *) VAR_IN_OUT // All motors associated with the state mover. astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // Fast fault output to fault to. fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // The number of motors we're actually using nActiveMotorCount: UINT; // Identifying name to use in group fast faults sDeviceName: STRING; END_VAR VAR_OUTPUT // Set to TRUE if the arrays don't have the same bounds. In this FB, that's an automatic fault. bMotorCountError: BOOL; END_VAR VAR afbEncError: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF FB_EncErrorFFO; ffProgrammerError: FB_FastFault; nIter: DINT; END_VAR CheckCount(); IF NOT bMotorCountError THEN HandleLoops(); END_IF HandleFFO(); END_FUNCTION_BLOCK ACTION CheckCount: // Make sure the motor count is valid (positive, nonzero, less or equal to the max) bMotorCountError S= nActiveMotorCount <= 0; bMotorCountError S= nActiveMotorCount > MotionConstants.MAX_STATE_MOTORS; END_ACTION ACTION HandleFFO: ffProgrammerError( i_xOK:=NOT bMotorCountError, i_xAutoReset:=TRUE, i_DevName:=sDeviceName, i_Desc:='Programmer error picking motor count', i_TypeCode:=E_MotionFFType.INTERNAL_ERROR, io_fbFFHWO:=fbFFHWO, ); END_ACTION ACTION HandleLoops: FOR nIter := 1 TO nActiveMotorCount DO afbEncError[nIter]( stMotionstage:=astMotionStage[nIter], fbFFHWO:=fbFFHWO, bAutoReset:=TRUE, ); END_FOR END_ACTION Related: * `E_MotionFFType`_ * `FB_EncErrorFFO`_ * `MotionConstants`_ * `ST_MotionStage`_ FB_PerMotorFFOND_Test ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PerMotorFFOND_Test EXTENDS TcUnit.FB_TestSuite (* FFO if an illogical motor count is passed FFO if any motor has what looks like an encoder error - We'll just simulate this without doing it legit - The legit test is done elsewhere More FFOs may be added later and these will need to be tested too *) VAR END_VAR TestBlankCount(); TestTwoMotorEncError(); END_FUNCTION_BLOCK METHOD TestBlankCount : BOOL VAR_INST fbFFO: FB_PerMotorFFOND; astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestBlankCount'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault', ); fbFFO( astMotionStage:=astMotionStage, fbFFHWO:=fbFFHWO, ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Blank count did not fault', ); AssertTrue( fbFFO.bMotorCountError, 'Blank count did not error', ); TEST_FINISHED(); END_METHOD METHOD TestTwoMotorEncError : BOOL VAR_INST fbFFO: FB_PerMotorFFOND; astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestTwoMotorEncError'); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Started with a fault', ); astMotionStage[2].bError := TRUE; astMotionStage[2].nErrorId := 16#4467; // Invalid encoder position data fbFFO( astMotionStage:=astMotionStage, fbFFHWO:=fbFFHWO, nActiveMotorCount:=2, ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Encoder error did not fault', ); TEST_FINISHED(); END_METHOD Related: * `FB_PerMotorFFOND`_ * `MotionConstants`_ * `ST_MotionStage`_ FB_PMPSJsonTestHelper ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PMPSJsonTestHelper (* Create a JSON doc in memory to match the input structs Assumes 1 device for simplicity Writes to the global pmps json doc *) VAR_IN_OUT astBeamParams: ARRAY[*] OF ST_DbStateParams; END_VAR VAR_INPUT bExecute: BOOL; sDevName: STRING; END_VAR VAR_OUTPUT END_VAR VAR rtExec: R_TRIG; jsonRoot: SJsonValue; jsonDevice: SJsonValue; ajsonState: ARRAY[0..GeneralConstants.MAX_STATES] OF SJsonValue; fbJson: FB_JsonDomParser; nIter: DINT; nId: DINT; aseVRange: ARRAY[0..GeneralConstants.MAX_STATES] OF STRING; asRate: ARRAY[0..GeneralConstants.MAX_STATES] OF STRING; asBCRange: ARRAY[0..GeneralConstants.MAX_STATES] OF STRING; asTran: ARRAY[0..GeneralConstants.MAX_STATES] OF STRING; END_VAR rtExec(CLK:=bExecute); IF NOT rtExec.Q THEN RETURN; END_IF jsonRoot := fbJson.NewDocument(); jsonDevice := fbJson.AddObjectMember(jsonRoot, sDevName); FOR nIter := LOWER_BOUND(astBeamParams, 1) TO UPPER_BOUND(astBeamParams, 1) DO ajsonState[nIter] := fbJson.AddObjectMember(jsonDevice, astBeamParams[nIter].sPmpsState); nId := nId + 1; fbJson.AddIntMember( v:=ajsonState[nIter], member:='id', value:=nId, ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='beamline', value:='tst', ); aseVRange[nIter] := bitmaskToString(astBeamParams[nIter].stBeamParams.neVRange, 32); fbJson.AddStringMember( v:=ajsonState[nIter], member:='neVRange', value:=aseVRange[nIter], ); asRate[nIter] := UDINT_TO_STRING(astBeamParams[nIter].stBeamParams.nRate); fbJson.AddStringMember( v:=ajsonState[nIter], member:='nRate', value:=asRate[nIter], ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='ap_ygap', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='ap_xgap', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='damage_limit', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='notes', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='reactive_temp', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='reactive_pressure', value:='', ); asBCRange[nIter] := bitmaskToString(astBeamParams[nIter].stBeamParams.nBCRange, 15); fbJson.AddStringMember( v:=ajsonState[nIter], member:='nBeamClassRange', value:=asBCRange[nIter], ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='name', value:=astBeamParams[nIter].sPmpsState, ); asTran[nIter] := REAL_TO_STRING(astBeamParams[nIter].stBeamParams.nTran); fbJson.AddStringMember( v:=ajsonState[nIter], member:='nTran', value:=asTran[nIter], ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='ap_name', value:='None', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='ap_ycenter', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='ap_xcenter', value:='', ); fbJson.AddStringMember( v:=ajsonState[nIter], member:='pulse_energy', value:='', ); fbJson.AddBoolMember( v:=ajsonState[nIter], member:='special', value:=FALSE, ); END_FOR PMPS_GVL.BP_jsonDoc := jsonRoot; END_FUNCTION_BLOCK METHOD bitmaskToString : STRING VAR_INPUT nBitmask: DWORD; nBits: UINT; END_VAR VAR nIter: UINT; END_VAR bitmaskToString := ''; FOR nIter := 1 TO nBits DO bitmaskToString := CONCAT(DWORD_TO_STRING(SHR(nBitmask, nIter - 1) AND 1), bitmaskToString); END_FOR END_METHOD FB_PositionState1D ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionState1D (* 1-Dimensional position state function block. This allows the user to move a motor among some set of named state positions. To use a states block, you must define enums that match the state options and give them pytmc pragmas. See FB_PositionState1D_InOut for a simple example. These enums must be passed in as the eEnumSet and eEnumGet VAR_IN_OUT variables. The enum values must match the array indices in astPositionState. A move will begin when eEnumSet is set to a positive, nonzero value. eEnumSet will be reset to 0 on every cycle, allowing us to accept a new, possibly conflicting, move request on the next cycle to interrupt the first. The motor must already be set up for point-to-point motion via FB_MotionStage, etc., for this function block to work properly. With no PMPS handling, this FB basically just links the state names with positions in both directions for set and readback. *) VAR_IN_OUT // The motor to move. stMotionStage: ST_MotionStage; // All possible position states, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE io: io expand: :%.2d '} astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnable: BOOL; // Normal EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stEpicsToPlc: ST_StateEpicsToPlc; END_VAR VAR_OUTPUT // Normal EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPlcToEpics: ST_StatePlcToEpics; END_VAR VAR fbCore: FB_PositionStateND_Core; astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR astMotionStageMax[1] := stMotionStage; astPositionStateMax[1] := astPositionState; fbCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPlcToEpics:=stPlcToEpics, eEnumSet:=eEnumSet, eEnumGet:=eEnumGet, bEnable:=bEnable, nActiveMotorCount:=1, ); stMotionStage := astMotionStageMax[1]; astPositionState := astPositionStateMax[1]; END_FUNCTION_BLOCK Related: * `FB_MotionStage`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateND_Core`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePlcToEpics`_ FB_PositionState1D_InOut ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionState1D_InOut (* An example and usable drop-in instance for a 1D state device with just an IN and an OUT state. *) VAR_IN_OUT // Include a stage that can be passed into the FB stMotionStage: ST_MotionStage; // Simplify the interface, the user just needs to construct and pass in and out position states stIn: ST_PositionState; stOut: ST_PositionState; END_VAR VAR_INPUT // Define an ENUM for EPICS to use to set a new value. It is expected this will be written to during one cycle, so don't continually apply a value here in the PLC code. // It is exposed as an input so we can test it using the PLC. {attribute 'pytmc' := ' pv: STATE:SET io: io '} eStateReq: E_EpicsInOut; END_VAR VAR_OUTPUT // Define an ENUM for EPICS to use to report the new value. {attribute 'pytmc' := ' pv: STATE:GET io: i '} eStateGet: E_EpicsInOut; END_VAR VAR // Include the standard fb with blank pv pragma {attribute 'pytmc' := 'pv:'} fbPositionState1D: FB_PositionState1D; // The standard fb expects a full array of position states astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR // Optional: default state names IF stIn.sName = '' THEN stIn.sName := 'IN'; END_IF IF stOut.sName = '' THEN stOut.sName := 'OUT'; END_IF // Assemble the states array, matching the enum values (IN is 1, OUT is 2) astPositionState[1] := stIn; astPositionState[2] := stOut; // Call the main function block, passing our motors, states, enums and an enable fbPositionState1D( stMotionStage:=stMotionStage, astPositionState:=astPositionState, eEnumSet:=eStateReq, eEnumGet:=eStateGet, bEnable:=TRUE, ); // Send updates made on the array back to the inputs (VAR_IN_OUT) for maximum clarity stIn := astPositionState[1]; stOut := astPositionState[2]; END_FUNCTION_BLOCK Related: * `E_EpicsInOut`_ * `FB_PositionState1D`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionState2D ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionState2D (* 2-Dimensional position state function block. This allows the user to move 2 motors among some set of named state positions. To use a states block, you must define enums that match the state options and give them pytmc pragmas. See FB_PositionState1D_InOut for a simple example. These enums must be passed in as the eEnumSet and eEnumGet VAR_IN_OUT variables. The enum values must match the array indices in astPositionState1 and astPositionState2. A move will begin when eEnumSet is set to a positive, nonzero value. eEnumSet will be reset to 0 on every cycle, allowing us to accept a new, possibly conflicting, move request on the next cycle to interrupt the first. The motors must already be set up for point-to-point motion via FB_MotionStage, etc., for this function block to work properly. With no PMPS handling, this FB basically just links the state names with positions in both directions for set and readback. *) VAR_IN_OUT // The 1st motor to move stMotionStage1: ST_MotionStage; // The 2nd motor to move stMotionStage2: ST_MotionStage; // All possible position states for motor 1, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:01 io: io expand: :%.2d '} astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // All possible position states for motor 2, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:02 io: io expand: :%.2d '} astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnable: BOOL; // Normal EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stEpicsToPlc: ST_StateEpicsToPlc; END_VAR VAR_OUTPUT // Normal EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPlcToEpics: ST_StatePlcToEpics; END_VAR VAR fbCore: FB_PositionStateND_Core; astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR astMotionStageMax[1] := stMotionStage1; astMotionStageMax[2] := stMotionStage2; astPositionStateMax[1] := astPositionState1; astPositionStateMax[2] := astPositionState2; fbCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPlcToEpics:=stPlcToEpics, eEnumSet:=eEnumSet, eEnumGet:=eEnumGet, bEnable:=bEnable, nActiveMotorCount:=2, ); stMotionStage1 := astMotionStageMax[1]; stMotionStage2 := astMotionStageMax[2]; astPositionState1 := astPositionStateMax[1]; astPositionState2 := astPositionStateMax[2]; END_FUNCTION_BLOCK Related: * `FB_MotionStage`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateND_Core`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePlcToEpics`_ FB_PositionState2D_InOut ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionState2D_InOut (* An example and usable drop-in instance for a 2D state device with just an IN and an OUT state. *) VAR_IN_OUT // Include stages that can be passed into the FB stMotionStage1: ST_MotionStage; stMotionStage2: ST_MotionStage; // Simplify the interface, the user just needs to construct and pass in and out position states stIn1: ST_PositionState; stOut1: ST_PositionState; stIn2: ST_PositionState; stOut2: ST_PositionState; END_VAR VAR_INPUT // Define an ENUM for EPICS to use to set a new value. It is expected this will be written to during one cycle, so don't continually apply a value here in the PLC code. // It is exposed as an input so we can test it using the PLC. {attribute 'pytmc' := ' pv: STATE:SET io: io '} eStateSet: E_EpicsInOut; END_VAR VAR_OUTPUT // Define an ENUM for EPICS to use to report the new value. {attribute 'pytmc' := ' pv: STATE:GET io: i '} eStateGet: E_EpicsInOut; END_VAR VAR // Include the standard fb with blank pv pragma {attribute 'pytmc' := 'pv:'} fbPositionState2D: FB_PositionState2D; // The standard fb expects a full array of position states per motor astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR // Optional: default state names IF stIn1.sName = '' THEN stIn1.sName := 'IN'; END_IF IF stOut1.sName = '' THEN stOut1.sName := 'OUT'; END_IF IF stIn2.sName = '' THEN stIn2.sName := 'IN'; END_IF IF stOut2.sName = '' THEN stOut2.sName := 'OUT'; END_IF // Assemble the states arrays, matching the enum values (IN is 1, OUT is 2) astPositionState1[1] := stIn1; astPositionState1[2] := stOut1; astPositionState2[1] := stIn2; astPositionState2[2] := stOut2; // Call the main function block, passing our motors, states, and an enable fbPositionState2D( stMotionStage1:=stMotionStage1, astPositionState1:=astPositionState1, stMotionStage2:=stMotionStage2, astPositionState2:=astPositionState2, eEnumSet:=eStateSet, eEnumGet:=eStateGet, bEnable:=TRUE, ); // Send updates made on the array back to the inputs (VAR_IN_OUT) for maximum clarity stIn1 := astPositionState1[1]; stOut1 := astPositionState1[2]; stIn2 := astPositionState2[1]; stOut2 := astPositionState2[2]; END_FUNCTION_BLOCK Related: * `E_EpicsInOut`_ * `FB_PositionState2D`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionState3D ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionState3D (* 3-Dimensional position state function block. This allows the user to move 3 motors among some set of named state positions. To use a states block, you must define enums that match the state options and give them pytmc pragmas. See FB_PositionState1D_InOut for a simple example. These enums must be passed in as the eEnumSet and eEnumGet VAR_IN_OUT variables. The enum values must match the array indices in astPositionState1, astPositionState2, and astPositionState3. A move will begin when eEnumSet is set to a positive, nonzero value. eEnumSet will be reset to 0 on every cycle, allowing us to accept a new, possibly conflicting, move request on the next cycle to interrupt the first. The motors must already be set up for point-to-point motion via FB_MotionStage, etc., for this function block to work properly. With no PMPS handling, this FB basically just links the state names with positions in both directions for set and readback. *) VAR_IN_OUT // The 1st motor to move stMotionStage1: ST_MotionStage; // The 2nd motor to move stMotionStage2: ST_MotionStage; // The 3rd motor to move stMotionStage3: ST_MotionStage; // All possible position states for motor 1, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:01 io: io expand: :%.2d '} astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // All possible position states for motor 2, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:02 io: io expand: :%.2d '} astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // All possible position states for motor 3, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:03 io: io expand: :%.2d '} astPositionState3: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnable: BOOL; // Normal EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stEpicsToPlc: ST_StateEpicsToPlc; END_VAR VAR_OUTPUT // Normal EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPlcToEpics: ST_StatePlcToEpics; END_VAR VAR fbCore: FB_PositionStateND_Core; astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR astMotionStageMax[1] := stMotionStage1; astMotionStageMax[2] := stMotionStage2; astMotionStageMax[3] := stMotionStage3; astPositionStateMax[1] := astPositionState1; astPositionStateMax[2] := astPositionState2; astPositionStateMax[3] := astPositionState3; fbCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPlcToEpics:=stPlcToEpics, eEnumSet:=eEnumSet, eEnumGet:=eEnumGet, bEnable:=bEnable, nActiveMotorCount:=3, ); stMotionStage1 := astMotionStageMax[1]; stMotionStage2 := astMotionStageMax[2]; stMotionStage3 := astMotionStageMax[3]; astPositionState1 := astPositionStateMax[1]; astPositionState2 := astPositionStateMax[2]; astPositionState3 := astPositionStateMax[3]; END_FUNCTION_BLOCK Related: * `FB_MotionStage`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateND_Core`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePlcToEpics`_ FB_PositionStateBase ^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionState1D instead'} 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: ST_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..GeneralConstants.MAX_STATES] OF ST_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: ST_PositionState; stGoal: ST_PositionState; fbStateMove: FB_PositionStateMove; fbStateInternal: ARRAY[1..GeneralConstants.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 GeneralConstants.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 GeneralConstants.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 := E_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 Related: * `E_MotionRequest`_ * `FB_PositionState1D`_ * `FB_PositionStateInOut`_ * `FB_PositionStateInternal`_ * `FB_PositionStateMove`_ * `F_AtPositionState`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateBase_WithPMPS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionStatePMPS1D instead'} 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 sPmpsDeviceName: STRING; sTransitionKey: STRING; {attribute 'pytmc' := ' pv: PMPS:ARB:ENABLE io: io '} bArbiterEnabled: BOOL := TRUE; tArbiterTimeout: TIME := T#1s; bMoveOnArbiterTimeout: BOOL := TRUE; fStateBoundaryDeadband: LREAL := 0; bBPOKAutoReset: BOOL := False; 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, sPmpsDeviceName:=sPmpsDeviceName, sTransitionKey:=sTransitionKey, stPmpsDoc:= PMPS_GVL.BP_jsonDoc, bRequestTransition:=setState <> 0, setState:=setState, getState:=getState, bArbiterEnabled:=bArbiterEnabled, tArbiterTimeout:=tArbiterTimeout, bMoveOnArbiterTimeout:=bMoveOnArbiterTimeout, fStateBoundaryDeadband:=fStateBoundaryDeadband, bBPOKAutoReset:=bBPOKAutoReset); fbEncErrFFO( stMotionStage:=stMotionStage, fbFFHWO:=fbFFHWO, bAutoReset:=TRUE); END_ACTION Related: * `FB_EncErrorFFO`_ * `FB_PositionStateBase`_ * `FB_PositionStateInOut_WithPMPS`_ * `FB_PositionStatePMPS`_ * `FB_PositionStatePMPS1D`_ FB_PositionStateBase_WithPMPS_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'No longer any reason to use this, all state classes can have unit tests.'} 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); END_ACTION Related: * `FB_PositionStateBase`_ * `FB_PositionStateInOut_WithPMPS_Test`_ * `FB_PositionStatePMPS_Test`_ FB_PositionStateInOut ^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionState1D_InOut instead'} 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_INT; // NOTE: Please copy this pragma exactly on your enumSet // Information about the OUT position stOut: ST_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN position stIn: ST_PositionState; END_VAR VAR_OUTPUT // The enum state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut_INT; // 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; stOut.sName := 'OUT'; stIn.sName := 'IN'; END_IF setState := enumSet; Exec(); enumGet := getState; enumSet := setState; END_FUNCTION_BLOCK Related: * `ENUM_EpicsInOut_INT`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateBase`_ * `ST_PositionState`_ FB_PositionStateInOut_WithPMPS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionStatePMPS1D_InOut instead'} 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_INT; // NOTE: Please copy this pragma exactly on your enumSet // Information about the OUT position stOut: ST_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN position stIn: ST_PositionState; END_VAR VAR_OUTPUT // The enum state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut_INT; // 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; stOut.sName := 'OUT'; stIn.sName := 'IN'; END_IF setState := enumSet; Exec(); enumGet := getState; enumSet := setState; END_FUNCTION_BLOCK Related: * `ENUM_EpicsInOut_INT`_ * `FB_PositionStateBase_WithPMPS`_ * `FB_PositionStatePMPS1D_InOut`_ * `ST_PositionState`_ FB_PositionStateInOut_WithPMPS_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'No longer any reason to use this, all state classes can have unit tests.'} 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_INT; // NOTE: Please copy this pragma exactly on your enumSet // Information about the OUT position stOut: ST_PositionState; // NOTE: Do not pragma these, let it happen in the manager. // Information about the IN position stIn: ST_PositionState; END_VAR VAR_OUTPUT // The enum state readback {attribute 'pytmc' := ' pv: GET io: i '} enumGet: ENUM_EpicsInOut_INT; // 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; stOut.sName := 'OUT'; stIn.sName := 'IN'; END_IF setState := enumSet; Exec(); enumGet := getState; enumSet := setState; END_FUNCTION_BLOCK Related: * `ENUM_EpicsInOut_INT`_ * `FB_PositionStateBase_WithPMPS_Test`_ * `ST_PositionState`_ FB_PositionStateInternal ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateInternal (* Routines that must be called on all ST_PositionState Currently, this FB: - ensures that a position state has both a proper encoder count and a proper position in engineering units with both of these quantities matching - handles the parameter locking feature, which nominally prevents the user from changing details about a locked state via EPICS *) VAR_IN_OUT stMotionStage: ST_MotionStage; stPositionState: ST_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 Related: * `FB_PositionStateLock`_ * `FB_RawCountConverter`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateInternalND ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateInternalND (* Given a standard ND state setup, call all the required state management FBs. *) VAR_IN_OUT // All the motors to apply the standard routines to astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // Each motor's respective position states along its direction astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR VAR // The individual instantiated internal FBs. Must have the same bounds as astPositionState. afbStateInternal: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF FB_PositionStateInternal; nIterMotors: DINT; nIterStates: DINT; END_VAR FOR nIterMotors := 1 TO MotionConstants.MAX_STATE_MOTORS DO FOR nIterStates := 1 TO GeneralConstants.MAX_STATES DO afbStateInternal[nIterMotors][nIterStates]( stMotionStage:=astMotionStage[nIterMotors], stPositionState:=astPositionState[nIterMotors][nIterStates], ); END_FOR END_FOR END_FUNCTION_BLOCK Related: * `FB_PositionStateInternal`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateLock ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateLock (* Implements immutability for a locked ST_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: ST_PositionState; END_VAR VAR_INPUT bEnable: BOOL; END_VAR VAR stCachedPositionState: ST_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 Related: * `ST_PositionState`_ FB_PositionStateManager ^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionState1D instead'} 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: ST_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 ST_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: ST_PositionState; stGoal: ST_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 := E_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 Related: * `E_MotionRequest`_ * `FB_EpicsInOut`_ * `FB_PositionState1D`_ * `FB_PositionStateInternal`_ * `FB_PositionStateMove`_ * `F_AtPositionState`_ * `ST_MotionStage`_ * `ST_PositionState`_ 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 another states function block. *) VAR_IN_OUT // Motor to move stMotionStage: ST_MotionStage; // State to move to {attribute 'pytmc' := ' pv: '} stPositionState: ST_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: E_MotionRequest := E_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 not 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 Related: * `E_MotionRequest`_ * `FB_MotionRequest`_ * `F_AtPositionState`_ * `F_MotionErrorCodeLookup`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateMove_Test ^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateMove_Test EXTENDS FB_MotorTestSuite (* Test that FB_PositionStateMove can be used to move motors to named state positions And that the API behaves exactly as described. *) VAR stMotionStage: ST_MotionStage; fbMotionStage: FB_MotionStage; astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbInternal: ARRAY[1..3] OF FB_PositionStateInternal; stDummyPos: ST_PositionState; stInvalid: ST_PositionState; stNotUpdated: ST_PositionState; stUnsafe: ST_PositionState; fbMove: FB_PositionStateMove; nTestCounter: UINT; bOneTestDone: BOOL; fTestStartPos: LREAL; tonTimer: TON; nIter: DINT; bStatesReady: BOOL; END_VAR astPositionState[1].sName := 'UNO'; astPositionState[1].fPosition := 10; astPositionState[1].fDelta := 1; astPositionState[1].fVelocity := 10; SetGoodState(astPositionState[1]); astPositionState[2].sName := 'DOS'; astPositionState[2].fPosition := 20; astPositionState[2].fDelta := 1; astPositionState[2].fVelocity := 10; SetGoodState(astPositionState[2]); astPositionState[3].sName := 'TRES'; astPositionState[3].fPosition := 30; astPositionState[3].fDelta := 1; astPositionState[3].fVelocity := 10; SetGoodState(astPositionState[3]); bStatesReady := TRUE; FOR nIter := 1 TO 3 DO afbInternal[nIter]( stMotionStage:=stMotionStage, stPositionState:=astPositionState[nIter], ); bStatesReady := bStatesReady AND astPositionState[nIter].bUpdated; END_FOR stInvalid.bValid := FALSE; stInvalid.bUpdated := TRUE; stInvalid.bMoveOk := TRUE; stNotUpdated.bValid := TRUE; stNotUpdated.bUpdated := FALSE; stNotUpdated.bMoveOk := TRUE; stUnsafe.bValid := TRUE; stUnsafe.bUpdated := TRUE; stUnsafe.bMoveOk := FALSE; SetEnables(stMotionStage); fbMotionStage(stMotionStage:=stMotionStage); IF bStatesReady AND nTestCounter = 0 THEN // Don't run any tests until the states are ready nTestCounter := 1; // Warm up the motion FB with a exec false runthrough fbMove( stMotionStage:=stMotionStage, stPositionState:=stDummyPos, bExecute:=FALSE, ); END_IF // Test that we can move to state 1 and the outputs are correct as we go TestMove(1, 1, FALSE); // Test that we can move to state 2 and the outputs are correct as we go TestMove(2, 2, FALSE); // Test that we can interrupt a move to state 3 by dropping bExecute TestMove(3, 3, TRUE); // Test that we cannot move to an invalid state TestBadStates(4); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; fbMove( stMotionStage:=stMotionStage, stPositionState:=stDummyPos, bExecute:=FALSE, bReset:=TRUE, ); fbMove( stMotionStage:=stMotionStage, stPositionState:=stDummyPos, bExecute:=FALSE, bReset:=FALSE, ); tonTimer(IN:=FALSE); END_IF // Use this timer to time out any tests that stall tonTimer( IN:=bStatesReady, PT:=T#5s, ); END_FUNCTION_BLOCK METHOD TestBadStates VAR_INPUT nTestIndex: UINT; END_VAR TEST(CONCAT('TestInvalid', UINT_TO_STRING(nTestIndex))); IF nTestCounter <> nTestIndex THEN RETURN; END_IF AssertFalse( fbMove.bError, 'Started with an error', ); fbMove( stMotionStage:=stMotionstage, stPositionState:=stInvalid, bExecute:=TRUE, ); AssertTrue( fbMove.bError, 'Invalid should have given an error', ); fbMove( stMotionStage:=stMotionstage, stPositionState:=stInvalid, bExecute:=FALSE, bReset:=TRUE, ); AssertFalse( fbMove.bError, 'Started with an error', ); fbMove( stMotionStage:=stMotionstage, stPositionState:=stNotUpdated, bExecute:=TRUE, bReset:=FALSE, ); AssertTrue( fbMove.bError, 'Not updated should have given an error', ); fbMove( stMotionStage:=stMotionstage, stPositionState:=stNotUpdated, bExecute:=FALSE, bReset:=TRUE, ); AssertFalse( fbMove.bError, 'Started with an error', ); fbMove( stMotionStage:=stMotionstage, stPositionState:=stUnsafe, bExecute:=TRUE, bReset:=FALSE, ); AssertTrue( fbMove.bError, 'Unsafe should have given an error', ); bOneTestDone := TRUE; TEST_FINISHED(); END_METHOD METHOD TestMove VAR_INPUT nTestIndex: UINT; nStateIndex: UINT; bInterrupt: BOOL; END_VAR VAR_INST bLocalInit: BOOL; bInterruptStarted: BOOL; END_VAR TEST(CONCAT('TestMove', UINT_TO_STRING(nTestIndex))); IF nTestCounter <> nTestIndex THEN RETURN; END_IF IF NOT bLocalInit THEN // Starting output checks AssertFalse( Condition:=fbMove.bBusy, Message:='Tried to start test with busy motor', ); AssertFalse( Condition:=fbMove.bError, Message:='Tried to start test with errored motor', ); bLocalInit := TRUE; END_IF bInterruptStarted S= bInterrupt AND stMotionStage.bBusy; fbMove( stMotionStage:=stMotionstage, stPositionState:=astPositionState[nStateIndex], bExecute:=NOT bInterruptStarted, ); IF fbMove.bDone OR tonTimer.Q OR (bInterruptStarted AND NOT fbMove.bBusy) THEN AssertFalse( tonTimer.Q, 'Test timed out', ); IF bInterrupt THEN AssertFalse( fbMove.bAtState, Message:='Should have been interrupted, but made it to the goal', ); ELSE AssertTrue( fbMove.bAtState, Message:='Did not end at the state', ); AssertEquals_LREAL( Expected:=astPositionState[nStateIndex].fPosition, Actual:=stMotionStage.stAxisStatus.fActPosition, Delta:=0.01, Message:='Did not reach the goal state', ); END_IF AssertFalse( fbMove.bBusy, Message:='Was busy while done', ); AssertFalse( fbMove.bError, Message:=CONCAT('Should not end in error: ', stMotionStage.sErrorMessage), ); bOneTestDone := TRUE; bLocalInit := FALSE; bInterruptStarted := FALSE; TEST_FINISHED(); END_IF END_METHOD Related: * `FB_MotionStage`_ * `FB_MotorTestSuite`_ * `FB_PositionStateInternal`_ * `FB_PositionStateMove`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateMoveND ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateMoveND (* This function block coordinates multidimensional state moves for groups of motors. It is a building block not meant for use outside of lcls-twintcat-motion. Use FB_PositionState1D, FB_PositionState2D, ... etc. instead *) VAR_IN_OUT // Array of motors to move together astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // 1D Position states: the current position to send each axis to astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_PositionState; END_VAR VAR_INPUT // The number of motors we're actually using nActiveMotorCount: UINT; // Start all moves on rising edge, stop all moves on falling edge bExecute: BOOL; // Reset any errors bReset: BOOL; // Define behavior for when a move request is interrupted with a new request enumMotionRequest: E_MotionRequest := E_MotionRequest.WAIT; END_VAR VAR_OUTPUT // TRUE if ALL of the motors are at their goal states bAtState: BOOL; // TRUE if ANY of this FB's moves are in progress bBusy: BOOL; // TRUE if ALL motors have completed the last move request from this FB bDone: BOOL; // TRUE if ANY of this FB's moves had an error bError: BOOL; // How many FBs are erroring nErrorCount: UINT; // Which component is the source of the example/summarized error nShownError: DINT; // One of the error ids nErrorID: UDINT; // The error error message matching the ID sErrorMessage: STRING; END_VAR VAR // 1D State movers: FBs to move the motors afbPositionStateMove: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF FB_PositionStateMove; nIndex: DINT; bMotorCountError: BOOL; nLowerBound: DINT; nUpperBound: DINT; END_VAR CheckCount(); IF NOT bMotorCountError THEN DoStateMoves(); CombineOutputs(); END_IF END_FUNCTION_BLOCK ACTION CheckCount: // Make sure the motor count is valid (positive, nonzero, less or equal to the max) bMotorCountError S= nActiveMotorCount <= 0; bMotorCountError S= nActiveMotorCount > MotionConstants.MAX_STATE_MOTORS; IF bMotorCountError THEN bError := TRUE; sErrorMessage := 'Internal Error: invalid motor count'; END_IF END_ACTION ACTION CombineOutputs: // bAtState is TRUE if ALL entries are TRUE bAtState := TRUE; FOR nIndex := 1 TO nActiveMotorCount DO bAtState := bAtState AND afbPositionStateMove[nIndex].bAtState; END_FOR // bBusy is TRUE if ANY entry is TRUE bBusy := FALSE; FOR nIndex := 1 TO nActiveMotorCount DO bBusy := bBusy OR afbPositionStateMove[nIndex].bBusy; END_FOR // bDone is TRUE if ALL entries are TRUE bDone := TRUE; FOR nIndex := 1 TO nActiveMotorCount DO bDone := bDone AND afbPositionStateMove[nIndex].bDone; END_FOR // bError is TRUE if ANY entry is TRUE // also set nShownError and increment nErrorCount bError := FALSE; nErrorCount := 0; FOR nIndex := 1 TO nActiveMotorCount DO bError := bError OR afbPositionStateMove[nIndex].bError; IF afbPositionStateMove[nIndex].bError THEN nShownError := nIndex; nErrorCount := nErrorCount + 1; END_IF END_FOR // Pick error id and message using nShownError IF bError THEN nErrorID := afbPositionStateMove[nShownError].nErrorID; sErrorMessage := afbPositionStateMove[nShownError].sErrorMessage; ELSE nErrorID := 0; sErrorMessage := ''; END_IF END_ACTION ACTION DoStateMoves: // Do the individual moves FOR nIndex := 1 TO nActiveMotorCount DO afbPositionStateMove[nIndex]( stMotionStage:=astMotionStage[nIndex], stPositionState:=astPositionState[nIndex], bExecute:=bExecute, bReset:=bReset, enumMotionRequest:=enumMotionRequest, ); END_FOR END_ACTION Related: * `E_MotionRequest`_ * `FB_PositionState1D`_ * `FB_PositionState2D`_ * `FB_PositionStateMove`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateMoveND_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateMoveND_Test EXTENDS FB_MotorTestSuite (* Test that FB_PositionStateMoveND can be used to move motors to named state positions And that the API behaves exactly as described. *) VAR astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; afbMotionStage: ARRAY[1..3] OF FB_MotionStage; astGoalPositions: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_PositionState; afbInternal: ARRAY[1..3] OF FB_PositionStateInternal; stDummyPos: ST_PositionState; fbMove: FB_PositionStateMoveND; bInit: BOOL; nTestCounter: UINT; bOneTestDone: BOOL; fTestStartPos: LREAL; tonTimer: TON; nIter: DINT; bStatesReady: BOOL; END_VAR IF NOT bInit THEN ResetGoals(); bInit := TRUE; END_IF bStatesReady := TRUE; FOR nIter := 1 TO 3 DO afbInternal[nIter]( stMotionStage:=astMotionStage[nIter], stPositionState:=astGoalPositions[nIter], ); bStatesReady := bStatesReady AND astGoalPositions[nIter].bUpdated; SetEnables(astMotionStage[nIter]); afbMotionStage[nIter](stMotionStage:=astMotionStage[nIter]); END_FOR IF bStatesReady AND nTestCounter = 0 THEN // Don't run any tests until the states are ready nTestCounter := 1; // Warm up the motion FB with a exec false runthrough fbMove( astMotionStage := astMotionStage, astPositionState := astGoalPositions, nActiveMotorCount := 3, bExecute := FALSE, ); END_IF // Move to somewhere TestMove(1, 1, 5, 10, FALSE); // Somewhere else TestMove(2, -10, 0, 5, FALSE); // Interrupt on the way to the last place TestMove(3, 0, 0, 0, TRUE); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; ResetGoals(); fbMove( astMotionStage := astMotionStage, astPositionState := astGoalPositions, nActiveMotorCount := 3, bExecute := FALSE, bReset := TRUE, ); tonTimer(IN:=FALSE); END_IF // Use this timer to time out any tests that stall tonTimer( IN:=bStatesReady, PT:=T#5s, ); END_FUNCTION_BLOCK METHOD ResetGoals VAR_INPUT END_VAR astGoalPositions[1].sName := 'Goal1'; astGoalPositions[1].fPosition := 0; astGoalPositions[1].fDelta := 1; astGoalPositions[1].fVelocity := 10; astGoalPositions[1].bUseRawCounts := FALSE; SetGoodState(astGoalPositions[1]); astGoalPositions[2].sName := 'Goal2'; astGoalPositions[2].fPosition := 0; astGoalPositions[2].fDelta := 1; astGoalPositions[2].fVelocity := 10; astGoalPositions[2].bUseRawCounts := FALSE; SetGoodState(astGoalPositions[2]); astGoalPositions[3].sName := 'Goal3'; astGoalPositions[3].fPosition := 0; astGoalPositions[3].fDelta := 1; astGoalPositions[3].fVelocity := 10; astGoalPositions[3].bUseRawCounts := FALSE; SetGoodState(astGoalPositions[3]); END_METHOD METHOD TestMove VAR_INPUT nTestIndex: UINT; fMotor1Pos: LREAL; fMotor2Pos: LREAL; fMotor3Pos: LREAL; bInterrupt: BOOL; END_VAR VAR_INST bLocalInit: BOOL; bInterruptStarted: BOOL; END_VAR TEST(CONCAT('TestMove', UINT_TO_STRING(nTestIndex))); IF nTestCounter <> nTestIndex THEN RETURN; END_IF IF NOT bLocalInit THEN // Starting output checks AssertFalse( Condition:=fbMove.bBusy, Message:='Tried to start test with busy motor', ); AssertFalse( Condition:=fbMove.bError, Message:='Tried to start test with errored motor', ); bLocalInit := TRUE; END_IF astGoalPositions[1].fPosition := fMotor1Pos; astGoalPositions[2].fPosition := fMotor2Pos; astGoalPositions[3].fPosition := fMotor3Pos; bInterruptStarted S= bInterrupt AND astMotionStage[1].bBusy AND astMotionStage[2].bBusy AND astMotionStage[3].bBusy; fbMove( astMotionStage:=astMotionStage, astPositionState:=astGoalPositions, nActiveMotorCount:=3, bExecute:=NOT bInterruptStarted, ); IF fbMove.bDone OR tonTimer.Q OR (bInterruptStarted AND NOT fbMove.bBusy) THEN IF bInterrupt THEN AssertFalse( fbMove.bAtState, Message:='Should have been interrupted, but made it to the goal', ); ELSE AssertTrue( fbMove.bAtState, Message:='Did not end at the state', ); FOR nIter := 1 TO 3 DO AssertEquals_LREAL( Expected:=astGoalPositions[nIter].fPosition, Actual:=astMotionStage[nIter].stAxisStatus.fActPosition, Delta:=0.01, Message:='Did not reach the goal state', ); END_FOR END_IF AssertFalse( fbMove.bBusy, Message:='Was busy while done', ); AssertFalse( fbMove.bError, Message:='Should not end in error', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD Related: * `FB_MotionStage`_ * `FB_MotorTestSuite`_ * `FB_PositionStateInternal`_ * `FB_PositionStateMoveND`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateND_Core ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateND_Core (* Collection of all the actions shared between all states FBs This is used in e.g. - FB_PositionState1D - FB_PositionState2D - ... etc. - FB_PositionStatePMPS1D - FB_PositionStatePMPS2D - ... etc. This is essentially input handling, position state reading, standard management blocks, and the motion state machine. *) VAR_IN_OUT // All motors to be used in the states move, including blank/uninitialized structs. astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // All position states for all motors, including unused/invalid states. astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Normal EPICS inputs, gathered into a single struct stEpicsToPlc: ST_StateEpicsToPlc; // Normal EPICS outputs, gathered into a single struct stPlcToEpics: ST_StatePlcToEpics; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnable: BOOL; // Set this to the number of motors to be included in astMotionStageMax nActiveMotorCount: UINT; END_VAR VAR_OUTPUT // The current position index goal, where the motors are supposed to be moving towards. nCurrGoal: UINT; END_VAR VAR fbInput: FB_StatesInputHandler; fbInternal: FB_PositionStateInternalND; fbMove: FB_PositionStateMoveND; fbRead: FB_PositionStateReadND; astMoveGoals: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_PositionState; stInvalidPos: ST_PositionState; nIterMotor: DINT; END_VAR stEpicsToPlc.nSetValue := eEnumSet; fbInternal( astMotionStage:=astMotionStageMax, astPositionState:=astPositionStateMax, ); fbRead( astMotionStage:=astMotionStageMax, astPositionState:=astPositionStateMax, nActiveMotorCount:=nActiveMotorCount, bKnownState=>, bMovingState=>, nPositionIndex=>stPlcToEpics.nGetValue, ); fbInput( stUserInput:=stEpicsToPlc, bMoveBusy:=fbMove.bBusy, nStartingState:=fbRead.nPositionIndex, nCurrGoal=>nCurrGoal, bExecMove=>, bResetMove=>, ); FOR nIterMotor := 1 TO MotionConstants.MAX_STATE_MOTORS DO IF nCurrGoal > 0 THEN astMoveGoals[nIterMotor] := astPositionStateMax[nIterMotor][nCurrGoal]; ELSE astMoveGoals[nIterMotor] := stInvalidPos; END_IF END_FOR fbMove( astMotionStage:=astMotionStageMax, astPositionState:=astMoveGoals, nActiveMotorCount:=nActiveMotorCount, bExecute:=fbInput.bExecMove AND bEnable, bReset:=fbInput.bResetMove, enumMotionRequest:=E_MotionRequest.INTERRUPT, bAtState=>, bError=>stPlcToEpics.bError, nErrorID=>stPlcToEpics.nErrorID, sErrorMessage=>stPlcToEpics.sErrorMsg, bBusy=>stPlcToEpics.bBusy, bDone=>stPlcToEpics.bDone, ); eEnumSet := stEpicsToPlc.nSetValue; eEnumGet := stPlcToEpics.nGetValue; END_FUNCTION_BLOCK Related: * `E_MotionRequest`_ * `FB_PositionState1D`_ * `FB_PositionState2D`_ * `FB_PositionStateInternalND`_ * `FB_PositionStateMoveND`_ * `FB_PositionStatePMPS1D`_ * `FB_PositionStatePMPS2D`_ * `FB_PositionStateReadND`_ * `FB_StatesInputHandler`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePlcToEpics`_ FB_PositionStateND_Test ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateND_Test EXTENDS FB_MotorTestSuite (* Sanity checks for the following: - FB_PositionState1D - FB_PositionState2D - FB_PositionState3D The internals have already been tested, but we need to make sure that they've been put together at least somewhat sensibly. This FB will simply use each FB to move and check the results. Additional tests: - Regression test for issue #197 (input deadlock) *) VAR stMotionStage1: ST_MotionStage; stMotionStage2: ST_MotionStage; stMotionStage3: ST_MotionStage; astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astPositionState3: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbInternal: ARRAY[1..3] OF ARRAY[1..3] OF FB_PositionStateInternal; afbMotionStage: ARRAY[1..3] OF FB_MotionStage; fb_Move1D: FB_PositionState1D; fb_Move2D: FB_PositionState2D; fb_Move3D: FB_PositionState3D; nTestCounter: UINT; bOneTestDone: BOOL; fTestStartPos: LREAL; tonTimer: TON; nIter: DINT; bStatesReady: BOOL; eSetPos: E_TestStates; eGetPos: E_TestStates; END_VAR bStatesReady := TRUE; FOR nIter := 1 TO 3 DO; astPositionState1[nIter].fPosition := nIter; astPositionState1[nIter].fDelta := 1; astPositionState1[nIter].fVelocity := 100; astPositionState2[nIter].fPosition := 10 + nIter; astPositionState2[nIter].fDelta := 1; astPositionState2[nIter].fVelocity := 100; astPositionState3[nIter].fPosition := 20 + nIter; astPositionState3[nIter].fDelta := 1; astPositionState3[nIter].fVelocity := 100; afbInternal[nIter][1]( stMotionStage:=stMotionStage1, stPositionState:=astPositionState1[nIter], ); afbInternal[nIter][2]( stMotionStage:=stMotionStage2, stPositionState:=astPositionState2[nIter], ); afbInternal[nIter][3]( stMotionStage:=stMotionStage3, stPositionState:=astPositionState3[nIter], ); SetGoodState(astPositionState1[nIter]); SetGoodState(astPositionState2[nIter]); SetGoodState(astPositionState3[nIter]); bStatesReady := bStatesReady AND astPositionState1[nIter].bUpdated; bStatesReady := bStatesReady AND astPositionState2[nIter].bUpdated; bStatesReady := bStatesReady AND astPositionState3[nIter].bUpdated; END_FOR SetEnables(stMotionStage1); SetEnables(stMotionStage2); SetEnables(stMotionStage3); afbMotionStage[1](stMotionStage:=stMotionStage1); afbMotionStage[2](stMotionStage:=stMotionStage2); afbMotionStage[3](stMotionStage:=stMotionStage3); IF bStatesReady AND nTestCounter = 0 THEN // Don't run any tests until the states are ready nTestCounter := 1; // Run all the motion FBs for one cycle to warm them up fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); fb_Move2D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); fb_Move3D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, stMotionStage3:=stMotionStage3, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, astPositionState3:=astPositionState3, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); END_IF Test1D(1, E_TestStates.OUT); Test1D(2, E_TestStates.TARGET1); Test1D(3, E_TestStates.TARGET2); Test2D(4, E_TestStates.OUT); Test2D(5, E_TestStates.TARGET1); Test2D(6, E_TestStates.TARGET2); Test3D(7, E_TestStates.OUT); Test3D(8, E_TestStates.TARGET1); Test3D(9, E_TestStates.TARGET2); TestInputDeadlock(10); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; tonTimer(IN:=FALSE); END_IF // Use this timer to time out any tests that stall tonTimer( IN:=bStatesReady, PT:=T#5s, ); END_FUNCTION_BLOCK METHOD Test1D VAR_INPUT nTestID: UINT; nState: E_TestStates; END_VAR VAR_INST nLocalInit: BOOL; END_VAR TEST(CONCAT('Test1D_state', DINT_TO_STRING(nState))); IF nTestCounter <> nTestID THEN RETURN; END_IF IF NOT nLocalInit THEN eSetPos := nState; nLocalInit := TRUE; END_IF fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); IF tonTimer.Q OR fb_Move1D.stPlcToEpics.bDone THEN AssertTrue( Condition:=fb_Move1D.stPlcToEpics.bDone, Message:='Done should be True after move', ); AssertFalse( Condition:=fb_Move1D.stPlcToEpics.bBusy, Message:='Busy should be False after move', ); AssertFalse( Condition:=fb_Move1D.stPlcToEpics.bError, Message:='Error should be False after move', ); AssertEquals_DINT( Expected:=nState, Actual:=eGetPos, Message:='Did not get to the input state', ); AssertEquals_LREAL( Expected:=astPositionState1[nState].fPosition, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); fb_Move1D.stEpicsToPlc.bReset := TRUE; fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); bOneTestDone := TRUE; nLocalInit := FALSE; TEST_FINISHED(); END_IF END_METHOD METHOD Test2D VAR_INPUT nTestID: UINT; nState: E_TestStates; END_VAR VAR_INST nLocalInit: BOOL; END_VAR TEST(CONCAT('Test2D_state', DINT_TO_STRING(nState))); IF nTestCounter <> nTestID THEN RETURN; END_IF IF NOT nLocalInit THEN eSetPos := nState; nLocalInit := TRUE; END_IF fb_Move2D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); IF tonTimer.Q OR fb_Move2D.stPlcToEpics.bDone THEN AssertTrue( Condition:=fb_Move2D.stPlcToEpics.bDone, Message:='Done should be True after move', ); AssertFalse( Condition:=fb_Move2D.stPlcToEpics.bBusy, Message:='Busy should be False after move', ); AssertFalse( Condition:=fb_Move2D.stPlcToEpics.bError, Message:='Error should be False after move', ); AssertEquals_DINT( Expected:=nState, Actual:=eGetPos, Message:='Did not get to the input state', ); AssertEquals_LREAL( Expected:=astPositionState1[nState].fPosition, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position for stage 1', ); AssertEquals_LREAL( Expected:=astPositionState2[nState].fPosition, Actual:=stMotionStage2.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position for stage 2', ); fb_Move2D.stEpicsToPlc.bReset := TRUE; fb_Move2D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); bOneTestDone := TRUE; nLocalInit := FALSE; TEST_FINISHED(); END_IF END_METHOD METHOD Test3D VAR_INPUT nTestID: UINT; nState: E_TestStates; END_VAR VAR_INST nLocalInit: BOOL; END_VAR TEST(CONCAT('Test3D_state', DINT_TO_STRING(nState))); IF nTestCounter <> nTestID THEN RETURN; END_IF IF NOT nLocalInit THEN eSetPos := nState; nLocalInit := TRUE; END_IF fb_Move3D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, stMotionStage3:=stMotionStage3, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, astPositionState3:=astPositionState3, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); IF tonTimer.Q OR fb_Move3D.stPlcToEpics.bDone THEN AssertTrue( Condition:=fb_Move3D.stPlcToEpics.bDone, Message:='Done should be True after move', ); AssertFalse( Condition:=fb_Move3D.stPlcToEpics.bBusy, Message:='Busy should be False after move', ); AssertFalse( Condition:=fb_Move3D.stPlcToEpics.bError, Message:='Error should be False after move', ); AssertEquals_DINT( Expected:=nState, Actual:=eGetPos, Message:='Did not get to the input state', ); AssertEquals_LREAL( Expected:=astPositionState1[nState].fPosition, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position for stage 1', ); AssertEquals_LREAL( Expected:=astPositionState2[nState].fPosition, Actual:=stMotionStage2.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position for stage 2', ); AssertEquals_LREAL( Expected:=astPositionState3[nState].fPosition, Actual:=stMotionStage3.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position for stage 3', ); fb_Move3D.stEpicsToPlc.bReset := TRUE; fb_Move3D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, stMotionStage3:=stMotionStage3, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, astPositionState3:=astPositionState3, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); bOneTestDone := TRUE; nLocalInit := FALSE; TEST_FINISHED(); END_IF END_METHOD METHOD TestInputDeadlock (* Regression test for issue #197 How to reproduce the issue: 1. Get the function block into any motion error 2. Ask for a move while in the error state Then, the state mover never moves again. To test we will follow steps 1 and 2, then reset the error, then move. With our fix, our second attempt at the move (after an error reset) will succeed. Our first attempt will fail regardless, since you cannot move a motor that is in an error state. Without our fix, the motor could never be moved ever again and this move will time out. *) VAR_INPUT nTestID: UINT; END_VAR VAR_INST nTestStep: UINT := 1; eNewGoal: E_TestStates; END_VAR TEST('TestInputDeadlock'); IF nTestCounter <> nTestID THEN RETURN; END_IF CASE nTestStep OF // Step 1: Normal move 1: // Normal move: pick any other state IF eGetPos = E_TestStates.OUT THEN eNewGoal := E_TestStates.TARGET1; ELSE eNewGoal := E_TestStates.OUT; END_IF eSetPos := eNewGoal; nTestStep := 2; // Step 2: Cause a motion error. Easiest is to just set bError to TRUE during a move. 2: // Cause the error if it's time IF stMotionStage1.bBusy THEN stMotionStage1.bError := TRUE; stMotionStage1.nErrorId := 16#4FFF; ELSIF stMotionStage1.bError THEN nTestStep := 3; END_IF // Step 3: another normal move. This should get us into the potential bugged state. 3: eSetPos := eNewGoal; nTestStep := 4; // Step 4: reset the error, which will allow fixed versions of the code to resume normal operations. 4: fb_Move1D.stEpicsToPlc.bReset := TRUE; IF NOT stMotionStage1.bError THEN nTestStep := 5; END_IF // Step 5: last normal move, which will succeed if we fixed the bug. 5: eSetPos := eNewGoal; IF eGetPos = E_TestStates.Unknown THEN nTestStep := 6; END_IF // Step 6: wait for the move to finish 6: IF eGetPos = eNewGoal THEN nTestStep := 7; END_IF END_CASE fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); // Timeout and checks IF tonTimer.Q OR nTestStep = 7 THEN AssertFalse( tonTimer.Q, 'Timeout in deadlock test', ); AssertEquals_UINT( Expected:=eNewGoal, Actual:=eGetPos, 'Did not reach a goal state in deadlock test', ); fb_Move1D.stEpicsToPlc.bReset := TRUE; fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, bEnable:=TRUE, ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD Related: * `E_TestStates`_ * `FB_MotionStage`_ * `FB_MotorTestSuite`_ * `FB_PositionState1D`_ * `FB_PositionState2D`_ * `FB_PositionState3D`_ * `FB_PositionStateInternal`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStatePMPS ^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionStatePMPS1D instead'} 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 bReadPmpsDb: BOOL; sPmpsDeviceName: STRING; sTransitionKey: STRING; stPmpsDoc: SJsonValue; stHighBeamThreshold: ST_BeamParams := PMPS_GVL.cstFullBeam; bBPOKAutoReset: BOOL := False; END_VAR VAR arrPMPS: ARRAY[0..GeneralConstants.MAX_STATES] OF ST_DbStateParams; nBPIndex: UINT; nTransitionAssertionID: UDINT; nLastReqAssertionID: UDINT; fbReadPmpsDb: FB_JsonDocToSafeBP; ftDbBusy: F_TRIG; rtReadDBExec: R_TRIG; ftRead: F_TRIG; bptm: BeamParameterTransitionManager; ffBeamParamsOk: FB_FastFault; ffZeroRate: FB_FastFault; ffBPTMTimeoutAndMove: FB_FastFault; ffBPTMError: FB_FastFault; ffMaint: FB_FastFault; ffUnknown: FB_FastFault; bFFOxOk: BOOL; bAtSafeState: BOOL; nIter: UINT; END_VAR Exec(); END_FUNCTION_BLOCK ACTION AssertHere: fbArbiter.AddRequest( sDevName := stMotionStage.sName, nReqID := stStateReq.stPMPS.nRequestAssertionID, stReqBP := stStateReq.stPMPS.stBeamParams); END_ACTION ACTION ClearAsserts: fbArbiter.RemoveRequest(nTransitionAssertionID); FOR nIter := 1 TO GeneralConstants.MAX_STATES DO fbArbiter.RemoveRequest(arrStates[nIter].stPMPS.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 IF stStateReq.stPMPS.nRequestAssertionID <> nTransitionAssertionID THEN (* Edge case: the request is swapped out without a move Same as above: we need a rising edge of bTransDone, so cause a falling edge and the let the rising edge happen next cycle This will already be false when we request a positional move *) bTransDone R= stStateReq.stPMPS.nRequestAssertionID <> nLastReqAssertionID; // Just normal bptm call bptm(fbArbiter:=fbArbiter, i_sDeviceName:=stMotionStage.sName, i_TransitionAssertionID:=nTransitionAssertionID, i_stTransitionAssertion:=stTransitionBeam, i_nRequestedAssertionID:=stStateReq.stPMPS.nRequestAssertionID, i_stRequestedAssertion:=stStateReq.stPMPS.stBeamParams, i_xDoneMoving:=bTransDone, stCurrentBeamParameters:=PMPS_GVL.stCurrentBeamParameters, q_xTransitionAuthorized=>bInternalAuth, bDone=>bBPTMDone); nLastReqAssertionID := stStateReq.stPMPS.nRequestAssertionID; END_IF 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_xAutoReset := bBPOKAutoReset; 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); // Trip the beam for unknown state ffUnknown( i_xOK := getState > 0 OR bInTransition, i_xAutoReset := TRUE, i_DevName := stMotionStage.sName, i_Desc := 'Unknown position between moves', i_TypeCode := 16#1005, io_fbFFHWO := fbFFHWO); END_ACTION ACTION HandlePmpsDb: // Automatically read from the pmps db when it updates // Assume update if no longer busy and no errors ftDbBusy(CLK:=MOTION_GVL.fbPmpsFileReader.bBusy); IF ftDbBusy.Q THEN bReadPmpsDb S= NOT MOTION_GVL.fbPmpsFileReader.bError; END_IF rtReadDBExec(CLK:=bReadPmpsDb); IF rtReadDBExec.Q THEN arrPMPS[0].sPmpsState := sTransitionKey; FOR nBPIndex := 1 TO GeneralConstants.MAX_STATES BY 1 DO arrPMPS[nBPIndex] := arrStates[nBPIndex].stPMPS; END_FOR END_IF fbReadPmpsDb( bExecute:=bReadPmpsDb, jsonDoc:=stPmpsDoc, sDeviceName:=sPmpsDeviceName, io_fbFFHWO:=fbFFHWO, arrStates:=arrPMPS, ); bReadPmpsDb R= NOT fbReadPmpsDb.bBusy; ftRead(CLK:=fbReadPmpsDb.bBusy); stTransitionState.sName := 'Transition'; IF ftRead.Q AND NOT fbReadPmpsDb.bError THEN stTransitionDb := arrPMPS[0]; stTransitionBeam := arrPMPS[0].stBeamParams; nTransitionAssertionID := arrPMPS[0].nRequestAssertionID; stTransitionState.stPMPS := arrPMPS[0]; FOR nBPIndex := 1 TO GeneralConstants.MAX_STATES BY 1 DO arrStates[nBPIndex].stPMPS := arrPMPS[nBPIndex]; END_FOR END_IF END_ACTION Related: * `FB_PositionStatePMPS1D`_ * `FB_PositionStatePMPS_Base`_ * `MOTION_GVL`_ FB_PositionStatePMPS1D ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS1D (* 1-Dimensional position state function block with PMPS. This allows the user to move a motor among some set of named state positions with PMPS protection. To use a states block, you must define enums that match the state options and give them pytmc pragmas. See FB_PositionState1D_InOut for a simple example. These enums must be passed in as the eEnumSet and eEnumGet VAR_IN_OUT variables. The enum values must match the array indices in astPositionState. A move will begin when eEnumSet is set to a positive, nonzero value. eEnumSet will be reset to 0 on every cycle, allowing us to accept a new, possibly conflicting, move request on the next cycle to interrupt the first. The motor must already be set up for point-to-point motion via FB_MotionStage, etc., for this function block to work properly. PMPS handling is done via database lookups by setting sPmpsState on each position state and on the transition state input appropriately. *) VAR_IN_OUT // The motor to move. stMotionStage: ST_MotionStage; // All possible position states, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE io: io expand: :%.2d '} astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; // The fast fault output to fault to. fbFFHWO: FB_HardwareFFOutput; // The arbiter to request beam conditions from. fbArbiter: FB_Arbiter; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnableMotion: BOOL; // Set this to TRUE to enable beam parameter checks, or FALSE to disable them. bEnableBeamParams: BOOL; // Set this to TRUE to enable position limit checks, or FALSE to disable them. bEnablePositionLimits: BOOL; // The name of the device for use in the PMPS DB lookup and diagnostic screens. sDeviceName: STRING; // The name of the transition state in the PMPS database. sTransitionKey: STRING; // Normal EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stEpicsToPlc: ST_StateEpicsToPlc; // PMPS EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPMPSEpicsToPlc: ST_StatePMPSEpicsToPlc; // Set this to TRUE to re-read the loaded database immediately (useful for debug) bReadDBNow: BOOL; END_VAR VAR_OUTPUT // Normal EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPlcToEpics: ST_StatePlcToEpics; // PMPS EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPMPSPlcToEpics: ST_StatePMPSPlcToEpics; // The PMPS database lookup associated with the current position state. stDbStateParams: ST_DbStateParams; END_VAR VAR fbCore: FB_PositionStateND_Core; fbPMPSCore: FB_PositionStatePMPSND_Core; astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR astMotionStageMax[1] := stMotionStage; astPositionStateMax[1] := astPositionState; fbCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPlcToEpics:=stPlcToEpics, eEnumSet:=eEnumSet, eEnumGet:=eEnumGet, bEnable:=bEnableMotion, nActiveMotorCount:=1, nCurrGoal=>, ); fbPMPSCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPMPSEpicsToPlc:=stPMPSEpicsToPlc, stPlcToEpics:=stPlcToEpics, stPMPSPlcToEpics:=stPMPSPlcToEpics, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter, bEnableBeamParams:=bEnableBeamParams, bEnablePositionLimits:=bEnablePositionLimits, nActiveMotorCount:=1, sDeviceName:=sDeviceName, sTransitionKey:=sTransitionKey, nCurrGoal:=fbCore.nCurrGoal, bReadDBNow:=bReadDBNow, stDbStateParams=>stDbStateParams, ); stMotionStage := astMotionStageMax[1]; astPositionState := astPositionStateMax[1]; END_FUNCTION_BLOCK Related: * `FB_MotionStage`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateND_Core`_ * `FB_PositionStatePMPSND_Core`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePMPSEpicsToPlc`_ * `ST_StatePMPSPlcToEpics`_ * `ST_StatePlcToEpics`_ FB_PositionStatePMPS1D_InOut ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS1D_InOut (* An example and usable drop-in instance for a 1D pmps state device with just an IN and an OUT state. Note that the outward-facing API is nearly identical to the non-PMPS version *) VAR_IN_OUT // Include a stage that can be passed into the FB stMotionStage: ST_MotionStage; // Simplify the interface, the user just needs to construct and pass in and out position states stIn: ST_PositionState; stOut: ST_PositionState; // Include PMPS output helpers fbFFHWO: FB_HardwareFFOutput; fbArbiter: FB_Arbiter; END_VAR VAR_INPUT // Define an ENUM for EPICS to use to set a new value. It is expected this will be written to during one cycle, so don't continually apply a value here in the PLC code. // It is exposed as an input so we can test it using the PLC. {attribute 'pytmc' := ' pv: STATE:SET io: io '} eStateSet: E_EpicsInOut; END_VAR VAR_OUTPUT // Define an ENUM for EPICS to use to report the new value. {attribute 'pytmc' := ' pv: STATE:GET io: i '} eStateGet: E_EpicsInOut; END_VAR VAR // Include the standard fb with blank pv pragma {attribute 'pytmc' := 'pv:'} fbPositionStatePMPS1D: FB_PositionStatePMPS1D; // The standard fb expects a full array of position states astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR // Optional: default state names IF stIn.sName = '' THEN stIn.sName := 'IN'; END_IF IF stOut.sName = '' THEN stOut.sName := 'OUT'; END_IF // Assemble the states array, matching the enum values (IN is 1, OUT is 2) astPositionState[1] := stIn; astPositionState[2] := stOut; // Call the main function block, passing our motors, states, and an enable fbPositionStatePMPS1D( fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter, stMotionStage:=stMotionStage, astPositionState:=astPositionState, eEnumSet:=eStateSet, eEnumGet:=eStateGet, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, ); // Send updates made on the array back to the inputs (VAR_IN_OUT) for maximum clarity stIn := astPositionState[1]; stOut := astPositionState[2]; END_FUNCTION_BLOCK Related: * `E_EpicsInOut`_ * `FB_PositionStatePMPS1D`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStatePMPS2D ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS2D (* 2-Dimensional position state function block with PMPS. This allows the user to move 2 motors among some set of named state positions with PMPS protection. To use a states block, you must define enums that match the state options and give them pytmc pragmas. See FB_PositionState1D_InOut for a simple example. These enums must be passed in as the eEnumSet and eEnumGet VAR_IN_OUT variables. The enum values must match the array indices in astPositionState1 and astPositionState2. A move will begin when eEnumSet is set to a positive, nonzero value. eEnumSet will be reset to 0 on every cycle, allowing us to accept a new, possibly conflicting, move request on the next cycle to interrupt the first. The motors must already be set up for point-to-point motion via FB_MotionStage, etc., for this function block to work properly. PMPS handling is done via database lookups by setting sPmpsState on each position state and on the transition state input appropriately. *) VAR_IN_OUT // The 1st motor to move stMotionStage1: ST_MotionStage; // The 2nd motor to move stMotionStage2: ST_MotionStage; // All possible position states for motor 1, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:M1 io: io expand: :%.2d '} astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // All possible position states for motor 2, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:M2 io: io expand: :%.2d '} astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; // The fast fault output to fault to. fbFFHWO: FB_HardwareFFOutput; // The arbiter to request beam conditions from. fbArbiter: FB_Arbiter; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnableMotion: BOOL; // Set this to TRUE to enable beam parameter checks, or FALSE to disable them. bEnableBeamParams: BOOL; // Set this to TRUE to enable position limit checks, or FALSE to disable them. bEnablePositionLimits: BOOL; // The name of the device for use in the PMPS DB lookup and diagnostic screens. sDeviceName: STRING; // The name of the transition state in the PMPS database. sTransitionKey: STRING; // Normal EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} // PMPS EPICS inputs, gathered into a single struct stEpicsToPlc: ST_StateEpicsToPlc; {attribute 'pytmc' := 'pv: STATE'} stPMPSEpicsToPlc: ST_StatePMPSEpicsToPlc; // Set this to TRUE to re-read the loaded database immediately (useful for debug) bReadDBNow: BOOL; END_VAR VAR_OUTPUT // Normal EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPlcToEpics: ST_StatePlcToEpics; // PMPS EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPMPSPlcToEpics: ST_StatePMPSPlcToEpics; // The PMPS database lookup associated with the current position state. stDbStateParams: ST_DbStateParams; END_VAR VAR fbCore: FB_PositionStateND_Core; fbPMPSCore: FB_PositionStatePMPSND_Core; astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR astMotionStageMax[1] := stMotionStage1; astMotionStageMax[2] := stMotionStage2; astPositionStateMax[1] := astPositionState1; astPositionStateMax[2] := astPositionState2; fbCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPlcToEpics:=stPlcToEpics, eEnumSet:=eEnumSet, eEnumGet:=eEnumGet, bEnable:=bEnableMotion, nActiveMotorCount:=2, nCurrGoal=>, ); fbPMPSCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPMPSEpicsToPlc:=stPMPSEpicsToPlc, stPlcToEpics:=stPlcToEpics, stPMPSPlcToEpics:=stPMPSPlcToEpics, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter, bEnableBeamParams:=bEnableBeamParams, bEnablePositionLimits:=bEnablePositionLimits, nActiveMotorCount:=2, sDeviceName:=sDeviceName, sTransitionKey:=sTransitionKey, nCurrGoal:=fbCore.nCurrGoal, bReadDBNow:=bReadDBNow, stDbStateParams=>stDbStateParams, ); stMotionStage1 := astMotionStageMax[1]; stMotionStage2 := astMotionStageMax[2]; astPositionState1 := astPositionStateMax[1]; astPositionState2 := astPositionStateMax[2]; END_FUNCTION_BLOCK Related: * `FB_MotionStage`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateND_Core`_ * `FB_PositionStatePMPSND_Core`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePMPSEpicsToPlc`_ * `ST_StatePMPSPlcToEpics`_ * `ST_StatePlcToEpics`_ FB_PositionStatePMPS2D_InOut ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS2D_InOut (* An example and usable drop-in instance for a 2D state device with just an IN and an OUT state. Note that the outward-facing API is nearly identical to the non-PMPS version *) VAR_IN_OUT // Include stages that can be passed into the FB stMotionStage1: ST_MotionStage; stMotionStage2: ST_MotionStage; // Simplify the interface, the user just needs to construct and pass in and out position states stIn1: ST_PositionState; stOut1: ST_PositionState; stIn2: ST_PositionState; stOut2: ST_PositionState; // Include PMPS output helpers fbFFHWO: FB_HardwareFFOutput; fbArbiter: FB_Arbiter; END_VAR VAR_INPUT // Define an ENUM for EPICS to use to set a new value. It is expected this will be written to during one cycle, so don't continually apply a value here in the PLC code. // It is exposed as an input so we can test it using the PLC. {attribute 'pytmc' := ' pv: STATE:SET io: io '} eStateSet: E_EpicsInOut; END_VAR VAR_OUTPUT // Define an ENUM for EPICS to use to report the new value. {attribute 'pytmc' := ' pv: STATE:GET io: i '} eStateGet: E_EpicsInOut; END_VAR VAR // Include the standard fb with blank pv pragma {attribute 'pytmc' := 'pv:'} fbPositionStatePMPS2D: FB_PositionStatePMPS2D; // The standard fb expects a full array of position states per motor astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR // Optional: default state names IF stIn1.sName = '' THEN stIn1.sName := 'IN'; END_IF IF stOut1.sName = '' THEN stOut1.sName := 'OUT'; END_IF IF stIn2.sName = '' THEN stIn2.sName := 'IN'; END_IF IF stOut2.sName = '' THEN stOut2.sName := 'OUT'; END_IF // Assemble the states arrays, matching the enum values (IN is 1, OUT is 2) astPositionState1[1] := stIn1; astPositionState1[2] := stOut1; astPositionState2[1] := stIn2; astPositionState2[2] := stOut2; // Call the main function block, passing our motors, states, and an enable fbPositionStatePMPS2D( fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter, stMotionStage1:=stMotionStage1, astPositionState1:=astPositionState1, stMotionStage2:=stMotionStage2, astPositionState2:=astPositionState2, eEnumSet:=eStateSet, eEnumGet:=eStateGet, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, ); // Send updates made on the array back to the inputs (VAR_IN_OUT) for maximum clarity stIn1 := astPositionState1[1]; stOut1 := astPositionState1[2]; stIn2 := astPositionState2[1]; stOut2 := astPositionState2[2]; END_FUNCTION_BLOCK Related: * `E_EpicsInOut`_ * `FB_PositionStatePMPS2D`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStatePMPS3D ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPS3D (* 3-Dimensional position state function block with PMPS. This allows the user to move 3 motors among some set of named state positions with PMPS protection. To use a states block, you must define enums that match the state options and give them pytmc pragmas. See FB_PositionState1D_InOut for a simple example. These enums must be passed in as the eEnumSet and eEnumGet VAR_IN_OUT variables. The enum values must match the array indices in astPositionState1, astPositionState2, and astPositionState3. A move will begin when eEnumSet is set to a positive, nonzero value. eEnumSet will be reset to 0 on every cycle, allowing us to accept a new, possibly conflicting, move request on the next cycle to interrupt the first. The motors must already be set up for point-to-point motion via FB_MotionStage, etc., for this function block to work properly. PMPS handling is done via database lookups by setting sPmpsState on each position state and on the transition state input appropriately. *) VAR_IN_OUT // The 1st motor to move stMotionStage1: ST_MotionStage; // The 2nd motor to move stMotionStage2: ST_MotionStage; // The 3rd motor to move stMotionStage3: ST_MotionStage; // All possible position states for motor 1, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:M1 io: io expand: :%.2d '} astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // All possible position states for motor 2, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:M2 io: io expand: :%.2d '} astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // All possible position states for motor 3, including unused/invalid states. {attribute 'pytmc' := ' pv: STATE:M3 io: io expand: :%.2d '} astPositionState3: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Set this to a nonzero value to request a new move. It will be reset to zero every cycle. This should be hooked up to a user's EPICS enum input. eEnumSet: UINT; // The current state index, or zero if we are not at a state. This should be hooked up to a user's EPICS enum output. eEnumGet: UINT; // The fast fault output to fault to. fbFFHWO: FB_HardwareFFOutput; // The arbiter to request beam conditions from. fbArbiter: FB_Arbiter; END_VAR VAR_INPUT // Set this to TRUE to enable input state moves, or FALSE to disable them. bEnableMotion: BOOL; // Set this to TRUE to enable beam parameter checks, or FALSE to disable them. bEnableBeamParams: BOOL; // Set this to TRUE to enable position limit checks, or FALSE to disable them. bEnablePositionLimits: BOOL; // The name of the device for use in the PMPS DB lookup and diagnostic screens. sDeviceName: STRING; // The name of the transition state in the PMPS database. sTransitionKey: STRING; // Normal EPICS inputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} // PMPS EPICS inputs, gathered into a single struct stEpicsToPlc: ST_StateEpicsToPlc; {attribute 'pytmc' := 'pv: STATE'} stPMPSEpicsToPlc: ST_StatePMPSEpicsToPlc; // Set this to TRUE to re-read the loaded database immediately (useful for debug) bReadDBNow: BOOL; END_VAR VAR_OUTPUT // Normal EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPlcToEpics: ST_StatePlcToEpics; // PMPS EPICS outputs, gathered into a single struct {attribute 'pytmc' := 'pv: STATE'} stPMPSPlcToEpics: ST_StatePMPSPlcToEpics; // The PMPS database lookup associated with the current position state. stDbStateParams: ST_DbStateParams; END_VAR VAR fbCore: FB_PositionStateND_Core; fbPMPSCore: FB_PositionStatePMPSND_Core; astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR astMotionStageMax[1] := stMotionStage1; astMotionStageMax[2] := stMotionStage2; astMotionStageMax[3] := stMotionStage3; astPositionStateMax[1] := astPositionState1; astPositionStateMax[2] := astPositionState2; astPositionStateMax[3] := astPositionState3; fbCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPlcToEpics:=stPlcToEpics, eEnumSet:=eEnumSet, eEnumGet:=eEnumGet, bEnable:=bEnableMotion, nActiveMotorCount:=3, nCurrGoal=>, ); fbPMPSCore( astMotionStageMax:=astMotionStageMax, astPositionStateMax:=astPositionStateMax, stEpicsToPlc:=stEpicsToPlc, stPMPSEpicsToPlc:=stPMPSEpicsToPlc, stPlcToEpics:=stPlcToEpics, stPMPSPlcToEpics:=stPMPSPlcToEpics, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter, bEnableBeamParams:=bEnableBeamParams, bEnablePositionLimits:=bEnablePositionLimits, nActiveMotorCount:=3, sDeviceName:=sDeviceName, sTransitionKey:=sTransitionKey, nCurrGoal:=fbCore.nCurrGoal, bReadDBNow:=bReadDBNow, stDbStateParams=>stDbStateParams, ); stMotionStage1 := astMotionStageMax[1]; stMotionStage2 := astMotionStageMax[2]; stMotionStage3 := astMotionStageMax[3]; astPositionState1 := astPositionStateMax[1]; astPositionState2 := astPositionStateMax[2]; astPositionState3 := astPositionStateMax[3]; END_FUNCTION_BLOCK Related: * `FB_MotionStage`_ * `FB_PositionState1D_InOut`_ * `FB_PositionStateND_Core`_ * `FB_PositionStatePMPSND_Core`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePMPSEpicsToPlc`_ * `ST_StatePMPSPlcToEpics`_ * `ST_StatePlcToEpics`_ FB_PositionStatePMPS_Base ^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionStatePMPS1D instead'} 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: ST_MotionStage; arrStates: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_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; tArbiterTimeout: TIME := T#1s; bMoveOnArbiterTimeout: BOOL := TRUE; END_VAR VAR_OUTPUT bTransitionAuthorized: BOOL; bForwardAuthorized: BOOL; bBackwardAuthorized: BOOL; bArbiterTimeout: BOOL; END_VAR VAR {attribute 'pytmc' := ' pv: TRANS io: i '} stTransitionDb: ST_DbStateParams; stTransitionBeam: ST_BeamParams := PMPS_GVL.cst0RateBeam; stTransitionState: ST_PositionState; 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: ST_PositionState; mcPower: MC_POWER; fUpperBound: LREAL; fLowerBound: LREAL; nGoalState: INT; stGoalState: ST_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: // Load the pmps parameters as needed HandlePmpsDb(); // 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; 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 ACTION HandlePmpsDb: END_ACTION METHOD GetBeamFromState : ST_BeamParams; VAR_INPUT nState: INT; END_VAR VAR stState: ST_PositionState; END_VAR stState := GetStateStruct(nState); GetBeamFromState := stState.stPMPS.stBeamParams; END_METHOD METHOD GetStateCode : INT VAR_INPUT nState: INT; END_VAR IF nState < 0 OR nState > GeneralConstants.MAX_STATES THEN GetStateCode := -1; ELSE GetStateCode := nState; END_IF END_METHOD METHOD GetStateStruct : ST_PositionState VAR_INPUT nState: INT; END_VAR {warning disable C0371} // Implicit VAR_IN_OUT reference inside a method needs special handling IF NOT __ISVALIDREF(arrStates) THEN GetStateStruct := stTransitionState; RETURN; END_IF CASE GetStateCode(nState) OF -1: GetStateStruct := stTransitionState; 0: GetStateStruct := stTransitionState; ELSE GetStateStruct := arrStates[nState]; END_CASE {warning restore C0371} END_METHOD Related: * `FB_PositionStatePMPS`_ * `FB_PositionStatePMPS1D`_ * `F_AtPositionState`_ * `F_PosOverLowerBound`_ * `F_PosUnderUpperBound`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStatePMPS_Test ^^^^^^^^^^^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'No longer any reason to use this, all state classes can have unit tests.'} 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 Related: * `FB_PositionStatePMPS_Base`_ FB_PositionStatePMPSND_Core ^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPSND_Core (* Collection of all core actions shared between all PMPS states FBs This is used in e.g. - FB_PositionStatePMPS1D - FB_PositionStatePMPS2D - ... etc. This handles database reading, BPTM, motor enables, virtual limits, and all FFOS. *) VAR_IN_OUT // All motors to be used in the states move, including blank/uninitialized structs. astMotionStageMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // All position states for all motors, including unused/invalid states. astPositionStateMax: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Normal EPICS inputs, gathered into a single struct stEpicsToPlc: ST_StateEpicsToPlc; // PMPS EPICS inputs, gathered into a single struct stPMPSEpicsToPlc: ST_StatePMPSEpicsToPlc; // Normal EPICS outputs, gathered into a single struct stPlcToEpics: ST_StatePlcToEpics; // PMPS EPICS outputs, gathered into a single struct stPMPSPlcToEpics: ST_StatePMPSPlcToEpics; // The fast fault output to fault to. fbFFHWO: FB_HardwareFFOutput; // The arbiter to request beam conditions from. fbArbiter: FB_Arbiter; END_VAR VAR_INPUT // Set this to TRUE to enable beam parameter checks, or FALSE to disable them. bEnableBeamParams: BOOL; // Set this to TRUE to enable position limit checks, or FALSE to disable them. bEnablePositionLimits: BOOL; // Set this to the number of motors to be included in astMotionStageMax nActiveMotorCount: UINT; // The name of the device for use in the PMPS DB lookup and diagnostic screens. sDeviceName: STRING; // The name of the transition state in the PMPS database. sTransitionKey: STRING; // The current position index goal, where the motors are supposed to be moving towards. nCurrGoal: UINT; // Set this to TRUE to re-read the loaded database immediately (useful for debug) bReadDBNow: BOOL; END_VAR VAR_OUTPUT // The PMPS database lookup associated with the current position state. stDbStateParams: ST_DbStateParams; END_VAR VAR fbMotionReadPMPSDB: FB_MotionReadPMPSDBND; fbMotionBPTM: FB_MotionBPTM; fbMotionClearAsserts: FB_MotionClearAsserts; fbStatePMPSEnables: FB_StatePMPSEnablesND; fbMiscStatesErrorFFO: FB_MiscStatesErrorFFO; fbPerMotorFFO: FB_PerMotorFFOND; eStatePMPSStatus: E_StatePMPSStatus; END_VAR IF stPMPSEpicsToPlc.bMaintMode OR NOT stPMPSEpicsToPlc.bArbiterEnabled THEN eStatePMPSStatus := E_StatePMPSStatus.DISABLED; ELSIF stPlcToEpics.nGetValue = 0 AND nCurrGoal = 0 THEN eStatePMPSStatus := E_StatePMPSStatus.UNKNOWN; ELSIF stPlcToEpics.nGetValue = nCurrGoal THEN eStatePMPSStatus := E_StatePMPSStatus.AT_STATE; ELSE eStatePMPSStatus := E_StatePMPSStatus.TRANSITION; END_IF fbMotionReadPMPSDB( astPositionState:=astPositionStateMax, fbFFHWO:=fbFFHWO, sTransitionKey:=sTransitionKey, sDeviceName:=sDeviceName, bReadNow:=bReadDBNow, astDbStateParams=>, bError=>, ); fbMotionBPTM( astMotionStage:=astMotionStageMax, fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, stGoalParams:=fbMotionReadPMPSDB.astDbStateParams[nCurrGoal], stTransParams:=fbMotionReadPMPSDB.astDbStateParams[0], nActiveMotorCount:=nActiveMotorCount, bEnable:=stPMPSEpicsToPlc.bArbiterEnabled AND bEnableBeamParams, bAtState:=stPlcToEpics.nGetValue = nCurrGoal AND nCurrGoal <> 0, sDeviceName:=sDeviceName, bTransitionAuthorized=>, bDone=>, bMotorCountError=>, ); fbMotionClearAsserts( astDbStateParams:=fbMotionReadPMPSDB.astDbStateParams, fbArbiter:=fbArbiter, bExecute:=NOT fbMotionBPTM.bEnable, ); fbStatePMPSEnables( astMotionStage:=astMotionStageMax, astPositionState:=astPositionStateMax, fbFFHWO:=fbFFHWO, bEnable:=bEnablePositionLimits, nActiveMotorCount:=nActiveMotorCount, nGoalStateIndex:=nCurrGoal, sDeviceName:=sDeviceName, bMaintMode:=stPMPSEpicsToPlc.bMaintMode, eStatePMPSStatus:=eStatePMPSStatus, bTransitionAuthorized:=fbMotionBPTM.bTransitionAuthorized, abEnabled=>, abForwardEnabled=>, abBackwardEnabled=>, abValidGoal=>, bMotorCountError=>, ); IF bEnableBeamParams THEN fbMiscStatesErrorFFO.stCurrentBeamReq := fbMotionReadPMPSDB.astDbStateParams[stPlcToEpics.nGetValue].stBeamParams; ELSE fbMiscStatesErrorFFO.stCurrentBeamReq := PMPS_GVL.cstFullBeam; END_IF fbMiscStatesErrorFFO( fbArbiter:=fbArbiter, fbFFHWO:=fbFFHWO, sDeviceName:=sDeviceName, bKnownState:=stPlcToEpics.nGetValue > 0, nTransitionID:=fbMotionReadPMPSDB.astDbStateParams[0].nRequestAssertionID, ); fbPerMotorFFO( astMotionStage:=astMotionStageMax, fbFFHWO:=fbFFHWO, nActiveMotorCount:=nActiveMotorCount, sDeviceName:=sDeviceName, bMotorCountError=>, ); stPMPSPlcToEpics.stTransitionDb := fbMotionReadPMPSDB.astDbStateParams[0]; stDbStateParams := fbMotionReadPMPSDB.astDbStateParams[stPlcToEpics.nGetValue]; END_FUNCTION_BLOCK Related: * `E_StatePMPSStatus`_ * `FB_MiscStatesErrorFFO`_ * `FB_MotionBPTM`_ * `FB_MotionClearAsserts`_ * `FB_MotionReadPMPSDBND`_ * `FB_PerMotorFFOND`_ * `FB_PositionStatePMPS1D`_ * `FB_PositionStatePMPS2D`_ * `FB_StatePMPSEnablesND`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ * `ST_StateEpicsToPlc`_ * `ST_StatePMPSEpicsToPlc`_ * `ST_StatePMPSPlcToEpics`_ * `ST_StatePlcToEpics`_ FB_PositionStatePMPSND_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStatePMPSND_Test EXTENDS FB_MotorTestSuite (* Sanity checks for the following: - FB_PositionStatePMPS1D - FB_PositionStatePMPS2D - FB_PositionStatePMPS3D The internals have already been tested, but we need to make sure that they've been put together at least somewhat sensibly. This FB will simply use each FB to move and check the results. In addition to reaching the goals, we need to check the beam assertions and the pmps limit enables. *) VAR stMotionStage1: ST_MotionStage; stMotionStage2: ST_MotionStage; stMotionStage3: ST_MotionStage; astPositionState1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astPositionState2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astPositionState3: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbInternal: ARRAY[1..3] OF ARRAY[1..3] OF FB_PositionStateInternal; afbMotionStage: ARRAY[1..3] OF FB_MotionStage; astBeam: ARRAY[0..3] OF ST_DbStateParams; fb_Move1D: FB_PositionStatePMPS1D; fb_Move2D: FB_PositionStatePMPS2D; fb_Move3D: FB_PositionStatePMPS3D; nTestCounter: UINT; bOneTestDone: BOOL; fTestStartPos: LREAL; tonTimer: TON; nIter: DINT; bStatesReady: BOOL; eSetPos: E_TestStates; eGetPos: E_TestStates; fbArbiter1D: FB_Arbiter(1); fbArbiter2D: FB_Arbiter(2); fbArbiter3D: FB_Arbiter(3); fbSubSysIO1D: FB_DummyArbIO; fbSubSysIO2D: FB_DummyArbIO; fbSubSysIO3D: FB_DummyArbIO; jsonHelper: FB_PMPSJsonTestHelper; END_VAR bStatesReady := TRUE; FOR nIter := 1 TO 3 DO; astPositionState1[nIter].fPosition := nIter; astPositionState1[nIter].fDelta := 0.5; astPositionState1[nIter].fVelocity := 100; SetGoodState(astPositionState1[nIter]); astPositionState2[nIter].fPosition := 10 + nIter; astPositionState2[nIter].fDelta := 0.5; astPositionState2[nIter].fVelocity := 100; SetGoodState(astPositionState2[nIter]); astPositionState3[nIter].fPosition := 20 + nIter; astPositionState3[nIter].fDelta := 0.5; astPositionState3[nIter].fVelocity := 100; SetGoodState(astPositionState3[nIter]); afbInternal[nIter][1]( stMotionStage:=stMotionStage1, stPositionState:=astPositionState1[nIter], ); afbInternal[nIter][2]( stMotionStage:=stMotionStage2, stPositionState:=astPositionState2[nIter], ); afbInternal[nIter][3]( stMotionStage:=stMotionStage3, stPositionState:=astPositionState3[nIter], ); bStatesReady := bStatesReady AND astPositionState1[nIter].bUpdated; bStatesReady := bStatesReady AND astPositionState2[nIter].bUpdated; bStatesReady := bStatesReady AND astPositionState3[nIter].bUpdated; END_FOR SetEnablesPMPS(stMotionStage1); SetEnablesPMPS(stMotionStage2); SetEnablesPMPS(stMotionStage3); IF nTestCounter <= 3 THEN // Startup tests, make sure the motor is enabled so we can inspect MC_Power's output // Otherwise we can't snoop on the PlcToNc control DWORD since this is all 0's if disabled // When enabled, some of the other bits go to 1 to represent directional enables stMotionStage1.nEnableMode := E_StageEnableMode.ALWAYS; stMotionStage2.nEnableMode := E_StageEnableMode.ALWAYS; stMotionStage3.nEnableMode := E_StageEnableMode.ALWAYS; ELSE // Otherwise, make sure the most complicated mode (and the default) works stMotionStage1.nEnableMode := E_StageEnableMode.DURING_MOTION; stMotionStage2.nEnableMode := E_StageEnableMode.DURING_MOTION; stMotionStage3.nEnableMode := E_StageEnableMode.DURING_MOTION; END_IF afbMotionStage[1](stMotionStage:=stMotionStage1); afbMotionStage[2](stMotionStage:=stMotionStage2); afbMotionStage[3](stMotionStage:=stMotionStage3); astBeam[E_TestStates.Unknown].stBeamParams := PMPS_GVL.cst0RateBeam; astBeam[E_TestStates.Unknown].nRequestAssertionID := 1; astBeam[E_TestStates.Unknown].sPmpsState := 'trans'; astBeam[E_TestStates.OUT].stBeamParams := PMPS_GVL.cstFullBeam; astBeam[E_TestStates.OUT].nRequestAssertionID := 2; astBeam[E_TestStates.OUT].sPmpsState := 'out'; astBeam[E_TestStates.TARGET1].stBeamParams := PMPS_GVL.cstFullBeam; astBeam[E_TestStates.TARGET1].stBeamParams.nTran := 0.1; astBeam[E_TestStates.TARGET1].nRequestAssertionID := 3; astBeam[E_TestStates.TARGET1].sPmpsState := 'target1'; astBeam[E_TestStates.TARGET2].stBeamParams := PMPS_GVL.cstFullBeam; astBeam[E_TestStates.TARGET2].stBeamParams.nTran := 0.01; astBeam[E_TestStates.TARGET2].nRequestAssertionID := 4; astBeam[E_TestStates.TARGET2].sPmpsState := 'target2'; // Assign beam params to states 1 astPositionState1[E_TestStates.OUT].stPMPS := astBeam[E_TestStates.OUT]; astPositionState1[E_TestStates.TARGET1].stPMPS := astBeam[E_TestStates.TARGET1]; astPositionState1[E_TestStates.TARGET2].stPMPS := astBeam[E_TestStates.TARGET2]; // Set some names for maybe help in debug astPositionState1[E_TestStates.OUT].sName := 'OUT'; astPositionState1[E_TestStates.TARGET1].sName := 'TARGET1'; astPositionState1[E_TestStates.TARGET2].sName := 'TARGET2'; astPositionState2[E_TestStates.OUT].sName := 'OUT'; astPositionState2[E_TestStates.TARGET1].sName := 'TARGET1'; astPositionState2[E_TestStates.TARGET2].sName := 'TARGET2'; astPositionState3[E_TestStates.OUT].sName := 'OUT'; astPositionState3[E_TestStates.TARGET1].sName := 'TARGET1'; astPositionState3[E_TestStates.TARGET2].sName := 'TARGET2'; // Load a fake json doc to be consumed by our FB jsonHelper( astBeamParams:=astBeam, bExecute:=TRUE, sDevName:='test', ); IF bStatesReady AND nTestCounter = 0 THEN // Don't run any tests until the states are ready nTestCounter := 1; END_IF TestStartup1D(1); TestStartup2D(2); TestStartup3D(3); Test1D(4, E_TestStates.OUT); Test1D(5, E_TestStates.TARGET1); Test1D(6, E_TestStates.TARGET2); Test2D(7, E_TestStates.OUT); Test2D(8, E_TestStates.TARGET1); Test2D(9, E_TestStates.TARGET2); Test3D(10, E_TestStates.OUT); Test3D(11, E_TestStates.TARGET1); Test3D(12, E_TestStates.TARGET2); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; tonTimer(IN:=FALSE); END_IF // Use this timer to time out any tests that stall tonTimer( IN:=bStatesReady, PT:=T#5s, ); END_FUNCTION_BLOCK METHOD AssertMotionLims: BOOL VAR_IN_OUT stMotionStage: ST_MotionStage; astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR VAR_INPUT eState: E_TestStates; sID: STRING; END_VAR VAR nExpected: DWORD; END_VAR IF F_AtPositionState(stMotionStage, astPositionState[eState]) THEN // Both allowed nExpected := 2#111; ELSIF stMotionStage.stAxisStatus.fActPosition < astPositionState[eState].fPosition THEN // Only + allowed nExpected := 2#011; ELSE // Only - allowed nExpected := 2#101; END_IF IF stMotionStage.Axis.PlcToNc.ControlDWord > 0 THEN AssertEquals_DWORD( Expected:=nExpected, Actual:=stMotionStage.Axis.PlcToNc.ControlDWord, Message:=CONCAT('Wrong control dword in test ', sID), ); AssertMotionLims := TRUE; END_IF END_METHOD METHOD Test1D VAR_INPUT nTestID: UINT; eState: E_TestStates; END_VAR VAR_INST fbFFHWO: FB_HardwareFFOutput; bInit: BOOL; bLimAsserted: BOOL; END_VAR TEST(CONCAT('Test1D_state', UINT_TO_STRING(eState))); IF nTestCounter <> nTestID THEN RETURN; END_IF IF NOT bInit THEN eSetPos := eState; END_IF fbSubSysIO1D( LA:=fbArbiter1D, FFO:=fbFFHWO, ); fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter1D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, bReadDBNow:=NOT bInit, sDeviceName:='test', sTransitionKey:='trans', ); // When ready: check that directonal enables are correct bLimAsserted S= AssertMotionLims(stMotionStage1, astPositionState1, eState, CONCAT('1D mot 1 state ', UINT_TO_STRING(eState))); IF NOT bInit THEN bInit := TRUE; END_IF IF tonTimer.Q OR fb_Move1D.stPlcToEpics.bDone THEN AssertTrue( bLimAsserted, 'Skipped limit assert test', ); AssertTrue( Condition:=fb_Move1D.stPlcToEpics.bDone, Message:='Done should be True after move', ); AssertFalse( Condition:=fb_Move1D.stPlcToEpics.bBusy, Message:='Busy should be False after move', ); AssertFalse( Condition:=fb_Move1D.stPlcToEpics.bError, Message:='Error should be False after move', ); AssertEquals_UINT( Expected:=eState, Actual:=eGetPos, Message:='Did not get to the input state', ); AssertEquals_LREAL( Expected:=astPositionState1[eState].fPosition, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); AssertTrue( fbArbiter1D.CheckRequestInPool(astBeam[eState].nRequestAssertionID), 'Destination bp should have been in the arbiter', ); fb_Move1D.stEpicsToPlc.bReset := TRUE; fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter1D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, ); bInit := FALSE; bLimAsserted := FALSE; bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD Test2D VAR_INPUT nTestID: UINT; eState: E_TestStates; END_VAR VAR_INST fbFFHWO: FB_HardwareFFOutput; bInit: BOOL; bLimAsserted1: BOOL; bLimAsserted2: BOOL; END_VAR TEST(CONCAT('Test2D_state', UINT_TO_STRING(eState))); IF nTestCounter <> nTestID THEN RETURN; END_IF IF NOT bInit THEN eSetPos := eState; END_IF fbSubSysIO2D( LA:=fbArbiter2D, FFO:=fbFFHWO, ); fb_Move2D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter2D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, bReadDBNow:=NOT bInit, sDeviceName:='test', sTransitionKey:='trans', ); // When ready: check that directonal enables are correct bLimAsserted1 S= AssertMotionLims(stMotionStage1, astPositionState1, eState, CONCAT('2D mot 1 state ', UINT_TO_STRING(eState))); bLimAsserted2 S= AssertMotionLims(stMotionStage2, astPositionState2, eState, CONCAT('2D mot 2 state ', UINT_TO_STRING(eState))); IF NOT bInit THEN bInit := TRUE; END_IF IF tonTimer.Q OR fb_Move2D.stPlcToEpics.bDone THEN AssertTrue( bLimAsserted1 AND bLimAsserted2, 'Skipped limit assert test', ); AssertTrue( Condition:=fb_Move2D.stPlcToEpics.bDone, Message:='Done should be True after move', ); AssertFalse( Condition:=fb_Move2D.stPlcToEpics.bBusy, Message:='Busy should be False after move', ); AssertFalse( Condition:=fb_Move2D.stPlcToEpics.bError, Message:='Error should be False after move', ); AssertEquals_UINT( Expected:=eState, Actual:=eGetPos, Message:='Did not get to the input state', ); AssertEquals_LREAL( Expected:=astPositionState1[eState].fPosition, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); AssertEquals_LREAL( Expected:=astPositionState2[eState].fPosition, Actual:=stMotionStage2.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); AssertTrue( fbArbiter2D.CheckRequestInPool(astBeam[eState].nRequestAssertionID), 'Destination bp should have been in the arbiter', ); fb_Move2D.stEpicsToPlc.bReset := TRUE; fb_Move2D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter2D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, ); bInit := FALSE; bLimAsserted1 := FALSE; bLimAsserted2 := FALSE; bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD Test3D VAR_INPUT nTestID: UINT; eState: E_TestStates; END_VAR VAR_INST fbFFHWO: FB_HardwareFFOutput; bInit: BOOL; bLimAsserted1: BOOL; bLimAsserted2: BOOL; bLimAsserted3: BOOL; END_VAR TEST(CONCAT('Test3D_state', UINT_TO_STRING(eState))); IF nTestCounter <> nTestID THEN RETURN; END_IF IF NOT bInit THEN eSetPos := eState; END_IF fbSubSysIO3D( LA:=fbArbiter3D, FFO:=fbFFHWO, ); fb_Move3D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, stMotionStage3:=stMotionStage3, eEnumSet:=eSetPos, eEnumGet:=eGetPos, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, astPositionState3:=astPositionState3, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter3D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, bReadDBNow:=NOT bInit, sDeviceName:='test', sTransitionKey:='trans', ); // When ready: check that directonal enables are correct bLimAsserted1 S= AssertMotionLims(stMotionStage1, astPositionState1, eState, CONCAT('3D mot 1 state ', UINT_TO_STRING(eState))); bLimAsserted2 S= AssertMotionLims(stMotionStage2, astPositionState2, eState, CONCAT('3D mot 2 state ', UINT_TO_STRING(eState))); bLimAsserted3 S= AssertMotionLims(stMotionStage3, astPositionState3, eState, CONCAT('3D mot 3 state ', UINT_TO_STRING(eState))); IF NOT bInit THEN bInit := TRUE; END_IF IF tonTimer.Q OR fb_Move3D.stPlcToEpics.bDone THEN AssertTrue( bLimAsserted1 AND bLimAsserted2 AND bLimAsserted3, 'Skipped limit assert test', ); AssertTrue( Condition:=fb_Move3D.stPlcToEpics.bDone, Message:='Done should be True after move', ); AssertFalse( Condition:=fb_Move3D.stPlcToEpics.bBusy, Message:='Busy should be False after move', ); AssertFalse( Condition:=fb_Move3D.stPlcToEpics.bError, Message:='Error should be False after move', ); AssertEquals_UINT( Expected:=eState, Actual:=eGetPos, Message:='Did not get to the input state', ); AssertEquals_LREAL( Expected:=astPositionState1[eState].fPosition, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); AssertEquals_LREAL( Expected:=astPositionState2[eState].fPosition, Actual:=stMotionStage2.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); AssertEquals_LREAL( Expected:=astPositionState3[eState].fPosition, Actual:=stMotionStage3.stAxisStatus.fActPosition, Delta:=0.1, Message:='Did not get to the input state position', ); AssertTrue( fbArbiter3D.CheckRequestInPool(astBeam[eState].nRequestAssertionID), 'Destination bp should have been in the arbiter', ); fb_Move3D.stEpicsToPlc.bReset := TRUE; fb_Move3D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, stMotionStage3:=stMotionStage3, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, astPositionState3:=astPositionState3, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter3D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, ); bInit := FALSE; bLimAsserted1 := FALSE; bLimAsserted2 := FALSE; bLimAsserted3 := FALSE; bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestStartup1D (* - On startup, there should be no move request - In this case, we start in an unknown state since (0, 0, 0) is not matching any state for any motor - Movement should be free but the transition assertion must be active *) VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbFFHWO: FB_HardwareFFOutput; bInit: BOOL; END_VAR TEST('TestStartup1D'); IF nTestCounter <> nTestID THEN RETURN; END_IF fb_Move1D( stMotionStage:=stMotionStage1, astPositionState:=astPositionState1, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter1D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, bReadDBNow:=NOT bInit, sDeviceName:='test', sTransitionKey:='trans', ); bInit := TRUE; // We sit in this fb for some timeout number of seconds on purpose, not an error IF tonTimer.Q THEN // We should neither be busy nor done (we didn't do anything) AssertFalse( Condition:=fb_Move1D.stPlcToEpics.bDone, Message:='Done should be False with no move', ); AssertFalse( Condition:=fb_Move1D.stPlcToEpics.bBusy, Message:='Busy should be False with no move', ); // We should still be at 0,0,0 AssertEquals_LREAL( Expected:=0, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.0001, Message:='Why did we move? motor1 should have default position', ); // We should be able to move both directions, which is control word 7 (2#111) AssertEquals_DWORD( Expected:=2#111, Actual:=stMotionStage1.Axis.PlcToNc.ControlDWord, Message:='Expected full +/- enables', ); AssertTrue( fbArbiter1D.CheckRequestInPool(astBeam[E_TestStates.Unknown].nRequestAssertionID), 'Transition assertion ID not in pool', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestStartup2D (* - On startup, there should be no move request - Starting from (0, 0, 0) all motors should only be allowed to move + *) VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbFFHWO: FB_HardwareFFOutput; bInit: BOOL; END_VAR TEST('TestStartup2D'); IF nTestCounter <> nTestID THEN RETURN; END_IF fb_Move2D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter2D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, bReadDBNow:=NOT bInit, sDeviceName:='test', sTransitionKey:='trans', ); bInit := TRUE; // We sit in this fb for some timeout number of seconds on purpose, not an error IF tonTimer.Q THEN // We should neither be busy nor done (we didn't do anything) AssertFalse( Condition:=fb_Move2D.stPlcToEpics.bDone, Message:='Done should be False with no move', ); AssertFalse( Condition:=fb_Move2D.stPlcToEpics.bBusy, Message:='Busy should be False with no move', ); // We should still be at 0,0,0 AssertEquals_LREAL( Expected:=0, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.0001, Message:='Why did we move? motor1 should have default position', ); AssertEquals_LREAL( Expected:=0, Actual:=stMotionStage2.stAxisStatus.fActPosition, Delta:=0.0001, Message:='Why did we move? motor2 should have default position', ); // We should be able to move both directions, which is control word 7 (2#111) AssertEquals_DWORD( Expected:=2#111, Actual:=stMotionStage1.Axis.PlcToNc.ControlDWord, Message:='Expected full +/- enables', ); AssertEquals_DWORD( Expected:=2#111, Actual:=stMotionStage2.Axis.PlcToNc.ControlDWord, Message:='Expected full +/- enables', ); AssertTrue( fbArbiter2D.CheckRequestInPool(astBeam[E_TestStates.Unknown].nRequestAssertionID), 'Transition assertion ID not in pool', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestStartup3D (* - On startup, there should be no move request - Starting from (0, 0, 0) all motors should only be allowed to move + *) VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbFFHWO: FB_HardwareFFOutput; bInit: BOOL; END_VAR TEST('TestStartup3D'); IF nTestCounter <> nTestID THEN RETURN; END_IF fb_Move3D( stMotionStage1:=stMotionStage1, stMotionStage2:=stMotionStage2, stMotionStage3:=stMotionStage3, astPositionState1:=astPositionState1, astPositionState2:=astPositionState2, astPositionState3:=astPositionState3, eEnumSet:=eSetPos, eEnumGet:=eGetPos, fbFFHWO:=fbFFHWO, fbArbiter:=fbArbiter3D, bEnableMotion:=TRUE, bEnableBeamParams:=TRUE, bEnablePositionLimits:=TRUE, bReadDBNow:=NOT bInit, sDeviceName:='test', sTransitionKey:='trans', ); bInit := TRUE; // We sit in this fb for some timeout number of seconds on purpose, not an error IF tonTimer.Q THEN // We should neither be busy nor done (we didn't do anything) AssertFalse( Condition:=fb_Move3D.stPlcToEpics.bDone, Message:='Done should be False with no move', ); AssertFalse( Condition:=fb_Move3D.stPlcToEpics.bBusy, Message:='Busy should be False with no move', ); // We should still be at 0,0,0 AssertEquals_LREAL( Expected:=0, Actual:=stMotionStage1.stAxisStatus.fActPosition, Delta:=0.0001, Message:='Why did we move? motor1 should have default position', ); AssertEquals_LREAL( Expected:=0, Actual:=stMotionStage2.stAxisStatus.fActPosition, Delta:=0.0001, Message:='Why did we move? motor2 should have default position', ); AssertEquals_LREAL( Expected:=0, Actual:=stMotionStage3.stAxisStatus.fActPosition, Delta:=0.0001, Message:='Why did we move? motor3 should have default position', ); // We should be able to move both directions, which is control word 7 (2#111) AssertEquals_DWORD( Expected:=2#111, Actual:=stMotionStage1.Axis.PlcToNc.ControlDWord, Message:='Expected full +/- enables', ); AssertEquals_DWORD( Expected:=2#111, Actual:=stMotionStage2.Axis.PlcToNc.ControlDWord, Message:='Expected full +/- enables', ); AssertEquals_DWORD( Expected:=2#111, Actual:=stMotionStage3.Axis.PlcToNc.ControlDWord, Message:='Expected full +/- enables', ); AssertTrue( fbArbiter3D.CheckRequestInPool(astBeam[E_TestStates.Unknown].nRequestAssertionID), 'Transition assertion ID not in pool', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD Related: * `E_StageEnableMode`_ * `E_TestStates`_ * `FB_MotionStage`_ * `FB_MotorTestSuite`_ * `FB_PMPSJsonTestHelper`_ * `FB_PositionStateInternal`_ * `FB_PositionStatePMPS1D`_ * `FB_PositionStatePMPS2D`_ * `FB_PositionStatePMPS3D`_ * `F_AtPositionState`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateRead ^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateRead (* This function block tells us what state a single motor is at. In the case of multiple valid overlapping states, one will be picked arbitrarily, but we can see a full description of which overlapping states are present using the abAtPosition array. This will only run properly if FB_PositionStateInternal has been called on each position state to initialize it. *) VAR_IN_OUT // The motor to check the state of stMotionStage: ST_MotionStage; // The allowed states for this motor astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR VAR_OUTPUT // TRUE if we're standing still at a known state, or moving within the bounds of a state to another location in the bounds. bKnownState: BOOL; // TRUE if we're moving to some other state or to another non-state position. bMovingState: BOOL; // If we're at a known state, this will be the index in the astPositionState array that matches the state. Otherwise, this will be 0, which is below the bounds of the array, so it cannot be confused with a valid output. nPositionIndex: UINT; // A copy of the details of the current position state, for convenience. This may be a moving state or an unknown state as a placeholder if we are not at a known state. stCurrentPosition: ST_PositionState; // A full description of whether we're at each of our states. This is used in 2D, 3D, etc. to clarify cases where states may overlap in 1D. abAtPosition: ARRAY[1..GeneralConstants.MAX_STATES] OF BOOL; END_VAR VAR nIter: UINT; END_VAR bKnownState := FALSE; bMovingState := FALSE; FOR nIter := 1 TO GeneralConstants.MAX_STATES DO IF astPositionState[nIter].bValid THEN MOTION_GVL.nMaxStates := MAX(MOTION_GVL.nMaxStates, nIter); END_IF IF F_AtPositionState(stMotionStage:=stMotionStage, stPositionState:=astPositionState[nIter]) THEN bKnownState := TRUE; nPositionIndex := nIter; stCurrentPosition := astPositionState[nIter]; abAtPosition[nIter] := TRUE; ELSE abAtPosition[nIter] := FALSE; END_IF END_FOR IF NOT bKnownState THEN nPositionIndex := 0; stCurrentPosition.fPosition := stMotionStage.stAxisStatus.fActPosition; stCurrentPosition.fDelta := 0; stCurrentPosition.bMoveOk := FALSE; stCurrentPosition.bValid := FALSE; stCurrentPosition.bUseRawCounts := FALSE; bMovingState := stMotionStage.bExecute; IF bMovingState THEN stCurrentPosition.sName := 'Moving'; stCurrentPosition.fVelocity := stMotionStage.fVelocity; stCurrentPosition.fAccel := stMotionStage.fAcceleration; ELSE stCurrentPosition.sName := 'Unknown'; END_IF END_IF END_FUNCTION_BLOCK Related: * `FB_PositionStateInternal`_ * `F_AtPositionState`_ * `MOTION_GVL`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateRead_Test ^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateRead_Test EXTENDS TcUnit.FB_TestSuite (* Test that FB_PositionStateRead works exactly how it should according to its API during normal and failure states. *) VAR stMotionStage: ST_MotionStage; fbMotionStage: FB_MotionStage; astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbInternal: ARRAY[1..3] OF FB_PositionStateInternal; stDummyPos: ST_PositionState; fbTestMove: FB_TestHelperSetAndMove; fbRead: FB_PositionStateRead; nTestCounter: UINT; bOneTestDone: BOOL; fTestStartPos: LREAL; tonTimer: TON; nIter: DINT; bStatesReady: BOOL; END_VAR astPositionState[1].sName := 'UNO'; astPositionState[1].fPosition := 10; astPositionState[1].fDelta := 1; astPositionState[1].bValid := TRUE; astPositionState[1].bUseRawCounts := FALSE; astPositionState[2].sName := 'DOS'; astPositionState[2].fPosition := 20; astPositionState[2].fDelta := 1; astPositionState[2].bValid := FALSE; astPositionState[2].bUseRawCounts := FALSE; astPositionState[3].sName := 'TRES'; astPositionState[3].fPosition := 30; astPositionState[3].fDelta := 1; astPositionState[3].bValid := TRUE; astPositionState[3].bUseRawCounts := FALSE; astPositionState[4].sName := 'QUATRO'; astPositionState[4].fPosition := 30; astPositionState[4].fDelta := 1; astPositionState[4].bValid := FALSE; astPositionState[4].bUseRawCounts := FALSE; bStatesReady := TRUE; FOR nIter := 1 TO 4 DO afbInternal[nIter]( stMotionStage:=stMotionStage, stPositionState:=astPositionState[nIter], ); bStatesReady := bStatesReady AND astPositionState[nIter].bUpdated; END_FOR fbMotionStage(stMotionStage:=stMotionStage); // At position 1 check TestStaticPosition( nTestIndex:=0, sTestName:='AtPos1', fPosition:=astPositionState[1].fPosition + 0.2 * astPositionState[1].fDelta, bKnownState:=TRUE, bMovingState:=FALSE, nPositionIndex:=1, stCurrentPosition:=astPositionState[1], ); // Outside the deltas check TestStaticPosition( nTestIndex:=1, sTestName:='OutsidePos1Delta', fPosition:=astPositionState[1].fPosition + 2 * astPositionState[1].fDelta, bKnownState:=FALSE, bMovingState:=FALSE, nPositionIndex:=0, stCurrentPosition:=stDummyPos, ); // At invalid state 2 check TestStaticPosition( nTestIndex:=2, sTestName:='AtInvalidPos', fPosition:=astPositionState[2].fPosition, bKnownState:=FALSE, bMovingState:=FALSE, nPositionIndex:=0, stCurrentPosition:=stDummyPos, ); // At position 3 check TestStaticPosition( nTestIndex:=3, sTestName:='AtPos3', fPosition:=astPositionState[3].fPosition - 0.5 * astPositionState[3].fDelta, bKnownState:=TRUE, bMovingState:=FALSE, nPositionIndex:=3, stCurrentPosition:=astPositionState[3], ); // At position 3 and moving within bounds check TestMovingPosition( nTestIndex:=4, sTestName:='MovingAt3', fStartPosition:=astPositionState[3].fPosition, fGoalPosition:=astPositionState[3].fPosition + 0.9 * astPositionState[3].fDelta, fVelocity:=0.001, bKnownState:=TRUE, bMovingState:=FALSE, nPositionIndex:=3, stCurrentPosition:=astPositionState[3], ); // At position 3 and moving away check TestMovingPosition( nTestIndex:=5, sTestName:='MovingFrom3', fStartPosition:=astPositionState[3].fPosition, fGoalPosition:=astPositionState[3].fPosition + 100 * astPositionState[3].fDelta, fVelocity:=0.001, bKnownState:=FALSE, bMovingState:=TRUE, nPositionIndex:=0, stCurrentPosition:=stDummyPos, ); TestDupe(nTestIndex:=6); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; fbTestMove( stMotionStage:=stMotionStage, bExecute:=FALSE, ); END_IF END_FUNCTION_BLOCK METHOD PRIVATE Asserts VAR_INPUT tTimeout: TIME; bKnownState: BOOL; bMovingState: BOOL; nPositionIndex: DINT; stCurrentPosition: ST_PositionState; END_VAR VAR abExpected: ARRAY[1..GeneralConstants.MAX_STATES] OF BOOL; END_VAR fbRead( stMotionStage:=stMotionStage, astPositionState:=astPositionstate, ); AssertEquals_BOOL( Expected:=FALSE, Actual:=fbTestMove.tElapsed > tTimeout, Message:='Test timed out', ); AssertEquals_BOOL( Expected:=bKnownState, Actual:=fbRead.bKnownState, Message:='Incorrect bKnownState', ); AssertEquals_BOOL( Expected:=bMovingState, Actual:=fbRead.bMovingState, Message:='Incorrect bMovingState', ); AssertEquals_DINT( Expected:=nPositionIndex, Actual:=fbRead.nPositionIndex, Message:='Incorrect nPositionIndex', ); IF nPositionIndex > 0 THEN IF nPositionIndex <= GeneralConstants.MAX_STATES THEN abExpected[nPositionIndex] := TRUE; END_IF END_IF AssertArrayEquals_BOOL( Expecteds:=abExpected, Actuals:=fbRead.abAtPosition, Message:='Wrong at position array', ); IF bKnownState THEN AssertEquals_STRING( Expected:=stCurrentPosition.sName, Actual:=fbRead.stCurrentPosition.sName, Message:='Did not provide correct current position struct', ); END_IF END_METHOD METHOD TestDupe VAR_INPUT nTestIndex: DINT; END_VAR VAR abExpected: ARRAY[1..GeneralConstants.MAX_STATES] OF BOOL; END_VAR TEST('TestDupe'); IF nTestIndex <> nTestCounter THEN RETURN; END_IF // In this test, and only in this test, we must make sure state 4 is valid astPositionState[4].bValid := TRUE; fbTestMove( stMotionStage:=stMotionStage, bExecute:=TRUE, fStartPosition:=astPositionState[4].fPosition, fGoalPosition:=astPositionState[4].fPosition, ); abExpected[3] := TRUE; abExpected[4] := TRUE; IF fbTestMove.tElapsed > T#1s OR (fbTestMove.bSetDone AND bStatesReady) THEN fbRead( stMotionStage:=stMotionStage, astPositionState:=astPositionstate, ); AssertEquals_BOOL( Expected:=FALSE, Actual:=fbTestMove.tElapsed > T#1s, Message:='Test timed out', ); AssertEquals_BOOL( Expected:=TRUE, Actual:=fbRead.bKnownState, Message:='Incorrect bKnownState', ); AssertArrayEquals_BOOL( Expecteds:=abExpected, Actuals:=fbRead.abAtPosition, Message:='Wrong at position array', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestMovingPosition VAR_INPUT nTestIndex: UINT; sTestName: STRING; fStartPosition: LREAL; fGoalPosition: LREAL; fVelocity: LREAL; bKnownState: BOOL; bMovingState: BOOL; nPositionIndex: DINT; stCurrentPosition: ST_PositionState; END_VAR TEST(sTestName); IF nTestIndex <> nTestCounter THEN RETURN; END_IF fbTestMove( stMotionStage:=stMotionStage, bExecute:=TRUE, fStartPosition:=fStartPosition, fGoalPosition:=fGoalPosition, fVelocity:=0.001, bHWEnable:=TRUE, ); IF fbTestMove.tElapsed > T#5s OR (fbTestMove.bMotionStarted AND bStatesReady) THEN Asserts( tTimeout:=T#5s, bKnownState:=bKnownState, bMovingState:=bMovingState, nPositionIndex:=nPositionIndex, stCurrentPosition:=stCurrentPosition, ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD PRIVATE TestStaticPosition VAR_INPUT nTestIndex: UINT; sTestName: STRING; fPosition: LREAL; bKnownState: BOOL; bMovingState: BOOL; nPositionIndex: DINT; stCurrentPosition: ST_PositionState; END_VAR TEST(sTestName); IF nTestIndex <> nTestCounter THEN RETURN; END_IF fbTestMove( stMotionStage:=stMotionStage, bExecute:=TRUE, fStartPosition:=fPosition, fGoalPosition:=fPosition, ); IF fbTestMove.tElapsed > T#1s OR (fbTestMove.bSetDone AND bStatesReady) THEN Asserts( tTimeout:=T#1s, bKnownState:=bKnownState, bMovingState:=bMovingState, nPositionIndex:=nPositionIndex, stCurrentPosition:=stCurrentPosition, ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD Related: * `FB_MotionStage`_ * `FB_PositionStateInternal`_ * `FB_PositionStateRead`_ * `FB_TestHelperSetAndMove`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateReadND ^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateReadND (* Function block to get the combined N-dimensional state of a group of motors. It is a building block not meant for use outside of lcls-twintcat-motion. Use FB_PositionStateRead1D, FB_PositionStateRead2D, ... etc. instead *) VAR_IN_OUT // The motors with a combined N-dimensional state astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // Each motor's respective position states along its direction astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR VAR_INPUT // The number of motors we're actually using nActiveMotorCount: UINT; END_VAR VAR_OUTPUT // TRUE if we're standing still at a known state. bKnownState: BOOL; // TRUE if we're moving, there can be no valid state if we are moving. bMovingState: BOOL; // If we're at a known state, this will be the state index along the enclosed states arrays. Otherwise, it will be zero, which is below the bounds of the states array. nPositionIndex: UINT; // TRUE if the active motor count was invalid bMotorCountError: BOOL; // A full description of whether we're at each of our states. This is used to clarify cases where states may overlap. abAtPosition: ARRAY[1..GeneralConstants.MAX_STATES] OF BOOL; END_VAR VAR // The individual position state reader function blocks afbPositionStateRead: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF FB_PositionStateRead; nIter: UINT; nIter2: UINT; END_VAR CheckCount(); IF NOT bMotorCountError THEN DoStateReads(); CombineOutputs(); END_IF END_FUNCTION_BLOCK ACTION CheckCount: // Make sure the motor count is valid (positive, nonzero, less or equal to the max) bMotorCountError S= nActiveMotorCount <= 0; bMotorCountError S= nActiveMotorCount > MotionConstants.MAX_STATE_MOTORS; END_ACTION ACTION CombineOutputs: // bKnownState is TRUE if ALL motors have the same known state bKnownState := TRUE; FOR nIter := 1 TO nActiveMotorCount DO bKnownState := bKnownState AND afbPositionStateRead[nIter].bKnownState; END_FOR // bMovingState is TRUE if ANY motor is moving bMovingState := FALSE; FOR nIter := 1 TO nActiveMotorCount DO bMovingState := bMovingState OR afbPositionStateRead[nIter].bMovingState; END_FOR // To account for redundant 1D states, we need to check the full output arrays. nPositionIndex := 0; FOR nIter := 1 TO GeneralConstants.MAX_STATES DO abAtPosition[nIter] := TRUE; FOR nIter2 := 1 TO nActiveMotorCount DO abAtPosition[nIter] R= NOT afbPositionStateRead[nIter2].abAtPosition[nIter]; END_FOR IF abAtPosition[nIter] THEN nPositionIndex := nIter; END_IF END_FOR // Position index 0 means different positions bKnownState := bKnownState AND nPositionIndex <> 0; END_ACTION ACTION DoStateReads: MOTION_GVL.nMaxStateMotorCount := MAX(MOTION_GVL.nMaxStateMotorCount, nActiveMotorCount); FOR nIter := 1 TO nActiveMotorCount DO afbPositionStateRead[nIter]( stMotionStage:=astMotionStage[nIter], astPositionState:=astPositionState[nIter], ); END_FOR END_ACTION Related: * `FB_PositionStateRead`_ * `MOTION_GVL`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_PositionStateReadND_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_PositionStateReadND_Test EXTENDS FB_MotorTestSuite (* Test that FB_PositionStateReadND can be used to read and summarize N-dimensional state positions where multiple motors must move in sync to a shared named state. *) VAR astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; afbMotionStage: ARRAY[1..3] OF FB_MotionStage; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY [1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbInternal: ARRAY[1..3] OF ARRAY [1..2] OF FB_PositionStateInternal; afbTestMove: ARRAY[1..3] OF FB_TestHelperSetAndMove; fbRead: FB_PositionStateReadND; bOneAssertDone: BOOL; nAssertCounter: UINT; nIter1: UINT; nIter2: UINT; nIter3: UINT; fbMisRead: FB_PositionStateReadND; astGoodStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astGoodStateShape: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY [1..GeneralConstants.MAX_STATES] OF ST_PositionState; astSqMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; afbSqMotionStage: ARRAY[1..2] OF FB_MotionStage; astSquareStates: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY [1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbSqInternal: ARRAY[1..2] OF ARRAY[1..4] OF FB_PositionStateInternal; afbSqTestMove: ARRAY[1..2] OF FB_TestHelperSetAndMove; fbSqRead: FB_PositionStateReadND; bSqAssertDone: BOOL; nSqAssertCounter: UINT; END_VAR VAR CONSTANT NO_STATE: UINT := 0; OUT_STATE: UINT := 1; IN_STATE: UINT := 2; IN_TWEAK: UINT := 3; AWAY: UINT := 4; LAST_TEST: UINT := AWAY; TEST_COUNT: UINT := 5; END_VAR PerMotor(1); PerMotor(2); PerMotor(3); SquareSetup(); // First check the case of mismatched arrays TestForgot(); TestCombos(nAssertCounter); TestSquare(nSqAssertCounter); IF bOneAssertDone THEN afbTestMove[1]( stMotionStage:=astMotionStage[1], bExecute:=FALSE, ); afbTestMove[2]( stMotionStage:=astMotionStage[2], bExecute:=FALSE, ); afbTestMove[3]( stMotionStage:=astMotionStage[3], bExecute:=FALSE, ); IF afbTestMove[1].bResetDone AND afbTestMove[2].bResetDone AND afbTestMove[3].bResetDone THEN bOneAssertDone := FALSE; nAssertCounter := nAssertCounter + 1; END_IF END_IF IF bSqAssertDone THEN afbSqTestMove[1]( stMotionStage:=astMotionStage[1], bExecute:=FALSE, ); afbSqTestMove[2]( stMotionStage:=astMotionStage[2], bExecute:=FALSE, ); IF afbSqTestMove[1].bResetDone AND afbSqTestMove[2].bResetDone THEN bSqAssertDone := FALSE; nSqAssertCounter := nSqAssertCounter + 1; END_IF END_IF END_FUNCTION_BLOCK METHOD PRIVATE DoAssert VAR_INPUT nMotorCase1: UINT; nMotorCase2: UINT; nMotorCase3: UINT; bReady1: BOOL; bReady2: BOOL; bReady3: BOOL; END_VAR VAR bKnownState: BOOL; bMovingState: BOOL; nPositionIndex: DINT; sTestCase: STRING; END_VAR sTestCase := CONCAT(CONCAT(UINT_TO_STRING(nMotorCase1), UINT_TO_STRING(nMotorCase2)), UINT_TO_STRING(nMotorCase3)); AssertTrue( Condition:=bReady1, Message:=CONCAT('Timeout for motor 1 during test case ', sTestCase), ); AssertTrue( Condition:=bReady2, Message:=CONCAT('Timeout for motor 2 during test case ', sTestCase), ); AssertTrue( Condition:=bReady3, Message:=CONCAT('Timeout for motor 3 during test case ', sTestCase), ); // All at OUT or all at IN should be OUT and IN respectively, even if doing a tweak move // Any other combination is at no state IF nMotorCase1 = OUT_STATE AND nMotorCase2 = OUT_STATE AND nMotorCase3 = OUT_STATE THEN bKnownState := TRUE; nPositionIndex := 1; ELSIF (nMotorCase1 = IN_STATE OR nMotorCase1 = IN_TWEAK) AND (nMotorCase2 = IN_STATE OR nMotorCase2 = IN_TWEAK) AND (nMotorCase3 = IN_STATE OR nMotorCase3 = IN_TWEAK) THEN bKnownState := TRUE; nPositionIndex := 2; END_IF // In addition, bMovingState must be set if any is moving away from a state IF nMotorCase1 = AWAY OR nMotorCase2 = AWAY OR nMotorCase3 = AWAY THEN bMovingState := TRUE; END_IF AssertEquals_BOOL( Expected:=bKnownState, Actual:=fbRead.bKnownState, Message:=CONCAT('Wrong bKnownState for test case ', sTestCase), ); AssertEquals_BOOL( Expected:=bMovingState, Actual:=fbRead.bMovingState, Message:=CONCAT('Wrong bMovingState for test case ', sTestCase), ); AssertEquals_DINT( Expected:=nPositionIndex, Actual:=fbRead.nPositionIndex, Message:=CONCAT('Wrong nPositionIndex for test case ', sTestCase), ); END_METHOD METHOD PRIVATE DoMove: BOOL VAR_INPUT nMotorIndex: UINT; nMotorCase: UINT; END_VAR VAR fStartPosition: LREAL; fGoalPosition: LREAL; END_VAR CASE nMotorCase OF // Somewhere smaller than OUT, static NO_STATE: fStartPosition := astPositionState[nMotorIndex][1].fPosition - 10 * astPositionState[nMotorIndex][1].fDelta; fGoalPosition := fStartPosition; // Exactly at OUT, static OUT_STATE: fStartPosition := astPositionState[nMotorIndex][1].fPosition; fGoalPosition := fStartPosition; // Exactly at IN, static IN_STATE: fStartPosition := astPositionState[nMotorIndex][2].fPosition; fGoalPosition := fStartPosition; // Start at IN, do a small tweak IN_TWEAK: fStartPosition := astPositionState[nMotorIndex][2].fPosition; fGoalPosition := fStartPosition + 0.9 * astPositionState[nMotorIndex][2].fDelta; // Start at IN, move positive a lot AWAY: fStartPosition := astPositionState[nMotorIndex][2].fPosition; fGoalPosition := fStartPosition + 100 * astPositionState[nMotorIndex][2].fDelta; END_CASE afbTestMove[nMotorIndex]( stMotionStage:=astMotionStage[nMotorIndex], bExecute:=TRUE, fStartPosition:=fStartPosition, fGoalPosition:=fGoalPosition, fVelocity:=0.001, bHWEnable:=TRUE, ); CASE nMotorCase OF // All static states: report ready when set is done NO_STATE: DoMove := afbTestMove[nMotorIndex].bSetDone; OUT_STATE: DoMove := afbTestMove[nMotorIndex].bSetDone; IN_STATE: DoMove := afbTestMove[nMotorIndex].bSetDone; // All moving states: report ready when move starts IN_TWEAK: DoMove := afbTestMove[nMotorIndex].bMotionStarted; AWAY: DoMove := afbTestMove[nMotorIndex].bMotionStarted; END_CASE // Universal 5s timeout DoMove S= afbTestMove[nMotorIndex].tElapsed > T#5s; END_METHOD METHOD PRIVATE DoRead VAR_INPUT END_VAR fbRead( astMotionStage:=astMotionStage, astPositionState:=astPositionState, nActiveMotorCount:=3, ); END_METHOD METHOD PRIVATE PerMotor : BOOL VAR_INPUT nIndex: DINT; END_VAR afbMotionStage[nIndex](stMotionStage:=astMotionStage[nIndex]); astPositionState[nIndex][1].sName := 'OUT'; astPositionState[nIndex][1].fPosition := nIndex * 100 + 10; astPositionState[nIndex][1].fDelta := 1; astPositionState[nIndex][1].bUseRawCounts := FALSE; SetGoodState(astPositionState[nIndex][1]); afbInternal[nIndex][1]( stMotionStage:=astMotionStage[nIndex], stPositionState:=astPositionState[nIndex][1], ); astPositionState[nIndex][2].sName := 'IN'; astPositionState[nIndex][2].fPosition := nIndex * 100 + 20; astPositionState[nIndex][2].fDelta := 1; astPositionState[nIndex][2].bUseRawCounts := FALSE; SetGoodState(astPositionState[nIndex][2]); afbInternal[nIndex][2]( stMotionStage:=astMotionStage[nIndex], stPositionState:=astPositionState[nIndex][2], ); END_METHOD METHOD SquareSetup VAR_INPUT END_VAR // 2 motors, 4 positions per motor, square geometry // Motor 1 is X, motor 2 is Y // Corners at (10,10), (10, 20), (20, 10), (20, 20) // So motor 1 is either LEFT=10 or RIGHT=20 // motor 2 is either BOT=10 or TOP=20 afbSqMotionStage[1](stMotionStage:=astSqMotionStage[1]); afbSqMotionStage[2](stMotionStage:=astSqMotionStage[2]); astSquareStates[1][1].sName := 'Top Left'; astSquareStates[1][1].fPosition := 10; astSquareStates[1][1].fDelta := 1; SetGoodState(astSquareStates[1][1]); afbSqInternal[1][1]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[1][1], ); astSquareStates[1][2].sName := 'Top Right'; astSquareStates[1][2].fPosition := 20; astSquareStates[1][2].fDelta := 1; SetGoodState(astSquareStates[1][2]); afbSqInternal[1][2]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[1][2], ); astSquareStates[1][3].sName := 'Bot Left'; astSquareStates[1][3].fPosition := 10; astSquareStates[1][3].fDelta := 1; SetGoodState(astSquareStates[1][3]); afbSqInternal[1][3]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[1][3], ); astSquareStates[1][4].sName := 'Bot Right'; astSquareStates[1][4].fPosition := 20; astSquareStates[1][4].fDelta := 1; SetGoodState(astSquareStates[1][4]); afbSqInternal[1][4]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[1][4], ); astSquareStates[2][1].sName := 'Top Left'; astSquareStates[2][1].fPosition := 20; astSquareStates[2][1].fDelta := 1; SetGoodState(astSquareStates[2][1]); afbSqInternal[2][1]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[2][1], ); astSquareStates[2][2].sName := 'Top Right'; astSquareStates[2][2].fPosition := 20; astSquareStates[2][2].fDelta := 1; SetGoodState(astSquareStates[2][2]); afbSqInternal[2][2]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[2][2], ); astSquareStates[2][3].sName := 'Bot Left'; astSquareStates[2][3].fPosition := 10; astSquareStates[2][3].fDelta := 1; SetGoodState(astSquareStates[2][3]); afbSqInternal[2][3]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[2][3], ); astSquareStates[2][4].sName := 'Bot Right'; astSquareStates[2][4].fPosition := 10; astSquareStates[2][4].fDelta := 1; SetGoodState(astSquareStates[2][4]); afbSqInternal[2][4]( stMotionStage:=astMotionStage[1], stPositionState:=astSquareStates[2][4], ); END_METHOD METHOD PRIVATE TestCombos VAR_INPUT nAssertID: UINT; END_VAR VAR nMotor1Case: UINT; nMotor2Case: UINT; nMotor3Case: UINT; bReady1: BOOL; bReady2: BOOL; bReady3: BOOL; END_VAR VAR_INST tonTimeout: TON; END_VAR // This should be one big test case with 125 asserts TEST('TestAllCombos'); nMotor1Case := nAssertID MOD TEST_COUNT; nMotor2Case := LREAL_TO_UINT(FLOOR(nAssertID / TEST_COUNT)) MOD TEST_COUNT; nMotor3Case := LREAL_TO_UINT(FLOOR(nAssertID / TEST_COUNT / TEST_COUNT)) MOD TEST_COUNT; bReady1 := DoMove(1, nMotor1Case); bReady2 := DoMove(2, nMotor2Case); bReady3 := DoMove(3, nMotor3Case); // There is a 5s timeout at a lower level, but that timeout could fail tonTimeout( IN:=TRUE, PT:=T#7s, ); IF tonTimeout.Q OR (bReady1 AND bReady2 AND bReady3) THEN DoRead(); DoAssert(nMotor1Case, nMotor2Case, nMotor3Case, bReady1, bReady2, bReady3); bOneAssertDone := TRUE; // The final assert case marks test as finished IF tonTimeout.Q OR (nMotor1Case = LAST_TEST AND nMotor2Case = LAST_TEST AND nMotor3Case = LAST_TEST) THEN // 11 extra tests // 1 from TestForgot // 10 (2*5) from TestSquare AssertEquals_UINT( Expected:=6 * TEST_COUNT * TEST_COUNT * TEST_COUNT + 11, Actual:=AssertResults.TotalAsserts, Message:='Some of the asserts were not run', ); AssertFalse( tonTimeout.Q, 'Level 2 timeout in test', ); TEST_FINISHED(); END_IF tonTimeout(IN:=FALSE); END_IF END_METHOD METHOD PRIVATE TestForgot VAR END_VAR TEST('ForgotCount'); fbMisRead( astMotionStage:=astGoodStage, astPositionState:=astGoodStateShape, ); AssertTrue( Condition:=fbMisRead.bMotorCountError, Message:='Failed to notice missing count', ); TEST_FINISHED(); END_METHOD METHOD TestSquare VAR_INPUT nAssertID: UINT; END_VAR VAR fMotor1Pos: LREAL; fMotor2Pos: LREAL; nGoal: UINT; END_VAR VAR_INST tonTimeout: TON; END_VAR // We'll do 5 tests, one at each square point and one more in the middle Test('TestSquare'); IF nAssertID > 4 THEN RETURN; END_IF IF nAssertID = 0 THEN fMotor1Pos := 10; fMotor2Pos := 10; nGoal := 3; ELSIF nAssertID = 1 THEN fMotor1Pos := 20; fMotor2Pos := 10; nGoal := 4; ELSIF nAssertID = 2 THEN fMotor1Pos := 10; fMotor2Pos := 20; nGoal := 1; ELSIF nAssertID = 3 THEN fMotor1Pos := 20; fMotor2Pos := 20; nGoal := 2; ELSIF nAssertID = 4 THEN fMotor1Pos := 15; fMotor2Pos := 15; nGoal := 0; END_IF afbSqTestMove[1]( stMotionStage:=astSqMotionStage[1], bExecute:=TRUE, fStartPosition:=fMotor1Pos, fGoalPosition:=fMotor1Pos, ); afbSqTestMove[2]( stMotionStage:=astSqMotionStage[2], bExecute:=TRUE, fStartPosition:=fMotor2Pos, fGoalPosition:=fMotor2Pos, ); tonTimeout( IN:=TRUE, PT:=T#5s, ); IF tonTimeout.Q OR afbSqTestMove[1].bSetDone AND afbSqTestMove[1].bSetDone THEN fbSqRead( astMotionStage:=astSqMotionStage, astPositionState:=astSquareStates, nActiveMotorCount:=2, ); AssertFalse( tonTimeout.Q, CONCAT('Timeout in square test ', UINT_TO_STRING(nAssertID)), ); AssertEquals_UINT( Expected:=nGoal, Actual:=fbSqRead.nPositionIndex, Message:=CONCAT('Wrong read in square test ', UINT_TO_STRING(nAssertID)), ); bSqAssertDone := TRUE; IF nAssertID = 4 THEN TEST_FINISHED(); END_IF tonTimeout(IN:=FALSE); END_IF END_METHOD Related: * `FB_MotionStage`_ * `FB_MotorTestSuite`_ * `FB_PositionStateInternal`_ * `FB_PositionStateReadND`_ * `FB_TestHelperSetAndMove`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ 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: ST_MotionStage; END_VAR stMotionStage.bAllForwardEnable := stMotionStage.bLimitForwardEnable AND (stMotionStage.bGantryForwardEnable OR NOT stMotionStage.bGantryAxis) AND stMotionStage.stEPSForwardEnable.bEPS_OK; stMotionStage.bAllBackwardEnable := stMotionStage.bLimitBackwardEnable AND (stMotionStage.bGantryBackwardEnable OR NOT stMotionStage.bGantryAxis) AND stMotionStage.stEPSBackwardEnable.bEPS_OK; stMotionStage.bAllEnable := stMotionStage.bEnable AND stMotionStage.bHardwareEnable AND stMotionStage.stEPSPowerEnable.bEPS_OK; stMotionStage.bAllEnable R= NOT stMotionStage.bUserEnable; END_FUNCTION_BLOCK Related: * `ST_MotionStage`_ FB_Standard_PMPSDB ^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_Standard_PMPSDB (* This FB should be run on every motion PLC to encapsulate the pmps file reader. It is pre-instantiated at MOTION_GVL.fbStandardPMPSDB *) VAR_IN_OUT // The fast fault output to fault to. io_fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // If TRUE, FB will run. Reads when enable goes TRUE. bEnable: BOOL; // E.g. lfe-motion sPlcName: STRING; {attribute 'pytmc' := ' pv: REFRESH io: io '} // Set to TRUE to cause an extra read. bRefresh: BOOL; // Directory where the DB is stored. sDirectory: STRING := '/Hard Disk/ftp/PMPS/'; END_VAR VAR_OUTPUT {attribute 'pytmc' := ' pv: LAST_REFRESH io: i '} nLastRefreshTime: DINT; bReadPmpsDb: BOOL; END_VAR VAR bExecute: BOOL; rtEnable: R_TRIG; rtRefresh: R_TRIG; ftBusy: F_TRIG; // Time tracking liften from Arbiter PLCs fbTime : FB_LocalSystemTime := ( bEnable := TRUE, dwCycle := 1 ); fbTime_to_UTC: FB_TzSpecificLocalTimeToSystemTime; fbGetTimeZone: FB_GetTimeZoneInformation; END_VAR IF NOT bEnable THEN RETURN; END_IF rtEnable(CLK:=bEnable); rtRefresh(CLK:=bRefresh); bRefresh := FALSE; IF rtEnable.Q OR rtRefresh.Q THEN // Make sure file reader gets a rising edge MOTION_GVL.fbPmpsFileReader( io_fbFFHWO:=io_fbFFHWO, bExecute:=FALSE, ); bExecute := TRUE; END_IF MOTION_GVL.fbPmpsFileReader( io_fbFFHWO:=io_fbFFHWO, bExecute:=bExecute, sSrcPathName:=CONCAT(CONCAT(sDirectory, sPlcName), '.json'), sPLCName:=sPLCName, PMPS_jsonDoc=>PMPS_GVL.BP_jsonDoc, ); ftBusy(CLK:=MOTION_GVL.fbPmpsFileReader.bBusy); IF ftBusy.Q THEN bReadPmpsDb := TRUE; ELSE bReadPmpsDb := FALSE; END_IF // Lifted from Arbiter PLCs: keep track of the time fbTime(sNetID:=''); fbGetTimeZone(sNetID:='', bExecute:=TRUE, tTimeout:=T#10S); fbTime_to_UTC(in:= fbTime.systemTime , tzInfo:=fbGetTimeZone.tzInfo); // Update the refresh time on successful read IF ftBusy.Q AND NOT MOTION_GVL.fbPmpsFileReader.bError THEN nLastRefreshTime := TO_DINT(TO_DT(SystemTime_TO_DT(fbTime_to_UTC.out))); END_IF bExecute R= ftBusy.Q; END_FUNCTION_BLOCK Related: * `MOTION_GVL`_ FB_StatePMPSEnables ^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StatePMPSEnables (* Function block to set virtual limit enables using MC_POWER for single dimensional state movers. It is a building block not meant for use outside of lcls-twintcat-motion. Each motor has a virtual "allowed" range of motion based on its goal position. When not at the goal, the motor can only move toward the goal. When at the goal, the motor can move within the position's delta. With no goals or other strange states, the motor is permitted to move in either direction to help restore it to a known position. *) VAR_IN_OUT // The motor with a position state. stMotionStage: ST_MotionStage; // All possible position states for this motor. astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Hardware output to fault to if there is a problem. fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // If TRUE, do the limits as normal. If FALSE, allow all moves regardless of the limits defined here. bEnable: BOOL; // The state that the motor is moving to. nGoalStateIndex: UINT; // The overal PMPS FB state eStatePMPSStatus: E_StatePMPSStatus; // Connect to the BPTM bTransitionAuthorized: BOOL; END_VAR VAR_OUTPUT // The enable state we send to MC_Power. This is a pass-through from stMotionStage. bEnabled: BOOL; // The forward enable state we send to MC_Power. This may be a pass-through or an override to FALSE. bForwardEnabled: BOOL; // The backwards enable state we send to MC_Power. This may be a pass-through or an override to FALSE. bBackwardEnabled: BOOL; // TRUE if there is a valid goal position and FALSE otherwise. This makes a fast fault if FALSE. bValidGoal: BOOL; END_VAR VAR mc_power: MC_POWER; nPrevStateIndex: DINT; fLowerPos: LREAL; fUpperPos: LREAL; ffNoGoal: FB_FastFault; END_VAR GetBounds(); SetEnables(); ApplyEnables(); RunFastFaults(); END_FUNCTION_BLOCK ACTION ApplyEnables: (* This action takes runs MC_POWER appropriately given the motor's own enables and the results of this FB's checks. *) bEnabled := stMotionStage.bAllEnable; bForwardEnabled := bForwardEnabled AND stMotionStage.bAllForwardEnable; bBackwardEnabled := bBackwardEnabled AND stMotionStage.bAllBackwardEnable; CASE eStatePMPSStatus OF E_StatePMPSStatus.UNKNOWN: stMotionStage.bSafetyReady := FALSE; E_StatePMPSStatus.TRANSITION: stMotionStage.bSafetyReady := bTransitionAuthorized; bForwardEnabled R= NOT bTransitionAuthorized; bBackwardEnabled R= NOT bTransitionAuthorized; E_StatePMPSStatus.AT_STATE: stMotionStage.bSafetyReady := stMotionStage.bExecute; E_StatePMPSStatus.DISABLED: stMotionStage.bSafetyReady := TRUE; END_CASE mc_power( Axis:=stMotionStage.Axis, Enable:=bEnabled, Enable_Positive:=bForwardEnabled, Enable_Negative:=bBackwardEnabled, ); END_ACTION ACTION GetBounds: (* This action sets fLowerPos and fUpperPos based on our goal position. *) IF nGoalStateIndex > 0 AND nGoalStateIndex <= GeneralConstants.MAX_STATES THEN IF astPositionState[nGoalStateIndex].bValid AND astPositionState[nGoalStateIndex].bUpdated THEN bValidGoal := TRUE; fLowerPos := astPositionState[nGoalStateIndex].fPosition - ABS(astPositionState[nGoalStateIndex].fDelta); fUpperPos := astPositionState[nGoalStateIndex].fPosition + ABS(astPositionState[nGoalStateIndex].fDelta); ELSE bValidGoal := FALSE; END_IF ELSE bValidGoal := FALSE; END_IF END_ACTION ACTION RunFastFaults: ffNoGoal( i_xOK:=bValidGoal, i_xAutoReset:=TRUE, i_DevName:=stMotionStage.sName, i_Desc:='Invalid goal position in state move', i_TypeCode:=E_MotionFFType.INVALID_GOAL, io_fbFFHWO:=fbFFHWO, ); END_ACTION ACTION SetEnables: (* This action sets bForwardEnable and bBackwardEnable based on the current position and the calculated bounds. *) IF bValidGoal AND bEnable THEN bForwardEnabled := stMotionStage.stAxisStatus.fActPosition < fUpperPos; bBackwardEnabled := stMotionStage.stAxisStatus.fActPosition > fLowerPos; ELSE // Either invalid state with a fault or FB not enabled bForwardEnabled := TRUE; bBackwardEnabled := TRUE; END_IF END_ACTION Related: * `E_MotionFFType`_ * `E_StatePMPSStatus`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_StatePMPSEnables_Test ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StatePMPSEnables_Test EXTENDS FB_MotorTestSuite (* Tests for FB_StatePMPSEnables This function block ensures that: - When not at our goal state, we cannot move away from our goal state - When at our goal state, we must stay within the state bounds - When at our goal state, we still obey other constraints like limit switches We also include a super basic real move test with our simulator motor to make sure the enables are set properly. *) VAR stMotionStage: ST_MotionStage; fbMotionStage: FB_MotionStage; astPositionState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; fbInternal1: FB_PositionStateInternal; fbInternal2: FB_PositionStateInternal; nInvalidState: UINT := 2; nGoalState: UINT := 4; nNotUpdatedState: UINT := 6; bInit: BOOL; nTestCounter: UINT; bOneTestDone: BOOL; fTestStartPos: LREAL; tonTimer: TON; bStatesReady: BOOL; END_VAR astPositionState[nGoalState].fPosition := 10; astPositionState[nGoalState].fDelta := 1; SetGoodState(astPositionState[nGoalState]); astPositionState[nInvalidState].fPosition := 20; astPositionState[nInvalidState].fDelta := 1; astPositionState[nNotUpdatedState].fPosition := 20; astPositionState[nNotUpdatedState].fDelta := 1; SetGoodState(astPositionState[nNotUpdatedState]); fbInternal1( stMotionStage:=stMotionStage, stPositionState:=astPositionState[nGoalState], ); fbInternal2( stMotionStage:=stMotionStage, stPositionState:=astPositionState[nInvalidState], ); fbMotionStage(stMotionStage:=stMotionStage); bStatesReady:=astPositionState[nGoalState].bUpdated AND astPositionState[nInvalidState].bUpdated; IF bStatesReady AND nTestCounter = 0 THEN // Don't run any tests until the states are ready nTestCounter := 1; END_IF TestInvalid(1); TestNotUpdated(2); TestBelow(3); TestAbove(4); TestAt(5); TestDisabled(6); TestLimits(7); TestMoveTo(8); TestMoveAt(9); IF bOneTestDone THEN bOneTestDone := FALSE; nTestCounter := nTestCounter + 1; tonTimer(IN:=FALSE); END_IF // Use this timer to time out any tests that stall tonTimer( IN:=bStatesReady, PT:=T#5s, ); END_FUNCTION_BLOCK METHOD TestAbove VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestAbove'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Set position to be above the goal's range fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition + 100 * astPositionState[nGoalState].fDelta, fGoalPosition:=astPositionState[nGoalState].fPosition + 100 * astPositionState[nGoalState].fDelta, fVelocity:=1, bHWEnable:=FALSE, ); bInit := TRUE; // Run our FB fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=TRUE, nGoalStateIndex:=nGoalState, ); fbFFHWO.EvaluateOutput(); // If we've set the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bSetDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=fbMove.fStartPosition, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Position was not set correctly', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertFalse( fbStateEnables.bForwardEnabled, 'Forward enabled when above goal', ); AssertTrue( fbStateEnables.bBackwardEnabled, 'Backward disabled when above goal', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestAt VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestAt'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Set position to be at the goal state fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition, fGoalPosition:=astPositionState[nGoalState].fPosition, fVelocity:=1, bHWEnable:=FALSE, ); bInit := TRUE; // Run our FB fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=TRUE, nGoalStateIndex:=nGoalState, ); fbFFHWO.EvaluateOutput(); // If we've set the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bSetDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=fbMove.fStartPosition, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Position was not set correctly', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertTrue( fbStateEnables.bForwardEnabled, 'Forward disabled when at goal', ); AssertTrue( fbStateEnables.bBackwardEnabled, 'Backward disabled when at goal', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestBelow VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestBelow'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Set position to be below the goal's range fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition - 100 * astPositionState[nGoalState].fDelta, fGoalPosition:=astPositionState[nGoalState].fPosition - 100 * astPositionState[nGoalState].fDelta, fVelocity:=1, bHWEnable:=FALSE, ); bInit := TRUE; // Run our FB fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=bInit, nGoalStateIndex:=nGoalState, ); fbFFHWO.EvaluateOutput(); // If we've set the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bSetDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=fbMove.fStartPosition, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Position was not set correctly', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertFalse( fbStateEnables.bBackwardEnabled, 'Backward enabled when below goal', ); AssertTrue( fbStateEnables.bForwardEnabled, 'Forward disabled when below goal', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestDisabled VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestDisabled'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Set position to be below the goal's range fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition - 100 * astPositionState[nGoalState].fDelta, fGoalPosition:=astPositionState[nGoalState].fPosition - 100 * astPositionState[nGoalState].fDelta, fVelocity:=1, bHWEnable:=FALSE, ); bInit := TRUE; // Run our FB fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=FALSE, nGoalStateIndex:=nGoalState, ); fbFFHWO.EvaluateOutput(); // If we've set the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bSetDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=fbMove.fStartPosition, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Position was not set correctly', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertTrue( fbStateEnables.bBackwardEnabled, 'Backward disabled when fb is supposed to be disabled', ); AssertTrue( fbStateEnables.bForwardEnabled, 'Forward disabled when fb is supposed to be disabled', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestInvalid VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestInvalid'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Faulted prior to test', ); // The invalid state should give us a fault fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=TRUE, nGoalStateIndex:=nInvalidState, ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Invalid state did not fault', ); bOneTestDone := TRUE; TEST_FINISHED(); END_METHOD METHOD TestLimits VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestLimits'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Kill the limit switches for this test stMotionStage.bLimitForwardEnable := FALSE; stMotionStage.bLimitBackwardEnable := FALSE; // Set position to be at the goal state fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition, fGoalPosition:=astPositionState[nGoalState].fPosition, fVelocity:=1, bHWEnable:=FALSE, ); bInit := TRUE; // Run our FB fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=TRUE, nGoalStateIndex:=nGoalState, ); fbFFHWO.EvaluateOutput(); // If we've set the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bSetDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=fbMove.fStartPosition, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Position was not set correctly', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertFalse( fbStateEnables.bForwardEnabled, 'Forward enabled with limit hit', ); AssertFalse( fbStateEnables.bBackwardEnabled, 'Backward enabled with limit hit', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestMoveAt VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestMoveAt'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Run our FB which should enable the real move fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=bInit, nGoalStateIndex:=nGoalState, eStatePMPSStatus:=E_StatePMPSStatus.AT_STATE, bTransitionAuthorized:=FALSE, ); // Set position to be below the goal's range, and move to the goal fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition - astPositionState[nGoalState].fDelta / 2, fGoalPosition:=astPositionState[nGoalState].fPosition + astPositionState[nGoalState].fDelta / 2, fVelocity:=5, bHWEnable:=FALSE, ); bInit := TRUE; fbFFHWO.EvaluateOutput(); // If we've reached the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bMoveDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=astPositionState[nGoalState].fPosition + astPositionState[nGoalState].fDelta / 2, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Did not reach the goal position', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertTrue( fbStateEnables.bBackwardEnabled, 'Backward disabled when at goal', ); AssertTrue( fbStateEnables.bForwardEnabled, 'Forward disabled when at goal', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestMoveTo VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); fbMove: FB_TestHelperSetAndMove; bInit: BOOL; END_VAR TEST('TestMoveTo'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); // Run our FB which should enable the real move fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=bInit, nGoalStateIndex:=nGoalState, eStatePMPSStatus:=E_StatePMPSStatus.TRANSITION, bTransitionAuthorized:=TRUE, ); // Set position to be below the goal's range, and move to the goal fbMove( stMotionStage:=stMotionStage, bExecute:=bInit, fStartPosition:=astPositionState[nGoalState].fPosition - 10, fGoalPosition:=astPositionState[nGoalState].fPosition, fVelocity:=5, bHWEnable:=FALSE, ); bInit := TRUE; fbFFHWO.EvaluateOutput(); // If we've reached the position OR ran out of time to set the position, check the asserts IF tonTimer.Q OR fbMove.bMoveDone THEN AssertFalse( tonTimer.Q, 'Timeout in test', ); AssertEquals_LREAL( Expected:=astPositionState[nGoalState].fPosition, Actual:=fbMove.fActPosition, Delta:=0.0001, Message:='Did not reach the goal position', ); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Fast fault in normal situation', ); AssertTrue( fbStateEnables.bBackwardEnabled, 'Backward disabled when at goal', ); AssertTrue( fbStateEnables.bForwardEnabled, 'Forward disabled when at goal', ); bOneTestDone := TRUE; TEST_FINISHED(); END_IF END_METHOD METHOD TestNotUpdated VAR_INPUT nTestID: UINT; END_VAR VAR_INST fbStateEnables: FB_StatePMPSEnables; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestNotUpdated'); IF nTestCounter <> nTestID THEN RETURN; END_IF SetEnablesPMPS(stMotionStage); fbFFHWO.EvaluateOutput(); AssertTrue( fbFFHWO.q_xFastFaultOut, 'Faulted prior to test', ); // The invalid state should give us a fault fbStateEnables( stMotionStage:=stMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=TRUE, nGoalStateIndex:=nNotUpdatedState, ); fbFFHWO.EvaluateOutput(); AssertFalse( fbFFHWO.q_xFastFaultOut, 'Not updated state did not fault', ); bOneTestDone := TRUE; TEST_FINISHED(); END_METHOD Related: * `E_StatePMPSStatus`_ * `FB_MotionStage`_ * `FB_MotorTestSuite`_ * `FB_PositionStateInternal`_ * `FB_StatePMPSEnables`_ * `FB_TestHelperSetAndMove`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_StatePMPSEnablesND ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StatePMPSEnablesND (* Function block to set virtual limit enables using MC_POWER for multidimensional state movers. It is a building block not meant for use outside of lcls-twintcat-motion. Each motor has a virtual "allowed" range of motion based on its goal position. Motors can move toward their goal delta ranges or within them, but not away from these ranges. *) VAR_IN_OUT // The motors with a combined N-dimensional state astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; // Each motor's respective position states along its direction astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; // Hardware output to fault to if there is a problem. fbFFHWO: FB_HardwareFFOutput; END_VAR VAR_INPUT // Whether or not to do anything bEnable: BOOL; // The number of motors we're actually using nActiveMotorCount: UINT; // The state that the motors are moving to, along dimension 2 of the position state array. This may be the same as the current state. nGoalStateIndex: UINT; // A name to use for this state mover in the case of fast faults. sDeviceName: STRING; // Set to TRUE to put motors into maintenance mode. This allows us to freely move the motors at the cost of a fast fault. bMaintMode: BOOL; // The overal PMPS FB state eStatePMPSStatus: E_StatePMPSStatus; // Connect from bptm bTransitionAuthorized bTransitionAuthorized: BOOL; END_VAR VAR_OUTPUT // Per-motor enable state we send to MC_Power. This is a pass-through from stMotionStage. abEnabled: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF BOOL; // Per-motor forward enable state we send to MC_Power. This may be a pass-through or an override to FALSE. abForwardEnabled: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF BOOL; // Per-motor backwards enable state we send to MC_Power. This may be a pass-through or an override to FALSE. abBackwardEnabled: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF BOOL; // Per-motor TRUE if there is a valid goal position and FALSE otherwise. This makes a fast fault if FALSE. abValidGoal: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF BOOL; // Set to TRUE if the arrays have mismatched sizing. For this FB, this means the motor won't ever get an enable. bMotorCountError: BOOL; END_VAR VAR // The individual state limit function blocks afbStateEnables: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF FB_StatePMPSEnables; ffMaint: FB_FastFault; ffProgrammerError: FB_FastFault; nIter: DINT; END_VAR CheckCount(); IF NOT bMotorCountError THEN DoLimits(); END_IF RunFastFaults(); END_FUNCTION_BLOCK ACTION CheckCount: // Make sure the motor count is valid (positive, nonzero, less or equal to the max) bMotorCountError S= nActiveMotorCount <= 0; bMotorCountError S= nActiveMotorCount > MotionConstants.MAX_STATE_MOTORS; END_ACTION ACTION DoLimits: FOR nIter := 1 TO nActiveMotorCount DO afbStateEnables[nIter]( stMotionStage:=astMotionStage[nIter], astPositionState:=astPositionState[nIter], fbFFHWO:=fbFFHWO, bEnable:=bEnable AND NOT bMaintMode, nGoalStateIndex:=nGoalStateIndex, eStatePMPSStatus:=eStatePMPSStatus, bTransitionAuthorized:=bTransitionAuthorized, bEnabled=>abEnabled[nIter], bForwardEnabled=>abForwardEnabled[nIter], bBackwardEnabled=>abBackwardEnabled[nIter], bValidGoal=>abValidGoal[nIter], ); END_FOR END_ACTION ACTION RunFastFaults: ffMaint( i_xOK := NOT bMaintMode, i_xAutoReset := TRUE, i_DevName := sDeviceName, i_Desc := 'Device is in maintenance mode', i_TypeCode := E_MotionFFType.MAINT_MODE, io_fbFFHWO := fbFFHWO, ); ffProgrammerError( i_xOK:=NOT bMotorCountError, i_xAutoReset:=TRUE, i_DevName:=sDeviceName, i_Desc:='Programmer error picking motor count', i_TypeCode:=E_MotionFFType.INTERNAL_ERROR, io_fbFFHWO:=fbFFHWO, ); END_ACTION Related: * `E_MotionFFType`_ * `E_StatePMPSStatus`_ * `FB_StatePMPSEnables`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_StatePMPSEnablesND_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StatePMPSEnablesND_Test EXTENDS FB_MotorTestSuite (* Unit tests for FB_StatePMPSEnablesND I'm confident that FB_StatePMPSEnables was tested in FB_StatePMPSEnables_Test There will be one core functionality check Then, the rest will be about the ND feature adds. Full checks: - Core motors not at goal can't move away check - bMaintMode = no move restrictions for all motors. - bMaintMode = fast fault - Wrong count = fast fault *) VAR astMotionStage: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ST_MotionStage; astPositionState: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; afbInternal: ARRAY[1..2] OF FB_PositionStateInternal; nIter: UINT; END_VAR // Spoof motor enables FOR nIter := 1 TO 3 DO astMotionStage[nIter].bAllEnable := TRUE; astMotionStage[nIter].bAllForwardEnable := TRUE; astMotionStage[nIter].bAllBackwardEnable := TRUE; END_FOR // Note: the fake motors show as position = 0, so they will be over/under the goals here astPositionState[1][1].fPosition := 10; astPositionState[1][1].fDelta := 1; afbInternal[1]( stMotionStage:=astMotionStage[1], stPositionState:=astPositionState[1][1], ); SetGoodState(astPositionState[1][1]); astPositionState[2][1].fPosition := -10; astPositionState[2][1].fDelta := 1; afbInternal[2]( stMotionStage:=astMotionStage[2], stPositionState:=astPositionState[2][1], ); SetGoodState(astPositionState[2][1]); TestUnderOverGoals(); TestMaint(); TestCount(); END_FUNCTION_BLOCK METHOD TestCount VAR_INST fbEnables: FB_StatePMPSEnablesND; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestCount'); fbFFHWO.EvaluateOutput(); // No faults please AssertTrue( fbFFHWO.q_xFastFaultOut, 'Had faults prior to test', ); fbEnables( astMotionStage:=astMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, nGoalStateIndex:=1, sDeviceName:='TestUnderOverGoals', bMaintMode:=FALSE, ); fbFFHWO.EvaluateOutput(); // Must fault with bad count AssertFalse( fbFFHWO.q_xFastFaultOut, 'Had no fault with bad count', ); TEST_FINISHED(); END_METHOD METHOD TestMaint VAR_INST fbEnables: FB_StatePMPSEnablesND; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestMaint'); fbFFHWO.EvaluateOutput(); // No faults please AssertTrue( fbFFHWO.q_xFastFaultOut, 'Had faults prior to test', ); fbEnables( astMotionStage:=astMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, nActiveMotorCount:=2, nGoalStateIndex:=1, sDeviceName:='TestUnderOverGoals', bMaintMode:=TRUE, ); fbFFHWO.EvaluateOutput(); // Must fault in maint mode AssertFalse( fbFFHWO.q_xFastFaultOut, 'Had no fault in maintenance mode', ); // All overrides should be relaxed AssertTrue( fbEnables.abForwardEnabled[1], 'In maintenance mode, we should be able to move anywhere', ); AssertTrue( fbEnables.abBackwardEnabled[1], 'In maintenance mode, we should be able to move anywhere', ); AssertTrue( fbEnables.abForwardEnabled[1], 'In maintenance mode, we should be able to move anywhere', ); AssertTrue( fbEnables.abBackwardEnabled[1], 'In maintenance mode, we should be able to move anywhere', ); TEST_FINISHED(); END_METHOD METHOD TestUnderOverGoals VAR_INST fbEnables: FB_StatePMPSEnablesND; fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE); END_VAR TEST('TestUnderOverGoals'); IF NOT astPositionState[1][1].bUpdated OR NOT astPositionState[2][1].bUpdated THEN // Cannot run this test until the one-time-setup runs RETURN; END_IF fbEnables( astMotionStage:=astMotionStage, astPositionState:=astPositionState, fbFFHWO:=fbFFHWO, bEnable:=TRUE, nActiveMotorCount:=2, nGoalStateIndex:=1, sDeviceName:='TestUnderOverGoals', bMaintMode:=FALSE, ); fbFFHWO.EvaluateOutput(); // No faults please AssertTrue( fbFFHWO.q_xFastFaultOut, 'Had faults in normal situation', ); // All core enables are true AssertTrue( astMotionStage[1].bAllForwardEnable, 'Core enables should be TRUE', ); AssertTrue( astMotionStage[1].bAllBackwardEnable, 'Core enables should be TRUE', ); AssertTrue( astMotionStage[2].bAllForwardEnable, 'Core enables should be TRUE', ); AssertTrue( astMotionStage[2].bAllBackwardEnable, 'Core enables should be TRUE', ); // But the overrides force us to move toward the goal // Motor 1 is below the goal, Motor 2 is above the goal AssertTrue( fbEnables.abForwardEnabled[1], 'Motor 1 should be able to move up to the goal', ); AssertFalse( fbEnables.abBackwardEnabled[1], 'Motor 1 should not be able to move down away from the goal', ); AssertFalse( fbEnables.abForwardEnabled[2], 'Motor 2 should not be able to move up away from the goal', ); AssertTrue( fbEnables.abBackwardEnabled[2], 'Motor 2 should be able to move down to the goal', ); TEST_FINISHED(); END_METHOD Related: * `FB_MotorTestSuite`_ * `FB_PositionStateInternal`_ * `FB_StatePMPSEnables`_ * `FB_StatePMPSEnablesND`_ * `FB_StatePMPSEnables_Test`_ * `MotionConstants`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_StatePTPMove ^^^^^^^^^^^^^^^ :: {attribute 'obsolete' := 'Use FB_PositionStateMove instead'} FUNCTION_BLOCK FB_StatePTPMove // Do not use, this is deprecated VAR_INPUT {attribute 'pytmc' := ' pv: '} stPositionState: ST_PositionState; {attribute 'pytmc' := ' pv: GO io: io field: ZNAM False field: ONAM True '} bExecute: BOOL; bMoveOk: BOOL; END_VAR VAR_IN_OUT stMotionStage: ST_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 := E_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 Related: * `E_EpicsMotorCmd`_ * `FB_PositionStateMove`_ * `ST_MotionStage`_ * `ST_PositionState`_ FB_StateSetupHelper ^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StateSetupHelper (* This is a helper for setting up large numbers of ST_PositionState instances. This is typically verbose to do by hand in the normal ways and can be error-prone. Calling with bSetDefault:=TRUE will set the default values to all the values from the input stPositionState. Note that the other args will be ignored in this case. This must be done at least once. If you forget to do this, there will be a warning and bValid will be set to FALSE, making it so we cannot move to that state. Calling without bSetDefault or with it set to FALSE will apply values to the input stPositionState with the following priority order: 1. The value used in the function block call 2. The value from the template stPositionState used in the most recent call with bSetDefault:=TRUE 3. The original default value as defined on ST_PositionState For ease of use, to enable EPICS writes if unlocked, and to avoid repeated self-overwrites in the encoder count use case, this function block will not reapply the values to the same state again after the state has been fully initialized by the states function blocks, as determined by the bUpdated struct member. If you want to force the function block to reapply every cycle you can set bForceUpdate to TRUE, but it is not recommended. Without this feature, you would be required to wrap this function block in a guard to make sure it was only called once per state, which is fairly annoying. Example expected usage: VAR fbStateSetup: FB_StateSetupHelper; stDefault: ST_PositionState := ( fDelta := 0.5, fVelocity := 10, bMoveOk := TRUE, bValid := TRUE ); astStates1: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astStates2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; astStates2: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState; END_VAR fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE); fbStateSetup(stPositionState:=astStates1[1], sName:='OUT', fPosition:=10); fbStateSetup(stPositionState:=astStates1[2], sName:='YAG', fPosition:=20); fbStateSetup(stPositionState:=astStates1[3], sName:='TT', fPosition:=30); fbStateSetup(stPositionState:=astStates2[1], sName:='OUT', fPosition:=-30); fbStateSetup(stPositionState:=astStates2[2], sName:='YAG', fPosition:=35); fbStateSetup(stPositionState:=astStates2[3], sName:='TT', fPosition:=70); fbStateSetup(stPositionState:=astStates3[1], sName:='OUT', fPosition:=0.4, fDelta:=0.1); fbStateSetup(stPositionState:=astStates3[2], sName:='YAG', fPosition:=2.3, fDelta:=0.1); fbStateSetup(stPositionState:=astStates3[3], sName:='TT', fPosition:=5.6, fDelta:=0.1; *) VAR_IN_OUT stPositionState: ST_PositionState; END_VAR VAR_INPUT bSetDefault: BOOL; bForceUpdate: BOOL; sName: STRING; fPosition: LREAL; nEncoderCount: UDINT; fDelta: LREAL; fVelocity: LREAL; fAccel: LREAL; fDecel: LREAL; bMoveOk: BOOL; bLocked: BOOL; bValid: BOOL; bUseRawCounts: BOOL; sPmpsState: STRING; END_VAR VAR stDefault: ST_PositionState; fbWarning: FB_LogMessage; bHasDefault: BOOL; bHasWarned: BOOL; sJson: STRING; END_VAR IF bSetDefault THEN bSetDefault := FALSE; bHasDefault := TRUE; stDefault := stPositionState; ELSIF bForceUpdate OR NOT stPositionState.bUpdated THEN stPositionState.sName := sName; stPositionState.fPosition := fPosition; stPositionState.nEncoderCount := nEncoderCount; stPositionState.fDelta := fDelta; stPositionState.fVelocity := fVelocity; stPositionState.fAccel := fAccel; stPositionState.fDecel := fDecel; stPositionState.bMoveOk := bMoveOk; stPositionState.bLocked := bLocked; stPositionState.bValid := bValid; stPositionState.bUseRawCounts := bUseRawCounts; stPositionState.stPMPS.sPmpsState := sPmpsState; IF NOT bHasDefault THEN stPositionState.bValid := FALSE; IF NOT bHasWarned THEN bHasWarned := TRUE; sJson := CONCAT(CONCAT(CONCAT(CONCAT('{"sName": "', sName), '", "sPmpsState": "'), sPmpsState), '"}'); fbWarning( sMsg:=CONCAT('Did not initialize any defaults in FB_StateSetupHelper! Some states are disabled, check your code! ', sJson), eSevr:=TcEventSeverity.Warning, eSubSystem:=E_Subsystem.MOTION, sJson:=sJson, ); END_IF END_IF END_IF // Overwrite the input args so that unset args are the defaults in the next call sName := stDefault.sName; fPosition := stDefault.fPosition; nEncoderCount := stDefault.nEncoderCount; fDelta := stDefault.fDelta; fVelocity := stDefault.fVelocity; fAccel := stDefault.fAccel; fDecel := stDefault.fDecel; bMoveOk := stDefault.bMoveOk; bLocked := stDefault.bLocked; bValid := stDefault.bValid; bUseRawCounts := stDefault.bUseRawCounts; sPmpsState := stDefault.stPMPS.sPmpsState; END_FUNCTION_BLOCK Related: * `ST_PositionState`_ FB_StateSetupHelper_Test ^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StateSetupHelper_Test EXTENDS FB_TestSuite VAR astStates: ARRAY[1..10] OF ST_PositionState; stDefaultDefault: ST_PositionState; END_VAR TestNormalCase(); TestDefaultOnly(); TestManyOverrides(); TestNoDefault(); TestOnlyOnce(); END_FUNCTION_BLOCK METHOD TestDefaultOnly VAR fbStateSetup: FB_StateSetupHelper; stDefault: ST_PositionState := ( sName := 'DEFAULT', fPosition := 100, nEncoderCount := 200, fDelta := 0.5, fVelocity := 10, fAccel := 12, fDecel := 24, bMoveOk := TRUE, bLocked := TRUE, bValid := TRUE, bUseRawCounts := TRUE ); stOriginal: ST_PositionState; stTarget: ST_PositionState; END_VAR TEST('TestDefaultOnly'); // Add a pmps key stDefault.stPMPS.sPmpsState := 'KEY'; // Cache the defaults to use as the check in case stDefault gets mutated by a bug stOriginal := stDefault; // Apply only defaults fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE); fbStateSetup(stPositionState:=stTarget); // Check everything AssertEquals_STRING(stOriginal.sName, stTarget.sName, 'Wrong sName'); AssertEquals_LREAL(stOriginal.fPosition, stTarget.fPosition, 0, 'Wrong fPosition'); AssertEquals_UDINT(stOriginal.nEncoderCount, stTarget.nEncoderCount, 'Wrong nEncoderCount'); AssertEquals_LREAL(stOriginal.fDelta, stTarget.fDelta, 0, 'Wrong fDelta'); AssertEquals_LREAL(stOriginal.fVelocity, stTarget.fVelocity, 0, 'Wrong fVelocity'); AssertEquals_LREAL(stOriginal.fAccel, stTarget.fAccel, 0, 'Wrong fAccel'); AssertEquals_LREAL(stOriginal.fDecel, stTarget.fDecel, 0, 'Wrong fDecel'); AssertEquals_BOOL(stOriginal.bMoveOk, stTarget.bMoveOk, 'Wrong bMoveOk'); AssertEquals_BOOL(stOriginal.bLocked, stTarget.bLocked, 'Wrong bLocked'); AssertEquals_BOOL(stOriginal.bValid, stTarget.bValid, 'Wrong bValid'); AssertEquals_BOOL(stOriginal.bUseRawCounts, stTarget.bUseRawCounts, 'Wrong bUseRawCounts'); AssertEquals_STRING(stOriginal.stPMPS.sPmpsState, stTarget.stPMPS.sPmpsState, 'Wrong sPmpsState'); TEST_FINISHED(); END_METHOD METHOD TestManyOverrides VAR fbStateSetup: FB_StateSetupHelper; stDefault: ST_PositionState := ( sName := 'POTATO', fPosition := 23, fDelta := 0.5, fVelocity := 10 ); stOne: ST_PositionState; stTwo: ST_PositionState; END_VAR TEST('TestManyOverrides'); // This is the case where the defaults are always overriden fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE); fbStateSetup(stPositionState:=stOne, sName:='ONE', fPosition:=10, fDelta:=0.1, fVelocity:=20); fbStateSetup(stPositionState:=stTWO, sName:='TWO', fPosition:=30, fDelta:=0.23, fVelocity:=4); // Check Everything AssertEquals_STRING('ONE', stOne.sName, 'Wrong sName in state 1'); AssertEquals_LREAL(10, stOne.fPosition, 0, 'Wrong fPosition in state 1'); AssertEquals_UDINT(stDefaultDefault.nEncoderCount, stOne.nEncoderCount, 'Wrong nEncoderCount in state 1'); AssertEquals_LREAL(0.1, stOne.fDelta, 0, 'Wrong fDelta in state 1'); AssertEquals_LREAL(20, stOne.fVelocity, 0, 'Wrong fVelocity in state 1'); AssertEquals_LREAL(stDefaultDefault.fAccel, stOne.fAccel, 0, 'Wrong fAccel in state 1'); AssertEquals_LREAL(stDefaultDefault.fDecel, stOne.fDecel, 0, 'Wrong fDecel in state 1'); AssertEquals_BOOL(stDefaultDefault.bMoveOk, stOne.bMoveOk, 'Wrong bMoveOk in state 1'); AssertEquals_BOOL(stDefaultDefault.bLocked, stOne.bLocked, 'Wrong bLocked in state 1'); AssertEquals_BOOL(stDefaultDefault.bValid, stOne.bValid, 'Wrong bValid in state 1'); AssertEquals_BOOL(stDefaultDefault.bUseRawCounts, stOne.bUseRawCounts, 'Wrong bUseRawCounts in state 1'); AssertEquals_STRING(stDefaultDefault.stPMPS.sPmpsState, stOne.stPMPS.sPmpsState, 'Wrong sPmpsState in state 1'); AssertEquals_STRING('TWO', stTwo.sName, 'Wrong sName in state 2'); AssertEquals_LREAL(30, stTwo.fPosition, 0, 'Wrong fPosition in state 2'); AssertEquals_UDINT(stDefaultDefault.nEncoderCount, stTwo.nEncoderCount, 'Wrong nEncoderCount in state 2'); AssertEquals_LREAL(0.23, stTwo.fDelta, 0, 'Wrong fDelta in state 2'); AssertEquals_LREAL(4, stTwo.fVelocity, 0, 'Wrong fVelocity in state 2'); AssertEquals_LREAL(stDefaultDefault.fAccel, stTwo.fAccel, 0, 'Wrong fAccel in state 2'); AssertEquals_LREAL(stDefaultDefault.fDecel, stTwo.fDecel, 0, 'Wrong fDecel in state 2'); AssertEquals_BOOL(stDefaultDefault.bMoveOk, stTwo.bMoveOk, 'Wrong bMoveOk in state 2'); AssertEquals_BOOL(stDefaultDefault.bLocked, stTwo.bLocked, 'Wrong bLocked in state 2'); AssertEquals_BOOL(stDefaultDefault.bValid, stTwo.bValid, 'Wrong bValid in state 2'); AssertEquals_BOOL(stDefaultDefault.bUseRawCounts, stTwo.bUseRawCounts, 'Wrong bUseRawCounts in state 2'); AssertEquals_STRING(stDefaultDefault.stPMPS.sPmpsState, stTwo.stPMPS.sPmpsState, 'Wrong sPmpsState in state 2'); TEST_FINISHED(); END_METHOD METHOD TestNoDefault VAR fbStateSetup: FB_StateSetupHelper; stTarget: ST_PositionState; END_VAR TEST('TestNoDefault'); // No default = invalid state + warning log message fbStateSetup(stPositionState:=stTarget, sName:='TestNoDefault', sPmpsState:='TestPMPS', bValid:=TRUE); // Only bValid matters, it must not be valid! AssertFalse(stTarget.bValid, 'bValid should be FALSE with no default set.'); TEST_FINISHED(); END_METHOD METHOD TestNormalCase VAR fbStateSetup: FB_StateSetupHelper; stDefault: ST_PositionState; END_VAR VAR CONSTANT cDelta: LREAL := 0.5; cVelocity: LREAL := 10; cMoveOk: BOOL := TRUE; cValid: BOOL := TRUE; END_VAR TEST('TestNormalCase'); // Mimic what I might do in a real project stDefault.fDelta := cDelta; stDefault.fVelocity := cVelocity; stDefault.bMoveOk := cMoveOk; stDefault.bValid := cValid; fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE); fbStateSetup(stPositionState:=astStates[1], sName:='OUT', fPosition:=10, sPmpsState:='FAKE_OUT'); fbStateSetup(stPositionState:=astStates[2], sName:='YAG', fPosition:=20, sPmpsState:='FAKE_YAG'); fbStateSetup(stPositionState:=astStates[3], sName:='TT', fPosition:=30, sPmpsState:='FAKE_TT'); // Check everything AssertEquals_STRING('OUT', astStates[1].sName, 'Wrong sName in state 1'); AssertEquals_LREAL(10, astStates[1].fPosition, 0, 'Wrong fPosition in state 1'); AssertEquals_UDINT(stDefaultDefault.nEncoderCount, astStates[1].nEncoderCount, 'Wrong nEncoderCount in state 1'); AssertEquals_LREAL(cDelta, astStates[1].fDelta, 0, 'Wrong fDelta in state 1'); AssertEquals_LREAL(cVelocity, astStates[1].fVelocity, 0, 'Wrong fVelocity in state 1'); AssertEquals_LREAL(stDefaultDefault.fAccel, astStates[1].fAccel, 0, 'Wrong fAccel in state 1'); AssertEquals_LREAL(stDefaultDefault.fDecel, astStates[1].fDecel, 0, 'Wrong fDecel in state 1'); AssertEquals_BOOL(cMoveOk, astStates[1].bMoveOk, 'Wrong bMoveOk in state 1'); AssertEquals_BOOL(stDefaultDefault.bLocked, astStates[1].bLocked, 'Wrong bLocked in state 1'); AssertEquals_BOOL(cValid, astStates[1].bValid, 'Wrong bValid in state 1'); AssertEquals_BOOL(stDefaultDefault.bUseRawCounts, astStates[1].bUseRawCounts, 'Wrong bUseRawCounts in state 1'); AssertEquals_STRING('FAKE_OUT', astStates[1].stPMPS.sPmpsState, 'Wrong sPmpsState in state 1'); AssertEquals_STRING('YAG', astStates[2].sName, 'Wrong sName in state 2'); AssertEquals_LREAL(20, astStates[2].fPosition, 0, 'Wrong fPosition in state 2'); AssertEquals_UDINT(stDefaultDefault.nEncoderCount, astStates[2].nEncoderCount, 'Wrong nEncoderCount in state 2'); AssertEquals_LREAL(cDelta, astStates[2].fDelta, 0, 'Wrong fDelta in state 2'); AssertEquals_LREAL(cVelocity, astStates[2].fVelocity, 0, 'Wrong fVelocity in state 2'); AssertEquals_LREAL(stDefaultDefault.fAccel, astStates[2].fAccel, 0, 'Wrong fAccel in state 2'); AssertEquals_LREAL(stDefaultDefault.fDecel, astStates[2].fDecel, 0, 'Wrong fDecel in state 2'); AssertEquals_BOOL(cMoveOk, astStates[2].bMoveOk, 'Wrong bMoveOk in state 2'); AssertEquals_BOOL(stDefaultDefault.bLocked, astStates[2].bLocked, 'Wrong bLocked in state 2'); AssertEquals_BOOL(cValid, astStates[2].bValid, 'Wrong bValid in state 2'); AssertEquals_BOOL(stDefaultDefault.bUseRawCounts, astStates[2].bUseRawCounts, 'Wrong bUseRawCounts in state 2'); AssertEquals_STRING('FAKE_YAG', astStates[2].stPMPS.sPmpsState, 'Wrong sPmpsState in state 2'); AssertEquals_STRING('TT', astStates[3].sName, 'Wrong sName in state 3'); AssertEquals_LREAL(30, astStates[3].fPosition, 0, 'Wrong fPosition in state 3'); AssertEquals_UDINT(stDefaultDefault.nEncoderCount, astStates[3].nEncoderCount, 'Wrong nEncoderCount in state 3'); AssertEquals_LREAL(cDelta, astStates[3].fDelta, 0, 'Wrong fDelta in state 3'); AssertEquals_LREAL(cVelocity, astStates[3].fVelocity, 0, 'Wrong fVelocity in state 3'); AssertEquals_LREAL(stDefaultDefault.fAccel, astStates[3].fAccel, 0, 'Wrong fAccel in state 3'); AssertEquals_LREAL(stDefaultDefault.fDecel, astStates[3].fDecel, 0, 'Wrong fDecel in state 3'); AssertEquals_BOOL(cMoveOk, astStates[3].bMoveOk, 'Wrong bMoveOk in state 3'); AssertEquals_BOOL(stDefaultDefault.bLocked, astStates[3].bLocked, 'Wrong bLocked in state 3'); AssertEquals_BOOL(cValid, astStates[3].bValid, 'Wrong bValid in state 3'); AssertEquals_BOOL(stDefaultDefault.bUseRawCounts, astStates[3].bUseRawCounts, 'Wrong bUseRawCounts in state 3'); AssertEquals_STRING('FAKE_TT', astStates[3].stPMPS.sPmpsState, 'Wrong sPmpsState in state 3'); TEST_FINISHED(); END_METHOD METHOD TestOnlyOnce VAR fbStateSetup: FB_StateSetupHelper; stDefault: ST_PositionState; stTarget: ST_PositionState; END_VAR TEST('TestOnlyOnce'); // Required call, even though we don't need anything here fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE); // Start with no position AssertEquals_LREAL(stTarget.fPosition, 0, 0, 'Start position sanity check failed'); // Apply a new position fbStateSetup(stPositionState:=stTarget, fPosition:=10); AssertEquals_LREAL(stTarget.fPosition, 10, 0, 'Basic set position failed'); // Simulate the position state being used and updated via EPICS or otherwise stTarget.bUpdated := TRUE; // Set by FB_PositionStateInternal stTarget.fPosition := 12; // Someone tweaked the value in EPICS // Run through the state setup again fbStateSetup(stPositionState:=stTarget, fPosition:=10); // But we should still be at position 12 AssertEquals_LREAL(stTarget.fPosition, 12, 0, 'FB_StateSetupHelper ran twice!'); // Unless we override the behavior fbStateSetup(stPositionState:=stTarget, bForceUpdate:=TRUE, fPosition:=10); AssertEquals_LREAL(stTarget.fPosition, 10, 0, 'bForceUpdate failed'); TEST_FINISHED(); END_METHOD Related: * `FB_PositionStateInternal`_ * `FB_StateSetupHelper`_ * `ST_PositionState`_ FB_StatesInputHandler ^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_StatesInputHandler (* Handle the state enum EPICS input for any of the ND state function blocks. The desired behavior is: - Trigger a move to a new state when the enum PV is written to - Interrupt an ongoing move with a new one if the enum changes mid-move - Stop the move if the enum is set to an invalid value *) VAR_IN_OUT // The user's inputs from EPICS. This is an IN_OUT variable because we will write values back to this to help us detect when the same value is re-caput stUserInput: ST_StateEpicsToPlc; END_VAR VAR_INPUT // The bBusy boolean from the motion FB bMoveBusy: BOOL; // The starting state number to seed nCurrGoal with nStartingState: UINT; END_VAR VAR_OUTPUT // The goal index to input to the motion FB. This will be clamped to the range 0..GeneralConstants.MAX_STATES nCurrGoal: UINT; // The bExecute boolean to input to the motion FB bExecMove: BOOL; // The bReset boolean to input to the motion FB bResetMove: BOOL; END_VAR VAR nState: UINT; bInit: BOOL; nQueuedGoal: UINT; bNewMove: BOOL; END_VAR VAR CONSTANT IDLE: UINT := 0; GOING: UINT := 1; WAIT_STOP: UINT := 2; END_VAR bResetMove := stUserInput.bReset; IF bResetMove OR NOT bInit THEN bInit := TRUE; stUserInput.nSetValue := 0; nCurrGoal := nStartingState; bExecMove := FALSE; nState := IDLE; bNewMove := FALSE; END_IF IF stUserInput.nSetValue <> 0 THEN nQueuedGoal := stUserInput.nSetValue; bNewMove := TRUE; END_IF CASE nState OF IDLE: IF bNewMove AND nQueuedGoal > 0 AND nQueuedGoal <= GeneralConstants.MAX_STATES THEN // New request, currently idle -> ask for a move nCurrGoal := nQueuedGoal; bExecMove := TRUE; bNewMove := FALSE; ELSIF bMoveBusy THEN // We're moving but used to be idle -> switch to GOING nState := GOING; END_IF GOING: IF bNewMove THEN // New request, currently moving -> ask for a stop nState := WAIT_STOP; bExecMove := FALSE; ELSIF NOT bMoveBusy THEN nState := IDLE; nQueuedGoal := 0; bExecMove := FALSE; END_IF WAIT_STOP: IF NOT bMoveBusy THEN nState := IDLE; END_IF END_CASE // Help us detect if there is an EPICS put before the next cycle stUserInput.nSetValue := 0; stUserInput.bReset := FALSE; END_FUNCTION_BLOCK Related: * `ST_StateEpicsToPlc`_ 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 Related: * `DUT_ErrorState`_ * `DUT_TerminalError`_ * `GVL_ErrorSystem`_ * `ST_ErrorSystem`_ FB_TestHelperSetAndMove ^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_TestHelperSetAndMove (* Set an stMotionStage to a start position, then cause a move. On rising edge of bExecute, errors are cleared, the user enable is set to FALSE, and the position is set. After the position is set, user enable is re-introduced and a move begins. The progress of this function block for the purposes of a unit test can be monitored via the outputs. The motion stage should have its own FB_MotionStage or some stand-in for it running on outside of the FB. This is, in itself, tested in FB_TestHelperSetAndMove_Test. *) VAR_IN_OUT // The motion state to test with stMotionStage: ST_MotionStage; END_VAR VAR_INPUT // Begin on rising edge, stop on falling edge. bExecute: BOOL; // The position to set the motor to fStartPosition: LREAL; // The goal to move the motor towards fGoalPosition: LREAL; // The velocity to move at fVelocity: LREAL; // If TRUE, we'll set all the hardware enable signals. Leave this as FALSE if your testing involves any of the hardware enables. bHWEnable: BOOL; END_VAR VAR_OUTPUT // Goes to TRUE once the motor has no errors and is deactivated. bResetDone: BOOL; // Goes to TRUE once the set position completes. bSetDone: BOOL; // Goes to TRUE once the motor begins moving. bMotionStarted: BOOL; // Goes to TRUE once the motor reaches its destination. bMoveDone: BOOL; // If bBusy is FALSE, the function block is ready for a new request. Goes to TRUE after bExecute rises and back to FALSE after bMoveDone or bError rises. Also goes to FALSE after the motor stops following bExecute being dropped to FALSE. bBusy: BOOL; // Goes to TRUE in case of an error from FB_MotionRequest bError: BOOL; // Counts up from bExecute's rising trigger. Can be used to implement a timeout. tElapsed: TIME; // Current motor position in case you're looking for it fActPosition: LREAL; // Goes to TRUE if we're trying to stop a move. We'll stop an in-progress move when bExecute goes to FALSE, and we'll make sure the motor fully stops before responding to the next bExecute rising edge. bStoppingMotor: BOOL; END_VAR VAR fbMoveRequest: FB_MotionRequest; mcSetPos: MC_SetPosition; tonTimeout: TON; rtExec: R_TRIG; ftExec: F_TRIG; bExecQueued: BOOL; nLatchError: UDINT; END_VAR tonTimeout( IN:=bExecute, PT:=T#1h, ET=>tElapsed, ); rtExec(CLK:=bExecute); ftExec(CLK:=bExecute); // Set the outputs to known values on exec bResetDone R= rtExec.Q; bSetDone R= rtExec.Q; bMotionStarted R= rtExec.Q; bMoveDone R= rtExec.Q; bBusy S= rtExec.Q; bError R= rtExec.Q; bExecQueued S= rtExec.Q; bExecQueued R= ftExec.Q; bStoppingMotor S= ftExec.Q; // Set the hardware enables if asked IF bHWEnable THEN stMotionStage.bLimitForwardEnable := TRUE; stMotionStage.bLimitBackwardEnable := TRUE; stMotionStage.bHardwareEnable := TRUE; stMotionStage.bPowerSelf := TRUE; END_IF // Note that we only reset if it's needed, e.g. the motor has an error or is enabled or moving etc. etc. bResetDone S= stMotionStage.bError = FALSE AND stMotionStage.bBusy = FALSE AND stMotionStage.bAllEnable = FALSE; // All uses of fbMoveRequest are in this IF/ELSE tree IF bStoppingMotor THEN // Continue stopping the motor from previous cycles fbMoveRequest( stMotionStage:=stMotionStage, bExecute:=FALSE, ); stMotionStage.bExecute := FALSE; bStoppingMotor R= NOT stMotionStage.Axis.Status.ControlLoopClosed AND NOT fbMoveRequest.bBusy; ELSIF bExecQueued THEN // First exec cycle: bring these to FALSE for later use bExecQueued := FALSE; stMotionStage.bUserEnable := FALSE; fbMoveRequest( stMotionStage:=stMotionStage, bExecute:=FALSE, bReset:=FALSE, ); ELSIF NOT bResetDone THEN // Other cycles: reset until we're done resetting fbMoveRequest( stMotionStage:=stMotionStage, bReset:=bExecute, ); ELSIF fStartPosition <> fGoalPosition THEN // Later: do the move // Important to not enable the motor until the set position is done stMotionStage.bUserEnable := bSetDone; fbMoveRequest( stMotionStage:=stMotionStage, bExecute:=bSetDone AND bExecute AND NOT bMoveDone AND NOT bError, bReset:=FALSE, enumMotionRequest:=E_MotionRequest.INTERRUPT, fPos:=fGoalPosition, fVel:=fVelocity, ); bMotionStarted S= bSetDone AND fbMoveRequest.bExecute AND stMotionStage.bBusy AND stMotionStage.stAxisStatus.fActPosition <> fStartPosition; bMoveDone S= bMotionStarted AND stMotionStage.stAxisStatus.fActPosition = fGoalPosition; END_IF // Set the position prior to the move but after the reset. mcSetPos( Axis:=stMotionStage.Axis, Execute:=bResetDone AND bExecute AND NOT bSetDone AND NOT bError, Position:=fStartPosition, ); IF mcSetPos.Done OR mcSetPos.Error THEN nLatchError := mcSetPos.ErrorID; END_IF bSetDone S= mcSetPos.Done AND NOT mcSetPos.Error AND stMotionStage.stAxisStatus.fActPosition = fStartPosition; // Any sub-error is an error here bError := fbMoveRequest.bError OR mcSetPos.Error; // Reset bBusy when appropriate bBusy R= (bMoveDone AND NOT bStoppingMotor) OR bError OR ftExec.Q; // Extract the motor position fActPosition := stMotionStage.stAxisStatus.fActPosition; END_FUNCTION_BLOCK Related: * `E_MotionRequest`_ * `FB_MotionRequest`_ * `FB_MotionStage`_ * `FB_TestHelperSetAndMove_Test`_ * `ST_MotionStage`_ FB_TestHelperSetAndMove_Test ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :: FUNCTION_BLOCK FB_TestHelperSetAndMove_Test EXTENDS TcUnit.FB_TestSuite (* Ensure that the test helper function block works as needed. Without this, all tests that need motion cannot pass. *) VAR stMotionStage: ST_MotionStage; fbMotionStage: FB_MotionStage; fbTestMove: FB_TestHelperSetAndMove; rtResetDone: R_TRIG; rtSetDone: R_TRIG; rtMotionStart: R_TRIG; rtMoveDone: R_TRIG; END_VAR fbMotionStage(stMotionStage:=stMotionstage); BasicMotion(); END_FUNCTION_BLOCK METHOD PRIVATE BasicMotion VAR_INPUT END_VAR TEST('BasicMotion'); IF NOT fbTestMove.bExecute THEN stMotionStage.bError := TRUE; END_IF fbTestMove( stMotionStage:=stMotionStage, bExecute:=TRUE, fStartPosition:=15.0, fGoalPosition:=17.0, fVelocity:=1.0, bHWEnable:=TRUE, ); rtResetDone(CLK:=fbTestMove.bResetDone); rtSetDone(CLK:=fbTestMove.bSetDone); rtMotionStart(CLK:=fbTestMove.bMotionStarted); rtMoveDone(CLK:=fbTestMove.bMoveDone); IF rtResetDone.Q THEN AssertFalse( Condition:=stMotionStage.bError, Message:='Reset did not clear error', ); AssertEquals_LREAL( Expected:=stMotionStage.stAxisStatus.fActPosition, Actual:=fbTestMove.fActPosition, Delta:=0, Message:='Real position output does not match real position.', ); END_IF IF rtSetDone.Q THEN AssertEquals_LREAL( Expected:=fbTestMove.fStartPosition, Actual:=fbTestMove.fActPosition, Delta:=0, Message:='Was not set to start position after set done', ); END_IF IF rtMotionStart.Q THEN AssertTrue( Condition:=stMotionStage.bBusy, Message:='stMotionStage is not busy, but motion is said to have started.', ); AssertTrue( Condition:=fbTestMove.fActPosition > fbTestMove.fStartPosition, Message:='stMotionStage has not moved, but motion is said to have started.', ); END_IF IF rtMoveDone.Q THEN AssertEquals_LREAL( Expected:=fbTestMove.fGoalPosition, Actual:=fbTestMove.fActPosition, Delta:=0.001, Message:='Did not reach destination at move done', ); END_IF IF fbTestMove.bError OR fbTestMove.tElapsed > T#5s OR (fbTestMove.bResetDone AND fbTestMove.bSetDone AND fbTestMove.bMotionStarted AND fbTestMove.bMoveDone) THEN AssertFalse( Condition:=fbTestMove.bError, Message:='Error in fbTestMove', ); AssertFalse( Condition:=fbTestMove.tElapsed > T#5s, Message:='Timeout in basic motion test', ); TEST_FINISHED(); END_IF END_METHOD Related: * `FB_MotionStage`_ * `FB_TestHelperSetAndMove`_ * `ST_MotionStage`_ 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 PRG_TEST ^^^^^^^^ :: PROGRAM PRG_TEST VAR fb_TestHelperSetAndMove_Test: FB_TestHelperSetAndMove_Test; fb_AtPositionState_Test: FB_AtPositionState_Test; fb_PositionStateRead_Test: FB_PositionStateRead_Test; fb_PositionStateReadND_Test: FB_PositionStateReadND_Test; fb_PositionStateMove_Test: FB_PositionStateMove_Test; fb_PositionStateMoveND_Test: FB_PositionStateMoveND_Test; fb_PositionStateND_Test: FB_PositionStateND_Test; fb_NCErrorFFO_Test: FB_NCErrorFFO_Test; fb_MiscStatesErrorFFO_Test: FB_MiscStatesErrorFFO_Test; fb_MotionBPTM_Test: FB_MotionBPTM_Test; fb_MotionClearAsserts_Test: FB_MotionClearAsserts_Test; fb_StatePMPSEnables_Test: FB_StatePMPSEnables_Test; fb_MotionReadPMPSDBND_Test: FB_MotionReadPMPSDBND_Test; fb_PerMotorFFOND_Test: FB_PerMotorFFOND_Test; fb_StatePMPSEnablesND_Test: FB_StatePMPSEnablesND_Test; fb_PositionStatePMPSND_Test: FB_PositionStatePMPSND_Test; fb_StateSetupHelper_Test: FB_StateSetupHelper_Test; END_VAR TcUnit.RUN(); END_PROGRAM Related: * `FB_AtPositionState_Test`_ * `FB_MiscStatesErrorFFO_Test`_ * `FB_MotionBPTM_Test`_ * `FB_MotionClearAsserts_Test`_ * `FB_MotionReadPMPSDBND_Test`_ * `FB_NCErrorFFO_Test`_ * `FB_PerMotorFFOND_Test`_ * `FB_PositionStateMoveND_Test`_ * `FB_PositionStateMove_Test`_ * `FB_PositionStateND_Test`_ * `FB_PositionStatePMPSND_Test`_ * `FB_PositionStateReadND_Test`_ * `FB_PositionStateRead_Test`_ * `FB_StatePMPSEnablesND_Test`_ * `FB_StatePMPSEnables_Test`_ * `FB_StateSetupHelper_Test`_ * `FB_TestHelperSetAndMove_Test`_