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_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:
DUT_MotionStage
{attribute 'obsolete' := 'DUT_MotionStage has been renamed to ST_MotionStage'}
TYPE DUT_MotionStage : ST_MotionStage; END_TYPE
- Related:
DUT_PositionState
{attribute 'obsolete' := 'DUT_PositionState has been renamed to ST_PositionState'}
TYPE DUT_PositionState : ST_PositionState; END_TYPE
- Related:
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:
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:
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_LCLSMotionError
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_LCLSMotionError :
(
ABORTED := 16#7900,
UNSAFE := 16#7901,
INVALID := 16#7902,
TEST := 16#FFFF
) UDINT;
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:
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:
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:
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:
ENUM_EpicsHomeCmd
{attribute 'obsolete' := 'Use E_EpicsHomeCmd'}
TYPE ENUM_EpicsHomeCmd : E_EpicsHomeCmd; END_TYPE
- Related:
ENUM_EpicsInOut
{attribute 'obsolete' := 'Use E_EpicsInOut'}
TYPE ENUM_EpicsInOut : E_EpicsInOut; END_TYPE
- Related:
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_EpicsMotorCmd
{attribute 'obsolete' := 'Use E_EpicsMotorCmd'}
TYPE ENUM_EpicsMotorCmd : E_EpicsMotorCmd; END_TYPE
- Related:
ENUM_MotionFFType
{attribute 'obsolete' := 'Use E_MotionFFType'}
TYPE ENUM_MotionFFType : E_MotionFFType; END_TYPE
- Related:
ENUM_MotionRequest
{attribute 'obsolete' := 'Use E_MotionRequest'}
TYPE ENUM_MotionRequest : E_MotionRequest; END_TYPE
- Related:
ENUM_PnuematicActuatorPositionState
{attribute 'obsolete' := 'Use E_PnuematicActuatorPositionState'}
TYPE ENUM_PnuematicActuatorPositionState : E_PnuematicActuatorPositionState; END_TYPE
- Related:
ENUM_StageBrakeMode
{attribute 'obsolete' := 'Use E_StageBrakeMode'}
TYPE ENUM_StageBrakeMode : E_StageBrakeMode; END_TYPE
- Related:
ENUM_StageEnableMode
{attribute 'obsolete' := 'Use E_StageEnableMode'}
TYPE ENUM_StageEnableMode : E_StageEnableMode; END_TYPE
- Related:
ENUM_StatePMPSStatus
{attribute 'obsolete' := 'Use E_StatePMPSStatus'}
TYPE ENUM_StatePMPSStatus : E_StatePMPSStatus; END_TYPE
- Related:
ST_AxisParameterSetExposed
TYPE ST_AxisParameterSetExposed :
// Collects the axis parameters that are intentionally exposed to EPICS.
STRUCT
// Maximum Dynamics
{attribute 'pytmc' := '
pv: MaxVel
io: i
field: DESC Maximum commandable speed of the axis in EU/s.
'}
fVeloMaximum : LREAL; // Maximum commandable speed of the axis in EU/s.
{attribute 'pytmc' := '
pv: MaxAccel
io: i
field: DESC Maximum rate of increase in speed of the axis in EU/s^2.
'}
fAccelerationMax : LREAL; // Maximum rate of increase in speed of the axis in EU/s^2.
{attribute 'pytmc' := '
pv: MaxDecel
io: i
field: DESC Maximum rate of decrease in speed of the axis in EU/s^2.
'}
fDecelerationMax : LREAL; // Maximum rate of decrease in speed of the axis in EU/s^2.
// Monitoring
{attribute 'pytmc' := '
pv: PosLagEn
io: i
field: DESC TRUE if position lag monitor (also known as stall monitor) is enabled.
'}
bCtrlEnablePosDiffControl : WORD; // Enable/Disable state of Position Lag Monitor.
{attribute 'pytmc' := '
pv: PosLagVal
io: i
field: DESC Maximum magnitude of position lag in EU.
'}
fCtrlPosDiffMax : LREAL; // Maximum magnitude of position lag in EU.
{attribute 'pytmc' := '
pv: PosLagTime
io: i
field: DESC Maximum allowable duration outside of maximum position lag value in seconds.
'}
fCtrlPosDiffMaxTime : LREAL; // Maximum allowable duration outside of maximum position lag value in seconds.
// Limit Switches
{attribute 'pytmc' := '
pv: SLimMinEn
io: i
field: DESC TRUE if controller static minimum limit is enabled.
'}
bEncEnableSoftEndMinControl : WORD; // Enable/Disable state of controller static minimum limit.
{attribute 'pytmc' := '
pv: SLimMaxEn
io: i
field: DESC TRUE if controller static maximum limit is enabled.
'}
bEncEnableSoftEndMaxControl : WORD; // Enable/Disable state of controller static maximum limit.
{attribute 'pytmc' := '
pv: SLimMin
io: i
field: DESC Minimum commandable position of the axis in EU.
'}
fEncSoftEndMin : LREAL; // Minimum commandable position of the axis in EU.
{attribute 'pytmc' := '
pv: SLimMax
io: i
field: DESC Maximum commandable position of the axis in EU.
'}
fEncSoftEndMax : LREAL; // Maximum commandable position of the axis in EU.
// Encoder Evaluation
{attribute 'pytmc' := '
pv: EncScaling
io: i
field: DESC Encoder scaling numerator / denominator in EU/COUNT.
'}
fEncScaleFactorInternal : LREAL; // Encoder scaling numerator / denominator in EU/COUNT.
{attribute 'pytmc' := '
pv: EncOffset
io: i
field: DESC Encoder offset in EU.
'}
fEncOffset : LREAL; // Encoder offset in EU.
END_STRUCT
END_TYPE
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:
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:
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;
// NC parameters that are exposed with pytmc pragmas
{attribute 'pytmc' := '
pv: PLC:AxisPar
io: i
field: DESC Axis configuration parameters in the numerical controller.
'}
stAxisParametersExposed: ST_AxisParameterSetExposed;
// 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
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
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:
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_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_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'}
{attribute 'no-analysis'}
{attribute 'linkalways'}
// This function has been automatically generated from the project information.
VAR_GLOBAL CONSTANT
{attribute 'const_non_replaced'}
stLibVersion_lcls_twincat_motion : ST_LibVersion := (iMajor := 4, iMinor := 1, iBuild := 1, iRevision := 0, nFlags := 1, sVersion := '4.1.1');
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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:
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
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#4466: msg:='Invalid I/O data for more than n continuous NC cycles (encoder)';
16#4467: msg:='Encoder error: invalid actual position data';
16#4550: msg:='Stall: position lag monitoring error';
16#4650: msg:='Drive hardware not ready to operate';
16#4655: msg:='Invalid IO data';
16#4B07: msg:='Timeout axis function block after 6 seconds';
16#4FFF: msg:='Unknown NC error (not in manual)';
// Custom error definitions
E_LCLSMotionError.ABORTED: msg:='Aborted move request with active move in progress';
E_LCLSMotionError.UNSAFE: msg:='Position state unsafe';
E_LCLSMotionError.INVALID: msg:='Position state invalid';
E_LCLSMotionError.TEST: msg:='Fake testing error';
// Fallbacks
0: msg:='';
ELSE
msg:='Contact PCDS to add new message';
END_CASE
F_MotionErrorCodeLookup := msg;
END_FUNCTION
- Related:
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:
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:
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
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
FB_AxisParameterSetExposed_Test
FUNCTION_BLOCK FB_AxisParameterSetExposed_Test EXTENDS FB_MotorTestSuite
(*
Quick Check to verify exposed NC parameters are copied properly.
*)
VAR
stMotionStage: ST_MotionStage;
fbMotionStage: FB_MotionStage;
nTestCounter: UINT;
bOneTestDone: BOOL;
tonTimer: TON;
END_VAR
IF nTestCounter = 0 THEN
nTestCounter := 1;
END_IF
// Test that parameters are copied to the exposed parameters properly.
TestParameterCopy(1);
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:=nTestCounter >= 1,
PT:=T#2s,
);
END_FUNCTION_BLOCK
METHOD TestParameterCopy
VAR_INPUT
nTestIndex: UINT;
END_VAR
VAR_INST
bLocalInit: BOOL := TRUE;
bAllModified: BOOL;
bReady: BOOL;
bDone: BOOL;
END_VAR
TEST(CONCAT('TestParameterCopy', UINT_TO_STRING(nTestIndex)));
IF nTestCounter <> nTestIndex OR bOneTestDone THEN
RETURN;
END_IF
fbMotionStage(stMotionStage:=stMotionStage);
IF bLocalInit THEN
stMotionStage.stAxisParametersExposed.bCtrlEnablePosDiffControl := 100;
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMaxControl := 101;
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMinControl := 102;
stMotionStage.stAxisParametersExposed.fAccelerationMax := 103;
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMax := 104;
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMaxTime := 105;
stMotionStage.stAxisParametersExposed.fDecelerationMax := 106;
stMotionStage.stAxisParametersExposed.fEncSoftEndMax := 107;
stMotionStage.stAxisParametersExposed.fEncSoftEndMin := 108;
stMotionStage.stAxisParametersExposed.fVeloMaximum := 109;
stMotionStage.stAxisParametersExposed.fEncOffset := 110;
stMotionStage.stAxisParametersExposed.fEncScaleFactorInternal := 111;
bLocalInit := FALSE;
bAllModified :=
stMotionStage.stAxisParametersExposed.bCtrlEnablePosDiffControl <> stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl AND
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMaxControl <> stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl AND
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMinControl <> stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl AND
stMotionStage.stAxisParametersExposed.fAccelerationMax <> stMotionStage.stAxisParameters.fAccelerationMax AND
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMax <> stMotionStage.stAxisParameters.fCtrlPosDiffMax AND
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMaxTime <> stMotionStage.stAxisParameters.fCtrlPosDiffMaxTime AND
stMotionStage.stAxisParametersExposed.fDecelerationMax <> stMotionStage.stAxisParameters.fDecelerationMax AND
stMotionStage.stAxisParametersExposed.fEncSoftEndMax <> stMotionStage.stAxisParameters.fEncSoftEndMax AND
stMotionStage.stAxisParametersExposed.fEncSoftEndMin <> stMotionStage.stAxisParameters.fEncSoftEndMin AND
stMotionStage.stAxisParametersExposed.fVeloMaximum <> stMotionStage.stAxisParameters.fVeloMaximum AND
stMotionStage.stAxisParametersExposed.fEncOffset <> stMotionStage.stAxisParameters.fEncOffset AND
stMotionStage.stAxisParametersExposed.fEncScaleFactorInternal <> stMotionStage.stAxisParameters.fEncScaleFactorInternal;
AssertTrue(
Condition:=bAllModified,
Message:='One or more parameters was not modified initially so the check is invalid.',
);
END_IF
bReady :=
stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl <> 0 AND
stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl <> 0 AND
stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl <> 0 AND
stMotionStage.stAxisParameters.fAccelerationMax <> 0 AND
stMotionStage.stAxisParameters.fCtrlPosDiffMax <> 0 AND
stMotionStage.stAxisParameters.fCtrlPosDiffMaxTime <> 0 AND
stMotionStage.stAxisParameters.fDecelerationMax <> 0 AND
stMotionStage.stAxisParameters.fEncSoftEndMax <> 0 AND
stMotionStage.stAxisParameters.fEncSoftEndMin <> 0 AND
stMotionStage.stAxisParameters.fVeloMaximum <> 0 AND
stMotionStage.stAxisParameters.fEncOffset <> 0 AND
stMotionStage.stAxisParameters.fEncScaleFactorInternal <> 0;
IF bReady THEN
bDone :=
stMotionStage.stAxisParametersExposed.bCtrlEnablePosDiffControl = stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl AND
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMaxControl = stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl AND
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMinControl = stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl AND
stMotionStage.stAxisParametersExposed.fAccelerationMax = stMotionStage.stAxisParameters.fAccelerationMax AND
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMax = stMotionStage.stAxisParameters.fCtrlPosDiffMax AND
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMaxTime = stMotionStage.stAxisParameters.fCtrlPosDiffMaxTime AND
stMotionStage.stAxisParametersExposed.fDecelerationMax = stMotionStage.stAxisParameters.fDecelerationMax AND
stMotionStage.stAxisParametersExposed.fEncSoftEndMax = stMotionStage.stAxisParameters.fEncSoftEndMax AND
stMotionStage.stAxisParametersExposed.fEncSoftEndMin = stMotionStage.stAxisParameters.fEncSoftEndMin AND
stMotionStage.stAxisParametersExposed.fVeloMaximum = stMotionStage.stAxisParameters.fVeloMaximum AND
stMotionStage.stAxisParametersExposed.fEncOffset = stMotionStage.stAxisParameters.fEncOffset AND
stMotionStage.stAxisParametersExposed.fEncScaleFactorInternal = stMotionStage.stAxisParameters.fEncScaleFactorInternal;
END_IF
IF bDone OR tonTimer.Q THEN
AssertFalse(
Condition:=tonTimer.Q,
Message:=CONCAT(CONCAT('Failed to update the parameters within ', TIME_TO_STRING(tonTimer.PT)),' seconds.'),
);
AssertTrue(
Condition:=bDone,
Message:='One or more parameters was not copied properly.',
);
bOneTestDone := TRUE;
TEST_FINISHED();
END_IF
END_METHOD
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 AND aBufferValue[nIndex-1]>=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<rTimeLast THEN
rTimeSpan:=rTimeLast-rTimeFirst;
ELSE //overflow of counter once (only 32bit timestamp =4.29seconds)
rTimeSpan:=4294967296.0-rTimeFirst+rTimeLast;
END_IF
fActFrequency:=1000000000.0/rTimeSpan*(nCrossings-1)/2; //two crossings per period => 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:
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_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:
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:
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
FB_ErrorList
FUNCTION_BLOCK FB_ErrorList
VAR_INPUT
En : BOOL; //Enable input
bReset : BOOL; //Delete all Error entry
lErrorID : ULINT; //ErrorID to be acknoledged
bACK : BOOL; //Acknoledge the given error and delete it from the list
END_VAR
VAR_OUTPUT
EnO : BOOL; //Enable output
nNoError: UINT; //Number of Errors
nNoOverflow : INT; //Number of Overflows
pErrorSystem : POINTER TO ST_ErrorSystem; //Pointer to ErrorSystem
END_VAR
VAR
nFreePos : UINT; //Number of free position in the list
nListCnt1 : UINT; //work variable
ErrorSystem : ST_ErrorSystem; //Data structure of the Error list
END_VAR
(*
* ================================================================================
* DESCRIPTION
* ================================================================================
* This Function Block implements the core structure of the Error/Warning Handling
* system. It realizes the datastructure containing every Error Entry, collect
* statistics about the usage and manage the Error Entries.
* Note: The system is under development, most of the functionalities are not
* implemented or existing functionalities may change with time.
* ================================================================================
*)
EnO := En;
//Ponter to ErrorSystem
pErrorSystem := ADR(ErrorSystem);
//Number of overflows
nNoOverflow := ErrorSystem.nNoOverflows;
IF bReset THEN
MEMSET ( ADR(ErrorSystem.aErrorData[0]), 0, GVL_ErrorSystem.cSizeOfErrorData * SIZEOF(DUT_TerminalError));
ErrorSystem.lNextErrorID := 1;
END_IF
//Number of errors in the system
nNoError := 0;
FOR nListCnt1 := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO
IF ErrorSystem.aErrorData[nListCnt1].Error_ID <> 0 THEN
nNoError := nNoError+1;
END_IF
END_FOR
ErrorSystem.nNoErrors := nNoError;
//Number of free position in the list
nFreePos := GVL_ErrorSystem.cSizeOfErrorData - nNoError;
//Acknoledge specified Error entry
IF bACK THEN
FOR nListCnt1 := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO
IF ErrorSystem.aErrorData[nListCnt1].Error_ID = lErrorID THEN
ErrorSystem.aErrorData[nListCnt1].ErrorState := DUT_ErrorState.Acknowledged;
END_IF
END_FOR
END_IF
//Deleting acknoledged errors
FOR nListCnt1 := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO
IF ErrorSystem.aErrorData[nListCnt1].ErrorState = DUT_ErrorState.Acknowledged THEN
MEMMOVE (ADR(ErrorSystem.aErrorData[nListCnt1]), ADR(ErrorSystem.aErrorData[nListCnt1+1]), (GVL_ErrorSystem.cSizeOfErrorData - 1 - nListCnt1) * SIZEOF(DUT_TerminalError));
MEMSET(ADR(ErrorSystem.aErrorData[GVL_ErrorSystem.cSizeOfErrorData - 1]), 0, SIZEOF(DUT_TerminalError));
END_IF
END_FOR
END_FUNCTION_BLOCK
FB_GantryAutoCoupling
FUNCTION_BLOCK FB_GantryAutoCoupling
VAR_INPUT
nGantryTol : LINT;
END_VAR
VAR_OUTPUT
bGantryAlreadyCoupled : BOOL;
END_VAR
VAR_IN_OUT
Master : 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
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_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_HomePrepare
FUNCTION_BLOCK FB_HomePrepare
VAR_INPUT
En: BOOL;
bReset: BOOL;
bExecute: BOOL;
nCmdData: UINT;
END_VAR
VAR_IN_OUT
Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
EnO: BOOL;
bBusy: BOOL;
bDone: BOOL;
bError: BOOL;
nErrorId: UDINT;
bSofLimEnableLowOriginal: BOOL:=TRUE;
bSofLimEnableHighOriginal: BOOL:=TRUE;
fVelocityToCam: LREAL:=0;
fVelocityFromCam: LREAL:=0;
END_VAR
VAR
fbHomeReadSoftLimEnable:FB_HomeReadSoftLimEnable;
fbHomeDisableSoftLimEnable:FB_HomeWriteSoftLimEnable;
fbHomeReadNCVelocities: FB_HomeReadNcVelocities;
fbHomeResetCalibrationFlag:MC_Home; //Only used for reset of calibration flag
fbExecuteRiseEdge: R_TRIG;
bExecuteReadNC:BOOL:=FALSE;
bExecuteWriteNC:BOOL:=FALSE;
nState:INT:=0;
END_VAR
En:=EnO;
IF bReset THEN
bError:=FALSE;
nErrorId:=0;
END_IF
IF NOT bExecute THEN
bExecuteReadNC:=FALSE;
bExecuteWriteNC:=FALSE;
nState:=0;
END_IF
fbExecuteRiseEdge(CLK:=bExecute);
IF fbExecuteRiseEdge.Q THEN
bExecuteReadNC:=TRUE;
END_IF
// Read from NC
fbHomeReadNCVelocities(
En:=En,
bExecute:=bExecuteReadNC, // Actualy not needed for sequence 15 (set position only, no movement))
bReset:=bReset,
Axis:=Axis,
);
fbHomeReadSoftLimEnable(
En:=En,
bExecute:=bExecuteReadNC AND bExecute,
Axis:=Axis,
bReset:=bReset,
);
// Reset calibration flag
fbHomeResetCalibrationFlag(
Execute:=bExecuteReadNC,
HomingMode:=MC_ResetCalibration,
Axis:=Axis
);
bSofLimEnableLowOriginal:=fbHomeReadSoftLimEnable.bSofLimEnableLow;
bSofLimEnableHighOriginal:=fbHomeReadSoftLimEnable.bSofLimEnableHigh;
fVelocityToCam:=fbHomeReadNCVelocities.fVelocityToCam;
fVelocityFromCam:=fbHomeReadNCVelocities.fVelocityFromCam;
IF bExecuteReadNC AND bExecute AND fbHomeReadSoftLimEnable.bDone THEN
fbHomeDisableSoftLimEnable.bSofLimEnableHigh:=FALSE;
fbHomeDisableSoftLimEnable.bSofLimEnableLow:=FALSE;
bExecuteWriteNC:=TRUE; //Always write (only needed if enabled actually)
END_IF
// Write to NC (disable soft limits)
fbHomeDisableSoftLimEnable(
En:=En,
bExecute:=bExecuteWriteNC AND bExecute,
Axis:=Axis,
bReset:=bReset,
);
bBusy:=fbHomeReadSoftLimEnable.bBusy OR fbHomeDisableSoftLimEnable.bBusy OR fbHomeReadNCVelocities.bBusy OR fbHomeResetCalibrationFlag.Busy;
bDone:=fbHomeReadSoftLimEnable.bDone AND fbHomeDisableSoftLimEnable.bDone AND fbHomeReadNCVelocities.bDone AND fbHomeResetCalibrationFlag.Done AND bExecute;
bError:=fbHomeReadSoftLimEnable.bError OR fbHomeDisableSoftLimEnable.bError OR fbHomeReadNCVelocities.bError OR fbHomeResetCalibrationFlag.Error;
IF fbHomeReadSoftLimEnable.bError THEN
nErrorId:=fbHomeReadSoftLimEnable.nErrorId;
ELSIF fbHomeDisableSoftLimEnable.bError THEN
nErrorId:=fbHomeDisableSoftLimEnable.nErrorId;
ELSIF fbHomeResetCalibrationFlag.Error THEN
nErrorId:=fbHomeResetCalibrationFlag.ErrorId;
END_IF
END_FUNCTION_BLOCK
FB_HomeReadNcVelocities
FUNCTION_BLOCK FB_HomeReadNcVelocities
VAR_INPUT
En: BOOL;
bReset: BOOL;
bExecute: BOOL;
END_VAR
VAR_IN_OUT
Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
EnO: BOOL;
bBusy: BOOL;
bDone: BOOL;
bError: BOOL;
nErrorId: UDINT;
fVelocityToCam: LREAL;
fVelocityFromCam: LREAL;
END_VAR
VAR
fbReadVelocityToCam:FB_ReadFloatParameter;
fbReadVelocityFromCam:FB_ReadFloatParameter;
END_VAR
En:=EnO;
IF bReset THEN
bError:=FALSE;
nErrorId:=0;
END_IF
fbReadVelocityToCam(
bExecute:=bExecute,
nDeviceGroup:= 16#4000,
nIndexOffset:= 16#6,
Axis:= Axis);
fbReadVelocityFromCam(
bExecute:=bExecute,
nDeviceGroup:= 16#4000,
nIndexOffset:= 16#7,
Axis:= Axis);
fVelocityToCam:=fbReadVelocityToCam.nData;
fVelocityFromCam:=fbReadVelocityFromCam.nData;
bBusy:=fbReadVelocityFromCam.bBusy OR fbReadVelocityToCam.bBusy;
bDone:=fbReadVelocityFromCam.bDone AND fbReadVelocityToCam.bDone AND bExecute;
bError:=fbReadVelocityToCam.bError OR fbReadVelocityFromCam.bError;
IF fbReadVelocityToCam.bError THEN
nErrorId:=fbReadVelocityToCam.nErrorId;
ELSIF fbReadVelocityFromCam.bError THEN
nErrorId:=fbReadVelocityFromCam.nErrorId;
END_IF
END_FUNCTION_BLOCK
- Related:
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_HomeToSwitch
FUNCTION_BLOCK FB_HomeToSwitch
VAR_INPUT
En: BOOL;
bReset: BOOL;
bExecute: BOOL;
bCamSensor:BOOL;
nSearchDirTwoardsCam: MC_Direction;
nSearchDirOffCam: MC_Direction;
fHomePosition:LREAL;
fVelocityToCamNC: LREAL; //Velcoity when searching for cam
fVelocityFromCamNC: LREAL; // Velocity after found cam (searching for next signal transition)
END_VAR
VAR_IN_OUT
Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
EnO: BOOL;
bBusy: BOOL;
bDone: BOOL;
bHomed:BOOL;
bError: BOOL;
nErrorId: UDINT;
END_VAR
VAR
fbHome: MC_Home;
fbWriteHomeDirCamToNC:FB_WriteParameterInNc_v1_00;
fbWriteHomeDirSyncToNC:FB_WriteParameterInNc_v1_00;
fbWriteHomeModeToNC:FB_WriteParameterInNc_v1_00;
fbWriteHomeVelocitiesToNC: FB_HomeWriteNcVelocities;
bConfigNCDone:BOOL:=FALSE;
fbRTrigg: R_TRIG;
END_VAR
En:=EnO;
IF bReset THEN
bConfigNCDone:=FALSE;
bError:=FALSE;
nErrorId:=0;
END_IF
//Start preparation of NC if rising edge on bExecute
fbRTrigg(CLK:=bExecute);
IF fbRTrigg.Q THEN
bConfigNCDone:=FALSE;
END_IF
fbWriteHomeDirCamToNC(
bExecute:=bExecute AND NOT bConfigNCDone,
nDeviceGroup:=16#5000,
nIndexOffset:=16#101, //Direction for Calibration Cam Search
nData:=BOOL_TO_DWORD(nSearchDirTwoardsCam=MC_Negative_Direction),//BOOL_TO_DWORD(NOT bSearchDirTwoardsCam),
Axis:=Axis
);
fbWriteHomeDirSyncToNC(
bExecute:= bExecute AND NOT bConfigNCDone,
nDeviceGroup:=16#5000 ,
nIndexOffset:=16#102 , //Direction for Sync Impuls Search
nData:=BOOL_TO_DWORD(nSearchDirOffCam=MC_Negative_Direction),//BOOL_TO_DWORD(NOT bSearchDirOffCam),
Axis:= Axis
);
fbWriteHomeModeToNC(
bExecute:=bExecute AND NOT bConfigNCDone,
nDeviceGroup:=16#5000,
nIndexOffset:=16#107, //Reference Mode
nData:=1,
Axis:=axis);
fbWriteHomeVelocitiesToNC(
En:=En,
bExecute:=bExecute AND NOT bConfigNCDone,
bReset:=bReset,
fVelocityFromCam:=fVelocityFromCamNC,
fVelocityToCam:=fVelocityToCamNC,
Axis:=Axis);
fbHome.bCalibrationCam:=bCamSensor;
fbHome(
Execute:=bExecute AND bConfigNCDone(* AND NOT bError*),
Position:=fHomePosition,
HomingMode:=0,
Axis:=Axis
);
bBusy:=(fbHome.Busy OR (NOT bConfigNCDone AND bExecute));
bDone:=fbHome.Done AND bConfigNCDone;
bHomed:=Axis.Status.Homed;
IF (NOT bConfigNCDone) AND fbWriteHomeDirCamToNC.bDone AND fbWriteHomeDirSyncToNC.bDone AND fbWriteHomeModeToNC.bDone AND fbWriteHomeVelocitiesToNC.bDone THEN
bConfigNCDone:=TRUE;
END_IF
//For some reason MC_HOME gives an Error for one cycle of NC-Task 1 SVB (10ms default..) so filter with bExecute
bError:=(fbHome.Error AND bExecute) OR fbWriteHomeDirCamToNC.bError OR fbWriteHomeDirSyncToNC.bError OR fbWriteHomeModeToNC.bError OR fbWriteHomeVelocitiesToNC.bError;
IF (fbHome.Error AND bExecute) THEN
nErrorId:=fbHome.ErrorID;
ELSIF fbWriteHomeDirCamToNC.bError THEN
nErrorId:=fbWriteHomeDirCamToNC.nErrorId;
ELSIF fbWriteHomeDirSyncToNC.bError THEN
nErrorId:=fbWriteHomeDirSyncToNC.nErrorId;
ELSIF fbWriteHomeModeToNC.bError THEN
nErrorId:=fbWriteHomeModeToNC.nErrorId;
ELSIF fbWriteHomeVelocitiesToNC.bError THEN
nErrorId:=fbWriteHomeVelocitiesToNC.nErrorId;
END_IF
END_FUNCTION_BLOCK
FB_HomeVirtual
FUNCTION_BLOCK FB_HomeVirtual
VAR_INPUT
En: BOOL;
bReset: BOOL;
bExecute: BOOL;
nCmdData: UINT;
bLimitFwd: BOOL;
bLimitBwd: BOOL;
bHomeSensor: BOOL;
fHomePosition:LREAL;
nHomeRevOffset: UINT;
END_VAR
VAR_IN_OUT
Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
EnO: BOOL;
bBusy: BOOL;
bDone: BOOL;
bHomed:BOOL;
bError: BOOL;
nErrorId: UDINT;
END_VAR
VAR
fbHomeToSwitch: FB_HomeToSwitch;
fbHomeDirect: FB_HomeDirect; //Only used for direct homing (set of position)
fbMoveVelocity:MC_MoveVelocity;
fbHomePrepare:FB_HomePrepare;
fbHomeFinish:FB_HomeFinish;
fbExecuteRiseEdge: R_TRIG;
nHomingState:INT:=0;
bExecuteHomeToSwitch:BOOL:=FALSE;
bExecuteMoveVelocity:BOOL:=FALSE;
bExecutePrepare: BOOL:=FALSE;
bExecuteFinish: BOOL:=FALSE;
bExecuteHomeDirect: BOOL;
nCmdDataLocal: UINT; //Ensure that nCmdData is not changed during sequence
bSequenceReady:BOOL:=TRUE;
bRestoreNCDataNeeded: BOOL:=FALSE;
END_VAR
EnO:=En;
IF bReset THEN
bError:=FALSE;
nErrorId:=0;
END_IF
// Reset when bExecute is low
IF NOT bExecute THEN
nHomingState:=0;
bSequenceReady:=TRUE;
bExecuteHomeToSwitch:=FALSE;
bExecuteHomeDirect:=FALSE;
bExecuteMoveVelocity:=FALSE;
bExecutePrepare:=FALSE;
bExecuteFinish:=FALSE;
END_IF
//Reset at rinsing edge of bExecute
fbExecuteRiseEdge(CLK:=bExecute);
IF fbExecuteRiseEdge.Q THEN
nCmdDataLocal:=nCmdData; //Ensure that nCmdData is not changed during sequence (nCmdData will only be read at a rising edge of bExecute)
bSequenceReady:=FALSE;
bExecutePrepare:=TRUE;
bRestoreNCDataNeeded:=FALSE;
//Check if valid nCmdDataLocal
CASE nCmdDataLocal OF
1:
2:
3:
4:
15:
ELSE //nCmdData not valid
bError:=TRUE;
nErrorId:=16#4FFF;
END_CASE
END_IF
//############# Prepare for homing (Read from NC and reset homed flag)
fbHomePrepare(
En:=En,
bExecute:=bExecutePrepare AND NOT bError, // Not needed for sequence 15 (set position only, no movement))
bReset:=bReset,
Axis:=Axis,
);
//############# Homing Sequences:
CASE nCmdDataLocal OF
1: // Home to low limit switch
CASE nHomingState OF
0:
bHomed:=Axis.Status.Homed;
// Wait for read of velocities from NC and reset of calibration flag
IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
bRestoreNCDataNeeded:=TRUE;
IF bLimitBwd THEN
nHomingState:=1;
ELSE
nHomingState:=2; //Standing on limit switch go direct to state 2
END_IF
END_IF
1: // wait for reach low limit then trigger fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
fbMoveVelocity.Direction:=MC_Negative_Direction;
bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity
IF NOT bLimitBwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
nHomingState:=2;
END_IF
2: // Wait for fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
bExecuteMoveVelocity:=FALSE;
bExecuteHomeToSwitch:=TRUE;
fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Positive_Direction;
fbHomeToSwitch.nSearchDirOffCam:=MC_Positive_Direction;
fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed
fbHomeToSwitch.bCamSensor:=NOT bLimitBwd;
IF fbHomeToSwitch.bDone THEN
nHomingState:=3;
bExecuteFinish:=TRUE;
fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
END_IF;
3: // restore softlimit enable
bHomed:=FALSE;
bSequenceReady:=FALSE;
IF fbHomeFinish.bDone THEN
bRestoreNCDataNeeded:=FALSE;
bSequenceReady:=TRUE;
nHomingState:=0;
bHomed:=Axis.Status.Homed;
END_IF;
END_CASE;
2: // Home to high limit switch
CASE nHomingState OF
0:
bHomed:=Axis.Status.Homed;
// Wait for read of velocities from NC and reset of calibration flag
IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
bRestoreNCDataNeeded:=TRUE;
IF bLimitFwd THEN
nHomingState:=1;
ELSE
nHomingState:=2; //Standing on limit switch go direct to state 2
END_IF
END_IF
1: // wait for reach low limit then trigger fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
fbMoveVelocity.Direction:=MC_Positive_Direction;
bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity
IF NOT bLimitFwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
nHomingState:=2;
END_IF
2: // Wait for fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
bExecuteMoveVelocity:=FALSE;
bExecuteHomeToSwitch:=TRUE;
fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Negative_Direction;
fbHomeToSwitch.nSearchDirOffCam:=MC_Negative_Direction;
fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed
fbHomeToSwitch.bCamSensor:=NOT bLimitFwd;
IF fbHomeToSwitch.bDone THEN
nHomingState:=3;
bExecuteFinish:=TRUE;
fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
END_IF;
3: // restore softlimit enable
bHomed:=FALSE;
bSequenceReady:=FALSE;
IF fbHomeFinish.bDone THEN
bRestoreNCDataNeeded:=FALSE;
bSequenceReady:=TRUE;
nHomingState:=0;
bHomed:=Axis.Status.Homed;
END_IF;
END_CASE;
3: // Home on bHomeSensor via bLimitBwd
CASE nHomingState OF
0:
bHomed:=Axis.Status.Homed;
// Wait for read of velocities from NC and reset of calibration flag
IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
bRestoreNCDataNeeded:=TRUE;
IF bLimitBwd THEN
nHomingState:=1;
ELSE
nHomingState:=2; //Standing on limit switch go direct to state 2
END_IF
END_IF
1: // wait for reach low limit then trigger fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
fbMoveVelocity.Direction:=MC_Negative_Direction;
bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity
IF NOT bLimitBwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
nHomingState:=2;
END_IF
2: // Wait for fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
bExecuteMoveVelocity:=FALSE;
bExecuteHomeToSwitch:=TRUE;
fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Positive_Direction;
fbHomeToSwitch.nSearchDirOffCam:=MC_Positive_Direction;
fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed
fbHomeToSwitch.bCamSensor:=bHomeSensor;
IF fbHomeToSwitch.bDone THEN
nHomingState:=3;
bExecuteFinish:=TRUE;
fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
END_IF;
3: // restore softlimit enable
bHomed:=FALSE;
bSequenceReady:=FALSE;
IF fbHomeFinish.bDone THEN
bRestoreNCDataNeeded:=FALSE;
bSequenceReady:=TRUE;
nHomingState:=0;
bHomed:=Axis.Status.Homed;
END_IF;
END_CASE;
4: // Home on bHomeSensor via bLimitFwd
CASE nHomingState OF
0:
bHomed:=Axis.Status.Homed;
// Wait for read of velocities from NC and reset of calibration flag
IF fbHomePrepare.bDone AND bExecute AND NOT bSequenceReady THEN
bRestoreNCDataNeeded:=TRUE;
IF bLimitFwd THEN
nHomingState:=1;
ELSE
nHomingState:=2; //Standing on limit switch go direct to state 2
END_IF
END_IF
1: // wait for reach low limit then trigger fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
fbMoveVelocity.Velocity:=ABS(fbHomePrepare.fVelocityToCam);
fbMoveVelocity.Direction:=MC_Positive_Direction;
bExecuteMoveVelocity:=bExecute; // Execute MC_MoveVelocity
IF NOT bLimitFwd AND NOT fbMoveVelocity.Busy AND Axis.Status.NotMoving THEN //MC_MoveVelocity.Busy goes down when ramp down initiates (not ends).
nHomingState:=2;
END_IF
2: // Wait for fbHomeToSwitch
bHomed:=FALSE;
bSequenceReady:=FALSE;
bExecuteMoveVelocity:=FALSE;
bExecuteHomeToSwitch:=TRUE;
fbHomeToSwitch.nSearchDirTwoardsCam:=MC_Negative_Direction;
fbHomeToSwitch.nSearchDirOffCam:=MC_Negative_Direction;
fbHomeToSwitch.fVelocityToCamNC:=fbHomePrepare.fVelocityToCam; // High speed
fbHomeToSwitch.fVelocityFromCamNC:=fbHomePrepare.fVelocityFromCam; // Low speed
fbHomeToSwitch.bCamSensor:=bHomeSensor;
IF fbHomeToSwitch.bDone THEN
nHomingState:=3;
bExecuteFinish:=TRUE;
fbHomeFinish.bSofLimEnableHigh:=fbHomePrepare.bSofLimEnableHighOriginal;
fbHomeFinish.bSofLimEnableLow:=fbHomePrepare.bSofLimEnableLowOriginal;
END_IF;
3: // Restore softlimit enable
bHomed:=FALSE;
bSequenceReady:=FALSE;
IF fbHomeFinish.bDone THEN
bRestoreNCDataNeeded:=FALSE;
bSequenceReady:=TRUE;
nHomingState:=0;
bHomed:=Axis.Status.Homed;
END_IF;
END_CASE;
15: //Set current position (simplest homing sequence)
bExecuteHomeDirect:=bExecute;
bHomed:=Axis.Status.Homed;
IF fbHomeDirect.bDone THEN //Homing ready
bExecuteHomeDirect:=FALSE;
bSequenceReady:=TRUE;
END_IF
ELSE
fbHomeToSwitch.bCamSensor:=FALSE;
bHomed:=Axis.Status.Homed;
END_CASE;
// Main homing block
fbHomeToSwitch(
bExecute:=bExecuteHomeToSwitch AND bExecute AND NOT bError AND NOT bExecuteHomeDirect AND NOT bExecuteMoveVelocity,
bReset:=bReset,
fHomePosition:=fHomePosition,
Axis:=Axis
);
// Approach limit switch (error if MC_Home is used)
fbMoveVelocity(
Execute:= bExecuteMoveVelocity AND bExecute AND NOT bError AND NOT bExecuteHomeToSwitch AND NOT bExecuteHomeDirect,
Axis:=Axis
);
// No sequence, just set position value (nCmdDataLocal=15). Can not run if fbHomeToSwitch is executed
fbHomeDirect(
bExecute:=bExecuteHomeDirect AND bExecute AND NOT bError AND NOT bExecuteHomeToSwitch AND NOT bExecuteMoveVelocity,
bReset:=bReset,
fHomePosition:=fHomePosition,
Axis:=Axis
);
//############# Finish homing
IF NOT bexecute AND bRestoreNCDataNeeded THEN //If homing is aborted restore is needed
bExecuteFinish:=TRUE;
IF fbHomeFinish.bDone THEN
bExecuteFinish:=FALSE;
bRestoreNCDataNeeded:=FALSE;
END_IF
END_IF
fbHomeFinish(
En:=En,
bExecute:=bExecuteFinish,
bReset:=bReset,
Axis:=Axis,
);
// Error handling
IF NOT bError THEN
IF fbHomeToSwitch.bError THEN
bError:=fbHomeToSwitch.bError;
nErrorId:=fbHomeToSwitch.nErrorId;
ELSIF fbHomeDirect.bError THEN
bError:=fbHomeDirect.bError;
nErrorId:=fbHomeDirect.nErrorId;
ELSIF fbMoveVelocity.Error THEN
bError:=fbMoveVelocity.Error;
nErrorId:=fbMoveVelocity.ErrorId;
END_IF;
END_IF
// Done and busy bit
bDone:=bSequenceReady AND bExecute;
bBusy:=NOT bSequenceReady;
RETURN;
END_FUNCTION_BLOCK
FB_HomeWriteNcVelocities
FUNCTION_BLOCK FB_HomeWriteNcVelocities
VAR_INPUT
En: BOOL;
bReset: BOOL;
bExecute: BOOL;
fVelocityToCam: LREAL;
fVelocityFromCam: LREAL;
END_VAR
VAR_IN_OUT
Axis: AXIS_REF;
END_VAR
VAR_OUTPUT
EnO: BOOL;
bBusy: BOOL;
bDone: BOOL;
bError: BOOL;
nErrorId: UDINT;
END_VAR
VAR
fbExecuteRiseEdge: R_TRIG;
fbWriteVelocityToCam:FB_WriteFloatParameter;
fbWriteVelocityFromCam:FB_WriteFloatParameter;
END_VAR
En:=EnO;
IF bReset THEN
bError:=FALSE;
nErrorId:=0;
END_IF
fbExecuteRiseEdge(CLK:=bExecute);
fbWriteVelocityToCam(
bExecute:=bExecute,
nDeviceGroup:= 16#4000,
nIndexOffset:= 16#6,
nData:=fVelocityToCam,
Axis:= Axis);
fbWriteVelocityFromCam(
bExecute:=bExecute,
nDeviceGroup:= 16#4000,
nIndexOffset:= 16#7,
nData:=fVelocityFromCam,
Axis:= Axis);
bBusy:=fbWriteVelocityFromCam.bBusy OR fbWriteVelocityToCam.bBusy;
bDone:=fbWriteVelocityFromCam.bDone AND fbWriteVelocityToCam.bDone AND bExecute;
bError:=fbWriteVelocityToCam.bError OR fbWriteVelocityFromCam.bError;
IF fbWriteVelocityToCam.bError THEN
nErrorId:=fbWriteVelocityToCam.nErrorId;
ELSIF fbWriteVelocityFromCam.bError THEN
nErrorId:=fbWriteVelocityFromCam.nErrorId;
END_IF
END_FUNCTION_BLOCK
- Related:
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_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:
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:
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_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:
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_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_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:
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
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
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
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 := E_LCLSMotionError.ABORTED;
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
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
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);
// Copy axis parameters that we want to expose to the EPICS layer.
stMotionStage.stAxisParametersExposed.bCtrlEnablePosDiffControl := stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl;
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMaxControl := stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl;
stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMinControl := stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl;
stMotionStage.stAxisParametersExposed.fAccelerationMax := stMotionStage.stAxisParameters.fAccelerationMax;
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMax := stMotionStage.stAxisParameters.fCtrlPosDiffMax;
stMotionStage.stAxisParametersExposed.fCtrlPosDiffMaxTime := stMotionStage.stAxisParameters.fCtrlPosDiffMaxTime;
stMotionStage.stAxisParametersExposed.fDecelerationMax := stMotionStage.stAxisParameters.fDecelerationMax;
stMotionStage.stAxisParametersExposed.fEncSoftEndMax := stMotionStage.stAxisParameters.fEncSoftEndMax;
stMotionStage.stAxisParametersExposed.fEncSoftEndMin := stMotionStage.stAxisParameters.fEncSoftEndMin;
stMotionStage.stAxisParametersExposed.fVeloMaximum := stMotionStage.stAxisParameters.fVeloMaximum;
stMotionStage.stAxisParametersExposed.fEncOffset := stMotionStage.stAxisParameters.fEncOffset;
stMotionStage.stAxisParametersExposed.fEncScaleFactorInternal := stMotionStage.stAxisParameters.fEncScaleFactorInternal;
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:
FB_MotionStageSetAndMoveHelper
FUNCTION_BLOCK FB_MotionStageSetAndMoveHelper
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;
// The allowable deviation from the goal position to be considered done moving.
fDelta: LREAL;
END_VAR
VAR_IN_OUT
stMotionStage: ST_MotionStage;
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;
END_VAR
VAR
rtExec: R_TRIG;
mcSetPos: MC_SetPosition;
END_VAR
rtExec(CLK:=bExecute);
IF rtExec.Q THEN
stMotionStage.bReset := TRUE;
stMotionStage.bEnable := FALSE;
stMotionStage.nEnableMode := E_StageEnableMode.NEVER;
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=FALSE
);
bResetDone := FALSE;
bSetDone := FALSE;
bMotionStarted := FALSE;
bMoveDone := FALSE;
END_IF
bResetDone := NOT stMotionStage.bError AND NOT stMotionStage.bBusy;
bMoveDone := stMotionStage.stAxisStatus.fActPosition <= fGoalPosition + fDelta AND
stMotionStage.stAxisStatus.fActPosition >= fGoalPosition - fDelta;
IF NOT bSetDone THEN
stMotionStage.bReset := TRUE;
stMotionStage.bEnable := FALSE;
IF stMotionStage.stAxisStatus.fActPosition <> fStartPosition THEN
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=NOT mcSetPos.Execute,
Position:=fStartPosition,
Mode:=FALSE,
Done=>
);
ELSE
bSetDone := TRUE;
END_IF
ELSE
stMotionStage.bEnable := TRUE;
IF NOT bMotionStarted AND NOT stMotionStage.Axis.Status.Disabled THEN
stMotionStage.fPosition := fGoalPosition;
stMotionStage.fVelocity := fVelocity;
stMotionStage.bMoveCmd := TRUE;
bMotionStarted := TRUE;
END_IF
END_IF
IF bMoveDone THEN
stMotionStage.bReset := TRUE;
END_IF
END_FUNCTION_BLOCK
- Related:
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
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:
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:
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
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
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
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
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
astPositionState[E_EpicsInOut.OUT] := stOut;
astPositionState[E_EpicsInOut.IN] := stIn;
// 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
stOut := astPositionState[E_EpicsInOut.OUT];
stIn := astPositionState[E_EpicsInOut.IN];
END_FUNCTION_BLOCK
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
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
astPositionState1[E_EpicsInOut.OUT] := stOut1;
astPositionState1[E_EpicsInOut.IN] := stIn1;
astPositionState2[E_EpicsInOut.OUT] := stOut2;
astPositionState2[E_EpicsInOut.IN] := stIn2;
// 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
stOut1 := astPositionState1[E_EpicsInOut.OUT];
stIn1 := astPositionState1[E_EpicsInOut.IN];
stOut2 := astPositionState2[E_EpicsInOut.OUT];
stIn2 := astPositionState2[E_EpicsInOut.IN];
END_FUNCTION_BLOCK
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
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:
<something to fill arrStates>
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
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:
<something to fill arrStates>
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
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:
<something to fill arrStates>
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
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
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
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
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
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
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:
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
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;
rtExec: R_TRIG;
rtReset: R_TRIG;
bInnerExec: BOOL;
bAllowMove: BOOL;
nLatchAllowErrorID: UDINT;
END_VAR
// Veto the move for uninitialized and unsafe states
bAllowMove := stPositionState.bMoveOk AND stPositionState.bValid AND stPositionState.bUpdated;
rtExec(CLK:=bExecute);
bInnerExec S= rtExec.Q AND bAllowMove AND NOT bError;
bInnerExec R= NOT bExecute;
// Do the move
fbMotionRequest(
stMotionStage := stMotionStage,
bExecute := bInnerExec,
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);
rtReset(CLK:=bReset);
IF rtReset.Q THEN
nLatchAllowErrorID := 0;
END_IF
// Inject custom error if we can't move because of bMoveOk or bValid
IF nLatchAllowErrorID <> 0 OR (bExecute AND NOT bAllowMove) THEN
bError := TRUE;
IF nLatchAllowErrorID <> 0 THEN
nErrorID := nLatchAllowErrorID;
ELSIF stPositionState.bValid THEN
nErrorID := E_LCLSMotionError.UNSAFE;
ELSE
nErrorID := E_LCLSMotionError.INVALID;
END_IF
// Keep error latched until it is cleared, otherwise it can be lost early
nLatchAllowErrorID := nErrorID;
sErrorMessage := CONCAT(CONCAT(F_MotionErrorCodeLookup(nErrorId := nErrorID), ' for '), stPositionState.sName);
END_IF
// This can be useful if we're running this FB standalone for some reason
bAtState := F_AtPositionState(stMotionStage:=stMotionStage, stPositionState:=stPositionState);
END_FUNCTION_BLOCK
FB_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;
tonCleanup: 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);
// Test that we need a new bExecute to start a new move if a move goes from unsafe to safe
TestMoveOk(5);
// Wait a few cycles for remaining errors to propagate
tonCleanup(IN:=bOneTestDone, PT:=T#1s);
IF tonCleanup.Q THEN
IF fbMove.bBusy THEN
fbMove(
stMotionStage:=stMotionStage,
stPositionState:=stDummyPos,
bExecute:=FALSE,
bReset:=FALSE,
);
ELSIF stMotionStage.bBusy THEN
stMotionStage.bExecute := FALSE;
ELSIF fbMove.bError THEN
fbMove(
stMotionStage:=stMotionStage,
stPositionState:=stDummyPos,
bExecute:=FALSE,
bReset:=TRUE,
);
ELSIF stMotionStage.bError THEN
stMotionStage.bReset := TRUE;
ELSE
fbMove(
stMotionStage:=stMotionStage,
stPositionState:=stDummyPos,
bExecute:=FALSE,
bReset:=FALSE,
);
stMotionStage.bReset := FALSE;
bOneTestDone := FALSE;
nTestCounter := nTestCounter + 1;
tonTimer(IN:=FALSE);
END_IF
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 OR bOneTestDone 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;
tonBusy: TON;
END_VAR
TEST(CONCAT('TestMove', UINT_TO_STRING(nTestIndex)));
IF nTestCounter <> nTestIndex OR bOneTestDone 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
// Simulate waiting a moment before stopping
tonBusy(IN:=stMotionStage.bBusy, PT:=T#100ms);
bInterruptStarted S= bInterrupt AND tonBusy.Q;
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
METHOD TestMoveOk
VAR_INPUT
nTestIndex: UINT;
END_VAR
VAR_INST
nStep: UINT;
fStartPos: LREAL;
stState: ST_PositionState := (
sName:='GOAL',
fPosition:=135,
fDelta:=0.5,
fVelocity:=1,
bValid:=TRUE,
bMoveOk:=FALSE,
bUpdated:=TRUE
);
tonInternal: TON;
bLocalExec: BOOL;
bLocalReset: BOOL;
END_VAR
VAR CONSTANT
END: UINT := 999;
END_VAR
TEST('TestMoveOk');
IF nTestCounter <> nTestIndex OR bOneTestDone THEN
RETURN;
END_IF
CASE nStep OF
// Initial setup and checks
0:
fStartPos := stMotionStage.stAxisStatus.fActPosition;
AssertFalse(
fbMove.bError,
'Started with a fbMove error',
);
AssertFalse(
stMotionStage.bError,
'Started with a motor error',
);
bLocalExec := FALSE;
bLocalReset := FALSE;
nStep := nStep + 1;
// Move to a state that is bad
1:
stState.bMoveOk := FALSE;
bLocalExec := TRUE;
nStep := nStep + 1;
// The move should have failed with an error on the FB that never propagated to the motor itself- we shouldn't have attempted the move at all
2:
AssertTrue(
fbMove.bError,
'Did not have the expected move not ok error',
);
AssertFalse(
stMotionStage.bError,
'Had a motion error, but we never should have asked for a move?',
);
nStep := nStep + 1;
// Make the state no longer bad, and wait a bit
3:
stState.bMoveOk := TRUE;
tonInternal(IN:=TRUE, PT:=T#500ms);
IF tonInternal.Q THEN
tonInternal(IN:=FALSE);
nStep := nStep + 1;
END_IF
// There should be no movement still
4:
AssertEquals_LREAL(
Expected:=fStartPos,
Actual:=stMotionStage.stAxisStatus.fActPosition,
Delta:=0.1,
Message:='Motor moved without bReset or a new bExecute!',
);
// Reset the error (rising edge)
bLocalReset := TRUE;
nStep := nStep + 1;
// After we reset the error, wait a bit
5:
// Drop for rising edges later
bLocalReset := FALSE;
tonInternal(IN:=TRUE, PT:=T#500ms);
IF tonInternal.Q THEN
tonInternal(IN:=FALSE);
nStep := nStep + 1;
END_IF
// There should be no error, and STILL be no movement
6:
AssertFalse(
fbMove.bError,
'The error should be reset',
);
AssertEquals_LREAL(
Expected:=fStartPos,
Actual:=stMotionStage.stAxisStatus.fActPosition,
Delta:=0.1,
Message:='Motor moved without a new bExecute!',
);
// Set bExecute to FALSE for an upcoming rising edge. It should be TRUE at this point.
bLocalExec := FALSE;
nStep := nStep + 1;
// We should be able to start a move via bExecute now
7:
stState.fVelocity := 2000;
stState.fAccel := 15000;
stState.fDecel := 15000;
bLocalExec := TRUE;
IF fbMove.bAtState AND stMotionStage.bDone THEN
nStep := END;
END_IF
END_CASE
// Run fbMove exactly once every cycle no matter what
fbMove(
stMotionStage:=stMotionStage,
stPositionState:=stState,
bExecute:=bLocalExec,
bReset:=bLocalReset,
);
IF nStep = END OR tonTimer.Q THEN
AssertFalse(
tonTimer.Q,
'Test timed out',
);
// Check that we've reached the goal
AssertEquals_LREAL(
Expected:=stState.fPosition,
Actual:=stMotionStage.stAxisStatus.fActPosition,
Delta:=0.1,
Message:='Motor did not reach the goal.',
);
bOneTestDone := TRUE;
TEST_FINISHED();
END_IF
END_METHOD
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
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
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,
bMoveError:=fbMove.bError,
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
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
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
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
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
astPositionState[E_EpicsInOut.OUT] := stOut;
astPositionState[E_EpicsInOut.IN] := stIn;
// 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
stOut := astPositionState[E_EpicsInOut.OUT];
stIn := astPositionState[E_EpicsInOut.IN];
END_FUNCTION_BLOCK
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
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
astPositionState1[E_EpicsInOut.OUT] := stOut1;
astPositionState1[E_EpicsInOut.IN] := stIn1;
astPositionState2[E_EpicsInOut.OUT] := stOut2;
astPositionState2[E_EpicsInOut.IN] := stIn2;
// 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
stOut1 := astPositionState1[E_EpicsInOut.OUT];
stIn1 := astPositionState1[E_EpicsInOut.IN];
stOut2 := astPositionState2[E_EpicsInOut.OUT];
stIn2 := astPositionState2[E_EpicsInOut.IN];
END_FUNCTION_BLOCK
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
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
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_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:
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
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
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
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
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
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:
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 := '';
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;
fbIPCReg: FB_IPCDiag_Register;
fbCheckOS: FB_IPCDiag_ReadParameter;
sOSName: STRING := '';
nCheckOSTries: UINT := 3;
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
IF sDirectory = '' THEN
// Check OS for default directory
fbIPCReg(
bExecute:=TRUE,
tTimeout:=T#1s,
);
fbCheckOS(
bExecute:=fbIPCReg.bValid,
eParameterKey:=E_IPCDiag_ParameterKey.OS_Name,
fbRegister:=fbIPCReg,
);
IF fbIPCReg.bError OR fbCheckOS.bError THEN
IF nCheckOSTries <= 0 THEN
// Retry counter down to zero: set to error name to try old default
sOSName := 'Error';
ELSE
// Decrement retry counter
nCheckOSTries := nCheckOSTries - 1;
// Reset FBs
fbCheckOS(bExecute:=FALSE, fbRegister:=fbIPCReg);
fbIPCReg(bExecute:=FALSE);
END_IF
ELSIF fbCheckOS.bValid THEN
fbCheckOS.GetParameter(
pBuffer:=ADR(sOSName),
nBufferSize:=SIZEOF(sOSName),
);
END_IF
IF sOSName = 'TwinCAT/BSD' THEN
sDirectory := '/home/ecs-user/pmpsdb/';
ELSIF sOSName <> '' THEN
sDirectory := '/Hard Disk/ftp/PMPS/';
END_IF
END_IF
MOTION_GVL.fbPmpsFileReader(
io_fbFFHWO:=io_fbFFHWO,
bExecute:=bExecute AND sDirectory <> '',
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:
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;
bLockBounds: BOOL;
bErrorMsg: BOOL;
END_VAR
GetBounds();
SetEnables();
ApplyEnables();
RunFastFaults();
END_FUNCTION_BLOCK
ACTION ApplyEnables:
(*
This action 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
// bPowerSelf MUST be false to use this function with FB_MotionStage, so
// automatically set it false here otherwise it will conflict with the
// MC_POWER call in FB_MotionStage.
stMotionStage.bPowerSelf := FALSE;
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;
bLockBounds := 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
IF NOT bEnable THEN
bLockBounds := 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 bLockBounds THEN
// Prevent forward/backward motion if the position of the axis is outside of the upper/lower bounds respectively.
// Prevent forward/backward motion if the command to the axis is outside of the upper/lower bounds respectively.
bForwardEnabled := stMotionStage.Axis.NcToPlc.ActPos < fUpperPos AND stMotionStage.fPosition < fUpperPos;
bBackwardEnabled := stMotionStage.Axis.NcToPlc.ActPos > fLowerPos AND stMotionStage.fPosition > fLowerPos;
IF (stMotionStage.nErrorId = 16#4223 OR stMotionStage.nErrorId = 16#4260) AND
((stMotionStage.bAllForwardEnable AND NOT bForwardEnabled) OR
(stMotionStage.bAllBackwardEnable AND NOT bBackwardEnabled)) THEN
// Cannot move forward/backward and not triggered by one of the stMotionStage disables so overwrite the error with a custom error message to give more context.
stMotionStage.sCustomErrorMessage := CONCAT(CONCAT(CONCAT(
'Limits exceeded for most recent PMPS position state: ', LREAL_TO_FMTSTR(fLowerPos,5,TRUE)), ' < pos < '), LREAL_TO_FMTSTR(fUpperPos,5,TRUE));
END_IF
ELSE
// Either invalid state with a fault or FB not enabled
bForwardEnabled := TRUE;
bBackwardEnabled := TRUE;
END_IF
END_ACTION
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);
TestBoundsAfterFallingIntoUnknownState(10,-20,-40);
TestBoundsAfterFallingIntoUnknownState(11,20,40);
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 TestBoundsAfterFallingIntoUnknownState
VAR_INPUT
nTestID: UINT;
fStartPosDelta: REAL;
fGoalPosDelta: REAL;
END_VAR
VAR_INST
fbStateEnables: FB_StatePMPSEnables;
fbMotionStageSetAndMove: FB_MotionStageSetAndMoveHelper;
fbFFHWO: FB_HardwareFFOutput := (bAutoReset := TRUE);
mcSetPos: MC_SetPosition;
bInit: BOOL;
bSetPos: BOOL;
bCommandedMove: BOOL;
nGoalStateIndex: UINT;
END_VAR
TEST(CONCAT('TestBoundsAfterFallingIntoUnknownState',UINT_TO_STRING(nTestID)));
IF nTestCounter <> nTestID THEN
RETURN;
END_IF
// On first pass, set the goal state index to a valid state to initialize the bounds.
// Then, set the goal state index to an unknown state to ensure the bounds remain
// active.
IF NOT bInit THEN
nGoalStateIndex := nGoalState;
SetEnables(stMotionStage);
ELSE
nGoalStateIndex := 0;
END_IF
// Run our FB which should enable the real move while within the allowable bounds.
// Once the position is set out of the bounds, the bounds should persist and prevent
// further movement away from the goal state.
fbStateEnables(
stMotionStage:=stMotionStage,
astPositionState:=astPositionState,
fbFFHWO:=fbFFHWO,
bEnable:=TRUE,
nGoalStateIndex:=nGoalStateIndex,
eStatePMPSStatus:=E_StatePMPSStatus.TRANSITION,
bTransitionAuthorized:=TRUE,
);
// Set position to be out of the goal's range, and try to move away from the goal
fbMotionStageSetAndMove(
bExecute:=bInit,
fStartPosition:=astPositionState[nGoalState].fPosition + fStartPosDelta,
fGoalPosition:=astPositionState[nGoalState].fPosition + fGoalPosDelta,
fVelocity:=1.0,
stMotionStage:=stMotionStage,
fDelta:=0.01,
bResetDone=>,
bSetDone=>,
bMotionStarted=>,
bMoveDone=>
);
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 fbMotionStageSetAndMove.bMoveDone THEN
AssertTrue(
tonTimer.Q,
'Timeout should have occurred. Move should have been unable to complete.',
);
AssertTrue(
fbMotionStageSetAndMove.bSetDone,
'Position was never set.',
);
AssertTrue(
fbMotionStageSetAndMove.bMotionStarted,
'Move was never commanded.',
);
AssertEquals_LREAL(
Expected:=astPositionState[nGoalState].fPosition + fStartPosDelta,
Actual:=stMotionStage.stAxisStatus.fActPosition,
Delta:=0.0001,
Message:='Bounds failed to persist after falling out of state',
);
AssertFalse(
fbFFHWO.q_xFastFaultOut,
'Fast fault should have triggered with unknown state',
);
IF fStartPosDelta < 0 AND abs(fStartPosDelta) > astPositionState[nGoalState].fDelta THEN
AssertFalse(
fbStateEnables.bBackwardEnabled,
'Backward should be disabled.',
);
AssertTrue(
fbStateEnables.bForwardEnabled,
'Forward should be enabled.',
);
ELSIF fStartPosDelta > 0 AND abs(fStartPosDelta) > astPositionState[nGoalState].fDelta THEN
AssertTrue(
fbStateEnables.bBackwardEnabled,
'Backward should be enabled.',
);
AssertFalse(
fbStateEnables.bForwardEnabled,
'Forward should be disabled.',
);
ELSE
AssertTrue(
fbStateEnables.bForwardEnabled,
'Forward should be enabled.',
);
AssertTrue(
fbStateEnables.bBackwardEnabled,
'Backward should be enabled.',
);
END_IF
bInit := FALSE;
stMotionStage.bReset := TRUE;
bOneTestDone := TRUE;
TEST_FINISHED();
END_IF
fbMotionStage(stMotionStage:=stMotionStage);
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
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
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
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
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:
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
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;
// TRUE if we have a move error, to prevent moves
bMoveError: BOOL;
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;
nCachedStart: UINT;
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;
nCachedStart := 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;
ELSIF nStartingState <> nCachedStart THEN
// Usually a late position init, sometimes a live change in encoder offset
// The state changed without a move, so we need to partially reinitialize.
nCurrGoal := nStartingState;
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
IF bMoveError THEN
bExecMove := FALSE;
END_IF
// Detect if the set/start position updates without a move
nCachedStart := nStartingState;
// Help us detect if there is an EPICS put before the next cycle
stUserInput.nSetValue := 0;
stUserInput.bReset := FALSE;
END_FUNCTION_BLOCK
- Related:
FB_TerminalError
FUNCTION_BLOCK FB_TerminalError
VAR_INPUT
En : BOOL;
iTerminal_ID : INT;
bWcState : BOOL;
uiInfoData_State : UINT;
pErrorSystem : POINTER TO ST_ErrorSystem; //Pointer to the error system
END_VAR
VAR_OUTPUT
EnO : BOOL;
bError : BOOL := FALSE;
END_VAR
VAR
iStateError : UINT;
iOtherError : UINT;
ErrorData : DUT_TerminalError;
nErrSysCNT : UINT;
//testing
bStateChanged : BOOL; //Indicate if state change happened
uiInfoData_State_Prev : UINT := 16#8; //Previous value of Infodata.State
bWcState_Prev : BOOL := FALSE; //Previous state of WcState
//FB-s
END_VAR
(*
Currently:
Problem:
TODO:
*)
//Connect EN to EnO
EnO:=En;
//Check if pointer is OK
IF pErrorSystem=0 THEN RETURN; END_IF
//Any difference from normal state creates an error
IF En AND (bWcState OR uiInfoData_State<>16#8) THEN
bError:=TRUE;
ELSE
bError:=FALSE;
END_IF
//Change detection
IF uiInfoData_State <> uiInfoData_State_Prev OR bWcState <> bWcState_Prev THEN
bStateChanged := TRUE;
ELSE
bStateChanged := FALSE;
END_IF
//Update previous values
uiInfoData_State_Prev := uiInfoData_State;
bWcState_Prev := bWcState;
//Decision tree
IF bStateChanged THEN
IF bError THEN
IF ErrorData.ErrorState = DUT_ErrorState.Active THEN
//Close active error
//Read system time
ErrorData.nDateTimeOff := Tc2_EtherCAT.F_GetActualDcTime64();
ErrorData.sDateTimeOff := Tc2_EtherCat.DCTIME64_TO_STRING(ErrorData.nDateTimeOff);
ErrorData.ErrorState := DUT_ErrorState.Inactive;
//Write Off-time to Error System
FOR nErrSysCNT := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO
IF pErrorSystem^.aErrorData[nErrSysCNT].Error_ID = ErrorData.Error_ID THEN
pErrorSystem^.aErrorData[nErrSysCNT].nDateTimeOff := ErrorData.nDateTimeOff;
pErrorSystem^.aErrorData[nErrSysCNT].sDateTimeOff := ErrorData.sDateTimeOff;
pErrorSystem^.aErrorData[nErrSysCNT].ErrorState := DUT_ErrorState.Inactive;
EXIT;
END_IF
END_FOR
//Clear ErrorData
MEMSET(ADR(ErrorData), 0, SIZEOF(ErrorData));
END_IF
//Open a new error
ErrorData.ErrorState := DUT_ErrorState.Active; //Set Error State
ErrorData.nDateTimeOn := Tc2_EtherCAT.F_GetActualDcTime64(); //Get system time
ErrorData.sDateTimeOn := Tc2_EtherCat.DCTIME64_TO_STRING(ErrorData.nDateTimeOn); //Convert to string
ErrorData.iTerminalID := iTerminal_ID; //Terminal_ID
ErrorData.bWcState := bWcState; //WcState bit
ErrorData.uiInfoDataState := uiInfoData_State; //uiInfoData_State
//Error message according to uiInfoData_State and WcState
iStateError := (uiInfoData_State AND 16#000F); //Mask for operation state
iOtherError := (uiInfoData_State AND 16#00F0); //Mask for the other 3 kind of errors
//Error messages according to the least significant digit
CASE iStateError OF
16#0001 : ErrorData.sErrorMessage := 'Slave in INIT state; ';
16#0002 : ErrorData.sErrorMessage := 'Slave in PREOP state; ';
16#0003 : ErrorData.sErrorMessage := 'Slave in BOOT state; ';
16#0004 : ErrorData.sErrorMessage := 'Slave in SAFEOP state; ';
16#0008 : ; //Normal operation state
ELSE
ErrorData.sErrorMessage := 'Undefined State of operation; '; //I hope we will never see this message
ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, UINT_TO_STRING(iStateError));
ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, ' ');
END_CASE
//Error messages according to the second least significant digit
CASE iOtherError OF
16#0000 : ; //No error case
16#0010 : ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Slave signals error; ');
16#0020 : ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Invalid vendorID/productCode read; ');
16#0040 : ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Initialisation error occured; ');
ELSE
ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Undefined Error ID: ');
ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, UINT_TO_STRING(iOtherError));
ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, ' ');
END_CASE
//Errormessage according to WcState bit
IF bWcState THEN
ErrorData.sErrorMessage := CONCAT (ErrorData.sErrorMessage, 'Invalid Data;');
END_IF
//Check for overflow
IF pErrorSystem^.nNoErrors = GVL_ErrorSystem.cSizeOfErrorData THEN
pErrorSystem^.nNoOverflows := pErrorSystem^.nNoOverflows+1;
END_IF
//Write Error Data into Error System
ErrorData.Error_ID := pErrorSystem^.lNextErrorID ;
MEMMOVE( ADR(pErrorSystem^.aErrorData[1]), ADR(pErrorSystem^.aErrorData[0]), (GVL_ErrorSystem.cSizeOfErrorData-1) * SIZEOF(DUT_TerminalError));
pErrorSystem^.aErrorData[0] := ErrorData;
pErrorSystem^.lNextErrorID := pErrorSystem^.lNextErrorID+1;
ELSE
//Close Active Error
//Read system time
ErrorData.nDateTimeOff := Tc2_EtherCAT.F_GetActualDcTime64();
ErrorData.sDateTimeOff := Tc2_EtherCat.DCTIME64_TO_STRING(ErrorData.nDateTimeOff);
ErrorData.ErrorState := DUT_ErrorState.Inactive;
//Write Off time to Error System
FOR nErrSysCNT := 0 TO GVL_ErrorSystem.cSizeOfErrorData - 1 DO
IF pErrorSystem^.aErrorData[nErrSysCNT].Error_ID = ErrorData.Error_ID THEN
pErrorSystem^.aErrorData[nErrSysCNT].nDateTimeOff := ErrorData.nDateTimeOff;
pErrorSystem^.aErrorData[nErrSysCNT].sDateTimeOff := ErrorData.sDateTimeOff;
pErrorSystem^.aErrorData[nErrSysCNT].ErrorState := DUT_ErrorState.Inactive;
EXIT;
END_IF
END_FOR
//Clear ErrorData
MEMSET(ADR(ErrorData), 0, SIZEOF(ErrorData));
END_IF
END_IF
END_FUNCTION_BLOCK
FB_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;
ELSIF fStartPosition = fGoalPosition THEN
// if the start position matches the goal position, make sure the commanded position is
// set to the current position.
stMotionStage.fPosition := 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
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
FB_TestStateInitTiming
FUNCTION_BLOCK FB_TestStateInitTiming EXTENDS TcUnit.FB_TestSuite
VAR
stMotionStage: ST_MotionStage;
fbMotionStageSim: FB_MotionStageSim;
END_VAR
fbMotionStageSim(
stMotionStage:=stMotionStage,
nEnableMode:=E_StageEnableMode.DURING_MOTION,
);
PassiveReinit();
END_FUNCTION_BLOCK
METHOD PassiveReinit : BOOL
VAR_INPUT
END_VAR
VAR_INST
fbStateSetup: FB_StateSetupHelper;
stDefault: ST_PositionState := (
fDelta := 0.5,
fVelocity := 3,
bMoveOk := TRUE,
bValid := TRUE
);
eEnumSet: UINT;
eEnumGet: UINT;
stEpicsToPlc: ST_StateEpicsToPlc;
stPlcToEpics: ST_StatePlcToEpics;
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;
mcSetPos: MC_SetPosition;
nCheckStep: UINT := 0;
nFurthestStep: UINT;
nLastState: UINT;
nTransitionState: UINT := 99;
nDoneState: UINT := 99;
timer: TON;
nSetPosErrCount: UINT := 0;
nSetPosErrorID: UDINT;
END_VAR
TEST('PassiveReinit');
// State setup
fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE);
fbStateSetup(stPositionState:=astPositionStateMax[1][1], sName:='ONE', fPosition:=10);
fbStateSetup(stPositionState:=astPositionStateMax[1][2], sName:='TWO', fPosition:=20);
// Run the state FB every cycle
astMotionStageMax[1] := stMotionStage;
fbCore(
astMotionStageMax:=astMotionStageMax,
astPositionStateMax:=astPositionStateMax,
stEpicsToPlc:=stEpicsToPlc,
stPlcToEpics:=stPlcToEpics,
eEnumSet:=eEnumSet,
eEnumGet:=eEnumGet,
bEnable:=TRUE,
nActiveMotorCount:=1,
);
stMotionStage := astMotionStageMax[1];
// Check and adjust different things as we go
CASE nCheckStep OF
0: // State begins at "Unknown", nCurrGoal begins at "Unknown"
AssertEquals_UINT(
Expected:=0,
Actual:=eEnumGet,
Message:='Did not start in unknown state',
);
AssertEquals_UINT(
Expected:=0,
Actual:=fbCore.nCurrGoal,
Message:='Did not start with nCurrGoal unknown',
);
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=FALSE,
);
nCheckStep := 1;
1: // Set the current position to ONE/10
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=TRUE,
Position:=10,
);
IF mcSetPos.Done THEN
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=FALSE,
);
nCheckStep := 2;
ELSIF mcSetPos.Error THEN
nSetPosErrCount := nSetPosErrCount + 1;
nSetPosErrorID := mcSetPos.ErrorID;
nCheckStep := 0;
END_IF
2: // Without a move, the state and goal should both change to "ONE" if the position updates
AssertEquals_UINT(
Expected:=1,
Actual:=eEnumGet,
Message:='Read state did not change to ONE after setpos',
);
AssertEquals_UINT(
Expected:=1,
Actual:=fbCore.nCurrGoal,
Message:='nCurrGoal did not change to ONE after setpos',
);
// Verify: no move requested
AssertEquals_LREAL(
Expected:=0,
Actual:=stMotionStage.fPosition,
Delta:=0.001,
Message:='Set pos routine 1 actually gave us a move!',
);
nCheckStep := 3;
3: // Same as before, but to 20/TWO
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=TRUE,
Position:=20,
);
IF mcSetPos.Done THEN
mcSetPos(
Axis:=stMotionStage.Axis,
Execute:=FALSE,
);
nCheckStep := 4;
END_IF
4:
AssertEquals_UINT(
Expected:=2,
Actual:=eEnumGet,
Message:='Read state did not change to TWO after setpos',
);
AssertEquals_UINT(
Expected:=2,
Actual:=fbCore.nCurrGoal,
Message:='nCurrGoal did not change to TWO after setpos',
);
// Verify: no move requested
AssertEquals_LREAL(
Expected:=0,
Actual:=stMotionStage.fPosition,
Delta:=0.001,
Message:='Set pos routine 2 actually gave us a move!',
);
nCheckStep := 5;
5: // Triggering a move should change the goal to the new state, without updating the readback
eEnumSet := 1;
nLastState := 2;
nCheckStep := 6;
6:
AssertEquals_UINT(
Expected:=1,
Actual:=fbCore.nCurrGoal,
Message:='nCurrGoal did not change to ONE in move',
);
// Looking for a readback transition 2 -> 0 -> 1
// Record the next two transitions
IF eEnumGet <> nLastState and nTransitionState = 99 THEN
nTransitionState := eEnumGet;
ELSIF eEnumGet <> nLastState and nDoneState = 99 THEN
nDoneState := eEnumGet;
END_IF
nLastState := eEnumGet;
IF stPlcToEpics.bDone THEN
AssertEquals_UINT(
Expected:=0,
Actual:=nTransitionState,
Message:='State did not transition 2 -> 0 in move',
);
AssertEquals_UINT(
Expected:=1,
Actual:=nDoneState,
Message:='State did not transition 2 -> 0 -> 1 in move',
);
nCheckStep := 7;
END_IF
7: // The readback and curr goal should match after the move, then end test suite
AssertEquals_UINT(
Expected:=1,
Actual:=eEnumGet,
Message:='Read state did not change to ONE after move',
);
AssertEquals_UINT(
Expected:=1,
Actual:=fbCore.nCurrGoal,
Message:='nCurrGoal did not stay at ONE after move',
);
AssertFalse(
Condition:=timer.Q,
Message:='Timeout in test',
);
TEST_FINISHED();
END_CASE
timer(IN:=TRUE, PT:=T#5s);
IF timer.Q THEN
nCheckStep := 7;
END_IF
IF nCheckStep < 7 AND nFurthestStep < nCheckStep THEN
nFurthestStep := nCheckStep;
END_IF
END_METHOD
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;
fb_TestStateInitTiming: FB_TestStateInitTiming;
fb_AxisParameterSetExposed_Test: FB_AxisParameterSetExposed_Test;
END_VAR
TcUnit.RUN();
END_PROGRAM
- Related: