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