DUTs
DUT_Sensor
TYPE DUT_Sensor :
STRUCT
fCntsInEGU: REAL;
{attribute 'pytmc' := '
pv: EU
io: i
'}
sEGU: STRING;
sName: STRING;
{attribute 'pytmc' := '
pv: VALUE
io: i
'}
fValue: REAL;
{attribute 'pytmc' := '
pv: RAWCOUNTS
io: i
'}
iRawCnts AT %I*: INT;
iOffset: INT;
END_STRUCT
END_TYPE
- Related:
DUT_SensorHGS
TYPE DUT_SensorHGS:
STRUCT
fCntsInEGU: REAL;
{attribute 'pytmc' := '
pv: EU
io: i
'}
sEGU: STRING;
sName: STRING;
{attribute 'pytmc' := '
pv: VALUE
io: i
'}
fValue: REAL;
{attribute 'pytmc' := '
pv: RAWCOUNTS
io: i
'}
iRawCnts AT %I*: DINT;
iOffset: DINT;
END_STRUCT
END_TYPE
E_DiffState
{attribute 'qualified_only'}
TYPE E_DiffState :
(
UNKNOWN := 0,
TRANSFER_ARM_INTERFACE,
NOMINAL_POSITION
) UINT;
END_TYPE
E_GantryEnableState
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_GantryEnableState :
(
STANDBY,
ERROR,
WAIT_FOR_FULLY_ENABLED,
WAIT_FOR_READY_STATE,
DELAY_FOR_MOVE_START
);
END_TYPE
E_MultiLeaderMotionCoupling
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_MultiLeaderMotionCoupling :
(
STATE_STANDBY,
STATE_WAIT_FOR_FOLLOWER_NOT_BUSY,
STATE_WAIT_FOR_FOLLOWER_ENABLED,
STATE_WAIT_FOR_FOLLOWER_READY,
STATE_ENABLE_EXTERNAL_SETPOINT_GENERATOR,
STATE_FOLLOWING_ACTIVE,
STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR,
STATE_ERROR,
STATE_MONITOR_TARGET_WINDOW,
STATE_HALT_MOTION
) UDINT;
END_TYPE
E_RealignCoupleState
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_RealignCoupleState :
(
STANDBY,
DECOUPLE_AXES,
COMMAND_AXES_INTO_REALIGNMENT,
WAIT_FOR_MOVE_START,
RECOUPLE_AXES,
CLEANUP,
ERROR
);
END_TYPE
E_SampleSlotStates
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_SampleSlotStates :
(
EMPTY,
FULL,
DISABLED
);
END_TYPE
E_SDSFaceState
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_SDSFaceState :
(
UNKNOWN := 0,
FACE_TRANSFER_ARM := 1,
FACE_SIDE_PORT := 2
) UINT;
END_TYPE
E_SDSGarageRStates
{attribute 'qualified_only'}
TYPE E_SDSGarageRStates :
(
UNKNOWN := 0,
T1 := 1,
T2 := 2,
T3 := 3,
T4 := 4,
M1 := 5,
M2 := 6,
M3 := 7,
M4 := 8,
B1 := 9,
B2 := 10,
B3 := 11,
B4 := 12,
PREDICTED_HOME := 13
) UINT;
END_TYPE
E_SDSGarageYStates
{attribute 'qualified_only'}
TYPE E_SDSGarageYStates :
(
UNKNOWN := 0,
T1 := 1,
T2 := 2,
T3 := 3,
T4 := 4,
M1 := 5,
M2 := 6,
M3 := 7,
M4 := 8,
B1 := 9,
B2 := 10,
B3 := 11,
B4 := 12,
RETRACTED := 13
) UINT;
END_TYPE
E_SDSHomingState
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_SDSHomingState :
(
INIT := 0,
WAIT_NOT_BUSY,
TRIGGER_HOMING,
WAIT_FOR_HOMED,
HOMED
) UINT;
END_TYPE
E_SDSSeqState
{attribute 'qualified_only'}
TYPE E_SDSSeqState :
(
Inactive,
Home_Transfer_Arm,
Check_Garage_Height,
Manually_Rotate_Garage_for_Clearance_to_Raise,
Retract_Garage_1,
Home_Garage,
Move_Diffractometer_to_Load_Unload_Position,
Confirm_Vacuum_Valve_Open,
Select_Garage_Slot_for_Removal,
Select_Garage_Slot_for_Loading,
Garage_Face_Transfer_Arm,
Rotate_Garage_for_Clearance_to_Lower,
Move_Garage_to_Slot_Height,
Prerotate_Transfer_Arm_for_Extraction,
Prerotate_Transfer_Arm_for_Insertion,
Extend_Transfer_Arm_for_Extraction,
Extend_Transfer_Arm_for_Insertion,
Extend_Transfer_Arm_for_Diffractometer_Extraction,
Extend_Transfer_Arm_for_Diffractometer_Insertion,
Screw_in_Half_Turn,
Fine_Adjustment,
Unscrew_Sample_from_Garage,
Unscrew_Sample_from_Diffractometer,
Screw_Sample_into_Garage,
Screw_Sample_into_Diffractometer,
Confirm_Extraction,
Confirm_Insertion,
Confirm_Diffractometer_Insertion,
Confirm_Diffractometer_Extraction,
Update_Sample_Database,
Retract_Transfer_Arm,
Rotate_Garage_for_Clearance_to_Raise,
Retract_Garage_2,
Return_Garage_to_Home
) UINT;
END_TYPE
E_SDSState
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_SDSState :
(
Standby,
Error,
Operating
) UINT;
END_TYPE
E_SDSSubstate
{attribute 'qualified_only'}
TYPE E_SDSSubstate :
(
Inactive,
Select_Desired_Action,
Check_Can_Home_Garage,
Home_Garage,
Check_Can_Home_Transfer_Arm,
Home_Transfer_Arm,
Check_Can_Load_Arm_From_Garage,
Load_Arm_from_Garage,
Check_Can_Unload_Arm_to_Garage,
Unload_Arm_to_Garage,
Check_Can_Load_Unload_Garage_from_Port,
Load_Unload_Garage_from_Port,
Check_Can_Load_Diffractometer_from_Arm,
Load_Diffractometer_from_Arm,
Check_Can_Unload_Diffractometer_to_Arm,
Unload_Diffractometer_to_Arm,
Check_Can_Manual_Control,
Manual_Control
) UINT;
END_TYPE
E_SDSTransferArmHStates
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_SDSTransferArmHStates :
(
UNKNOWN := 0,
RETRACTED,
GARAGE_SAMPLE_REMOVAL,
CONFIRM_GARAGE_EXTRACTION,
CONFIRM_DIFF_EXTRACTION,
DIFF_SAMPLE_EXTRACTION,
GARAGE_SAMPLE_INSERTION,
CONFIRM_GARAGE_INSERTION,
CONFIRM_DIFF_INSERTION,
DIFF_SAMPLE_INSERTION
) UINT;
END_TYPE
E_SDSTransferArmRStates
{attribute 'qualified_only'}
{attribute 'strict'}
TYPE E_SDSTransferArmRStates :
(
UNKNOWN := 0,
FULLY_CCW,
PREROTATE_SAMPLE_REMOVAL,
PREROTATE_SAMPLE_INSERTION,
UNSCREW_FROM_GARAGE,
UNSCREW_FROM_DIFF,
SCREW_INTO_GARAGE,
SCREW_INTO_DIFF,
SCREW_IN_HALF_TURN
) UINT;
END_TYPE
ENUM_LAS_VIS_States
{attribute 'qualified_only'}
TYPE ENUM_LAS_VIS_States :
(
UNKNOWN := 0,
Position7to24mm := 1,
Position62to77mm := 2
) UINT;
END_TYPE
ST_AutoRealignCouple
TYPE ST_AutoRealignCouple :
STRUCT
{attribute 'pytmc' := '
pv: eState
io: i
field: DESC Gives the current coupling realignment state.
'}
eState: E_RealignCoupleState;
{attribute 'pytmc' := '
pv: bExecute
io: io
field: DESC Hold TRUE to run an automatic realignment sequence. FALSE to cancel.
'}
bExecute: BOOL;
{attribute 'pytmc' := '
pv: bExecuteCouple
io: io
field: DESC Write TRUE to couple axes.
'}
bExecuteCouple: BOOL;
{attribute 'pytmc' := '
pv: bExecuteDecouple
io: io
field: DESC Write FALSE to decouple axes.
'}
bExecuteDecouple: BOOL;
{attribute 'pytmc' := '
pv: fRealignSpeed
io: io
field: DESC The speed at which the axis will be moved when realigning.
'}
fRealignSpeed: LREAL;
{attribute 'pytmc' := '
pv: bCoupled
io: i
field: DESC TRUE if the axes are coupled. FALSE if they are not coupled.
'}
bCoupled: BOOL;
END_STRUCT
END_TYPE
- Related:
ST_BoundedValue
TYPE ST_BoundedValue :
STRUCT
{attribute 'pytmc' := '
pv: fIn
io: io
field: DESC The input value before bounds are applied.
'}
fIn : LREAL; // The input value before bounds are applied.
{attribute 'pytmc' := '
pv: fMax
io: io
field: DESC The maximum bound for the output value.
'}
fMax : LREAL; // The maximum bound for the output value.
{attribute 'pytmc' := '
pv: fMin
io: io
field: DESC The minimum bound for the output value.
'}
fMin : LREAL; // The minimum bound for the output value.
{attribute 'pytmc' := '
pv: fOut
io: i
field: DESC The output value after bounds are applied.
'}
fOut : LREAL; // The output value after bounds are applied.
{attribute 'pytmc' := '
pv: bMaxLim
io: i
field: DESC TRUE if the output value has been limited by the upper bound.
'}
bMaxLim: BOOL; // TRUE if the output value has been limited by the upper bound.
{attribute 'pytmc' := '
pv: bMinLim
io: i
field: DESC TRUE if the output value has been limited by the lower bound.
'}
bMinLim: BOOL; // TRUE if the output value has been limited by the lower bound.
END_STRUCT
END_TYPE
ST_CoordinateGantryAxisEnable
TYPE ST_CoordinateGantryAxisEnable :
STRUCT
{attribute 'pytmc' := '
pv: bEnable
io: i
field: DESC Set false to prevent this function block from allowing the motors to enable.
'}
bEnable: BOOL; // Set false to prevent this function block from allowing the motors to enable.
{attribute 'pytmc' := '
pv: bLeaderDriveReady
io: i
field: DESC Intended to be linked to the leader drive ready status.
'}
bLeaderDriveReady AT %I*: BOOL; // Intended to be linked to the leader drive ready status.
{attribute 'pytmc' := '
pv: bFollowDriveReady
io: i
field: DESC Intended to be linked to the follow drive ready status.
'}
bFollowDriveReady AT %I*: BOOL; // Intended to be linked to the follow drive ready status.
tTimeout: TIME := T#5s; // Timeout before giving up on a state.
{attribute 'pytmc' := '
pv: eState
io: i
field: DESC Present enable sequence state.
'}
eState: E_GantryEnableState; // Present enable sequence state.
END_STRUCT
END_TYPE
- Related:
ST_DiffractometerSlot
TYPE ST_DiffractometerSlot EXTENDS ST_SampleSlot :
(*
Represents a slot in the SDS garage.
*)
STRUCT
{attribute 'pytmc' := '
pv: fYPos
io: io
field: DESC The y-position of the garage, in units of mm, which corresponds to the slot being approximately level with the transfer arm.
'}
fYPos: REAL;
{attribute 'pytmc' := '
pv: fDeg
io: io
field: DESC The angular position of the slot in the garage in units of degrees.
'}
fDeg: REAL;
END_STRUCT
END_TYPE
- Related:
ST_GarageSlot
TYPE ST_GarageSlot EXTENDS ST_SampleSlot :
(*
Represents a slot in the SDS garage.
*)
STRUCT
{attribute 'pytmc' := '
pv: fYPos
io: io
field: DESC The y-position of the garage, in units of mm, which corresponds to the slot being approximately level with the transfer arm.
'}
fYPos: REAL;
{attribute 'pytmc' := '
pv: fDeg
io: io
field: DESC The angular position of the slot in the garage in units of degrees.
'}
fDeg: REAL;
END_STRUCT
END_TYPE
- Related:
ST_MultiLeaderMotionCoupling
TYPE ST_MultiLeaderMotionCoupling :
STRUCT
{attribute 'pytmc' := '
pv: bEnable
io: io
field: DESC Set and hold TRUE to allow the axis to run in multi-leader mode.
'}
bEnable: BOOL;
{attribute 'pytmc' := '
pv: bReset
io: io
field: DESC Set and release TRUE to fully reset errors and state machine.
'}
bReset: BOOL;
{attribute 'pytmc' := '
pv: bBusy
io: i
field: DESC TRUE indicates state machine running and not in error state.
'}
bBusy: BOOL;
{attribute 'pytmc' := '
pv: sStatusMsg
io: i
field: DESC String used to give helpful messages.
'}
sStatusMsg: T_MaxString;
// Ex: TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_11^STM Status^Status^Ready
{attribute 'pytmc' := '
pv: bFollowerDriveReady
io: i
field: DESC Linked to the ready status of the drive.
'}
bFollowerDriveReady AT %I*: BOOL; // Link to the ready status of the drive
{attribute 'pytmc' := '
pv: bUseDynamicLimits
io: io
field: DESC Set AND hold TRUE to enable dynamic limits to acc, vel, AND pos.
'}
bUseDynamicLimits: BOOL;
{attribute 'pytmc' := '
pv: fTargetAcc
io: io
field: DESC The target acceleration.
'}
fTargetAcc: LREAL; // Set in software to allow axis to target an acceleration based on multiple leaders.
{attribute 'pytmc' := '
pv: fTargetVel
io: io
field: DESC The target velocity.
'}
fTargetVel: LREAL; // Set in software to allow axis to target a velocity based on multiple leaders.
{attribute 'pytmc' := '
pv: fTargetPos
io: io
field: DESC The target position.
'}
fTargetPos: LREAL; // Set in software to allow axis to target a position based on multiple leaders.
{attribute 'pytmc' := '
pv: fPosDelta
io: i
field: DESC Target position minus position.
'}
fPosDelta : LREAL; // Difference between current position and target position.
{attribute 'pytmc' := '
pv: fTTGain
io: io
field: DESC Target tracking gain.
'}
fTargetTrackingGain: LREAL := 1; // Gain used to tune target tracking when within target position window.
{attribute 'pytmc' := '
pv: fHaltPos
io: i
field: DESC Position at which axis halted.
'}
fHaltPos: LREAL; // Position at which halt is called. Allows for centering deadband.
{attribute 'pytmc' := 'pv: stAcc'}
stAcc: ST_BoundedValue;
{attribute 'pytmc' := 'pv: stVel'}
stVel: ST_BoundedValue;
{attribute 'pytmc' := 'pv: stPos'}
stPos: ST_BoundedValue;
{attribute 'pytmc' := '
pv: nDir
io: i
field: DESC Internally calculated direction value for external setpoint generator.
'}
nDir: DINT;
{attribute 'pytmc' := '
pv: bError
io: i
field: DESC TRUE if an error is active.
'}
bError: BOOL;
{attribute 'pytmc' := '
pv: sErrorMsg
io: i
field: DESC Displays error message relevant to the active error. Empty otherwise.
'}
sErrorMsg: T_MaxString;
{attribute 'pytmc' := '
pv: sErrorHelp
io: i
field: DESC Displays additional help for the active rror. Empty otherwise.
'}
sErrorHelp: T_MaxString;
{attribute 'pytmc' := '
pv: eState
io: i
field: DESC Records the current state of the multi-leader function block.
'}
eState: E_MultiLeaderMotionCoupling;
END_STRUCT
END_TYPE
ST_SampleSlot
TYPE ST_SampleSlot :
(*
Represents a general slot for a sample puck.
*)
STRUCT
{attribute 'pytmc' := '
pv: eState
io: io
field: DESC Enumeration of possible states for a slot in the sample delivery system. (empty, full, or disabled)
'}
eState: E_SampleSlotStates;
{attribute 'pytmc' := '
pv: sTag
io: i
field: DESC A two character identifier for the slot. e.g., T1, B3, etc...
'}
sTag: STRING;
{attribute 'pytmc' := '
pv: sDesc
io: io
field: DESC If the slot is in the full state, this value is supposed to describe what is in the slot.
'}
sDesc: T_MaxString;
END_STRUCT
END_TYPE
- Related:
ST_SDS
TYPE ST_SDS :
(*
Structure to represent the data used for the sample delivery system that is not
needed to be persistent.
*)
STRUCT
{attribute 'pytmc' := '
pv: Initialized
io: i
field: DESC TRUE if the non-persistent data has been initialized. FALSE if it has not.
'}
bInitialized: BOOL;
{attribute 'pytmc' := '
pv: Enable
io: io
field: DESC TRUE to enable SDS system. FALSE to disable.
'}
bEnable : BOOL := TRUE;
{attribute 'pytmc' := '
pv: Maint
io: io
field: DESC TRUE if in maintenance mode. FALSE if NOT.
'}
bMaint: BOOL;
{attribute 'pytmc' := '
pv: Error
io: i
field: DESC TRUE if an error is active. FALSE if not.
'}
bError : BOOL;
{attribute 'pytmc' := '
pv: ErrorMsg
io: i
field: DESC Error message to help describe the active error.
'}
sErrorMsg : T_MaxString;
{attribute 'pytmc' := '
pv: Success
io: i
field: DESC TRUE if successfully completed state machine step.
'}
bSuccess : BOOL;
// Variables used for homing
{attribute 'pytmc' := '
pv: HomeReq
io: i
field: DESC TRUE if homing is required for the sample transfer arm.
'}
bHomingRequired : BOOL := TRUE;
tHomingRequiredTimeoutTime : TIME := T#1h;
tonHomingRequiredTimeout : TON;
tonHomingStateTimeout : TON;
nHomingState : INT := 0;
{attribute 'pytmc' := '
pv: MinHomeReq
io: i
field: DESC Minutes until homing is required again for the sample transfer arm.
'}
fMinUntilHomingRequired: REAL;
{attribute 'pytmc' := '
pv: FaceState
io: i
field: DESC Enum to describe which way the sample garage should face.
'}
eFaceState: E_SDSFaceState;
{attribute 'pytmc' := '
pv: FacePortOffset
io: io
field: DESC Degree offset when trying to face the side port.
'}
fFacePortOffset: REAL := 90; // degree offset when trying to face the side port.
// Transfer Arm State Positioner
stTransferArmHEpicsToPlc: ST_StateEpicsToPlc;
stTransferArmHPlcToEpics: ST_StatePlcToEpics;
{attribute 'pytmc' := '
pv: ArmHStateGet
io: i
'}
eTransferArmHStateGet: E_SDSTransferArmHStates; // get state enum for transfer arm horizontal position states.
{attribute 'pytmc' := '
pv: ArmHStateSet
io: io
'}
eTransferArmHStateSet: E_SDSTransferArmHStates; // set state enum for transfer arm horizontal position states.
astTransferArmHPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
{attribute 'pytmc' := '
pv: ARMH:PS1D
field: DESC Position State 1D used for transfer arm horizontal position states.
'}
fbTransferArmHPos1D: FB_PositionState1D; // Position State 1D used for transfer arm horizontal position states.
// Transfer Arm State Positioner
stTransferArmREpicsToPlc: ST_StateEpicsToPlc;
stTransferArmRPlcToEpics: ST_StatePlcToEpics;
{attribute 'pytmc' := '
pv: ArmRStateGet
io: i
'}
eTransferArmRStateGet: E_SDSTransferArmRStates; // get state enum for transfer arm rotational position states.
{attribute 'pytmc' := '
pv: ArmRStateSet
io: io
'}
eTransferArmRStateSet: E_SDSTransferArmRStates; // set state enum for transfer arm rotational position states.
astTransferArmRPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
{attribute 'pytmc' := '
pv: ARMR:PS1D
field: DESC Position State 1D used for transfer arm rotational position states.
'}
fbTransferArmRPos1D: FB_PositionState1D; // Position State 1D used for transfer arm rotational position states.
// Garage Y State Positioner
stGarageYEpicsToPlc: ST_StateEpicsToPlc;
stGarageYPlcToEpics: ST_StatePlcToEpics;
{attribute 'pytmc' := '
pv: GarYStateGet
io: i
'}
eGarageYStateGet: E_SDSGarageYStates; // get state enum for garage y and rotational position states.
{attribute 'pytmc' := '
pv: GarYStateSet
io: io
'}
eGarageYStateSet: E_SDSGarageYStates; // set state enum for garage y and rotational position states.
astGarageYPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
{attribute 'pytmc' := '
pv: GARY:PS1D
field: DESC Position State 1D used for garage y-position.
'}
fbGarageYPos1D: FB_PositionState1D; // Position State 1D used for garage y-position.
// Garage R State Positioner
stGarageREpicsToPlc: ST_StateEpicsToPlc;
stGarageRPlcToEpics: ST_StatePlcToEpics;
{attribute 'pytmc' := '
pv: GarRStateGet
io: i
'}
eGarageRStateGet: E_SDSGarageRStates; // get state enum for garage y and rotational position states.
{attribute 'pytmc' := '
pv: GarRStateSet
io: io
'}
eGarageRStateSet: E_SDSGarageRStates; // set state enum for garage y and rotational position states.
astGarageRPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
{attribute 'pytmc' := '
pv: GARR:PS1D
field: DESC Position State 1D used for garage rotation..
'}
fbGarageRPos1D: FB_PositionState1D; // Position State 1D used for garage rotation.
{attribute 'pytmc' := '
pv: State
field: DESC Present state for sample delivery system.
'}
eState : E_SDSState;
{attribute 'pytmc' := '
pv: SubState
field: DESC Present sub-state for sample delivery system.
'}
eSubState : E_SDSSubState;
{attribute 'pytmc' := '
pv: SeqState
field: DESC Present sequence state for sample delivery system.
'}
eSeqState: E_SDSSeqState;
{attribute 'pytmc' := '
pv: SecRemaining
field: DESC Seconds remaining before current sequence step times out.
'}
fSecRemaining: REAL;
{attribute 'pytmc' := '
pv: StatusMsg
field: DESC Status message.
'}
sStatusMsg: T_MaxString;
// Booleans to track what is currently possible to do.
{attribute 'pytmc' := '
pv: GarLoadable
io: i
field: DESC True if it is currently possible to load a garage slot. False if all garage slots are full or disabled.
'}
bGarageLoadable : BOOL;
{attribute 'pytmc' := '
pv: GarUnloadable
io: i
field: DESC True if it is currently possible to unload a garage slot. False if all garage slots are empty or disabled.
'}
bGarageUnloadable : BOOL;
{attribute 'pytmc' := '
pv: ArmLoadable
io: i
field: DESC True if it is currently possible to load the transfer arm slot. False if the transfer arm slot is full or disabled.
'}
bTransferArmLoadable : BOOL;
{attribute 'pytmc' := '
pv: ArmUnloadable
io: i
field: DESC True if it is currently possible to unload the transfer arm slot. False if the transfer arm slot is empty or disabled.
'}
bTransferArmUnloadable : BOOL;
{attribute 'pytmc' := '
pv: DiffLoadable
io: i
field: DESC True if it is currently possible to load the diffractometer slot. False if the diffractometer slot is full or disabled.
'}
bDiffractometerLoadable : BOOL;
{attribute 'pytmc' := '
pv: DiffUnloadable
io: i
field: DESC True if it is currently possible to unload the diffractometer slot. False if the diffractoemter slot is empty or disabled.
'}
bDiffractometerUnloadable : BOOL;
{attribute 'pytmc' := 'pv: Sel'}
stUISelect : ST_UISelect;
tonForwardHomingLimit: TON;
stGarageYStage : REFERENCE TO ST_MotionStage;
stGarageRStage : REFERENCE TO ST_MotionStage;
stTransferArmHStage : REFERENCE TO ST_MotionStage;
stTransferArmRStage : REFERENCE TO ST_MotionStage;
eDiffXStateGet : REFERENCE TO E_DiffState;
eDiffXStateSet : REFERENCE TO E_DiffState;
stDiffXStage : REFERENCE TO ST_MotionStage;
eDiffYStateGet : REFERENCE TO E_DiffState;
eDiffYStateSet : REFERENCE TO E_DiffState;
stDiffYStage : REFERENCE TO ST_MotionStage;
eDiffZStateGet : REFERENCE TO E_DiffState;
eDiffZStateSet : REFERENCE TO E_DiffState;
stDiffZStage : REFERENCE TO ST_MotionStage;
eDiff2ThetaYStateGet : REFERENCE TO E_DiffState;
eDiff2ThetaYStateSet : REFERENCE TO E_DiffState;
stDiff2ThetaYStage : REFERENCE TO ST_MotionStage;
eDiffPhiStateGet : REFERENCE TO E_DiffState;
eDiffPhiStateSet : REFERENCE TO E_DiffState;
stDiffPhiStage : REFERENCE TO ST_MotionStage;
eDiffChiStateGet : REFERENCE TO E_DiffState;
eDiffChiStateSet : REFERENCE TO E_DiffState;
stDiffChiStage : REFERENCE TO ST_MotionStage;
eDiff2ThetaStateGet : REFERENCE TO E_DiffState;
eDiff2ThetaStateSet : REFERENCE TO E_DiffState;
stDiff2ThetaStage : REFERENCE TO ST_MotionStage;
eDiffThetaStateGet : REFERENCE TO E_DiffState;
eDiffThetaStateSet : REFERENCE TO E_DiffState;
stDiffThetaStage : REFERENCE TO ST_MotionStage;
{attribute 'pytmc' := '
pv: GarAboveColl
io: i
field: DESC True if garage above collision height.
'}
bGarageAboveCollisionHeight : BOOL;
{attribute 'pytmc' := '
pv: TAHHomeState
io: i
field: DESC Transfer arm horizontal homing state.
'}
eTransferArmHHomingState : E_SDSHomingState;
{attribute 'pytmc' := '
pv: TARHomeState
io: i
field: DESC Transfer arm rotational homing state.
'}
eTransferArmRHomingState : E_SDSHomingState;
{attribute 'pytmc' := '
pv: GarRHomeState
io: i
field: DESC Garage rotational homing state.
'}
eGarageRHomingState : E_SDSHomingState;
{attribute 'pytmc' := '
pv: GarRotSpeed
io: i
field: DESC Garage rotational speed.
'}
fGarageRotationSpeed : LREAL := 10.0;
{attribute 'pytmc' := '
pv: TARotSpeed
io: i
field: DESC Garage rotational speed.
'}
fTransferArmRotationSpeed : LREAL := 10.0;
{attribute 'pytmc' := '
pv: GarRotClearancePos
io: i
field: DESC Garage rotational position to avoid collision with loaded transfer arm when moving up and down.
'}
fGarageRotationClearancePos : LREAL;
{attribute 'pytmc' := '
pv: LoadingSel
io: i
field: DESC True if loading from port.
'}
bLoadingSelected : BOOL;
{attribute 'pytmc' := '
pv: UnloadingSel
io: i
field: DESC True if unloading from port.
'}
bUnloadingSelected : BOOL;
{attribute 'pytmc' := '
pv: NumGarSlots
io: i
field: DESC Number of garage sample slots.
'}
nGarageSlots : UINT := 12;
{attribute 'pytmc' := '
pv: SelGarSlot
io: i
field: DESC Selected garage slot for loading/unloading.
'}
eSelectedGarageSlot : E_SDSGarageYStates;
END_STRUCT
END_TYPE
ST_SDSPersistent
TYPE ST_SDSPersistent :
(*
Structure to represent the data used for the sample delivery system that is
needed to be persistent.
*)
STRUCT
{attribute 'pytmc' := '
pv: bInitialized
io: i
field: DESC TRUE if the persistent data has been initialized. FALSE if it has not.
'}
bInitialized: BOOL;
{attribute 'pytmc' := 'pv: Garage:Slot'}
astGarageSlot: ARRAY[E_SDSGarageYStates.T1..E_SDSGarageYStates.B4] OF ST_GarageSlot;
{attribute 'pytmc' := 'pv: Transfer:Slot'}
stTransferArmSlot: ST_TransferSlot;
{attribute 'pytmc' := 'pv: Diff:Slot'}
stDiffractometerSlot: ST_DiffractometerSlot;
{attribute 'pytmc' := '
pv: GarHSafeFromCol
io: i
field: DESC Height at which garage consider safe from collisions.
'}
fGarageHeightSafeFromCollision : LREAL := -10;
{attribute 'pytmc' := '
pv: GarRClearOff
io: i
field: DESC Garage rotational offset for clearance when moving up and down with loading transfer arm.
'}
fGarageRotationClearanceOffset : LREAL := 45;
END_STRUCT
END_TYPE
ST_TransferSlot
TYPE ST_TransferSlot EXTENDS ST_SampleSlot :
(*
Represents a slot in the SDS transfer arm.
*)
STRUCT
END_STRUCT
END_TYPE
- Related:
ST_UISelect
TYPE ST_UISelect :
STRUCT
// Booleans to allow operator to choose what they want to do.
{attribute 'pytmc' := '
pv: Load
io: io
field: DESC Write True to command a loading sequence.
'}
bLoad: BOOL;
{attribute 'pytmc' := '
pv: Unload
io: io
field: DESC Write True to command a unloading sequence.
'}
bUnload: BOOL;
{attribute 'pytmc' := '
pv: ManualControl
io: io
field: DESC Write True to command manual control.
'}
bManualControl : BOOL;
{attribute 'pytmc' := '
pv: ExitManualControl
io: io
field: DESC Write True to command exit manual control.
'}
bExitManualControl : BOOL;
{attribute 'pytmc' := '
pv: LoadArmFromGar
io: io
field: DESC Write True to command load arm from garage.
'}
bLoadArmFromGarage : BOOL;
{attribute 'pytmc' := '
pv: UnloadArmToGar
io: io
field: DESC Write True to command unload arm from garage.
'}
bUnloadArmToGarage : BOOL;
{attribute 'pytmc' := '
pv: LoadGarFromPort
io: io
field: DESC Write True to command load garage from port.
'}
bLoadGarageFromPort : BOOL;
{attribute 'pytmc' := '
pv: UnloadGarFromPort
io: io
field: DESC Write True to command load garage from port.
'}
bLoadUnloadGarageFromPort : BOOL;
{attribute 'pytmc' := '
pv: LoadDiffFromArm
io: io
field: DESC Write True to command load diffractometer from arm.
'}
bLoadDiffractometerFromArm : BOOL;
{attribute 'pytmc' := '
pv: UnloadDiffToArm
io: io
field: DESC Write True to command unload diffractometer to arm.
'}
bUnloadDiffractometerToArm : BOOL;
{attribute 'pytmc' := '
pv: HomeArm
io: io
field: DESC Write True to command home transfer arm.
'}
bHomeTransferArm : BOOL;
{attribute 'pytmc' := '
pv: HomeGar
io: io
field: DESC Write True to command home garage.
'}
bHomeGarage : BOOL;
{attribute 'pytmc' := '
pv: GarSlot
io: io
field: DESC Select garage slot to unload or load.
'}
eGarageSlot : E_SDSGarageYStates;
{attribute 'pytmc' := '
pv: Proceed
io: io
field: DESC Write True to proceed to next sequence step, if applicable.
'}
bProceed: BOOL;
{attribute 'pytmc' := '
pv: Retry
io: io
field: DESC Write True when prompted to retry a defined previous step, if applicable.
'}
bRetry: BOOL;
{attribute 'pytmc' := '
pv: Cancel
io: io
field: DESC TRUE to cancel next sequence step.
'}
bCancel: BOOL;
{attribute 'pytmc' := '
pv: TextInput
io: io
field: DESC General purpose text input field.
'}
sTextInput: T_MaxString;
END_STRUCT
END_TYPE
- Related:
GVLs
Global_Version
{attribute 'TcGenerated'}
{attribute 'no-analysis'}
{attribute 'linkalways'}
// This function has been automatically generated from the project information.
VAR_GLOBAL CONSTANT
{attribute 'const_non_replaced'}
stLibVersion_qrix_motion : ST_LibVersion := (iMajor := 0, iMinor := 4, iBuild := 0, iRevision := 0, nFlags := 1, sVersion := '0.4.0');
END_VAR
GVL_EPS
VAR_GLOBAL
{attribute 'pytmc' := 'pv: QRIX:SA:ESTOP_RST'}
bResetIClk: BOOL;
{attribute 'pytmc' := 'pv: QRIX:SA:ESTOP'}
{attribute 'TcLinkTo' := 'TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 18'}
bESTOP AT %I*: BOOL;
{attribute 'TcLinkTo' := 'TIIB[Rack#2B-05 SV_AIR (EL2004)]^Channel 1^Output'}
{attribute 'pytmc' := 'pv: QRIX:SA:SV1'}
bOpenSV1 AT %Q*: BOOL;
{attribute 'TcLinkTo' := 'TIIB[Rack#2B-05 SV_AIR (EL2004)]^Channel 2^Output'}
{attribute 'pytmc' := 'pv: QRIX:SA:SV2'}
bOpenSV2 AT %Q*: BOOL;
{attribute 'TcLinkTo' := 'TIIB[Rack#1C_ServoDR (NCR-HD Series)]^260th receive PDO Mapping^Positive torque limit value'}
iServoTorqueLimitPositive AT %Q*: UINT;
{attribute 'TcLinkTo' := 'TIIB[Rack#1C_ServoDR (NCR-HD Series)]^260th receive PDO Mapping^Negative torque limit value'}
iServoTorqueLimitNegative AT %Q*: UINT;
END_VAR
GVL_Interface
{attribute 'qualified_only'}
VAR_GLOBAL
(*I/O Outputs to QRIX Vacuum PLC*)
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Outputs^QRIX_MOT_SDS_MMS_H_RAW_ENC_CNTS
'}
QRIX_MOT_SDS_MMS_H_RAW_ENC_CNTS AT %Q* : UINT;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Outputs^QRIX_MOT_SDS_MMS_H_FORWARD_EN
'}
QRIX_MOT_SDS_MMS_H_FORWARD_EN AT %Q* : BOOL;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Outputs^QRIX_MOT_SDS_MMS_H_BACKWARD_EN
'}
QRIX_MOT_SDS_MMS_H_BACKWARD_EN AT %Q* : BOOL;
(*I/O Inputs from QRIX Vacuum PLC*)
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Inputs^QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH
'}
QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH AT %I* : BOOL;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Inputs^QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH
'}
QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH AT %I* : BOOL;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Inputs^QRIX_VAC_VGC_03_OP_CMD
'}
QRIX_VAC_VGC_03_OP_CMD AT %I* : BOOL;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-R03-DRL-01 (EK1200)^Term 141 (EL6692)^IO Inputs^QRIX_VAC_VGC_03_CL_CMD
'}
QRIX_VAC_VGC_03_CL_CMD AT %I* : BOOL;
END_VAR
GVL_PMPS
{attribute 'qualified_only'}
VAR_GLOBAL
{attribute 'pytmc' := 'pv: PLC:QRIX:MOTION:ARB'}
fbArbiter: FB_Arbiter(1);
{attribute 'pytmc' := 'pv: PLC:QRIX:MOTION:FFO:01'}
{attribute 'TcLinkTo' := '.q_xFastFaultOut:=TIIB[PMPS_FFO]^Channel 1^Output'}
fbFastFaultOutput1: FB_HardwareFFOutput := (bAutoReset := TRUE, i_sNetID:='172.21.92.73.1.1');
{attribute 'pytmc' := 'pv: PLC:QRIX:MOTION:FFO:02'}
{attribute 'TcLinkTo' := '.q_xFastFaultOut:=TIIB[PMPS_FFO]^Channel 2^Output'}
fbFastFaultOutput2: FB_HardwareFFOutput := (bAutoReset := TRUE, i_sNetID:='172.21.92.73.1.1');
END_VAR
GVL_Sensor
{attribute 'qualified_only'}
VAR_GLOBAL
{attribute 'pytmc' := '
pv: QRIX:SA:FLOATING
io: i
'}
bFloating: BOOL;
// YDF1
{attribute 'pytmc' := '
pv: QRIX:SA:DDS:YDF1
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(In) TxPDO-Map^Response Data Unit1'}
stYDF1: DUT_SensorHGS;
nYDF1 AT %Q*: UDINT;
// YDF2
{attribute 'pytmc' := '
pv: QRIX:SA:DDS:YDF2
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(In) TxPDO-Map^Response Data Unit2'}
stYDF2: DUT_SensorHGS;
nYDF2 AT %Q*: UDINT;
// YDF3
{attribute 'pytmc' := '
pv: QRIX:SA:DDS:YDF3
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(In) TxPDO-Map^Response Data Unit3'}
stYDF3: DUT_SensorHGS;
nYDF3 AT %Q*: UDINT;
// Height difference btw granite shelf and detector frame.
{attribute 'pytmc' := '
pv: QRIX:SA:DDS:HDF
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(In) TxPDO-Map^Response Data Unit4'}
stHDF: DUT_SensorHGS;
// Granite TiltZ : Pitch
{attribute 'pytmc' := '
pv: QRIX:GS:INM:Z
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-03 AI_TiltSensor (EL3174-0002)]^AI Standard Channel 2^Value'}
stGraniteP: DUT_Sensor;
// Granite TiltX : Roll
{attribute 'pytmc' := '
pv: QRIX:GS:INM:X
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-03 AI_TiltSensor (EL3174-0002)]^AI Standard Channel 1^Value'}
stGraniteR: DUT_Sensor;
// Frame TiltZ : Pitch
{attribute 'pytmc' := '
pv: QRIX:DF:INM:Z
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-03 AI_TiltSensor (EL3174-0002)]^AI Standard Channel 4^Value'}
stFrameP: DUT_Sensor;
// Frame TiltX : Roll
{attribute 'pytmc' := '
pv: QRIX:DF:INM:X
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-03 AI_TiltSensor (EL3174-0002)]^AI Standard Channel 3^Value'}
stFrameR: DUT_Sensor;
{attribute 'pytmc' := '
pv: QRIX:SA:GPT:PS1
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-04 PS_P (EL3064)]^AI Standard Channel 1^Value'}
stPS1: DUT_Sensor;
{attribute 'pytmc' := '
pv: QRIX:SA:GPT:PS2
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-04 PS_P (EL3064)]^AI Standard Channel 2^Value'}
stPS2: DUT_Sensor;
{attribute 'pytmc' := '
pv: QRIX:SA:GPT:PS3
io: i
'}
{attribute 'TcLinkTo' := ' .iRawCnts := TIIB[Rack#2B-04 PS_P (EL3064)]^AI Standard Channel 3^Value'}
stPS3: DUT_Sensor;
END_VAR
- Related:
GVL_VAR
VAR_GLOBAL
{attribute 'pytmc' := '
pv: QRIX:SA:bDoneJackOff
io: i
'}
bDoneJackOff: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:bDoneLevitation
io: i
'}
bDoneLevitation: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:bDoneLanding
io: i
'}
bDoneLanding: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:bDoneAdjustingRoll
io: i
'}
bDoneAdjustingRoll: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:bDoneAdjustingPitch
io: i
'}
bDoneAdjustingPitch: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:bDoneAdjustingHeight
io: i
'}
bDoneAdjustingHeight: BOOL;
// Reset and monitor circuit breakers on spectrometer arm
// BOX0108 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:B0108:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-08 CP_SDC (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerB0108Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:B0108:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-08 CP_SDC (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerB0108Tripped AT %I*: BOOL;
// BOX0113 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:B0113:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-13 CP_DC (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerB0113Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:B0113:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-13 CP_DC (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerB0113Tripped AT %I*: BOOL;
// BOX0118 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:B0118:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-18 CP_YF1_YF2 (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerB0118Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:B0118:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-18 CP_YF1_YF2 (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerB0118Tripped AT %I*: BOOL;
// Rack 2A02 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2A02:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2A-01 Coupler (EK1100)^Rack#2A-02 CP_SLIT (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerR2A02Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2A02:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2A-01 Coupler (EK1100)^Rack#2A-02 CP_SLIT (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerR2A02Tripped AT %I*: BOOL;
// Rack 2A07 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2A07:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2A-01 Coupler (EK1100)^Rack#2A-07 CP_G (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerR2A07Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2A07:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2A-01 Coupler (EK1100)^Rack#2A-07 CP_G (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerR2A07Tripped AT %I*: BOOL;
// Rack 2A13 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2A13:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2A-01 Coupler (EK1100)^Rack#2A-13 CP_PM (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerR2A13Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2A13:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2A-01 Coupler (EK1100)^Rack#2A-13 CP_PM (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerR2A13Tripped AT %I*: BOOL;
// Rack 2B14 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2B14:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2B-01 Coupler (EK1100)^Rack#2B-14 CP_YDF (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerR2B14Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2B14:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2B-01 Coupler (EK1100)^Rack#2B-14 CP_YDF (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerR2B14Tripped AT %I*: BOOL;
// Rack 2B17 Circuit Breaker
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2B17:Reset
io: io
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2B-01 Coupler (EK1100)^Rack#2B-17 CP_YDF_ZF (EL9221-5000)^OCP Outputs^Control^Reset'}
bCircuitBreakerR2B17Reset AT %Q*: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:CB:R2B17:Tripped
io: i
'}
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#2B-01 Coupler (EK1100)^Rack#2B-17 CP_YDF_ZF (EL9221-5000)^OCP Inputs^Status^Tripped'}
bCircuitBreakerR2B17Tripped AT %I*: BOOL;
fCryoZNeutralPositionAtDiffZ0 : LREAL;
fCryoYNeutralPositionAtDiffY0 : LREAL;
fBraidDiameter: LREAL := 70; // mm
END_VAR
Main
{attribute 'qualified_only'}
VAR_GLOBAL
(*
Only includes motor definitions for the IOC
These are hard-coded to be Main.M#,
but are very convenient to store in a GVL,
hence the confusing namespace here
This should be refactored once the IOC
supports arbitrary ads paths for motors
*)
// Sample Chamber Sliding Seal - 1 axis
//connect EPS limits
{attribute 'pytmc' := 'pv: QRIX:SC:SSL:MMS'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_02_01 - SSL]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[EL7047_02_01 - SSL]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[EL5042_02_02]^FB Inputs Channel 1^Position'}
M1: ST_MotionStage := (sName := 'QRIX:SC:SSL:MMS');
(* Start Adding Spectometer Arm Axes Here*)
// 2Theta Stepper
{attribute 'pytmc' := 'pv: QRIX:SA:MMS:2Theta'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#1A-04 DR_2Th (EL7047)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#1A-04 DR_2Th (EL7047)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#1A-06 ENC_2Th (EL5042)]^FB Inputs Channel 1^Position;
.bBrakeRelease := TIIB[Rack#1A-05 MB_2Th (EL2602)]^Channel 1^Output
'}
M2: ST_MotionStage := (sName := 'QRIX:SA:MMS:2Theta');
// XS1
{attribute 'pytmc' := 'pv: QRIX:OPTSL:MMS:NORTH'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-03 DR_XS1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-03 DR_XS1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-07 ENC_X1_X2 (EL5042)]^FB Inputs Channel 1^Position
'}
M3: ST_MotionStage :=(sName := 'QRIX:OPTSL:MMS:NORTH');
// XS2
{attribute 'pytmc' := 'pv: QRIX:OPTSL:MMS:SOUTH'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-04 DR_XS2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-04 DR_XS2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-07 ENC_X1_X2 (EL5042)]^FB Inputs Channel 2^Position
'}
M4: ST_MotionStage:=(sName := 'QRIX:OPTSL:MMS:SOUTH');
// YS1
{attribute 'pytmc' := 'pv: QRIX:OPTSL:MMS:TOP'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-05 DR_YS1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-05 DR_YS1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-08 ENC_Y1_Y2 (EL5042)]^FB Inputs Channel 1^Position
'}
M5: ST_MotionStage:=(sName := 'QRIX:OPTSL:MMS:TOP');
// YS2
{attribute 'pytmc' := 'pv: QRIX:OPTSL:MMS:BOTTOM'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-06 DR_YS2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-06 DR_YS2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-08 ENC_Y1_Y2 (EL5042)]^FB Inputs Channel 1^Position
'}
M6: ST_MotionStage :=(sName := 'QRIX:OPTSL:MMS:BOTTOM');;
// YG1
{attribute 'pytmc' := 'pv: QRIX:OPT:MMS:Y1'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-10 DR_YG1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-10 DR_YG1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-10 ENC_YG1_YG2 (EL5042)]^FB Inputs Channel 1^Position
'}
M7: ST_MotionStage:= (sName := 'QRIX:OPT:MMS:Y1');
// YG2
{attribute 'pytmc' := 'pv: QRIX:OPT:MMS:Y2'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-11 DR_YG2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-11 DR_YG2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-10 ENC_YG1_YG2 (EL5042)]^FB Inputs Channel 2^Position
'}
M8: ST_MotionStage:= (sName := 'QRIX:OPT:MMS:Y2');
// YG3
{attribute 'pytmc' := 'pv: QRIX:OPT:MMS:Y3'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-12 DR_YG3 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-12 DR_YG3 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-11 ENC_YG3_X1PM (EL5042)]^FB Inputs Channel 1^Position
'}
M9: ST_MotionStage:= (sName := 'QRIX:OPT:MMS:Y3');
// RxG
{attribute 'pytmc' := 'pv: QRIX:G:MMS:Rx'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-08 DR_RxG (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-08 DR_RxG (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-09 ENC_RxG_XG (EL5042)]^FB Inputs Channel 1 compact^Position
'}
M10: ST_MotionStage:=(sName := 'QRIX:G:MMS:Rx');
// XG
{attribute 'pytmc' := 'pv: QRIX:G:MMS:X'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-09 DR_XG (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-09 DR_XG (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-09 ENC_RxG_XG (EL5042)]^FB Inputs Channel 2^Position
'}
M11: ST_MotionStage:=(sName := 'QRIX:G:MMS:X');
// XPM1
{attribute 'pytmc' := 'pv: QRIX:PM:MMS:X1'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-14 DR_XPM1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-14 DR_XPM1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-11 ENC_YG3_X1PM (EL5042)]^FB Inputs Channel 2^Position
'}
M12: ST_MotionStage :=(sName := 'QRIX:PM:MMS:X1');
// XPM2
{attribute 'pytmc' := 'pv: QRIX:PM:MMS:X2'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-15 DR_XPM2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-15 DR_XPM2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-12 ENC_X2PM_RzPM (EL5042)]^FB Inputs Channel 1^Position
'}
M13: ST_MotionStage:=(sName := 'QRIX:PM:MMS:X2');
// RzPM
{attribute 'pytmc' := 'pv: QRIX:PM:MMS:Rz'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2A-16 DR_RzPM (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2A-16 DR_RzPM (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[Rack#2B-12 ENC_X2PM_RzPM (EL5042)]^FB Inputs Channel 2^Position
'}
M14: ST_MotionStage:=(sName := 'QRIX:PM:MMS:Rz');
// YDF1
{attribute 'pytmc' := 'pv: QRIX:DF:MMS:Y1'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2B-15 DR_YDF1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2B-15 DR_YDF1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIID^Device 2 (EtherCAT)^Rack#2D LinearGaugeAmp (SC-HG1-ETC)^Process Data(In) TxPDO-Map^Response Data Unit1
'}
M15: ST_MotionStage :=(sName := 'QRIX:DF:MMS:Y1');
// YDF2
{attribute 'pytmc' := 'pv: QRIX:DF:MMS:Y2'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2B-16 DR_YDF2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2B-16 DR_YDF2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIID^Device 2 (EtherCAT)^Rack#2D LinearGaugeAmp (SC-HG1-ETC)^Process Data(In) TxPDO-Map^Response Data Unit2
'}
M16: ST_MotionStage:=(sName := 'QRIX:DF:MMS:Y2');
// YDF3
{attribute 'pytmc' := 'pv: QRIX:DF:MMS:Y3'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#2B-19 DR_YDF3 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[Rack#2B-19 DR_YDF3 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIID^Device 2 (EtherCAT)^Rack#2D LinearGaugeAmp (SC-HG1-ETC)^Process Data(In) TxPDO-Map^Response Data Unit3
'}
M17: ST_MotionStage :=(sName := 'QRIX:DF:MMS:Y3');
// XSDC1
{attribute 'pytmc' := 'pv: QRIX:DETSL:MMS:NORTH'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-09 DR_XSDC1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-09 DR_XSDC1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-03 ENC_X1_X2 (EL5042)]^FB Inputs Channel 1^Position
'}
M18: ST_MotionStage:=(sName := 'QRIX:DETSL:MMS:NORTH');
// XSDC2
{attribute 'pytmc' := 'pv: QRIX:DETSL:MMS:SOUTH'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-10 DR_XSDC2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-10 DR_XSDC2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-03 ENC_X1_X2 (EL5042)]^FB Inputs Channel 2^Position
'}
M19: ST_MotionStage :=(sName := 'QRIX:DETSL:MMS:SOUTH');
// YSDC1
{attribute 'pytmc' := 'pv: QRIX:DETSL:MMS:TOP'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-11 DR_YSDC1 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-11 DR_YSDC1 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-04 ENC_Y1_Y2 (EL5042)]^FB Inputs Channel 1^Position
'}
M20: ST_MotionStage :=(sName := 'QRIX:DETSL:MMS:TOP');
// YSDC2
{attribute 'pytmc' := 'pv: QRIX:DETSL:MMS:BOTTOM'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-12 DR_YSDC2 (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-12 DR_YSDC2 (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-04 ENC_Y1_Y2 (EL5042)]^FB Inputs Channel 1^Position
'}
M21: ST_MotionStage :=(sName := 'QRIX:DETSL:MMS:BOTTOM');
// XDC
{attribute 'pytmc' := 'pv: QRIX:DC:MMS:X'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-14 DR_XDC (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-14 DR_XDC (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-05 ENC_XDC_RyDC (EL5042)]^FB Inputs Channel 1^Position
'}
M22: ST_MotionStage:=(sName := 'QRIX:DC:MMS:X');
// RyDC
{attribute 'pytmc' := 'pv: QRIX:DC:MMS:Ry'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-15 DR_RyDC (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-15 DR_RyDC (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-05 ENC_XDC_RyDC (EL5042)]^FB Inputs Channel 2^Position
'}
M23: ST_MotionStage :=(sName := 'QRIX:DC:MMS:Ry');
// ZDC
{attribute 'pytmc' := 'pv: QRIX:DC:MMS:Z'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[BOX-16 DR_ZDC (EL7041-0052)]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[BOX-16 DR_ZDC (EL7041-0052)]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[BOX-06 ENC_ZDC_YF1 (EL5042)]^FB Inputs Channel 1^Position
'}
M24: ST_MotionStage:=(sName := 'QRIX:DC:MMS:Z');
// YF1
{attribute 'pytmc' := 'pv: QRIX:DA:MMS:Y1'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 10;
.bLimitBackwardEnable := TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 11;
.nRawEncoderULINT := TIIB[BOX-06 ENC_ZDC_YF1 (EL5042)]^FB Inputs Channel 2^Position;
.bBrakeRelease := TIIB[BOX-19 MB_YF1_YF2 (EL2602)]^Channel 1^Output
'}
M25: ST_MotionStage:=(sName := 'QRIX:DA:MMS:Y1');
// YF2
{attribute 'pytmc' := 'pv: QRIX:DA:MMS:Y2'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 12;
.bLimitBackwardEnable := TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 13;
.nRawEncoderULINT := TIIB[BOX-07 ENC_YF2_ZF (EL5042)]^FB Inputs Channel 1^Position;
.bBrakeRelease := TIIB[BOX-19 MB_YF1_YF2 (EL2602)]^Channel 2^Output
'}
M26: ST_MotionStage:=(sName := 'QRIX:DA:MMS:Y2');
// ZF
{attribute 'pytmc' := 'pv: QRIX:DA:MMS:Z'}
{attribute 'TcLinkTo' := ' .bLimitForwardEnable := TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 14;
.bLimitBackwardEnable := TIIB[Rack#1A-02 SFIN (EL1918)]^StandardInputs^Standard Out Var 15;
.nRawEncoderULINT := TIIB[BOX-07 ENC_YF2_ZF (EL5042)]^FB Inputs Channel 2^Position;
.bBrakeRelease := TIIB[Rack#2B-18 MB_ZF (EL2602)]^Channel 1^Output
'}
M27: ST_MotionStage:=(sName := 'QRIX:DA:MMS:Z');
// Diffractometer - 8 axes START AT M28
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:X'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_04]^Channel 2^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_04]^Channel 1^Input;
.bHome := TIIB[EL1088_03_10]^Channel 1^Input;
.nRawEncoderULINT := TIIB[EL5042_03_02-Diff-X-Y]^FB Inputs Channel 1^Position'}
M28: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:X');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:Y'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_04]^Channel 4^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_04]^Channel 3^Input;
.bHome := TIIB[EL1088_03_10]^Channel 2^Input;
.nRawEncoderULINT := TIIB[EL5042_03_02-Diff-X-Y]^FB Inputs Channel 2^Position'}
M29: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:Y');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:Z'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_08]^Channel 2^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_08]^Channel 1^Input;
.bHome := TIIB[EL1088_03_10]^Channel 3^Input;
.nRawEncoderULINT := TIIB[EL5042_03_06-DIff-Z-2ThetaY]^FB Inputs Channel 1^Position'}
M30: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:Z');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:DETY'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_08]^Channel 5^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_08]^Channel 6^Input;
.bHome := TIIB[EL1088_03_10]^Channel 4^Input;
.nRawEncoderULINT := TIIB[EL5042_03_06-DIff-Z-2ThetaY]^FB Inputs Channel 2^Position'}
M31: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:DETY');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:PHI'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_08]^Channel 3^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_08]^Channel 4^Input;
.bHome := TIIB[EL1088_03_10]^Channel 4^Input;
.nRawEncoderULINT := TIIB[EL5042_03_13 - Diff - Phi - Chi]^FB Inputs Channel 1^Position'}
M32: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:PHI');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:CHI'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_15]^Channel 2^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_15]^Channel 1^Input;
.bHome := TIIB[EL1088_03_10]^Channel 6^Input;
.nRawEncoderULINT := TIIB[EL5042_03_13 - Diff - Phi - Chi]^FB Inputs Channel 2^Position'}
M33: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:CHI');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:THETA'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_15]^Channel 3^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_15]^Channel 4^Input;
.bHome := TIIB[EL1088_03_10]^Channel 7^Input;
.nRawEncoderULINT := TIIB[EL5042_03_17 - Diff - Theta - 2Theta]^FB Inputs Channel 1^Position'}
M34: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:THETA');
{attribute 'pytmc' := 'pv: QRIX:DIFF:MMS:2THETA'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_03_15]^Channel 5^Input;
.bLimitBackwardEnable := TIIB[EL1088_03_15]^Channel 6^Input;
.bHome := TIIB[EL1088_03_10]^Channel 8^Input;
.nRawEncoderULINT := TIIB[EL5042_03_17 - Diff - Theta - 2Theta]^FB Inputs Channel 2^Position'}
M35: ST_MotionStage := (sName := 'QRIX:DIFF:MMS:2THETA');
// Laser Table - 3 axes
{attribute 'pytmc' := 'pv: QRIX:LAS:MMS:VIS'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_02_06]^Channel 6^Input;
.bLimitBackwardEnable := TIIB[EL1088_02_06]^Channel 5^Input;
.nRawEncoderULINT := TIIB[EL5042_02_05 - LAS_VIS]^FB Inputs Channel 1^Position'}
M36: ST_MotionStage := (sName := 'QRIX:LAS:MMS:VIS');
{attribute 'pytmc' := 'pv: QRIX:DIAG:MMS:H'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_02_06]^Channel 2^Input;
.bLimitBackwardEnable := TIIB[EL1088_02_06]^Channel 1^Input;
.nRawEncoderULINT := TIIB[EL5042_02_08 - LAS_D_V-H]^FB Inputs Channel 2^Position'}
M37: ST_MotionStage := (sName := 'QRIX:DIAG:MMS:H');
{attribute 'pytmc' := 'pv: QRIX:DIAG:MMS:V'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL1088_02_06]^Channel 4^Input;
.bLimitBackwardEnable := TIIB[EL1088_02_06]^Channel 3^Input;
.nRawEncoderULINT := TIIB[EL5042_02_08 - LAS_D_V-H]^FB Inputs Channel 1^Position'}
M38: ST_MotionStage := (sName := 'QRIX:DIAG:MMS:V');
//Sample Delivery - 6 Axes
{attribute 'pytmc' := 'pv: QRIX:SDS:MMS:X'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_04_01]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[EL7047_04_01]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[EL5042_04_02]^FB Inputs Channel 1^Position;
.bHome := TIIB[EL1004_04_04]^Channel 1^Input'}
M39: ST_MotionStage := (sName := 'QRIX:SDS:MMS:X');
{attribute 'pytmc' := 'pv: QRIX:SDS:MMS:Y'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_04_03]^STM Status^Status^Digital input 2;
.bLimitBackwardEnable := TIIB[EL7047_04_03]^STM Status^Status^Digital input 1;
.nRawEncoderULINT := TIIB[EL5042_04_02]^FB Inputs Channel 2^Position;
.bHome := TIIB[EL1004_04_04]^Channel 2^Input'}
M40: ST_MotionStage := (sName := 'QRIX:SDS:MMS:Y');
{attribute 'pytmc' := 'pv: QRIX:SDS:MMS:Z'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_04_05]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[EL7047_04_05]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[EL5042_04_06]^FB Inputs Channel 1^Position;
.bHome := TIIB[EL1004_04_04]^Channel 3^Input'}
M41: ST_MotionStage := (sName := 'QRIX:SDS:MMS:Z');
{attribute 'pytmc' := 'pv: QRIX:SDS:MMS:ROT_V'}
{attribute 'TcLinkTo' := '.nRawEncoderUINT := TIIB[EL5101_05_01]^ENC Status compact^Counter value;
.bHome := TIIB[EL1004_04_04]^Channel 4^Input'}
M42: ST_MotionStage := (sName := 'QRIX:SDS:MMS:ROT_V');
{attribute 'pytmc' := 'pv: QRIX:SDS:MMS:ROT_H'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_04_10]^STM Status^Status^Digital input 2;
.bLimitBackwardEnable := TIIB[EL7047_04_10]^STM Status^Status^Digital input 1;
.nRawEncoderUINT := TIIB[EL5101_05_02]^ENC Status compact^Counter value;
.bHome := TIIB[EL1004_04_09]^Channel 1^Input'}
M43: ST_MotionStage := (sName := 'QRIX:SDS:MMS:ROT_H');
{attribute 'pytmc' := 'pv: QRIX:SDS:MMS:H'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_04_12]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[EL7047_04_12]^STM Status^Status^Digital input 2;
.bHome := TIIB[EL1004_04_09]^Channel 3^Input;
.nRawEncoderUINT := TIIB[EL5101_05_03]^ENC Status compact^Counter value'}
M44: ST_MotionStage := (sName := 'QRIX:SDS:MMS:H');
//Detector Rotation
{attribute 'pytmc' := 'pv: QRIX:DET:MMS:ROT'}
{attribute 'TcLinkTo' := '.nRawEncoderULINT := TIIB[BOX-22 DET_ROT (EL5042)]^FB Inputs Channel 1^Position'}
M45: ST_MotionStage := (sName := 'QRIX:DET:MMS:ROT');
// Cryo - 4 axes
//Cryo -X
{attribute 'pytmc' := 'pv: QRIX:CRYO:MMS:X'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_02_12]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[EL7047_02_12]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[EL5042_02_13]^FB Inputs Channel 1^Position'}
M46: ST_MotionStage := (sName := 'QRIX:CRYO:MMS:X');
//CRYO Y
{attribute 'pytmc' := 'pv: QRIX:CRYO:MMS:Y'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_02_14]^STM Status^Status^Digital input 2;
.bLimitBackwardEnable := TIIB[EL7047_02_14]^STM Status^Status^Digital input 1;
.nRawEncoderULINT := TIIB[EL5042_02_13]^FB Inputs Channel 2^Position'}
M47: ST_MotionStage := (sName := 'QRIX:CRYO:MMS:Y');
//CRYO Z
{attribute 'pytmc' := 'pv: QRIX:CRYO:MMS:Z'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[EL7047_02_15]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[EL7047_02_15]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[EL5042_02_16]^FB Inputs Channel 1^Position'}
M48: ST_MotionStage := (sName := 'QRIX:CRYO:MMS:Z');
// CRYO-ROT
{attribute 'pytmc' := 'pv: QRIX:CRYO:MMS:ROT'}
{attribute 'TcLinkTo' := '.nRawEncoderULINT := TIIB[EL5042_02_16]^FB Inputs Channel 2^Position'}
M49: ST_MotionStage := (sName := 'QRIX:CRYO:MMS:ROT');
(*
// Questar - 2 axes
{attribute 'pytmc' := 'pv: QRIX:QUESTAR:MMS:01'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[SSL-EL7041]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[SSL-EL7041]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[IM1K3-EL5042]^FB Inputs Channel 1^Position'}
M49: ST_MotionStage := (sName := 'QRIX:QUESTAR:MMS:01');
{attribute 'pytmc' := 'pv: QRIX:QUESTAR:MMS:02'}
{attribute 'TcLinkTo' := '.bLimitForwardEnable := TIIB[SSL-EL7041]^STM Status^Status^Digital input 1;
.bLimitBackwardEnable := TIIB[SSL-EL7041]^STM Status^Status^Digital input 2;
.nRawEncoderULINT := TIIB[IM1K3-EL5042]^FB Inputs Channel 1^Position'}
M50: ST_MotionStage := (sName := 'QRIX:QUESTAR:MMS:02');
*)
END_VAR
POUs
F_CheckAxis
FUNCTION F_CheckAxis : BOOL
VAR_INPUT
stMotionStage: ST_MotionStage;
nComparisonCode: UINT;
fQueryValue: LREAL;
END_VAR
VAR
END_VAR
CASE nComparisonCode OF
0: F_CheckAxis := stMotionStage.stAxisStatus.fActPosition < fQueryValue AND stMotionStage.fPosition < fQueryValue; // <
1: F_CheckAxis := stMotionStage.stAxisStatus.fActPosition <= fQueryValue AND stMotionStage.fPosition <= fQueryValue; // <=
2: F_CheckAxis := stMotionStage.stAxisStatus.fActPosition > fQueryValue AND stMotionStage.fPosition > fQueryValue; // >
3: F_CheckAxis := stMotionStage.stAxisStatus.fActPosition >= fQueryValue AND stMotionStage.fPosition >= fQueryValue; // <=
ELSE
return;
END_CASE;
END_FUNCTION
F_CheckAxisGEQ
FUNCTION F_CheckAxisGEQ : BOOL
VAR_INPUT
stMotionStage: ST_MotionStage;
fQueryValue: LREAL;
END_VAR
VAR
END_VAR
F_CheckAxisGEQ := F_CheckAxis(stMotionStage, 3, fQueryValue);
END_FUNCTION
- Related:
F_CheckAxisGRT
FUNCTION F_CheckAxisGRT : BOOL
VAR_INPUT
stMotionStage: ST_MotionStage;
fQueryValue: LREAL;
END_VAR
VAR
END_VAR
F_CheckAxisGRT := F_CheckAxis(stMotionStage, 2, fQueryValue);
END_FUNCTION
- Related:
F_CheckAxisLEQ
FUNCTION F_CheckAxisLEQ : BOOL
VAR_INPUT
stMotionStage: ST_MotionStage;
fQueryValue: LREAL;
END_VAR
VAR
END_VAR
F_CheckAxisLEQ := F_CheckAxis(stMotionStage, 1, fQueryValue);
END_FUNCTION
- Related:
F_CheckAxisLES
FUNCTION F_CheckAxisLES : BOOL
VAR_INPUT
stMotionStage: ST_MotionStage;
fQueryValue: LREAL;
END_VAR
VAR
END_VAR
F_CheckAxisLES := F_CheckAxis(stMotionStage, 0, fQueryValue);
END_FUNCTION
- Related:
F_Limit
FUNCTION F_Limit : BOOL
VAR_INPUT
fVal : LREAL;
fLLim : LREAL;
fHLim : LREAL;
bInclusive : BOOL;
END_VAR
VAR
END_VAR
IF bInclusive THEN
IF fLLim <= fHLim THEN
F_Limit := fLLim <= fVal AND fHLim >= fVal;
ELSE
F_Limit := fLLim >= fVal OR fHLim <= fVal;
END_IF
ELSE
IF fLLim <= fHLim THEN
F_Limit := fLLim < fVal AND fHLim > fVal;
ELSE
F_Limit := fLLim > fVal OR fHLim < fVal;
END_IF
END_IF
END_FUNCTION
F_TargetCryoYPosition
FUNCTION F_TargetCryoYPosition : LREAL
VAR_INPUT
fNeutralPosition : LREAL;
fBraidDiameter : LREAL;
fPhiAngleDeg : LREAL;
END_VAR
F_TargetCryoYPosition := fNeutralPosition - ABS(fBraidDiameter * (1 - Cos(fPhiAngleDeg * 3.1415 / 180)));
END_FUNCTION
FB_2AxesTrans
FUNCTION_BLOCK FB_2AxesTrans
VAR_IN_OUT
stFirstAxis: ST_MotionStage;
stSecondAxis: ST_MotionStage;
bExecuteMotion:BOOL ;
io_fbFFHWO : FB_HardwareFFOutput;
fbArbiter: FB_Arbiter();
END_VAR
VAR_INPUT
{attribute 'pytmc' := '
pv: PMPS_OK;
io: i;
field: ZNAM False
field: ONAM True
'}
bMoveOk:BOOL;
(*Offsets*)
rEncoderOffsetFirst: REAL;
rEncoderOffsetSecound: REAL;
(*Distance between 1st stage and 2nd stage*)
rDistance: REAL := 0.48;
i_DevName : STRING; //device name for FFO and PMPS diagnostics
bHome:BOOL:=FALSE;
END_VAR
VAR
fbFirstStage: FB_MotionStage;
fbSecondStage: FB_MotionStage;
fPosFirstStage: LREAL;
fPosSecondStage: LREAL;
(*Motion Parameters*)
fSmallDelta: LREAL := 0.01;
fBigDelta: LREAL := 10;
fMaxVelocity: LREAL := 0.03;
fHighAccel: LREAL := 0.8;
fLowAccel: LREAL := 0.1;
stFirst: ST_PositionState;
stSecond: ST_PositionState;
{attribute 'pytmc' := 'pv: FIRST'}
fbFirst: FB_StatePTPMove;
{attribute 'pytmc' := 'pv: SECOND'}
fbSecond: FB_StatePTPMove;
(*EPICS pvs*)
{attribute 'pytmc' := '
pv: X_REQ;
io: io;
'}
rReqPosition : REAL;
{attribute 'pytmc' := '
pv: PITCH_REQ;
io: io;
'}
rReqAngle : REAL;
{attribute 'pytmc' := '
pv: X;
io: i;
'}
rActPosition : REAL;
{attribute 'pytmc' := '
pv: PITCH;
io: i;
'}
rActAngle : REAL;
{attribute 'pytmc' := '
pv: HOME_READY;
io: i;
field: ZNAM False
field: ONAM True
'}
bHomeReady:BOOL:=FALSE;
//Local variables
bInit: BOOL :=true;
rTrig_Block: R_TRIG;
rTrig_Open: R_TRIG;
rTrig_Close: R_TRIG;
//old values
rOldReqPosition : REAL;
rOldReqAngle: REAL;
bExecuteMotionX: BOOL;
fPosBlock: LREAL;
fPosClose: LREAL;
fPosOpen: LREAL;
stSetPositionOptions: ST_SetPositionOptions;
fbSetPosition_Pos: MC_SetPosition;
fbSetPosition_Angle: MC_SetPosition;
// For logging
fbLogger : FB_LogMessage := (eSubsystem:=E_SubSystem.MOTION);
tErrorPresent : R_TRIG;
tAction : R_TRIG;
tOverrideActivated : R_TRIG;
FFO : FB_FastFault :=(
i_DevName := 'OPT_XPM_Translation',
i_Desc := 'Fault occurs when center is greated than that requested',
i_TypeCode := 16#1010);
bTest: BOOL;
AptArrayStatus AT %Q* : ST_PMPS_Aperture_IO;
AptArrayReq AT %I* : ST_PMPS_Aperture_IO;
END_VAR
// Initialize
ACT_init();
// Instantiate Function block for all the blades
ACT_Motion();
//SET and GET the requested and Actual values
ACT_CalculatePositions();
END_FUNCTION_BLOCK
ACTION ACT_CalculatePositions:
//check if requested center or gap has changed
//check that the requested values are within acceptable motion range
IF (rOldReqPosition <> rReqPosition) THEN
rOldReqPosition := rReqPosition;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new Position.', eSevr:=TcEventSeverity.Verbose);
END_IF
IF (rOldReqAngle <> rReqAngle) THEN
rOldReqAngle := rReqAngle;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new Angle', eSevr:=TcEventSeverity.Verbose);
END_IF
//Calculate requested target positions from requested gap and center
fPosFirstStage := rReqPosition - rDistance / 2 * rReqAngle + rEncoderOffsetFirst;
fPosSecondStage := rReqPosition + rDistance / 2 * rReqAngle + rEncoderOffsetSecound;
//Calculate actual gap and center from actual stages positions
rActPosition := LREAL_TO_REAL( ((stFirstAxis.stAxisStatus.fActPosition - rEncoderOffsetFirst) + (stSecondAxis.stAxisStatus.fActPosition - rEncoderOffsetSecound)) / 2 );
rActAngle := LREAL_TO_REAL( ((stSecondAxis.stAxisStatus.fActPosition - rEncoderOffsetSecound) - (stFirstAxis.stAxisStatus.fActPosition - rEncoderOffsetFirst)) / rDistance );
END_ACTION
ACTION ACT_Init:
// init the motion stages parameters
IF ( bInit) THEN
stFirstAxis.bHardwareEnable := TRUE;
stSecondAxis.bHardwareEnable := TRUE;
stFirstAxis.bPowerSelf :=TRUE;
stSecondAxis.bPowerSelf :=TRUE;
stFirstAxis.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
stSecondAxis.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
FFO.i_DevName := i_DevName;
END_IF
END_ACTION
ACTION ACT_Motion:
// Instantiate Function block for all the blades
fbFirstStage(stMotionStage:=stFirstAxis);
fbSecondStage(stMotionStage:=stSecondAxis);
// PTP Motion for each blade
stFirst.sName := 'First';
stFirst.fPosition := fPosFirstStage;
stFirst.fDelta := fSmallDelta;
stFirst.fVelocity := fMaxVelocity;
stFirst.fAccel := fHighAccel;
stFirst.fDecel := fHighAccel;
stSecond.sName := 'Second';
stSecond.fPosition := fPosSecondStage;
stSecond.fDelta := fSmallDelta;
stSecond.fVelocity := fMaxVelocity;
stSecond.fAccel := fHighAccel;
stSecond.fDecel := fHighAccel;
IF (bExecuteMotionX) THEN
fbFirst.bExecute := fbSecond.bExecute := bExecuteMotionX;
bExecuteMotionX:= FALSE;
END_IF
fbFirst(
stPositionState:=stFirst,
bMoveOk:=bMoveOk,
stMotionStage:=stFirstAxis);
fbSecond(
stPositionState:=stSecond,
bMoveOk:=bMoveOk,
stMotionStage:=stSecondAxis);
END_ACTION
FB_3AxesJack
FUNCTION_BLOCK FB_3AxesJack
VAR_IN_OUT
// Jack 1 is the axis that, when higher than Jack 2, creates a negative pitch/roll
stJack1Axis: ST_MotionStage;
// Jack 2 is the axis that, when higher than Jack1, creates a positive pitch/roll
stJack2Axis: ST_MotionStage;
// Jack 3 must be used as the jack that only contributes to Roll OR only contributes to Pitch
// Assign your axes such that this jack is the jack that can only do roll OR pitch.
// Jack 3 is assumed on the positive side of the pitch/roll, so to reverse the sign simply
// supply a negative rDistanceX or rDistanceY, the choice of which is negative depends on
// whether JAck3 is an axis that does only roll or only pitch.
stJack3Axis: ST_MotionStage;
END_VAR
VAR_INPUT
{attribute 'pytmc' := '
pv: bMoveToPosition;
io: i;
'}
bMoveToPosition: BOOL;
{attribute 'pytmc' := '
pv: bMoveToPitch;
io: i;
'}
bMoveToPitch: BOOL;
{attribute 'pytmc' := '
pv: bMoveToRoll;
io: i;
'}
bMoveToRoll: BOOL;
{attribute 'pytmc' := '
pv: PMPS_OK;
io: i;
field: ZNAM False
field: ONAM True
'}
bMoveOk:BOOL;
// Desired height, pitch, and roll
{attribute 'pytmc' := '
pv: Y_REQ;
io: io;
'}
rReqPosition : REAL;
{attribute 'pytmc' := '
pv: PITCH_REQ;
io: io;
'}
rReqPitch : REAL;
{attribute 'pytmc' := '
pv: ROLL_REQ;
io: io;
'}
rReqRoll : REAL;
(*Offsets*)
{attribute 'pytmc' := '
pv: ZeroOffset_Jack1;
io: io;
'}
rEncoderOffsetJack1: REAL;
{attribute 'pytmc' := '
pv: ZeroOffset_Jack2;
io: io;
'}
rEncoderOffsetJack2: REAL;
{attribute 'pytmc' := '
pv: ZeroOffset_Jack3;
io: io;
'}
rEncoderOffsetJack3: REAL;
// The distance along the pitch axis between the two sides of jacks.
rDistanceX: REAL := 0.24;
// The distance along the roll axis between the two sides of jacks.
rDistanceZ: REAL := 0.88;
bJack3DoesPitch: BOOL;
rLimitPositionMax: REAL := 8;
rLimitPositionMin: REAL := -8;
rLimitPitchMin: REAL := -18;
rLimitPitchMax: REAL := 18;
rLimitRollMin: REAL := -18;
rLimitRollMax: REAL := 18;
(*Motion Parameters*)
fSmallDelta: LREAL := 0.01;
fBigDelta: LREAL := 10;
fMaxVelocity: LREAL := 0.1;
fHighAccel: LREAL := 0.8;
fLowAccel: LREAL := 0.1;
{attribute 'pytmc' := '
pv: Home;
io: i;
field: ZNAM False
field: ONAM True
'}
bHome:BOOL:=FALSE;
END_VAR
VAR
fbJack1Stage: FB_MotionStage;
fbJack2Stage: FB_MotionStage;
fbJack3Stage: FB_MotionStage;
fPosJack1Stage: LREAL;
fPosJack2Stage: LREAL;
fPosJack3Stage: LREAL;
stJack1: ST_PositionState;
stJack2: ST_PositionState;
stJack3: ST_PositionState;
{attribute 'pytmc' := 'pv: JACK1'}
fbJack1: FB_StatePTPMove;
{attribute 'pytmc' := 'pv: JACK2'}
fbJack2: FB_StatePTPMove;
{attribute 'pytmc' := 'pv: JACK3'}
fbJack3: FB_StatePTPMove;
//old values
rOldReqPosition : REAL;
rOldReqPitch: REAL;
rOldReqRoll: REAL;
(*EPICS pvs*)
{attribute 'pytmc' := '
pv: Y;
io: i;
'}
rActPosition : REAL;
{attribute 'pytmc' := '
pv: PITCH;
io: i;
'}
rActPitch : REAL;
{attribute 'pytmc' := '
pv: ROLL;
io: i;
'}
rActRoll : REAL;
{attribute 'pytmc' := '
pv: HOME_READY;
io: i;
field: ZNAM False
field: ONAM True
'}
bHomeReady:BOOL:=FALSE;
//Local variables
bInit: BOOL :=true;
bExecuteMotionX: BOOL;
fPosBlock: LREAL;
fPosClose: LREAL;
fPosOpen: LREAL;
stSetPositionOptions: ST_SetPositionOptions;
fbSetPosition_Pos: MC_SetPosition;
fbSetPosition_Angle: MC_SetPosition;
// For logging
fbLogger : FB_LogMessage := (eSubsystem:=E_SubSystem.MOTION);
tErrorPresent : R_TRIG;
tAction : R_TRIG;
tOverrideActivated : R_TRIG;
bTest: BOOL;
fbPower: MC_POWER;
y1: LREAL;
y2: LREAL;
y3: LREAL;
y12Ave: LREAL;
y12Diff: LREAL;
END_VAR
// Instantiate Function block for all the blades
ACT_Motion();
//SET and GET the requested and Actual values
ACT_CalculatePositions();
// Software Limits to protect blades
ACT_VirtualLimitSW();
END_FUNCTION_BLOCK
ACTION ACT_CalculatePositions:
//check if requested center or gap has changed
//check that the requested values are within acceptable motion range
IF abs(rReqPosition - rOldReqPosition) > 0.01 OR bMoveToPosition THEN
rOldReqPosition := rReqPosition;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new Position.', eSevr:=TcEventSeverity.Verbose);
ELSE
rReqPosition := rActPosition;
rOldReqPosition := rReqPosition;
END_IF
IF abs(rReqPitch - rOldReqPitch) > 0.01 OR bMoveToPitch THEN
rOldReqPitch := rReqPitch;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new Pitch', eSevr:=TcEventSeverity.Verbose);
ELSE
rReqPitch := rActPitch;
rOldReqPitch := rReqPitch;
END_IF
IF abs(rReqRoll - rOldReqRoll) > 0.01 OR bMoveToRoll THEN
rOldReqRoll := rReqRoll;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new Roll', eSevr:=TcEventSeverity.Verbose);
ELSE
rReqRoll := rActRoll;
rOldReqRoll := rReqRoll;
END_IF
//Calculate requested target positions from requested roll, pitch, and midpoint height.
IF bJack3DoesPitch THEN
fPosJack1Stage := rReqPosition - rDistanceX / 2.0 * rReqRoll - rDistanceZ / 2.0 * rReqPitch + rEncoderOffsetJack1;
fPosJack2Stage := rReqPosition + rDistanceX / 2.0 * rReqRoll - rDistanceZ / 2.0 * rReqPitch + rEncoderOffsetJack2;
fPosJack3Stage := rReqPosition + rDistanceZ / 2.0 * rReqPitch + rEncoderOffsetJack3;
ELSE
fPosJack1Stage := rReqPosition - rDistanceX / 2.0 * rReqRoll - rDistanceZ / 2.0 * rReqPitch + rEncoderOffsetJack1;
fPosJack2Stage := rReqPosition - rDistanceX / 2.0 * rReqRoll + rDistanceZ / 2.0 * rReqPitch + rEncoderOffsetJack2;
fPosJack3Stage := rReqPosition + rDistanceX / 2.0 * rReqRoll + rEncoderOffsetJack3;
END_IF;
//Calculate actual gap and center from actual stages positions
y1 := stJack1Axis.stAxisStatus.fActPosition;
y2 := stJack2Axis.stAxisStatus.fActPosition;
y3 := stJack3Axis.stAxisStatus.fActPosition;
y12Ave := (y1 + y2) / 2.0;
y12Diff := y2 - y1;
// Compute the actual height of the midpoint of the three jacks.
rActPosition := (y12Ave + y3) / 2;
// Compute the roll and pitch based on the the relative position of the three jacks.
IF bJack3DoesPitch THEN
rActPitch := (y3 - y12Ave) / rDistanceZ;
rActRoll := y12Diff / rDistanceX;
ELSE
rActRoll := (y3 - y12Ave) / rDistanceX;
rActPitch := y12Diff / rDistanceZ;
END_IF;
END_ACTION
ACTION ACT_Motion:
// Instantiate Function block for all the blades
fbJack1Stage(stMotionStage:=stJack1Axis);
fbJack2Stage(stMotionStage:=stJack2Axis);
fbJack3Stage(stMotionStage:=stJack3Axis);
// PTP Motion for each blade
stJack1.sName := 'Jack1';
stJack1.fPosition := fPosJack1Stage;
stJack1.fDelta := fSmallDelta;
stJack1.fVelocity := fMaxVelocity;
stJack1.fAccel := fHighAccel;
stJack1.fDecel := fHighAccel;
stJack2.sName := 'Jack2';
stJack2.fPosition := fPosJack2Stage;
stJack2.fDelta := fSmallDelta;
stJack2.fVelocity := fMaxVelocity;
stJack2.fAccel := fHighAccel;
stJack2.fDecel := fHighAccel;
stJack3.sName := 'Jack3';
stJack3.fPosition := fPosJack3Stage;
stJack3.fDelta := fSmallDelta;
stJack3.fVelocity := fMaxVelocity;
stJack3.fAccel := fHighAccel;
stJack3.fDecel := fHighAccel;
IF (bExecuteMotionX) THEN
fbJack1.bExecute := fbJack2.bExecute := fbJack3.bExecute := bExecuteMotionX;
bExecuteMotionX:= FALSE;
END_IF
fbJack1(
stPositionState:=stJack1,
bMoveOk:=bMoveOk,
stMotionStage:=stJack1Axis);
fbJack2(
stPositionState:=stJack2,
bMoveOk:=bMoveOk,
stMotionStage:=stJack2Axis);
fbJack3(
stPositionState:=stJack3,
bMoveOk:=bMoveOk,
stMotionStage:=stJack3Axis);
END_ACTION
ACTION ACT_VirtualLimitSW:
// Set SafetyReady flags manually
stJack1Axis.bSafetyReady:= TRUE;
stJack2Axis.bSafetyReady:= TRUE;
stJack3Axis.bSafetyReady:= TRUE;
// Note: FB_MotionStage calls FB_SetEnables internally that overwrites .bAllForwardEnable/.bAllBackwardEnable flags
fbPower(
Axis := stJack1Axis.Axis,
Enable := stJack1Axis.bAllEnable,
Enable_Positive := stJack1Axis.bAllForwardEnable AND rActPosition <= rLimitPositionMax AND rActPitch <= rLimitPitchMax,
Enable_Negative := stJack1Axis.bAllBackwardEnable AND rActPosition >= rLimitPositionMin AND rActPitch >= rLimitPitchMin,
Override := 100.000
);
fbPower(
Axis := stJack2Axis.Axis,
Enable := stJack2Axis.bAllEnable,
Enable_Positive := stJack2Axis.bAllForwardEnable AND rActPosition <= rLimitPositionMax AND rActRoll <= rLimitRollMax,
Enable_Negative := stJack2Axis.bAllBackwardEnable AND rActPosition >= rLimitPositionMin AND rActRoll >= rLimitRollMin,
Override := 100.000
);
fbPower(
Axis := stJack3Axis.Axis,
Enable := stJack3Axis.bAllEnable,
Enable_Positive := stJack3Axis.bAllForwardEnable AND rActPosition <= rLimitPositionMax AND rActPitch >= rLimitPitchMin,
Enable_Negative := stJack3Axis.bAllBackwardEnable AND rActPosition >= rLimitPositionMin AND rActPitch <= rLimitPitchMax,
Override := 100.000
);
END_ACTION
FB_AutoRealignCouple
FUNCTION_BLOCK FB_AutoRealignCouple
VAR_IN_OUT
stMotionStageLeader: ST_MotionStage;
stMotionStageFollow: ST_MotionStage;
stAutoRealignCouple: ST_AutoRealignCouple;
END_VAR
VAR
bInit: BOOL := TRUE;
tTimeout: TIME;
tStateTimeout: TIME := T#1s;
tonTimeout: TON;
tonRecouple: TON;
END_VAR
IF bInit THEN
bInit := FALSE;
stAutoRealignCouple.eState := E_RealignCoupleState.STANDBY;
END_IF
stAutoRealignCouple.bCoupled := stMotionStageLeader.Axis.NcToPlc.CoupleState = 1 AND stMotionStageFollow.Axis.NcToPlc.CoupleState = 3;
CASE stAutoRealignCouple.eState OF
E_RealignCoupleState.STANDBY:
IF stAutoRealignCouple.bExecute THEN
stAutoRealignCouple.bExecute := FALSE;
stAutoRealignCouple.eState := E_RealignCoupleState.DECOUPLE_AXES;
END_IF
tonTimeout(IN:=FALSE);
E_RealignCoupleState.DECOUPLE_AXES:
// If the gantry is already coupled, the motor is not busy, and it is enabled, then decouple the axes.
tonTimeout(IN:=TRUE,PT:=tStateTimeout);
IF stAutoRealignCouple.bCoupled AND NOT stMotionStageFollow.bBusy AND NOT stMotionStageFollow.bError THEN
stAutoRealignCouple.bExecuteDecouple := TRUE;
stAutoRealignCouple.eState := E_RealignCoupleState.COMMAND_AXES_INTO_REALIGNMENT;
tonTimeout(IN:=FALSE);
ELSIF NOT stAutoRealignCouple.bCoupled AND NOT stMotionStageFollow.bBusy AND NOT stMotionStageFollow.bError THEN
stAutoRealignCouple.eState := E_RealignCoupleState.COMMAND_AXES_INTO_REALIGNMENT;
tonTimeout(IN:=FALSE);
ELSIF stMotionStageFollow.bError THEN
stAutoRealignCouple.eState := E_RealignCoupleState.ERROR;
stMotionStageFollow.bError := TRUE;
stMotionStageFollow.sCustomErrorMessage := 'Cannot autorealign axes if the follower axis is in an error state.';
ELSIF tonTimeout.Q THEN
stAutoRealignCouple.eState := E_RealignCoupleState.ERROR;
stMotionStageFollow.bError := TRUE;
stMotionStageFollow.sCustomErrorMessage := 'Failed to decouple axes before timeout.';
END_IF
E_RealignCoupleState.COMMAND_AXES_INTO_REALIGNMENT:
// If the gantry is no longer coupled, the motor is not busy, and it is enabled, then reset the decouple command and
// command the follower axis to align with the leader axis.
tonTimeout(IN:=TRUE,PT:=tStateTimeout);
IF NOT stAutoRealignCouple.bCoupled AND NOT stMotionStageFollow.bBusy AND NOT stMotionStageFollow.bError THEN
stAutoRealignCouple.bExecuteDecouple := FALSE;
stMotionStageFollow.fPosition := stMotionStageLeader.Axis.NcToPlc.ActPos;
stMotionStageFollow.fVelocity := stAutoRealignCouple.fRealignSpeed;
stMotionStageFollow.bMoveCmd := TRUE;
stAutoRealignCouple.eState := E_RealignCoupleState.WAIT_FOR_MOVE_START;
tonTimeout(IN:=FALSE);
ELSIF stMotionStageFollow.bError THEN
stAutoRealignCouple.eState := E_RealignCoupleState.ERROR;
stMotionStageFollow.bError := TRUE;
stMotionStageFollow.sCustomErrorMessage := 'Cannot autorealign axes if the follower axis is in an error state.';
ELSIF tonTimeout.Q THEN
stAutoRealignCouple.eState := E_RealignCoupleState.ERROR;
stMotionStageFollow.bError := TRUE;
stMotionStageFollow.sCustomErrorMessage := 'Failed to decouple axes before timeout.';
tonTimeout(IN:=FALSE);
ELSIF NOT stMotionStageFollow.bBusy AND NOT stMotionStageFollow.bError THEN
stAutoRealignCouple.bExecuteDecouple := TRUE;
tonTimeout(IN:=FALSE);
END_IF
E_RealignCoupleState.WAIT_FOR_MOVE_START:
// Wait for the following motor to reach the busy status, then proceed.
tonTimeout(IN:=TRUE,PT:=tStateTimeout);
IF stMotionStageFollow.bBusy THEN
stAutoRealignCouple.eState := E_RealignCoupleState.RECOUPLE_AXES;
// Estimate the amount of time it should take to make the move, use a minimum time of the state timeout time.
tTimeout := LREAL_TO_TIME(abs(stMotionStageFollow.Axis.NcToPlc.ActPos - stMotionStageLeader.Axis.NcToPlc.ActPos) / stAutoRealignCouple.fRealignSpeed * 3.0 * 1000.0);
IF tTimeout < tStateTimeout THEN
tTimeout := tStateTimeout;
END_IF
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
stAutoRealignCouple.eState := E_RealignCoupleState.ERROR;
stMotionStageFollow.bError := TRUE;
stMotionStageFollow.sCustomErrorMessage := 'Failed to start motor move before timeout.';
END_IF
E_RealignCoupleState.RECOUPLE_AXES:
// If the follower motor is no longer busy (finished or errored or stopped) then recouple the follower motor to
// the leader motor.
tonTimeout(IN:=TRUE,PT:=tTimeout + T#250ms);
tonRecouple(IN:=NOT stMotionStageFollow.bBusy AND NOT stMotionStageFollow.bError,PT:=T#250ms);
IF tonRecouple.Q THEN
stAutoRealignCouple.bExecuteCouple := TRUE;
stAutoRealignCouple.eState := E_RealignCoupleState.CLEANUP;
tonRecouple(IN:=FALSE);
ELSIF tonTimeout.Q THEN
stAutoRealignCouple.eState := E_RealignCoupleState.ERROR;
stMotionStageFollow.bError := TRUE;
stMotionStageFollow.sCustomErrorMessage := 'Failed to recouple motors after move.';
tonRecouple(IN:=FALSE);
tonTimeout(IN:=FALSE);
END_IF
E_RealignCoupleState.CLEANUP:
// If the motors are coupled again then reinitialize the bits to
// get them ready for another realignment attempt in the future.
IF stAutoRealignCouple.bCoupled THEN
stAutoRealignCouple.eState := E_RealignCoupleState.STANDBY;
stAutoRealignCouple.bExecute := FALSE;
stAutoRealignCouple.bExecuteDecouple := FALSE;
stAutoRealignCouple.bExecuteCouple := FALSE;
tonTimeout(IN:=FALSE);
ELSE
stAutoRealignCouple.eState := E_RealignCoupleState.RECOUPLE_AXES;
END_IF
E_RealignCoupleState.ERROR:
// Timeout: In case something bad happens, have the timeout timer in place to reset the bits to guarantee we do not get
// stuck in this recoupling and alignment sequence.
IF stAutoRealignCouple.bCoupled OR stMotionStageFollow.bError THEN
stAutoRealignCouple.bExecuteCouple := FALSE;
ELSIF NOT stMotionStageFollow.bBusy THEN
stAutoRealignCouple.bExecuteCouple := TRUE;
END_IF
stAutoRealignCouple.bExecute := FALSE;
stAutoRealignCouple.bExecuteDecouple := FALSE;
stAutoRealignCouple.eState := E_RealignCoupleState.STANDBY;
tonTimeout(IN:=FALSE);
tonRecouple(IN:=FALSE);
END_CASE
END_FUNCTION_BLOCK
- Related:
FB_CoordinateGantryAxisEnable
FUNCTION_BLOCK FB_CoordinateGantryAxisEnable
VAR_IN_OUT
stMotionStageLeader: ST_MotionStage;
stMotionStageFollow: ST_MotionStage;
stCoordinateGantryAxisEnable: ST_CoordinateGantryAxisEnable;
END_VAR
VAR
bInit: BOOL := TRUE;
rtExecuteMove: R_TRIG;
tonTimeout: TON;
bMovePos: BOOL;
bMoveNeg: BOOL;
bMovePosAndOK: BOOL;
bMoveNegAndOK: BOOL;
END_VAR
IF bInit THEN
bInit := FALSE;
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.STANDBY;
END_IF
CASE stCoordinateGantryAxisEnable.eState OF
E_GantryEnableState.STANDBY:
IF stMotionStageLeader.bExecute AND stCoordinateGantryAxisEnable.bEnable THEN
rtExecuteMove(CLK:=FALSE);
stMotionStageLeader.bExecute := FALSE;
stMotionStageLeader.bEnable := TRUE;
stMotionStageFollow.bEnable := TRUE;
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.WAIT_FOR_FULLY_ENABLED;
ELSIF stMotionStageLeader.bExecute AND NOT stCoordinateGantryAxisEnable.bEnable THEN
stMotionStageLeader.bExecute := FALSE;
stMotionStageLeader.bError := TRUE;
stMotionStageFollow.bError := TRUE;
stMotionStageLeader.sCustomErrorMessage := 'Move Failed. The gantry is currently disabled.';
stMotionStageFollow.sCustomErrorMessage := stMotionStageLeader.sCustomErrorMessage;
ELSE
stMotionStageLeader.bEnable := FALSE;
stMotionStageFollow.bEnable := FALSE;
END_IF
tonTimeout(IN:=FALSE);
E_GantryEnableState.WAIT_FOR_FULLY_ENABLED:
// Wait until the logic confirms both motors are fully enabled
tonTimeout(IN:=NOT stMotionStageLeader.bAllEnable OR NOT stMotionStageFollow.bAllEnable,PT:=stCoordinateGantryAxisEnable.tTimeout);
// Check which direction we are moving, and if the directional enables will allow that move.
bMovePos := stMotionStageLeader.stAxisStatus.fActPosition < stMotionStageLeader.fPosition;
bMoveNeg := stMotionStageLeader.stAxisStatus.fActPosition > stMotionStageLeader.fPosition;
bMovePosAndOK := bMovePos AND stMotionStageLeader.bAllForwardEnable AND stMotionStageFollow.bAllForwardEnable;
bMoveNegAndOK := bMoveNeg AND stMotionStageLeader.bAllBackwardEnable AND stMotionStageFollow.bAllBackwardEnable;
// Check for fully enabled
IF stMotionStageLeader.bAllEnable AND stMotionStageFollow.bAllEnable THEN
// Check direction of motion and if that direction is enabled
IF bMovePosAndOK OR bMoveNegAndOK THEN
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.WAIT_FOR_READY_STATE;
tonTimeout(IN:=FALSE);
ELSE
// Direction of motion is not enabled, so let's report the error and give a helpful error message.
stMotionStageLeader.bError := TRUE;
stMotionStageFollow.bError := TRUE;
IF bMovePos THEN
IF NOT stMotionStageLeader.bLimitForwardEnable THEN
stMotionStageLeader.sCustomErrorMessage := 'Move forward failed due to forward limit switch.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move forward failed due to forward limit switch for ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bLimitForwardEnable THEN
stMotionStageFollow.sCustomErrorMessage := 'Move forward failed due to forward limit switch.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move forward failed due to forward limit switch for ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.bGantryForwardEnable AND stMotionStageLeader.bGantryAxis THEN
stMotionStageLeader.sCustomErrorMessage := 'Move forward failed due to gantry motion tolerance limit.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move forward failed due to gantry motion tolerance limit for ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bGantryForwardEnable AND stMotionStageFollow.bGantryAxis THEN
stMotionStageFollow.sCustomErrorMessage := 'Move forward failed due to gantry motion tolerance limit.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move forward failed due to gantry motion tolerance limit for ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.stEPSForwardEnable.bEPS_OK THEN
stMotionStageLeader.sCustomErrorMessage := 'Move forward failed due to EPS condition.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move forward failed due to EPS condition for ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.stEPSForwardEnable.bEPS_OK THEN
stMotionStageFollow.sCustomErrorMessage := 'Move forward failed due to EPS condition.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move forward failed due to EPS condition for ',stMotionStageFollow.sName);
ELSE
stMotionStageFollow.sCustomErrorMessage := 'Move forward failed for unspecified reason.';
stMotionStageLeader.sCustomErrorMessage := 'Move forward failed for unspecified reason.';
END_IF
ELSIF bMoveNeg THEN
IF NOT stMotionStageLeader.bLimitBackwardEnable THEN
stMotionStageLeader.sCustomErrorMessage := 'Move backward failed due to forward limit switch.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move backward failed due to forward limit switch for ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bLimitBackwardEnable THEN
stMotionStageFollow.sCustomErrorMessage := 'Move backward failed due to forward limit switch.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move backward failed due to forward limit switch for ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.bGantryBackwardEnable AND stMotionStageLeader.bGantryAxis THEN
stMotionStageLeader.sCustomErrorMessage := 'Move backward failed due to gantry motion tolerance limit.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move backward failed due to gantry motion tolerance limit for ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bGantryBackwardEnable AND stMotionStageFollow.bGantryAxis THEN
stMotionStageFollow.sCustomErrorMessage := 'Move backward failed due to gantry motion tolerance limit.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move backward failed due to gantry motion tolerance limit for ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.stEPSBackwardEnable.bEPS_OK THEN
stMotionStageLeader.sCustomErrorMessage := 'Move backward failed due to EPS condition.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move backward failed due to EPS condition for ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.stEPSBackwardEnable.bEPS_OK THEN
stMotionStageFollow.sCustomErrorMessage := 'Move backward failed due to EPS condition.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move backward failed due to EPS condition for ',stMotionStageFollow.sName);
ELSE
stMotionStageLeader.sCustomErrorMessage := 'Move backward failed for unspecified reason.';
stMotionStageFollow.sCustomErrorMessage := 'Move backward failed for unspecified reason.';
END_IF
ELSE
stMotionStageLeader.sCustomErrorMessage := 'Move failed because it was not large enough to register.';
stMotionStageFollow.sCustomErrorMessage := 'Move failed because it was not large enough to register.';
END_IF
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.ERROR;
END_IF
ELSIF tonTimeout.Q THEN
stMotionStageLeader.bError := TRUE;
stMotionStageFollow.bError := TRUE;
IF NOT stMotionStageLeader.bEnable THEN
stMotionStageLeader.sCustomErrorMessage := 'Move never enabled because bEnable never received.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move never enabled because bEnable never received on ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bEnable THEN
stMotionStageFollow.sCustomErrorMessage := 'Move never enabled because bEnable never received.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move never enabled because bEnable never received on ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.bHardwareEnable THEN
stMotionStageLeader.sCustomErrorMessage := 'Move never enabled because bHardwareEnable set false.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move never enabled because bHardwareEnable set false on ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bHardwareEnable THEN
stMotionStageFollow.sCustomErrorMessage := 'Move never enabled because bHardwareEnable set false.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move never enabled because bHardwareEnable set false on ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.stEPSPowerEnable.bEPS_OK THEN
stMotionStageLeader.sCustomErrorMessage := 'Move never enabled because of EPS condition.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move never enabled because of EPS condition on ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.stEPSPowerEnable.bEPS_OK THEN
stMotionStageFollow.sCustomErrorMessage := 'Move never enabled because of EPS condition.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move never enabled because of EPS condition on ',stMotionStageFollow.sName);
ELSIF NOT stMotionStageLeader.bUserEnable THEN
stMotionStageLeader.sCustomErrorMessage := 'Move never enabled because user enable is set to false.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move never enabled because user enable is set to false on ',stMotionStageLeader.sName);
ELSIF NOT stMotionStageFollow.bUserEnable THEN
stMotionStageFollow.sCustomErrorMessage := 'Move never enabled because user enable is set to false.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move never enabled because user enable is set to false on ',stMotionStageFollow.sName);
ELSE
stMotionStageLeader.sCustomErrorMessage := 'Move never enabled in the timeout duration.';
stMotionStageFollow.sCustomErrorMessage := 'Move never enabled in the timeout duration.';
END_IF
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.ERROR;
END_IF
E_GantryEnableState.WAIT_FOR_READY_STATE:
// Wait for the motor drives to switch to the ready state before commanding the move.
tonTimeout(IN:=NOT stCoordinateGantryAxisEnable.bLeaderDriveReady OR NOT stCoordinateGantryAxisEnable.bFollowDriveReady,PT:=stCoordinateGantryAxisEnable.tTimeout);
// Are both drives reporting that they are ready?
IF stCoordinateGantryAxisEnable.bLeaderDriveReady AND stCoordinateGantryAxisEnable.bFollowDriveReady THEN
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.DELAY_FOR_MOVE_START;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
stMotionStageLeader.bError := TRUE;
stMotionStageFollow.bError := TRUE;
IF NOT stCoordinateGantryAxisEnable.bLeaderDriveReady THEN
stMotionStageLeader.sCustomErrorMessage := 'Move failed because motor drive did not get ready status before timeout.';
stMotionStageFollow.sCustomErrorMessage := CONCAT('Move failed because ',CONCAT(stMotionStageLeader.sName,' drive did not get ready status before timeout.'));
ELSE
stMotionStageFollow.sCustomErrorMessage := 'Move failed because motor drive did not get ready status before timeout.';
stMotionStageLeader.sCustomErrorMessage := CONCAT('Move failed because ',CONCAT(stMotionStageFollow.sName,' did not get ready status before timeout.'));
END_IF
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.ERROR;
END_IF
E_GantryEnableState.DELAY_FOR_MOVE_START:
// Allow the move execution to begin
rtExecuteMove(CLK:=TRUE);
IF rtExecuteMove.Q THEN
stMotionStageLeader.bExecute := TRUE;
END_IF
tonTimeout(IN:=NOT stMotionStageLeader.bBusy,PT:=T#100ms);
IF tonTimeout.Q THEN
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.STANDBY;
END_IF
E_GantryEnableState.ERROR:
// Error handling
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.STANDBY;
ELSE
stCoordinateGantryAxisEnable.eState := E_GantryEnableState.ERROR;
END_CASE
END_FUNCTION_BLOCK
FB_MeasureReferenceVelocity
FUNCTION_BLOCK FB_MeasureReferenceVelocity
VAR_IN_OUT
stMotionStage: ST_MotionStage;
END_VAR
VAR_INPUT
nMicrostepsPerStep: UINT := 64; // 64 for Beckhoff stepper drives EL7047, EL7037, EL7041, EL7031
END_VAR
VAR_OUTPUT
bBusy: BOOL;
fMeasuredReferenceVelocity: LREAL;
END_VAR
VAR
nState: INT := -1;
fbReadDriveAddress: MC_ReadDriveAddress;
nMicrostepCounter AT %I*: UINT; // In microsteps (64 microsteps per step)
bCounterOverflow AT %I*: BOOL;
bCounterUnderflow AT %I*: BOOL;
nMaxStepFrequencyBuffer: BYTE; // An enum corresponding to max step frequency
fbReadCoEMaxStepFrequency: FB_ReadCoE;
nMaxStepFrequency: UINT; // Fullsteps per Second
fStartingInternalPosition: DINT;
fEndingInternalPosition: DINT;
fStartingEUPosition: LREAL;
fEndingEUPosition: LREAL;
fDeltaEUPosition: LREAL;
fDeltaInternalPosition: LREAL;
fDeltaSteps: LREAL;
fEUPerStep: LREAL;
rtOverflow: R_TRIG;
rtUnderflow: R_TRIG;
nDriveAddressReadState: INT := -1;
bDriveAddressValid: BOOL;
END_VAR
CASE nDriveAddressReadState OF
-1:
IF NOT bDriveAddressValid THEN
nDriveAddressReadState := nDriveAddressReadState + 1;
fbReadDriveAddress(
Axis:=stMotionStage.Axis,
Execute:=FALSE
);
END_IF
0: // Read drive address
fbReadDriveAddress(
Axis:=stMotionStage.Axis,
Execute:=TRUE
);
IF NOT fbReadDriveAddress.Busy THEN
nDriveAddressReadState := -1;
bDriveAddressValid := TRUE;
ELSIF fbReadDriveAddress.Error THEN
nDriveAddressReadState := 100;
END_IF
100:
nDriveAddressReadState := -1;
END_CASE
CASE nState OF
-1:
IF stMotionStage.bBusy AND bDriveAddressValid THEN
nState := 0;
fStartingInternalPosition := 0;
fEndingInternalPosition := 0;
rtOverflow(CLK:=bCounterOverflow);
rtUnderflow(CLK:=bCounterUnderflow);
fbReadCoEMaxStepFrequency(
bExecute:=FALSE,
pBuffer:=ADR(nMaxStepFrequencyBuffer)
);
ELSIF NOT bDriveAddressValid THEN
nState := 100;
END_IF
0: // A move has started so read the starting position information
fbReadCoEMaxStepFrequency(
bExecute:=TRUE,
pBuffer:=ADR(nMaxStepFrequencyBuffer),
sNetId:=stMotionStage.Axis.DriveAddress.NetID,
nSlaveAddr:=stMotionStage.Axis.DriveAddress.SlaveAddress,
nDesiredCoEIndex:=16#8012,
nDesiredCoESubIndex:=5,
nBufferLength:=1,
bBusy=>,
bError=>
);
IF fbReadCoEMaxStepFrequency.bBusy THEN
nState := nState + 1;
ELSIF fbReadCoEMaxStepFrequency.bError THEN
nState := 100;
END_IF
1:
fbReadCoEMaxStepFrequency(
bExecute:=TRUE,
pBuffer:=ADR(nMaxStepFrequencyBuffer),
sNetId:=stMotionStage.Axis.DriveAddress.NetID,
nSlaveAddr:=stMotionStage.Axis.DriveAddress.SlaveAddress,
nDesiredCoEIndex:=16#8012,
nDesiredCoESubIndex:=5,
nBufferLength:=1,
bBusy=>,
bError=>
);
IF NOT fbReadCoEMaxStepFrequency.bBusy THEN
nState := nState + 1;
fStartingEUPosition := stMotionStage.stAxisStatus.fActPosition;
fStartingInternalPosition := nMicrostepCounter;
CASE nMaxStepFrequencyBuffer OF
0:
nMaxStepFrequency := 1000;
1:
nMaxStepFrequency := 2000;
2:
nMaxStepFrequency := 4000;
3:
nMaxStepFrequency := 8000;
4:
nMaxStepFrequency := 16000;
5:
nMaxStepFrequency := 32000;
END_CASE
ELSIF fbReadCoEMaxStepFrequency.bError THEN
nState := 100;
END_IF
2: // Wait until the move ends and then read the last position information
rtOverflow(CLK:=bCounterOverflow);
rtUnderflow(CLK:=bCounterUnderflow);
IF rtOverflow.Q THEN
fEndingInternalPosition := fEndingInternalPosition + 65535;
ELSIF rtUnderflow.Q THEN
fEndingInternalPosition := fEndingInternalPosition - 65535;
END_IF
IF NOT stMotionStage.bBusy THEN
nState := nState + 1;
fEndingEUPosition := stMotionStage.stAxisStatus.fActPosition;
fEndingInternalPosition := fEndingInternalPosition + nMicrostepCounter;
END_IF
3: // Calculate the reference velocity from the measured move
fDeltaEUPosition := fEndingEUPosition - fStartingEUPosition;
fDeltaInternalPosition := fEndingInternalPosition - fStartingInternalPosition;
fDeltaSteps := fDeltaInternalPosition / UINT_TO_LREAL(nMicrostepsPerStep);
IF fDeltaSteps <> 0 THEN
fEUPerStep := fDeltaEUPosition / fDeltaSteps;
fMeasuredReferenceVelocity := UINT_TO_LREAL(nMaxStepFrequency) * fEUPerStep;
nState := -1;
ELSE
nState := 100;
fMeasuredReferenceVelocity := 0;
END_IF
100: // Reserve for error handling
nState := -1;
END_CASE
END_FUNCTION_BLOCK
- Related:
FB_MultiLeaderMotionCoupling
FUNCTION_BLOCK FB_MultiLeaderMotionCoupling
VAR_IN_OUT
stMotionStageFollow: ST_MotionStage;
stMultiLeaderMotionCoupling: ST_MultiLeaderMotionCoupling;
END_VAR
VAR_INPUT
fTargetPos : LREAL;
fTargetVel : LREAL;
fTargetAcc : LREAL;
bError : BOOL;
tTimeout : TIME := T#5s;
END_VAR
VAR
bInit : BOOL := TRUE;
nTimeout : ULINT;
tonTimeout : TON;
tonHaltDelay : TON;
tonTargetPositionTimer: TON;
fMoveTime : LREAL; // in seconds
tMove : TIME;
tonMoveProgressCounter : TON;
GetCurrentTaskIndex : GETCURTASKINDEX;
fSecPerScan : LREAL;
fDistanceThisScan : LREAL;
fPosSetpoint : LREAL;
EnableSetPointGenerator : MC_ExtSetPointGenEnable;
DisableSetPointGenerator : MC_ExtSetPointGenDisable;
mcHalt : MC_Halt;
END_VAR
// During initialization or reset, restore state values to defaults.
IF bInit OR stMultiLeaderMotionCoupling.bReset THEN
bInit := FALSE;
stMotionStageFollow.bReset := TRUE;
stMultiLeaderMotionCoupling.bReset := FALSE;
stMultiLeaderMotionCoupling.bBusy := FALSE;
stMultiLeaderMotionCoupling.sStatusMsg := '';
stMultiLeaderMotionCoupling.bError := FALSE;
stMultiLeaderMotionCoupling.sErrorMsg := '';
stMultiLeaderMotionCoupling.sErrorHelp := '';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_STANDBY;
METH_Init(
stMotionStage := stMotionStageFollow,
stMultiLeader := stMultiLeaderMotionCoupling,
bEnableByDefault := stMultiLeaderMotionCoupling.bEnable,
bUseDynamicLimits := stMultiLeaderMotionCoupling.bUseDynamicLimits
);
END_IF
stMultiLeaderMotionCoupling.fTargetPos := fTargetPos;
stMultiLeaderMotionCoupling.fTargetVel := ABS(fTargetVel);
stMultiLeaderMotionCoupling.fTargetAcc := fTargetAcc;
stMultiLeaderMotionCoupling.fPosDelta := stMultiLeaderMotionCoupling.stPos.fOut - stMotionStageFollow.Axis.NcToPlc.ActPos;
nTimeout := TIME_TO_ULINT(tTimeout - tonTimeout.ET); // Get the remaining timeout time in ms.
CASE stMultiLeaderMotionCoupling.eState OF
E_MultiLeaderMotionCoupling.STATE_STANDBY:
// Wait here for multi-leader mode to be enabled.
// If we get here and there is an active error, request the user to clear the error.
ACT_01_STATE_STANDBY();
E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_NOT_BUSY:
// The external setpoint generator cannot be activated when the axis is currently busy.
// Give the duration of the timeout to wait for the axis to stop being busy.
// If the timeout duration expires, give a helpful error message and give up.
ACT_02_STATE_WAIT_FOR_FOLLOWER_NOT_BUSY();
E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_ENABLED:
// The external setpoint generator cannot be activated until the axis is enabled.
// Give the duration of the timeout to wait for the axis to be enabled.
// If the timeout duration expires, give a helpful error message and give up.
ACT_03_STATE_WAIT_FOR_FOLLOWER_ENABLED();
E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_READY:
// The external setpoint generator cannot be activated until the motor drive ready status is received.
// Give the duration of the timeout to wait for the motor drive to report its ready status.
// If the timeout duration expires, give a helpful error message and give up.
ACT_04_STATE_WAIT_FOR_FOLLOWER_READY();
E_MultiLeaderMotionCoupling.STATE_ENABLE_EXTERNAL_SETPOINT_GENERATOR:
// With all the prerequisites satisfied, we can attempt to enable the external setpoint generator.
// Give the duration of the timeout to wait for the external setpoint generator to enable.
// If the timeout duration expires, give a helpful error message and give up.
ACT_05_STATE_ENABLE_EXTERNAL_SETPOINT_GENERATOR();
E_MultiLeaderMotionCoupling.STATE_FOLLOWING_ACTIVE:
// The external setpoint generator allows us to feed arbitrary setpoint, velocity, acceleration, and direction
// commands to the follower axis. This is the backbone of the multi-leader control logic.
// The position and velocity inputs will be able to be used to control the position of an axis based on the
// positions of one or more other axes by feeding the desired position output of an arbitrary function or
// multi-dimensional lookup table into this function block.
// In this state, any other position commands to the follower axis will be overwritten every time
// this function block is executed. The function block can be disabled to allow manual commands to
// be used by this axis.
// On a manual disable command or an error in the follower axis, this state will move into the
// disable external setpoint generator state or the error state respectively.
ACT_06_STATE_FOLLOWING_ACTIVE();
E_MultiLeaderMotionCoupling.STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR:
// Attempt to disable the external setpoint generator.
// Give the duration of the timeout to wait for the external setpoint generator to disable.
// If the timeout duration expires, give a helpful error message and give up.
ACT_07_STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR();
E_MultiLeaderMotionCoupling.STATE_ERROR:
// Allow a state for handling errors.
// First, try to disable the external setpoint generator.
// If the timeout duration expires, give a helpful error message and give up.
ACT_08_STATE_ERROR();
E_MultiLeaderMotionCoupling.STATE_MONITOR_TARGET_WINDOW:
// Check to see if the axis leaves the target window. If it does,
// switch back to enabling the external setpoint generator.
ACT_09_STATE_MONITOR_TARGET_WINDOW();
E_MultiLeaderMotionCoupling.STATE_HALT_MOTION:
// Stop the motion on the axis so that it doesn't sit there using full current
// when it can be using holding current.
ACT_10_STATE_HALT_MOTION();
ELSE
// Catch-all for invalid states. Report the number of the invalid state and move into error state.
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := CONCAT('Attempted to enter invalid state with ID: ', UDINT_TO_STRING(stMultiLeaderMotionCoupling.eState));
stMultiLeaderMotionCoupling.sErrorHelp := 'Contact ECS to investigate.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
END_CASE
END_FUNCTION_BLOCK
ACTION ACT_01_STATE_STANDBY:
(* ACTION - STATE STANDBY *)
stMotionStageFollow.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
IF stMultiLeaderMotionCoupling.bEnable AND NOT stMultiLeaderMotionCoupling.bError THEN
stMultiLeaderMotionCoupling.bBusy := TRUE;
stMultiLeaderMotionCoupling.sStatusMsg := 'Moving from STANDBY to WAIT_FOR_FOLLOWER_NOT_BUSY.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_NOT_BUSY;
ELSIF NOT stMultiLeaderMotionCoupling.bEnable THEN
stMultiLeaderMotionCoupling.bBusy := FALSE;
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting to be enabled.';
ELSIF stMultiLeaderMotionCoupling.bError THEN
stMultiLeaderMotionCoupling.bBusy := FALSE;
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for reset button to be pressed to clear the error.';
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'STATE_STANDBY: Unexpected condition.';
END_IF
END_ACTION
ACTION ACT_02_STATE_WAIT_FOR_FOLLOWER_NOT_BUSY:
(* ACTION - STATE WAIT FOR FOLLOWER NOT BUSY *)
tonTimeout(IN:=TRUE,PT:=tTimeout);
IF NOT stMotionStageFollow.bBusy THEN
stMultiLeaderMotionCoupling.sStatusMsg := 'Moving from WAIT_FOR_FOLLOWER_NOT_BUSY to WAIT_FOR_FOLLOWER_ENABLED.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_ENABLED;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_WAIT_FOR_FOLLOWER_NOT_BUSY.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Make sure the axis is not busy before enabling multi-leader mode.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for follower to not be busy.';
END_IF
END_ACTION
ACTION ACT_03_STATE_WAIT_FOR_FOLLOWER_ENABLED:
(* ACTION - STATE WAIT FOR FOLLOWER ENABLED *)
tonTimeout(IN:=TRUE,PT:=tTimeout);
stMotionStageFollow.nEnableMode := ENUM_StageEnableMode.NEVER;
stMotionStageFollow.bEnable := TRUE;
// NEED TO ADD IN DIRECTIONAL ENABLES TOO.
// Make function that handles directional enables and gives proper error messages for them.
IF stMotionStageFollow.bAllEnable THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_READY;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_WAIT_FOR_FOLLOWER_ENABLED.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Check to see what condition is preventing the axis from enabling.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for follower to be enabled.';
END_IF
END_ACTION
ACTION ACT_04_STATE_WAIT_FOR_FOLLOWER_READY:
(* ACTION - STATE WAIT FOR FOLLOWER READY *)
tonTimeout(IN:=TRUE,PT:=tTimeout);
IF stMultiLeaderMotionCoupling.bFollowerDriveReady THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ENABLE_EXTERNAL_SETPOINT_GENERATOR;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_WAIT_FOR_FOLLOWER_READY.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Something is preventing the motor drive from giving the ready status.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for follower drive to give ready status.';
END_IF
END_ACTION
ACTION ACT_05_STATE_ENABLE_EXTERNAL_SETPOINT_GENERATOR:
(* ACTION - STATE ENABLE EXTERNAL SETPOINT GENERATOR *)
ACT_ComputeTargets();
tonTimeout(IN:=TRUE,PT:=tTimeout);
MC_ExtSetPointGenFeed(
Position := stMotionStageFollow.Axis.NcToPlc.ActPos,
Velocity := 0.0,
Acceleration := 0.0,
Direction := 1,
Axis := stMotionStageFollow.Axis
);
EnableSetPointGenerator(
Axis := stMotionStageFollow.Axis,
Execute := TRUE,
PositionType := E_PositionType.POSITIONTYPE_ABSOLUTE,
Done=>,
Busy=>,
Error=>,
ErrorID=>,
Enabled=>
);
// See: https://infosys.beckhoff.com/english.php?content=../content/1033/tcplclib_tc2_mc2/70136971.html&id=
// StateDWord.14 gives a boolean for the enable/disable state of the external setpoint generator.
// TRUE = ENABLED. FALSE = DISABLED.
IF stMotionStageFollow.Axis.NcToPlc.StateDWord.14 THEN
EnableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_FOLLOWING_ACTIVE;
tonTimeout(IN:=FALSE);
ELSIF EnableSetPointGenerator.Error THEN
EnableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := CONCAT('EnableSetPointGenerator returned error code: ', UDINT_TO_STRING(EnableSetPointGenerator.ErrorID));
CASE EnableSetPointGenerator.ErrorID OF
ELSE
stMultiLeaderMotionCoupling.sErrorHelp := 'This error code is missing explanation from Beckhoff.';
END_CASE
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
EnableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_ENABLE_EXTERNAL_SETPOINT_GENERATOR.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Failed to enable setpoint generator. Check enable and ready statuses.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for setpoint generator to be enabled.';
END_IF
END_ACTION
ACTION ACT_06_STATE_FOLLOWING_ACTIVE:
(* ACTION - STATE FOLLOWING ACTIVE *)
ACT_ComputeTargets();
stMultiLeaderMotionCoupling.fPosDelta := stMultiLeaderMotionCoupling.stPos.fOut - stMotionStageFollow.Axis.NcToPlc.ActPos;
IF stMultiLeaderMotionCoupling.stVel.fOut <> 0 THEN
fMoveTime := ABS(stMultiLeaderMotionCoupling.fPosDelta / stMultiLeaderMotionCoupling.stVel.fOut);
ELSE
fMoveTime := 0;
END_IF
tMove := LREAL_TO_TIME(fMoveTime * 1000);
// Determine the direction in which the axis is being commanded.
IF stMultiLeaderMotionCoupling.stVel.fOut = 0 AND stMultiLeaderMotionCoupling.stAcc.fOut = 0 THEN
stMultiLeaderMotionCoupling.nDir := 0; (* stand still *)
ELSIF stMultiLeaderMotionCoupling.stPos.fOut >= stMotionStageFollow.Axis.NcToPlc.ActPos THEN
stMultiLeaderMotionCoupling.nDir := 1; (* positive motion *)
ELSE
stMultiLeaderMotionCoupling.nDir := -1; (* negative motion *)
END_IF
stMultiLeaderMotionCoupling.stVel.fOut := stMultiLeaderMotionCoupling.stVel.fOut * stMultiLeaderMotionCoupling.nDir;
IF ABS(stMultiLeaderMotionCoupling.fPosDelta) < stMotionStageFollow.stAxisParameters.fTargetPosControlRange THEN
tonTargetPositionTimer(IN:=TRUE,PT:=LREAL_TO_TIME(stMotionStageFollow.stAxisParameters.fTargetPosControlTime * 1000));
ELSE
tonTargetPositionTimer(IN:=FALSE);
END_IF
IF NOT stMultiLeaderMotionCoupling.bEnable THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR;
ELSIF stMotionStageFollow.bError THEN
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'MultiLeaderMotionCoupling suspended due to follower axis error.';
stMultiLeaderMotionCoupling.sErrorHelp := stMotionStageFollow.sErrorMessage;
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := CONCAT('Actively controlling [', CONCAT(stMotionStageFollow.sName,'] in multi-leader mode.'));
IF NOT tonTargetPositionTimer.Q THEN
// Feed the dynamic parameters into the external setpoint generator interface of the follower axis
IF GetCurrentTaskIndex.index = 0 THEN
(* get cycle time *)
GetCurrentTaskIndex();
END_IF
fSecPerScan := UDINT_TO_LREAL(TwinCAT_SystemInfoVarList._TaskInfo[GetCurrentTaskIndex.index].cycleTime) / 1E7;
IF fMoveTime <> 0 THEN
fDistanceThisScan := stMultiLeaderMotionCoupling.fPosDelta / fMoveTime * stMultiLeaderMotionCoupling.nDir * fSecPerScan;
ELSE
fDistanceThisScan := 0;
END_IF
fPosSetpoint := stMotionStageFollow.Axis.NcToPlc.ActPos + fDistanceThisScan;
IF WORD_TO_BOOL(stMotionStageFollow.stAxisParameters.bEncEnableSoftEndMaxControl) AND fPosSetpoint > stMotionStageFollow.stAxisParameters.fEncSoftEndMax THEN
fPosSetpoint := stMotionStageFollow.stAxisParameters.fEncSoftEndMax;
ELSIF WORD_TO_BOOL(stMotionStageFollow.stAxisParameters.bEncEnableSoftEndMinControl) AND fPosSetpoint < stMotionStageFollow.stAxisParameters.fEncSoftEndMin THEN
fPosSetpoint := stMotionStageFollow.stAxisParameters.fEncSoftEndMin;
END_IF
MC_ExtSetPointGenFeed(
Position := fPosSetpoint,
Velocity := stMultiLeaderMotionCoupling.stVel.fOut,
Acceleration := stMultiLeaderMotionCoupling.stAcc.fOut,
Direction := stMultiLeaderMotionCoupling.nDir,
Axis := stMotionStageFollow.Axis
);
tonTimeout(IN:=FALSE);
tonHaltDelay(IN:=FALSE);
ELSE
MC_ExtSetPointGenFeed(
Position := stMultiLeaderMotionCoupling.stPos.fOut, // stMotionStageFollow.Axis.NcToPlc.ActPos,
Velocity := stMultiLeaderMotionCoupling.fPosDelta * stMultiLeaderMotionCoupling.fTargetTrackingGain, // 0,
Acceleration := stMultiLeaderMotionCoupling.stAcc.fOut,
Direction := stMultiLeaderMotionCoupling.nDir,
Axis := stMotionStageFollow.Axis
);
tonHaltDelay(IN:=TRUE,PT:=tTimeout);
IF tonHaltDelay.Q THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_HALT_MOTION;
tonHaltDelay(IN:=FALSE);
END_IF
END_IF
END_IF
END_ACTION
ACTION ACT_07_STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR:
(* ACTION - STATE DISABLE EXTERNAL SETPOINT GENERATOR *)
tonTimeout(IN:=TRUE,PT:=tTimeout);
DisableSetPointGenerator(
Axis := stMotionStageFollow.Axis,
Execute := TRUE,
Done=>,
Busy=>,
Error=>,
ErrorID=>,
Enabled=>
);
stMotionStageFollow.bEnable := FALSE;
// See: https://infosys.beckhoff.com/english.php?content=../content/1033/tcplclib_tc2_mc2/70136971.html&id=
// StateDWord.14 gives a boolean for the enable/disable state of the external setpoint generator.
// TRUE = ENABLED. FALSE = DISABLED.
IF NOT stMotionStageFollow.Axis.NcToPlc.StateDWord.14 THEN
DisableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_STANDBY;
tonTimeout(IN:=FALSE);
ELSIF DisableSetPointGenerator.Error THEN
DisableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := CONCAT('DisableSetPointGenerator returned error code: ', UDINT_TO_STRING(DisableSetPointGenerator.ErrorID));
CASE DisableSetPointGenerator.ErrorID OF
ELSE
stMultiLeaderMotionCoupling.sErrorHelp := 'This error code is missing explanation from Beckhoff.';
END_CASE
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
DisableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Failed to disable setpoint generator.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for setpoint generator to be disabled.';
END_IF
END_ACTION
ACTION ACT_08_STATE_ERROR:
(* ACTION - STATE ERROR *)
tonTimeout(IN:=TRUE,PT:=tTimeout);
stMultiLeaderMotionCoupling.bBusy := FALSE;
DisableSetPointGenerator(
Axis := stMotionStageFollow.Axis,
Execute := TRUE,
Done=>,
Busy=>,
Error=>,
ErrorID=>,
Enabled=>
);
stMotionStageFollow.bEnable := FALSE;
// See: https://infosys.beckhoff.com/english.php?content=../content/1033/tcplclib_tc2_mc2/70136971.html&id=
// StateDWord.14 gives a boolean for the enable/disable state of the external setpoint generator.
// TRUE = ENABLED. FALSE = DISABLED.
IF NOT stMotionStageFollow.Axis.NcToPlc.StateDWord.14 THEN
DisableSetPointGenerator(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE
);
tonTimeout(IN:=FALSE);
IF NOT stMultiLeaderMotionCoupling.bError THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_STANDBY;
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting for reset button to be pressed to clear error.';
END_IF
ELSIF tonTimeout.Q THEN
// Timeout would occur if the disable setpoint generator function block failed to complete.
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_ERROR.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Failed to disable setpoint generator.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Waiting setpoint generator to be disabled [STATE_ERROR].';
END_IF
END_ACTION
ACTION ACT_09_STATE_MONITOR_TARGET_WINDOW:
(* ACTION - STATE MONITOR TARGET WINDOW *)
ACT_ComputeTargets();
stMotionStageFollow.bEnable := FALSE;
stMultiLeaderMotionCoupling.fPosDelta := stMultiLeaderMotionCoupling.stPos.fOut - stMultiLeaderMotionCoupling.fHaltPos;
IF NOT stMultiLeaderMotionCoupling.bEnable THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_STANDBY;
ELSIF ABS(stMultiLeaderMotionCoupling.fPosDelta) >= stMotionStageFollow.stAxisParameters.fTargetPosControlRange THEN
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_WAIT_FOR_FOLLOWER_ENABLED;
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Holding mode active. Monitoring target position window for changes.';
END_IF
END_ACTION
ACTION ACT_10_STATE_HALT_MOTION:
(* ACTION - STATE HALT MOTION *)
// Halt and switch to waiting state.
tonTimeout(IN:=TRUE,PT:=tTimeout);
mcHalt(
Axis:=stMotionStageFollow.Axis,
Execute:=TRUE,
Deceleration:=stMultiLeaderMotionCoupling.stAcc.fOut,
BufferMode:=MC_BufferMode.MC_Aborting,
Done=>,
Busy=>,
Active=>,
CommandAborted=>,
Error=>,
ErrorID=>
);
IF NOT stMultiLeaderMotionCoupling.bEnable THEN
mcHalt(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE,
Deceleration:=stMultiLeaderMotionCoupling.stAcc.fOut,
BufferMode:=MC_BufferMode.MC_Aborting
);
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_DISABLE_EXTERNAL_SETPOINT_GENERATOR;
ELSIF mcHalt.Done THEN
mcHalt(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE,
Deceleration:=stMultiLeaderMotionCoupling.stAcc.fOut,
BufferMode:=MC_BufferMode.MC_Aborting
);
stMultiLeaderMotionCoupling.fHaltPos := stMotionStageFollow.Axis.NcToPlc.ActPos;
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_MONITOR_TARGET_WINDOW;
tonTimeout(IN:=FALSE);
ELSIF mcHalt.Error THEN
mcHalt(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE,
Deceleration:=stMultiLeaderMotionCoupling.stAcc.fOut,
BufferMode:=MC_BufferMode.MC_Aborting
);
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := CONCAT('MC_HALT returned error code: ', UDINT_TO_STRING(mcHalt.ErrorID));
CASE DisableSetPointGenerator.ErrorID OF
ELSE
stMultiLeaderMotionCoupling.sErrorHelp := 'This error code is missing explanation from Beckhoff.';
END_CASE
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSIF tonTimeout.Q THEN
mcHalt(
Axis:=stMotionStageFollow.Axis,
Execute:=FALSE,
Deceleration:=stMultiLeaderMotionCoupling.stAcc.fOut,
BufferMode:=MC_BufferMode.MC_Aborting
);
stMultiLeaderMotionCoupling.bError := TRUE;
stMultiLeaderMotionCoupling.sErrorMsg := 'Timeout occured during STATE_HALT_MOTION.';
stMultiLeaderMotionCoupling.sErrorHelp := 'Failed to halt the axis within the timeout duration.';
stMultiLeaderMotionCoupling.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
tonTimeout(IN:=FALSE);
ELSE
stMultiLeaderMotionCoupling.sStatusMsg := 'Halting axis.';
END_IF
END_ACTION
ACTION ACT_ComputeTargets:
// ACTION - Compute Targets
// Clip the commanded speed and position to the same limits
// set for the axis. The purpose of this is to allow an axis to automatically ignore commands that
// would otherwise lead to errors from the axis being commanded. Flags are set to indicate if one
// of the limiters is active.
stMultiLeaderMotionCoupling.stAcc.fMax := stMotionStageFollow.stAxisParameters.fAccelerationMax;
stMultiLeaderMotionCoupling.stAcc.fMin := -stMotionStageFollow.stAxisParameters.fDecelerationMax;
stMultiLeaderMotionCoupling.stVel.fMax := stMotionStageFollow.stAxisParameters.fVeloMaximum;
stMultiLeaderMotionCoupling.stVel.fMin := -stMotionStageFollow.stAxisParameters.fVeloMaximum;
stMultiLeaderMotionCoupling.stPos.fMax := stMotionStageFollow.stAxisParameters.fEncSoftEndMax;
stMultiLeaderMotionCoupling.stPos.fMin := stMotionStageFollow.stAxisParameters.fEncSoftEndMin;
stMultiLeaderMotionCoupling.stAcc.fIn := stMultiLeaderMotionCoupling.fTargetAcc;
stMultiLeaderMotionCoupling.stVel.fIn := stMultiLeaderMotionCoupling.fTargetVel;
stMultiLeaderMotionCoupling.stPos.fIn := stMultiLeaderMotionCoupling.fTargetPos;
IF stMultiLeaderMotionCoupling.bUseDynamicLimits THEN
METH_ApplyKinematicLimits(
bEnable:=TRUE,
stAcc:=stMultiLeaderMotionCoupling.stAcc,
stVel:=stMultiLeaderMotionCoupling.stVel,
stPos:=stMultiLeaderMotionCoupling.stPos
);
ELSE
METH_ApplyKinematicLimits(
bEnable:=FALSE,
stAcc:=stMultiLeaderMotionCoupling.stAcc,
stVel:=stMultiLeaderMotionCoupling.stVel,
stPos:=stMultiLeaderMotionCoupling.stPos
);
END_IF
END_ACTION
METHOD METH_ApplyKinematicLimits : BOOL
VAR_IN_OUT
stAcc: ST_BoundedValue;
stVel: ST_BoundedValue;
stPos: ST_BoundedValue;
END_VAR
VAR_INPUT
bEnable: BOOL;
END_VAR
IF bEnable THEN
METH_MaxMinLim(stVal:=stAcc);
METH_MaxMinLim(stVal:=stVel);
METH_MaxMinLim(stVal:=stPos);
ELSE
stAcc.fOut := stAcc.fIn;
stVel.fOut := stVel.fIn;
stPos.fOut := stPos.fIn;
END_IF
END_METHOD
METHOD METH_Init : BOOL
VAR_IN_OUT
stMotionStage : ST_MotionStage;
stMultiLeader : ST_MultiLeaderMotionCoupling;
END_VAR
VAR_INPUT
bEnableByDefault : BOOL;
bUseDynamicLimits : BOOL;
END_VAR
VAR
mcReadParams: MC_ReadParameterSet;
END_VAR
mcReadParams(
Parameter := stMotionStage.stAxisParameters,
Axis := stMotionStage.Axis,
Execute := TRUE,
Error =>
);
IF NOT mcReadParams.Error THEN
stMultiLeader.stAcc.fMax := stMotionStage.stAxisParameters.fAccelerationMax;
stMultiLeader.stAcc.fMin := -stMotionStage.stAxisParameters.fDecelerationMax;
stMultiLeader.stVel.fMax := stMotionStage.stAxisParameters.fVeloMaximum;
stMultiLeader.stVel.fMin := -stMotionStage.stAxisParameters.fVeloMaximum;
stMultiLeader.stPos.fMax := stMotionStage.stAxisParameters.fEncSoftEndMax;
stMultiLeader.stPos.fMin := stMotionStage.stAxisParameters.fEncSoftEndMin;
ELSE
stMultiLeader.bError := TRUE;
stMultiLeader.sErrorMsg := 'MultiLeaderMotionCoupling failed to load axis parameters.';
stMultiLeader.sErrorHelp := stMotionStageFollow.sErrorMessage;
stMultiLeader.eState := E_MultiLeaderMotionCoupling.STATE_ERROR;
END_IF
mcReadParams(
Parameter := stMotionStage.stAxisParameters,
Axis := stMotionStage.Axis,
Execute := FALSE,
Error =>
);
stMultiLeader.fTargetPos := stMotionStage.Axis.NcToPlc.ActPos;
stMultiLeader.bEnable := bEnableByDefault;
stMultiLeader.bUseDynamicLimits := bUseDynamicLimits;
END_METHOD
METHOD METH_MaxMinLim : BOOL
VAR_IN_OUT
stVal: ST_BoundedValue;
END_VAR
stVal.bMaxLim := FALSE;
stVal.bMinLim := FALSE;
IF stVal.fIn > stVal.fMax THEN
stVal.fOut := stVal.fMax;
stVal.bMaxLim := TRUE;
ELSIF stVal.fIn < stVal.fMin THEN
stVal.fOut := stVal.fMin;
stVal.bMinLim := TRUE;
ELSE
stVal.fOut := stVal.fIn;
END_IF
END_METHOD
FB_ReadCoE
FUNCTION_BLOCK FB_ReadCoE
VAR_INPUT
bExecute: BOOL;
pBuffer: PVOID; // ADR(buffer)
sNetId: T_AmsNetId; // Have to be entered
nSlaveAddr: UINT; // Have to be entered
nDesiredCoEIndex: WORD; // Ex: CoE 16#9000 (9000:1)
nDesiredCoESubIndex: Byte; // Ex: CoE 1 (9000:1)
nBufferLength: UDINT; // number of bytes in the CoE to be read
END_VAR
VAR_OUTPUT
bBusy: BOOL;
bError: BOOL;
END_VAR
VAR
fbCoERead: FB_EcCoESdoRead; // Function Block for reading from CoE
nState: INT := -1; // RW-status
rtExecute: R_TRIG;
END_VAR
CASE nState OF
-1:
rtExecute(CLK:=bExecute);
IF rtExecute.Q THEN
nState := 0;
bBusy := TRUE;
bError := FALSE;
ELSE
bBusy := FALSE;
END_IF
0:
// Read entry
fbCoERead(
sNetId:= sNetId,
nSlaveAddr:= nSlaveAddr,
nSubIndex:= nDesiredCoeSubIndex,
nIndex:= nDesiredCoEIndex,
pDstBuf:= pBuffer,
cbBufLen:= nBufferLength,
bExecute:= FALSE,
tTimeout:= T#1S
);
fbCoERead(
sNetId:= sNetId,
nSlaveAddr:= nSlaveAddr,
nSubIndex:= nDesiredCoeSubIndex,
nIndex:= nDesiredCoEIndex,
pDstBuf:= pBuffer,
cbBufLen:= nBufferLength,
bExecute:= TRUE,
tTimeout:= T#1S
);
nState := nState + 1; // Next state
1: // Execute CoE read until done
fbCoERead(
sNetId:= sNetId,
nSlaveAddr:= nSlaveAddr,
nSubIndex:= nDesiredCoeSubIndex,
nIndex:= nDesiredCoEIndex,
pDstBuf:= pBuffer,
cbBufLen:= nBufferLength,
bExecute:= TRUE,
tTimeout:= T#1S
);
IF fbCoERead.bError THEN
nState := 100; // Error case
ELSE
IF NOT fbCoERead.bBusy THEN
nState := -1; // Done
bBusy := FALSE;
END_IF
END_IF
100: // Error handling..
nState := -1;
bBusy := FALSE;
bError := TRUE;
END_CASE
END_FUNCTION_BLOCK
FB_SDS
FUNCTION_BLOCK FB_SDS
VAR_IN_OUT
stSDSin : ST_SDS;
stSDSPersistent : ST_SDSPersistent;
END_VAR
VAR
nIndex: UINT;
fbStateSetup: FB_StateSetupHelper;
stDefault: ST_PositionState := (
fDelta := 0.5,
fVelocity := 1,
bMoveOk := TRUE,
bValid := TRUE
);
END_VAR
// Check the present homing status
ACT_CheckHomeStatus();
IF NOT stSDSPersistent.bInitialized THEN
// Do all the required initialization for the persistent data of the sample delivery system.
stSDSPersistent.stTransferArmSlot.sTag := 'TA';
stSDSPersistent.stDiffractometerSlot.sTag := 'DF';
FOR nIndex := E_SDSGarageYStates.T1 to E_SDSGarageYStates.B4 BY 1 DO
stSDSPersistent.astGarageSlot[nIndex].eState := E_SampleSlotStates.EMPTY;
stSDSPersistent.astGarageSlot[nIndex].sDesc := '';
END_FOR
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T1].sTag := 'T1';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T2].sTag := 'T2';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T3].sTag := 'T3';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T4].sTag := 'T4';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M1].sTag := 'M1';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M2].sTag := 'M2';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M3].sTag := 'M3';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M4].sTag := 'M4';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B1].sTag := 'B1';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B2].sTag := 'B2';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B3].sTag := 'B3';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B4].sTag := 'B4';
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T1].fDeg := 0.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M1].fDeg := 0.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B1].fDeg := 0.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T2].fDeg := 90.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M2].fDeg := 90.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B2].fDeg := 90.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T3].fDeg := 180.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M3].fDeg := 180.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B3].fDeg := 180.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T4].fDeg := 270.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M4].fDeg := 270.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B4].fDeg := 270.0;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T1].fYPos := -100.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T2].fYPos := -100.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T3].fYPos := -100.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.T4].fYPos := -100.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M1].fYPos := -74.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M2].fYPos := -74.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M3].fYPos := -74.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.M4].fYPos := -74.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B1].fYPos := -44.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B2].fYPos := -44.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B3].fYPos := -44.5;
stSDSPersistent.astGarageSlot[E_SDSGarageYStates.B4].fYPos := -44.5;
stSDSPersistent.fGarageHeightSafeFromCollision := -40.0;
stSDSPersistent.fGarageRotationClearanceOffset := 45.0;
stSDSPersistent.bInitialized := TRUE;
END_IF
IF NOT stSDSin.bInitialized THEN
// Do all the required initialization for the non-persistent data of the sample delivery system.
stSDSin.bHomingRequired := TRUE;
fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE);
stSDSin.bInitialized := TRUE;
END_IF
// Update load/unloadable statuses for the garage, transfer arm, and the diffractometer.
UpdateLoadUnloadAbleStatuses(
stSDS := stSDSin,
stSDSP := stSDSPersistent
);
// Check if garage is above the threshold to rotate freely without fear of hitting the transfer arm.
ConfirmGarageFreeToRotate(
stSDS := stSDSin,
stSDSP := stSDSPersistent
);
// Control the sequencing of the sample delivery system.
StateMachine(
stSDS := stSDSin,
stSDSP := stSDSPersistent
);
// Update the position states
UpdatePositionStates(
stSDS := stSDSin,
stSDSP := stSDSPersistent
);
// Call state position function blocks.
stSDSin.fbTransferArmHPos1D(
stMotionStage:=stSDSin.stTransferArmHStage,
astPositionState:=stSDSin.astTransferArmHPosState,
eEnumSet:=stSDSin.eTransferArmHStateSet,
eEnumGet:=stSDSin.eTransferArmHStateGet,
bEnable:=TRUE,
stEpicsToPlc:=stSDSin.stTransferArmHEpicsToPlc,
stPlcToEpics=>stSDSin.stTransferArmHPlcToEpics
);
stSDSin.fbTransferArmRPos1D(
stMotionStage:=stSDSin.stTransferArmRStage,
astPositionState:=stSDSin.astTransferArmRPosState,
eEnumSet:=stSDSin.eTransferArmRStateSet,
eEnumGet:=stSDSin.eTransferArmRStateGet,
bEnable:=TRUE,
stEpicsToPlc:=stSDSin.stTransferArmREpicsToPlc,
stPlcToEpics=>stSDSin.stTransferArmRPlcToEpics
);
stSDSin.fbGarageYPos1D(
stMotionStage:=stSDSin.stGarageYStage,
astPositionState:=stSDSin.astGarageYPosState,
eEnumSet:=stSDSin.eGarageYStateSet,
eEnumGet:=stSDSin.eGarageYStateGet,
bEnable:=TRUE,
stEpicsToPlc:=stSDSin.stGarageYEpicsToPlc,
stPlcToEpics=>stSDSin.stGarageYPlcToEpics
);
stSDSin.fbGarageRPos1D(
stMotionStage:=stSDSin.stGarageRStage,
astPositionState:=stSDSin.astGarageRPosState,
eEnumSet:=stSDSin.eGarageRStateSet,
eEnumGet:=stSDSin.eGarageRStateGet,
bEnable:=TRUE,
stEpicsToPlc:=stSDSin.stGarageREpicsToPlc,
stPlcToEpics=>stSDSin.stGarageRPlcToEpics
);
// Ensure UI boolean commands are cleared after holding for one second.
stSDSin.stUISelect.bExitManualControl := FALSE;
stSDSin.stUISelect.bHomeGarage := FALSE;
stSDSin.stUISelect.bHomeTransferArm := FALSE;
stSDSin.stUISelect.bLoad := FALSE;
stSDSin.stUISelect.bLoadArmFromGarage := FALSE;
stSDSin.stUISelect.bLoadDiffractometerFromArm := FALSE;
stSDSin.stUISelect.bLoadGarageFromPort := FALSE;
stSDSin.stUISelect.bLoadUnloadGarageFromPort := FALSE;
stSDSin.stUISelect.bManualControl := FALSE;
stSDSin.stUISelect.bProceed := FALSE;
stSDSin.stUISelect.bCancel := FALSE;
stSDSin.stUISelect.bUnload := FALSE;
stSDSin.stUISelect.bUnloadArmToGarage := FALSE;
stSDSin.stUISelect.bUnloadDiffractometerToArm := FALSE;
// Align with the saved selection at the end of each cycle.
stSDSin.stUISelect.eGarageSlot := stSDSin.eSelectedGarageSlot;
END_FUNCTION_BLOCK
ACTION ACT_CheckHomeStatus:
// ACTION - Check home status
CASE stSDSin.nHomingState OF
0: // Initialization
stSDSin.tonForwardHomingLimit(IN:=FALSE);
IF stSDSin.stTransferArmHStage.nCommand = E_EpicsMotorCmd.HOME AND stSDSin.stTransferArmHStage.bBusy THEN
stSDSin.nHomingState := stSDSin.nHomingState + 1;
END_IF
1: // Homing Sequence Triggered
stSDSin.tonForwardHomingLimit(IN:=stSDSin.stTransferArmHStage.Axis.NcToPlc.SetVelo > 0,PT:=T#1s);
IF stSDSin.stTransferArmHStage.nCommand = E_EpicsMotorCmd.HOME AND stSDSin.stTransferArmHStage.bHome THEN
stSDSin.nHomingState := stSDSin.nHomingState + 1;
stSDSin.tonHomingStateTimeout(IN:=FALSE);
stSDSin.tonForwardHomingLimit(IN:=FALSE);
ELSIF NOT stSDSin.stTransferArmHStage.bBusy THEN
stSDSin.tonHomingStateTimeout(IN:=TRUE,PT:=T#5s);
stSDSin.tonForwardHomingLimit(IN:=FALSE);
IF stSDSin.tonHomingStateTimeout.Q THEN
stSDSin.nHomingState := -1;
END_IF
END_IF
2: // Homing Sequence Successful
IF F_Limit(stSDSin.stTransferArmHStage.stAxisStatus.fActPosition, stSDSin.stTransferArmHStage.fHomePosition-0.1, stSDSin.stTransferArmHStage.fHomePosition+0.1, TRUE) THEN
stSDSin.bHomingRequired := FALSE;
stSDSin.nHomingState := 0;
stSDSin.tonHomingRequiredTimeout(IN:=FALSE);
stSDSin.tonHomingStateTimeout(IN:=FALSE);
ELSIF NOT stSDSin.stTransferArmHStage.bBusy THEN
stSDSin.tonHomingStateTimeout(IN:=TRUE,PT:=T#5s);
IF stSDSin.tonHomingStateTimeout.Q THEN
stSDSin.nHomingState := -1;
END_IF
END_IF
-1: // Error Handling
stSDSin.nHomingState := 0;
END_CASE
stSDSin.tonHomingRequiredTimeout(IN:=NOT stSDSin.bHomingRequired,PT:=stSDSin.tHomingRequiredTimeoutTime);
IF stSDSin.tonHomingRequiredTimeout.Q THEN
stSDSin.bHomingRequired := TRUE;
END_IF
IF stSDSin.stTransferArmHStage.bHome AND F_Limit(
fVal := stSDSin.stTransferArmHStage.Axis.NcToPlc.ActPos,
fLLim := -0.01,
fHLim := 0.01,
bInclusive := TRUE) THEN
stSDSin.bHomingRequired := FALSE;
END_IF
stSDSin.fMinUntilHomingRequired := TIME_TO_REAL(stSDSin.tonHomingRequiredTimeout.PT - stSDSin.tonHomingRequiredTimeout.ET) / 60000;
IF stSDSin.stTransferArmHStage.bExecute AND stSDSin.bHomingRequired AND stSDSin.stTransferArmHStage.fPosition > stSDSin.stTransferArmHStage.stAxisStatus.fActPosition AND
stSDSin.stTransferArmHStage.nCommand <> E_EpicsMotorCmd.HOME THEN
stSDSin.stTransferArmHStage.bError := TRUE;
stSDSin.stTransferArmHStage.sCustomErrorMessage := 'Axis homing status overdue. Please home the axis.';
END_IF
END_ACTION
METHOD PRIVATE CheckCanDo2Criteria
VAR_IN_OUT
bCanDo : BOOL;
bCantDo : BOOL;
sErrorMsg : T_MaxString;
END_VAR
VAR_INPUT
b1 : BOOL;
b2 : BOOL;
sErrorMsgTF : T_MaxString;
sErrorMsgFT : T_MaxString;
sErrorMsgFF : T_MaxString;
END_VAR
bCanDo := FALSE;
IF b1 AND b2 THEN
bCanDo := TRUE;
ELSIF b1 AND NOT b2 THEN
bCantDo := TRUE;
sErrorMsg := sErrorMsgTF;
ELSIF NOT b1 AND b2 THEN
bCantDo := TRUE;
sErrorMsg := sErrorMsgFT ;
ELSE
bCantDo := TRUE;
sErrorMsg := sErrorMsgFF;
END_IF
END_METHOD
METHOD PRIVATE CheckCanHomeGarage
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if garage can be homed.';
InitHomeGarage(
stSDS := stSDS
);
IF stSDS.bGarageAboveCollisionHeight THEN
stSDS.bSuccess := TRUE;
ELSE
stSDS.bError := TRUE;
stSDS.sErrorMsg := CONCAT('The garage must be above ',
CONCAT(LREAL_TO_STRING(stSDSP.fGarageHeightSafeFromCollision),
' mm for the rotation axis to be homed.'));
END_IF
END_METHOD
METHOD PRIVATE CheckCanHomeTransferArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if transfer arm can be homed.';
InitHomeTransferArm(
stSDS := stSDS
);
stSDS.bSuccess := TRUE;
END_METHOD
METHOD PRIVATE CheckCanLoadArmFromGarage
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if there is an open slot in the transfer arm and at least one full slot in the garage.';
CheckCanDo2Criteria(
bCanDo := stSDS.bSuccess,
bCantDo := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
b1 := stSDS.bGarageUnloadable,
b2 := stSDS.bTransferArmLoadable,
sErrorMsgTF := 'The transfer arm is currently full and cannot be used to remove a sample from the garage.',
sErrorMsgFT := 'The garage is currently empty and therefore no samples can be removed.',
sErrorMsgFF := 'The garage is currently empty and the transfer arm is full. Therefore, no samples can be removed.'
);
END_METHOD
METHOD PRIVATE CheckCanLoadDiffractometerFromArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if the diffractometer slot is open and if the transfer arm is carrying a sample.';
CheckCanDo2Criteria(
bCanDo := stSDS.bSuccess,
bCantDo := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
b1 := stSDS.bDiffractometerLoadable,
b2 := stSDS.bTransferArmUnloadable,
sErrorMsgTF := 'The transfer arm is currently empty and cannot be used to place a sample in the diffractometer.',
sErrorMsgFT := 'The diffractometer already has a sample in it.',
sErrorMsgFF := 'The diffractoemter already has a sample and the transfer arm is empty.'
);
END_METHOD
METHOD PRIVATE CheckCanLoadUnloadGarageFromPort
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if the garage can be loaded or unloaded from the port.';
stSDS.bSuccess := TRUE;
END_METHOD
METHOD PRIVATE CheckCanManualControl
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if we can switch to manual control.';
stSDS.bSuccess := TRUE;
END_METHOD
METHOD PRIVATE CheckCanUnloadArmToGarage
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if there is an open slot in the garage and if the transfer arm is carrying a sample.';
CheckCanDo2Criteria(
bCanDo := stSDS.bSuccess,
bCantDo := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
b1 := stSDS.bGarageLoadable,
b2 := stSDS.bTransferArmUnloadable,
sErrorMsgTF := 'The transfer arm is currently empty and cannot be used to load sample into the garage.',
sErrorMsgFT := 'The garage is currently full and therefore no samples can be loaded into it.',
sErrorMsgFF := 'The garage is currently full and the transfer arm is empty.'
);
END_METHOD
METHOD PRIVATE CheckCanUnloadDiffractometerToArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Checking if the diffractometer slot is full and if the transfer arm is empty.';
CheckCanDo2Criteria(
bCanDo := stSDS.bSuccess,
bCantDo := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
b1 := stSDS.bDiffractometerUnloadable,
b2 := stSDS.bTransferArmLoadable,
sErrorMsgTF := 'The transfer arm is currently full and cannot be used to unload a sample from the diffractometer.',
sErrorMsgFT := 'The diffractometer does not have a sample in it.',
sErrorMsgFF := 'The diffractometeris empty and the transfer arm is full.'
);
END_METHOD
METHOD PRIVATE ConfirmDiffractometerExtraction
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm back a few mm. Please confirm the sample is held by the diffractometer and then click "proceed". If the sample is still in the transfer arm, click "retry" to go back to fine adjustment.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.CONFIRM_DIFF_EXTRACTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ConfirmDiffractometerInsertion
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm back a few mm. Please confirm the sample is held by the diffractometer and then click "proceed". If the sample is still in the transfer arm, click "retry" to go back to fine adjustment.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.CONFIRM_DIFF_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ConfirmExtraction
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm back a few mm. Please confirm the sample is held by the arm and then click "proceed". If the sample is still in the holder, click "retry" to go back to fine adjustment.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.CONFIRM_GARAGE_EXTRACTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ConfirmGarageFreeToRotate
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
VAR_INST
tonHoldDelay : TON;
END_VAR
// Missing handling for erroring encoder.
tonHoldDelay(
IN := stSDS.stGarageYStage.Axis.NcToPlc.ActPos >= stSDSP.fGarageHeightSafeFromCollision,
PT := T#1s
);
stSDS.bGarageAboveCollisionHeight := tonHoldDelay.Q;
END_METHOD
METHOD PRIVATE ConfirmInsertion
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm back a few mm. Please confirm the sample is held by the garage and then click "proceed". If the sample is still in the transfer arm, click "retry" to go back to fine adjustment.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.CONFIRM_GARAGE_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ConfirmVacuumValveOpen
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.sStatusMsg := 'Verifying that the vacuum valve between SDS and Diff is open.';
stSDS.bSuccess := GVL_Interface.QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH AND
NOT GVL_Interface.QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH;
END_METHOD
METHOD PRIVATE ExtendPrerotateTransferArmForLoading
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
VAR
bHArrived : BOOL;
bRArrived : BOOL;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm to nominal sample loading position and pre-rotating to allow full rotation for screwing in sample.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.GARAGE_SAMPLE_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bHArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.PREROTATE_SAMPLE_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bRArrived
);
IF bHArrived AND bRArrived THEN
stSDS.bSuccess := TRUE;
END_IF
END_METHOD
METHOD PRIVATE ExtendTransferArmForDiffractometerExtraction
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm to nominal sample unloading position for diffractometer.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.DIFF_SAMPLE_EXTRACTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ExtendTransferArmForDiffractometerInsertion
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm to nominal sample loading position for diffractometer.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.DIFF_SAMPLE_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ExtendTransferArmForExtraction
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm to nominal sample extraction position for garage.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.GARAGE_SAMPLE_REMOVAL,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ExtendTransferArmForInsertion
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
VAR
bHArrived : BOOL;
bRArrived : BOOL;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving transfer arm to nominal sample loading position for garage.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmHStateGet,
stMotionStage := stSDS.stTransferArmHStage,
eStateSet := stSDS.eTransferArmHStateSet,
eEnumSet := E_SDSTransferArmHStates.GARAGE_SAMPLE_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE FineAdjustment
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.sStatusMsg := 'Use the individual motor control to do fine adjustment. Click "proceed" to move to the next step.';
END_METHOD
METHOD PRIVATE GarageFaceTransferArm
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
VAR
bYArrived : BOOL;
bRArrived : BOOL;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Facing garage towards the transfer arm.';
stSDS.eFaceState := E_SDSFaceState.FACE_TRANSFER_ARM;
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eGarageRStateGet,
stMotionStage := stSDS.stGarageRStage,
eStateSet := stSDS.eGarageRStateSet,
eEnumSet := stSDS.eSelectedGarageSlot,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE HoldThenClear
VAR_IN_OUT
b1 : BOOL;
END_VAR
VAR_INPUT
tHoldDuration : TIME := T#1s;
END_VAR
VAR_INST
tonHoldTimer : TON;
END_VAR
tonHoldTimer(IN := b1, PT := tHoldDuration);
IF tonHoldTimer.Q THEN
b1 := FALSE;
END_IF
END_METHOD
METHOD PRIVATE HomeGarage
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Homing the garage.';
IF stSDS.stGarageRStage.bError THEN
stSDS.bError := TRUE;
stSDS.sErrorMsg := 'Garage rotational stage has an active error. Aborting sequence.';
END_IF
HomeStateMachine(
eHomingState := stSDS.eGarageRHomingState,
stMotionStage := stSDS.stGarageRStage,
bError => stSDS.bError,
sErrorMsg => stSDS.sErrorMsg
);
IF stSDS.eGarageRHomingState = E_SDSHomingState.HOMED THEN
stSDS.bSuccess := TRUE;
END_IF
END_METHOD
METHOD PRIVATE HomeStateMachine
VAR_IN_OUT
eHomingState : E_SDSHomingState;
stMotionStage : ST_MotionStage;
END_VAR
VAR_OUTPUT
bError : BOOL;
sErrorMsg : T_MaxString;
END_VAR
IF stMotionStage.bHome AND F_Limit(
fVal := stMotionStage.Axis.NcToPlc.ActPos,
fLLim := -0.01,
fHLim := 0.01,
bInclusive := TRUE) THEN
eHomingState := E_SDSHomingState.HOMED;
END_IF
CASE eHomingState OF
E_SDSHomingState.INIT:
eHomingState := E_SDSHomingState.WAIT_NOT_BUSY;
E_SDSHomingState.WAIT_NOT_BUSY:
IF NOT stMotionStage.bBusy THEN
eHomingState := E_SDSHomingState.TRIGGER_HOMING;
END_IF
E_SDSHomingState.TRIGGER_HOMING:
stMotionStage.bHomeCmd := TRUE;
IF stMotionStage.nCommand = E_EpicsMotorCmd.HOME AND stMotionStage.bBusy THEN
eHomingState := E_SDSHomingState.WAIT_FOR_HOMED;
END_IF
E_SDSHomingState.WAIT_FOR_HOMED:
IF stMotionStage.bDone THEN
eHomingState := E_SDSHomingState.HOMED;
END_IF
ELSE
bError := TRUE;
sErrorMsg := 'Homing sequence triggered an unknown state. Aborting sequence.';
END_CASE
END_METHOD
METHOD PRIVATE HomeTransferArm
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Homing the transfer arm.';
IF stSDS.stTransferArmHStage.bError THEN
stSDS.bError := TRUE;
stSDS.sErrorMsg := 'Transfer arm horizontal stage has an active error. Aborting sequence.';
END_IF
IF stSDS.stTransferArmRStage.bError THEN
stSDS.bError := TRUE;
stSDS.sErrorMsg := 'Transfer arm rotational stage has an active error. Aborting sequence.';
END_IF
HomeStateMachine(
eHomingState := stSDS.eTransferArmHHomingState,
stMotionStage := stSDS.stTransferArmHStage,
bError => stSDS.bError,
sErrorMsg => stSDS.sErrorMsg
);
HomeStateMachine(
eHomingState := stSDS.eTransferArmRHomingState,
stMotionStage := stSDS.stTransferArmRStage,
bError => stSDS.bError,
sErrorMsg => stSDS.sErrorMsg
);
IF stSDS.eTransferArmHHomingState = E_SDSHomingState.HOMED AND
stSDS.eTransferArmRHomingState = E_SDSHomingState.HOMED THEN
stSDS.bSuccess := TRUE;
END_IF
END_METHOD
METHOD PRIVATE InitHomeGarage
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.eGarageRHomingState := E_SDSHomingState.INIT;
END_METHOD
METHOD PRIVATE InitHomeTransferArm
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.eTransferArmHHomingState := E_SDSHomingState.INIT;
stSDS.eTransferArmRHomingState := E_SDSHomingState.INIT;
END_METHOD
METHOD PRIVATE InitRotateGarageForClearance
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.fGarageRotationClearancePos := stSDS.stGarageRStage.Axis.NcToPlc.ActPos + stSDSP.fGarageRotationClearanceOffset;
END_METHOD
METHOD PRIVATE InitSelectGarageSlotForLoading
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
FOR nIndex := E_SDSGarageYStates.T1 TO E_SDSGarageYStates.B4 BY 1 DO
CASE stSDSP.astGarageSlot[nIndex].eState OF
E_SampleSlotStates.EMPTY:
stSDS.eSelectedGarageSlot := nIndex;
END_CASE
END_FOR
IF nIndex > E_SDSGarageYStates.B4 THEN
stSDS.bError := TRUE;
stSDS.sErrorMsg := 'There are no empty slots in the garage to load a sample to.';
END_IF
END_METHOD
METHOD PRIVATE InitSelectGarageSlotForRemoval
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
FOR nIndex := E_SDSGarageYStates.T1 TO E_SDSGarageYStates.B4 BY 1 DO
CASE stSDSP.astGarageSlot[nIndex].eState OF
E_SampleSlotStates.FULL:
stSDS.eSelectedGarageSlot := nIndex;
END_CASE
END_FOR
IF nIndex > E_SDSGarageYStates.B4 THEN
stSDS.bError := TRUE;
stSDS.sErrorMsg := 'There are no full slots in the garage to extract a sample from.';
END_IF
END_METHOD
METHOD PRIVATE LoadArmFromGarage
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
CASE stSDS.eSeqState OF
E_SDSSeqState.Inactive:
stSDS.eSeqState := E_SDSSeqState.Home_Transfer_Arm;
E_SDSSeqState.Home_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Check_Garage_Height;
END_IF
E_SDSSeqState.Check_Garage_Height:
IF stSDS.bGarageAboveCollisionHeight THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
ELSE
stSDS.eSeqState := E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise;
END_IF
E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise:
RotateGarageForClearanceManual(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Garage_1;
END_IF
E_SDSSeqState.Retract_Garage_1:
RetractGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
END_IF
E_SDSSeqState.Home_Garage:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Select_Garage_Slot_for_Removal;
END_IF
E_SDSSeqState.Select_Garage_Slot_for_Removal:
SelectGarageSlotForRemoval(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Garage_Face_Transfer_Arm;
END_IF
E_SDSSeqState.Garage_Face_Transfer_Arm:
GarageFaceTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Rotate_Garage_for_Clearance_to_Lower;
END_IF
E_SDSSeqState.Rotate_Garage_for_Clearance_to_Lower:
RotateGarageForClearance(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Move_Garage_to_Slot_Height;
END_IF
E_SDSSeqState.Move_Garage_to_Slot_Height:
MoveGarageToSlotHeight(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Prerotate_Transfer_Arm_for_Extraction;
END_IF
E_SDSSeqState.Prerotate_Transfer_Arm_for_Extraction:
PrerotateTransferArmForExtraction(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Extend_Transfer_Arm_for_Extraction;
END_IF
E_SDSSeqState.Extend_Transfer_Arm_for_Extraction:
ExtendTransferArmForExtraction(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Screw_in_Half_Turn;
END_IF
E_SDSSeqState.Screw_in_Half_Turn:
ScrewInHalfTurn(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Fine_Adjustment:
FineAdjustment(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Unscrew_Sample_from_Garage;
END_IF
E_SDSSeqState.Unscrew_Sample_from_Garage:
UnscrewSampleFromGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Confirm_Extraction;
END_IF
E_SDSSeqState.Confirm_Extraction:
ConfirmExtraction(
stSDS := stSDS
);
IF stSDS.bSuccess AND stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Update_Sample_Database;
ELSIF stSDS.stUISelect.bRetry THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Update_Sample_Database:
UpdateSampleLocationDatabaseGarageToArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess OR stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Transfer_Arm;
END_IF
E_SDSSeqState.Retract_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Rotate_Garage_for_Clearance_to_Raise;
END_IF
E_SDSSeqState.Rotate_Garage_for_Clearance_to_Raise:
RotateGarageForClearance(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Garage_2;
END_IF
E_SDSSeqState.Retract_Garage_2:
RetractGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Return_Garage_to_Home;
END_IF
E_SDSSeqState.Return_Garage_to_Home:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eState := E_SDSState.Standby;
END_IF
END_CASE
END_METHOD
METHOD PRIVATE LoadDiffractometerFromArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
CASE stSDS.eSeqState OF
E_SDSSeqState.Inactive:
stSDS.eSeqState := E_SDSSeqState.Home_Transfer_Arm;
E_SDSSeqState.Home_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Check_Garage_Height;
END_IF
E_SDSSeqState.Check_Garage_Height:
IF stSDS.bGarageAboveCollisionHeight THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
ELSE
stSDS.eSeqState := E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise;
END_IF
E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise:
RotateGarageForClearanceManual(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Garage_1;
END_IF
E_SDSSeqState.Retract_Garage_1:
RetractGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
END_IF
E_SDSSeqState.Home_Garage:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Move_Diffractometer_to_Load_Unload_Position;
END_IF
E_SDSSeqState.Move_Diffractometer_to_Load_Unload_Position:
MoveDiffractometerToLoadUnloadPosition(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Confirm_Vacuum_Valve_Open;
END_IF
E_SDSSeqState.Confirm_Vacuum_Valve_Open:
ConfirmVacuumValveOpen(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Prerotate_Transfer_Arm_for_Insertion;
END_IF
E_SDSSeqState.Prerotate_Transfer_Arm_for_Insertion:
PrerotateTransferArmForInsertion(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Extend_Transfer_Arm_for_Diffractometer_Insertion;
END_IF
E_SDSSeqState.Extend_Transfer_Arm_for_Diffractometer_Insertion:
ExtendTransferArmForDiffractometerInsertion(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Fine_Adjustment:
FineAdjustment(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Screw_Sample_into_Diffractometer;
END_IF
E_SDSSeqState.Screw_Sample_into_Diffractometer:
ScrewSampleIntoDiffractometer(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Confirm_Diffractometer_Insertion;
END_IF
E_SDSSeqState.Confirm_Diffractometer_Insertion:
ConfirmDiffractometerInsertion(
stSDS := stSDS
);
IF stSDS.bSuccess AND stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Update_Sample_Database;
ELSIF stSDS.stUISelect.bRetry THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Update_Sample_Database:
UpdateSampleLocationDatabaseDiffToArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess OR stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Transfer_Arm;
END_IF
E_SDSSeqState.Retract_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Rotate_Garage_for_Clearance_to_Raise;
END_IF
END_CASE
END_METHOD
METHOD PRIVATE LoadUnloadGarageFromPort
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
END_METHOD
METHOD PRIVATE ManualControl
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
END_METHOD
METHOD PRIVATE MoveDiffractometerToLoadUnloadPosition
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
VAR
bXArrived : BOOL;
bYArrived : BOOL;
bZArrived : BOOL;
b2ThetaYArrived : BOOL;
bPhiArrived : BOOL;
bChiArrived : BOOL;
b2ThetaArrived : BOOL;
bThetaArrived : BOOL;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving the diffractometer to load/unload position.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiffXStateGet,
stMotionStage := stSDS.stDiffXStage,
eStateSet := stSDS.eDiffXStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bXArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiffYStateGet,
stMotionStage := stSDS.stDiffYStage,
eStateSet := stSDS.eDiffYStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bYArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiffZStateGet,
stMotionStage := stSDS.stDiffZStage,
eStateSet := stSDS.eDiffZStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bZArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiff2ThetaYStateGet,
stMotionStage := stSDS.stDiff2ThetaYStage,
eStateSet := stSDS.eDiff2ThetaYStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => b2ThetaYArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiffPhiStateGet,
stMotionStage := stSDS.stDiffPhiStage,
eStateSet := stSDS.eDiffPhiStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bPhiArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiffChiStateGet,
stMotionStage := stSDS.stDiffChiStage,
eStateSet := stSDS.eDiffChiStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bChiArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiff2ThetaStateGet,
stMotionStage := stSDS.stDiff2ThetaStage,
eStateSet := stSDS.eDiff2ThetaStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => b2ThetaArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eDiffThetaStateGet,
stMotionStage := stSDS.stDiffThetaStage,
eStateSet := stSDS.eDiffThetaStateSet,
eEnumSet := E_DiffState.TRANSFER_ARM_INTERFACE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bThetaArrived
);
IF bXArrived AND
bYArrived AND
bZArrived AND
b2ThetaYArrived AND
bPhiArrived AND
bChiArrived AND
b2ThetaArrived AND
bThetaArrived THEN
stSDS.bSuccess := TRUE;
END_IF
END_METHOD
METHOD PRIVATE MoveGarageToSlotHeight
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving garage to selected slot height.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eGarageYStateGet,
stMotionStage := stSDS.stGarageYStage,
eStateSet := stSDS.eGarageYStateSet,
eEnumSet := stSDS.eSelectedGarageSlot,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE MoveToGarageSlotFaceTransferArm
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
VAR
bYArrived : BOOL;
bRArrived : BOOL;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Moving garage to selected slot and facing it towards transfer arm.';
stSDS.eFaceState := E_SDSFaceState.FACE_TRANSFER_ARM;
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eGarageYStateGet,
stMotionStage := stSDS.stGarageYStage,
eStateSet := stSDS.eGarageYStateSet,
eEnumSet := stSDS.eSelectedGarageSlot,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bYArrived
);
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eGarageRStateGet,
stMotionStage := stSDS.stGarageRStage,
eStateSet := stSDS.eGarageRStateSet,
eEnumSet := stSDS.eSelectedGarageSlot,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => bRArrived
);
IF bYArrived AND bRArrived THEN
stSDS.bSuccess := TRUE;
END_IF
END_METHOD
METHOD PRIVATE PrerotateTransferArmForExtraction
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Pre-rotating transfer arm to allow full rotation for unscrewing the sample.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.PREROTATE_SAMPLE_REMOVAL,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE PrerotateTransferArmForInsertion
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Pre-rotating to allow full rotation for screwing in sample.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.PREROTATE_SAMPLE_INSERTION,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE RetractGarage
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Retracting garage.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eGarageYStateGet,
stMotionStage := stSDS.stGarageYStage,
eStateSet := stSDS.eGarageYStateSet,
eEnumSet := E_SDSGarageYStates.RETRACTED,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE RotateGarageForClearance
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Rotating garage to prevent a collision with the transfer arm sample during movement of the garage.';
IF F_Limit(
fVal := stSDS.stGarageRStage.Axis.NcToPlc.ActPos,
fLLim := stSDS.fGarageRotationClearancePos - stSDS.stGarageRStage.stAxisParameters.fTargetPosControlRange,
fHLim := stSDS.fGarageRotationClearancePos + stSDS.stGarageRStage.stAxisParameters.fTargetPosControlRange,
bInclusive := FALSE
) AND NOT stSDS.stGarageRStage.bBusy THEN
stSDS.bSuccess := TRUE;
ELSE
stSDS.stGarageRStage.fPosition := stSDS.fGarageRotationClearancePos;
stSDS.stGarageRStage.fVelocity := stSDS.fGarageRotationSpeed;
stSDS.stGarageRStage.bMoveCmd := TRUE;
END_IF
END_METHOD
METHOD PRIVATE RotateGarageForClearanceManual
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.sStatusMsg := 'Please manually rotate the garage to 45 degrees to prevent it from colliding with the sample on the transfer arm when raising it up. Click "proceed" when complete.';
END_METHOD
METHOD PRIVATE RotateToFacePort
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Rotating garage so that the selected slot faces the side port.';
stSDS.eFaceState := E_SDSFaceState.FACE_SIDE_PORT;
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eGarageRStateGet,
stMotionStage := stSDS.stGarageRStage,
eStateSet := stSDS.eGarageRStateSet,
eEnumSet := stSDS.eSelectedGarageSlot,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ScrewInHalfTurn
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Screwing transfer arm a half turn to catch the sample pin.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.SCREW_IN_HALF_TURN,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ScrewSampleIntoDiffractometer
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Screwing sample into diffractometer.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.SCREW_INTO_DIFF,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE ScrewSampleIntoGarage
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Screwing sample into garage.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.SCREW_INTO_GARAGE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE SelectDesiredAction
VAR_INPUT
stSDS : ST_SDS;
END_VAR
stSDS.sStatusMsg := 'Please select the desired action.';
END_METHOD
METHOD PRIVATE SelectGarageSlotForLoading
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.sStatusMsg := 'Please select a valid garage slot to load a sample to and then click "proceed".';
IF stSDS.stUISelect.eGarageSlot >= E_SDSGarageYStates.T1 AND stSDS.stUISelect.eGarageSlot <= E_SDSGarageYStates.B4 THEN
CASE stSDSP.astGarageSlot[stSDS.stUISelect.eGarageSlot].eState OF
E_SampleSlotStates.DISABLED:
stSDS.sErrorMsg := 'The selected garage slot is currently disabled.';
E_SampleSlotStates.EMPTY:
stSDS.eSelectedGarageSlot := stSDS.stUISelect.eGarageSlot;
E_SampleSlotStates.FULL:
stSDS.sErrorMsg := 'The selected garage slot is currently full.';
END_CASE
ELSE
stSDS.sErrorMsg := 'Please select a valid slot from T1 to B4.';
END_IF
END_METHOD
METHOD PRIVATE SelectGarageSlotForRemoval
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.sStatusMsg := 'Please select a valid garage slot to retrieve a sample from and then click "proceed".';
IF stSDS.stUISelect.eGarageSlot >= E_SDSGarageYStates.T1 AND stSDS.stUISelect.eGarageSlot <= E_SDSGarageYStates.B4 THEN
CASE stSDSP.astGarageSlot[stSDS.stUISelect.eGarageSlot].eState OF
E_SampleSlotStates.DISABLED:
stSDS.sErrorMsg := 'The selected garage slot is currently disabled.';
E_SampleSlotStates.EMPTY:
stSDS.sErrorMsg := 'The selected garage slot is currently empty.';
E_SampleSlotStates.FULL:
stSDS.eSelectedGarageSlot := stSDS.stUISelect.eGarageSlot;
END_CASE
ELSE
stSDS.sErrorMsg := 'Please select a valid slot from T1 to B4.';
END_IF
END_METHOD
METHOD PRIVATE SelectLoadUnloadGarage
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.sStatusMsg := 'Select whether you would like to load a sample to the garage or remove a sample to the garage.';
stSDS.bLoadingSelected := FALSE;
stSDS.bUnloadingSelected := FALSE;
IF stSDS.stUISelect.bLoad THEN
stSDS.bLoadingSelected := TRUE;
ELSIF stSDS.stUISelect.bUnload THEN
stSDS.bUnloadingSelected := TRUE;
END_IF
END_METHOD
METHOD PRIVATE SetPositionStateWaitForArrivalQuitIfError
VAR_IN_OUT
eStateGet : UINT;
stMotionStage : ST_MotionStage;
eStateSet : UINT;
bError : BOOL;
sErrorMsg : T_MaxString;
END_VAR
VAR_INPUT
eEnumSet : UINT;
END_VAR
VAR_OUTPUT
bArrived : BOOL;
END_VAR
IF eStateGet = eEnumSet AND
NOT stMotionStage.bBusy THEN
bArrived := TRUE;
ELSIF stMotionStage.bError THEN
bError := TRUE;
sErrorMsg := 'And axis error occurred. Aborting sequence.';
END_IF
IF eStateGet <> eEnumSet THEN
eStateSet := eEnumSet;
END_IF
END_METHOD
METHOD PRIVATE SetSeqState
VAR_INPUT
stSDS : ST_SDS;
eSeqState : E_SDSState;
END_VAR
stSDS.eSeqState := eSeqState;
END_METHOD
METHOD PRIVATE SetState
VAR_INPUT
stSDS : ST_SDS;
eState : E_SDSState;
END_VAR
stSDS.eState := eState;
END_METHOD
METHOD PRIVATE SetSubstate
VAR_INPUT
stSDS : ST_SDS;
eSubState : E_SDSState;
END_VAR
stSDS.eSubState := eSubState;
END_METHOD
METHOD PRIVATE StateMachine
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
//IF stSDS.bError THEN
// stSDS.eState := E_SDSState.Error;
//END_IF
IF stSDS.stUISelect.bCancel THEN
stSDS.eState := E_SDSState.Standby;
stSDS.eSubState := E_SDSSubState.Inactive;
stSDS.eSeqState := E_SDSSeqState.Inactive;
stSDS.bError := FALSE;
stSDS.sErrorMsg := '';
END_IF
stSDS.sStatusMsg := '';
CASE stSDS.eState OF
E_SDSState.Standby:
IF stSDS.bEnable THEN
stSDS.eState := E_SDSState.Operating;
stSDS.eSubState := E_SDSSubstate.Select_Desired_Action;
stSDS.eSeqState := E_SDSSeqState.Inactive;
ELSE
stSDS.eSubState := E_SDSSubState.Inactive;
stSDS.eSeqState := E_SDSSeqState.Inactive;
END_IF
E_SDSState.Operating:
IF NOT stSDS.bEnable THEN
stSDS.eState := E_SDSState.Standby;
ELSE
SubStateMachine(
stSDS := stSDS,
stSDSP := stSDSP
);
END_IF
E_SDSState.Error:
IF NOT stSDS.bError THEN
stSDS.eState := E_SDSState.Standby;
END_IF
stSDS.eSubState := E_SDSSubState.Inactive;
stSDS.eSeqState := E_SDSSeqState.Inactive;
END_CASE
END_METHOD
METHOD PRIVATE SubStateMachine
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
CASE stSDS.eSubState OF
E_SDSSubState.Inactive:
; // Do nothing
E_SDSSubState.Select_Desired_Action:
IF stSDS.stUISelect.bHomeGarage THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Home_Garage;
ELSIF stSDS.stUISelect.bHomeTransferArm THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Home_Transfer_Arm;
ELSIF stSDS.stUISelect.bLoadArmFromGarage THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Load_Arm_from_Garage;
ELSIF stSDS.stUISelect.bUnloadArmToGarage THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Unload_Arm_to_Garage;
ELSIF stSDS.stUISelect.bLoadUnloadGarageFromPort THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Load_Unload_Garage_from_Port;
ELSIF stSDS.stUISelect.bLoadDiffractometerFromArm THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Load_Diffractometer_from_Arm;
ELSIF stSDS.stUISelect.bUnloadDiffractometerToArm THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Unload_Diffractometer_to_Arm;
ELSIF stSDS.stUISelect.bManualControl THEN
stSDS.eSubState := E_SDSSubstate.Check_Can_Manual_Control;
END_IF
E_SDSSubState.Check_Can_Home_Garage:
CheckCanHomeGarage(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Home_Garage;
END_IF
E_SDSSubState.Home_Garage:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSState.Standby;
END_IF
E_SDSSubState.Check_Can_Home_Transfer_Arm:
CheckCanHomeTransferArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Home_Transfer_Arm;
END_IF
E_SDSSubState.Home_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
E_SDSSubState.Check_Can_Load_Arm_From_Garage:
CheckCanLoadArmFromGarage(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Load_Arm_from_Garage;
END_IF
E_SDSSubState.Load_Arm_from_Garage:
LoadArmFromGarage(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
E_SDSSubState.Check_Can_Load_Diffractometer_from_Arm:
CheckCanLoadDiffractometerFromArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Load_Diffractometer_from_Arm;
END_IF
E_SDSSubState.Load_Diffractometer_from_Arm:
LoadDiffractometerFromArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
E_SDSSubState.Check_Can_Load_Unload_Garage_from_Port:
CheckCanLoadUnloadGarageFromPort(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Load_Unload_Garage_from_Port;
END_IF
E_SDSSubState.Load_Unload_Garage_from_Port:
LoadUnloadGarageFromPort(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
E_SDSSubState.Check_Can_Unload_Arm_to_Garage:
CheckCanUnloadArmToGarage(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Unload_Arm_to_Garage;
END_IF
E_SDSSubState.Unload_Arm_to_Garage:
UnloadArmToGarage(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
E_SDSSubState.Check_Can_Unload_Diffractometer_to_Arm:
CheckCanUnloadDiffractometerToArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Unload_Diffractometer_to_Arm;
END_IF
E_SDSSubState.Unload_Diffractometer_to_Arm:
UnloadDiffractometerToArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
E_SDSSubState.Check_Can_Manual_Control:
CheckCanManualControl(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSubState := E_SDSSubstate.Manual_Control;
END_IF
E_SDSSubState.Manual_Control:
ManualControl(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eState := E_SDSstate.Standby;
END_IF
END_CASE
END_METHOD
METHOD PRIVATE UnloadArmToGarage
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
CASE stSDS.eSeqState OF
E_SDSSeqState.Inactive:
stSDS.eSeqState := E_SDSSeqState.Home_Transfer_Arm;
E_SDSSeqState.Home_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.bSuccess := FALSE;
stSDS.eSeqState := E_SDSSeqState.Check_Garage_Height;
END_IF
E_SDSSeqState.Check_Garage_Height:
IF stSDS.bGarageAboveCollisionHeight THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
ELSE
stSDS.eSeqState := E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise;
END_IF
E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise:
RotateGarageForClearanceManual(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.stUISelect.bProceed := FALSE;
stSDS.eSeqState := E_SDSSeqState.Retract_Garage_1;
END_IF
E_SDSSeqState.Retract_Garage_1:
RetractGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
END_IF
E_SDSSeqState.Home_Garage:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Select_Garage_Slot_for_Loading;
END_IF
E_SDSSeqState.Select_Garage_Slot_for_Loading:
SelectGarageSlotForLoading(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Garage_Face_Transfer_Arm;
END_IF
E_SDSSeqState.Garage_Face_Transfer_Arm:
GarageFaceTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Rotate_Garage_for_Clearance_to_Lower;
END_IF
E_SDSSeqState.Rotate_Garage_for_Clearance_to_Lower:
RotateGarageForClearance(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Move_Garage_to_Slot_Height;
END_IF
E_SDSSeqState.Move_Garage_to_Slot_Height:
MoveGarageToSlotHeight(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Extend_Transfer_Arm_for_Insertion;
END_IF
E_SDSSeqState.Extend_Transfer_Arm_for_Insertion:
ExtendTransferArmForInsertion(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Fine_Adjustment:
FineAdjustment(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Screw_Sample_into_Garage;
END_IF
E_SDSSeqState.Screw_Sample_into_Garage:
ScrewSampleIntoGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Confirm_Insertion;
END_IF
E_SDSSeqState.Confirm_Insertion:
ConfirmInsertion(
stSDS := stSDS
);
IF stSDS.bSuccess AND stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Update_Sample_Database;
ELSIF stSDS.stUISelect.bRetry THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Update_Sample_Database:
UpdateSampleLocationDatabaseGarageToArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess OR stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Transfer_Arm;
END_IF
E_SDSSeqState.Retract_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Rotate_Garage_for_Clearance_to_Raise;
END_IF
E_SDSSeqState.Rotate_Garage_for_Clearance_to_Raise:
RotateGarageForClearance(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Garage_2;
END_IF
E_SDSSeqState.Retract_Garage_2:
RetractGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Return_Garage_to_Home;
END_IF
E_SDSSeqState.Return_Garage_to_Home:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eState := E_SDSState.Standby;
END_IF
END_CASE
END_METHOD
METHOD PRIVATE UnloadDiffractometerToArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
CASE stSDS.eSeqState OF
E_SDSSeqState.Inactive:
stSDS.eSeqState := E_SDSSeqState.Home_Transfer_Arm;
E_SDSSeqState.Home_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Check_Garage_Height;
END_IF
E_SDSSeqState.Check_Garage_Height:
IF stSDS.bGarageAboveCollisionHeight THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
ELSE
stSDS.eSeqState := E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise;
END_IF
E_SDSSeqState.Manually_Rotate_Garage_for_Clearance_to_Raise:
RotateGarageForClearanceManual(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Garage_1;
END_IF
E_SDSSeqState.Retract_Garage_1:
RetractGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Home_Garage;
END_IF
E_SDSSeqState.Home_Garage:
HomeGarage(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Move_Diffractometer_to_Load_Unload_Position;
END_IF
E_SDSSeqState.Move_Diffractometer_to_Load_Unload_Position:
MoveDiffractometerToLoadUnloadPosition(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Confirm_Vacuum_Valve_Open;
END_IF
E_SDSSeqState.Confirm_Vacuum_Valve_Open:
ConfirmVacuumValveOpen(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Prerotate_Transfer_Arm_for_Extraction;
END_IF
E_SDSSeqState.Prerotate_Transfer_Arm_for_Extraction:
PrerotateTransferArmForExtraction(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Extend_Transfer_Arm_for_Diffractometer_Extraction;
END_IF
E_SDSSeqState.Extend_Transfer_Arm_for_Diffractometer_Extraction:
ExtendTransferArmForDiffractometerExtraction(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Fine_Adjustment:
FineAdjustment(
stSDS := stSDS
);
IF stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Unscrew_Sample_from_Diffractometer;
END_IF
E_SDSSeqState.Unscrew_Sample_from_Diffractometer:
UnscrewSampleFromDiffractometer(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Confirm_Diffractometer_Extraction;
END_IF
E_SDSSeqState.Confirm_Diffractometer_Extraction:
ConfirmDiffractometerExtraction(
stSDS := stSDS
);
IF stSDS.bSuccess AND stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Update_Sample_Database;
ELSIF stSDS.stUISelect.bRetry THEN
stSDS.eSeqState := E_SDSSeqState.Fine_Adjustment;
END_IF
E_SDSSeqState.Update_Sample_Database:
UpdateSampleLocationDatabaseDiffToArm(
stSDS := stSDS,
stSDSP := stSDSP
);
IF stSDS.bSuccess OR stSDS.stUISelect.bProceed THEN
stSDS.eSeqState := E_SDSSeqState.Retract_Transfer_Arm;
END_IF
E_SDSSeqState.Retract_Transfer_Arm:
HomeTransferArm(
stSDS := stSDS
);
IF stSDS.bSuccess THEN
stSDS.eSeqState := E_SDSSeqState.Rotate_Garage_for_Clearance_to_Raise;
END_IF
END_CASE
END_METHOD
METHOD PRIVATE UnscrewSampleFromDiffractometer
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Unscrewing sample from diffractometer.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.UNSCREW_FROM_DIFF,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE UnscrewSampleFromGarage
VAR_IN_OUT
stSDS : ST_SDS;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Unscrewing sample from garage.';
SetPositionStateWaitForArrivalQuitIfError(
eStateGet := stSDS.eTransferArmRStateGet,
stMotionStage := stSDS.stTransferArmRStage,
eStateSet := stSDS.eTransferArmRStateSet,
eEnumSet := E_SDSTransferArmRStates.UNSCREW_FROM_GARAGE,
bError := stSDS.bError,
sErrorMsg := stSDS.sErrorMsg,
bArrived => stSDS.bSuccess
);
END_METHOD
METHOD PRIVATE UpdateLoadUnloadAbleStatuses
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
// Determine whether the transfer arm is loadable or unloadable.
// If the transfer arm is disabled, then nothing is allowed.
// If the transfer arm is empty, it can be loaded.
// If the transfer arm is full, it can be unloaded.
CASE stSDSP.stTransferArmSlot.eState OF
E_SampleSlotStates.EMPTY:
stSDS.bTransferArmLoadable := TRUE;
stSDS.bTransferArmUnloadable := FALSE;
E_SampleSlotStates.FULL:
stSDS.bTransferArmLoadable := FALSE;
stSDS.bTransferArmUnloadable := TRUE;
E_SampleSlotStates.DISABLED:
stSDS.bTransferArmLoadable := FALSE;
stSDS.bTransferArmUnloadable := FALSE;
END_CASE
// Determine whether the diffractometer slot is loadable or unloadable.
// If the diffractometer slot is disabled, then nothing is allowed.
// If the diffractometer slot is empty, it can be loaded.
// If the diffractometer slot is full, it can be unloaded.
CASE stSDSP.stDiffractometerSlot.eState OF
E_SampleSlotStates.EMPTY:
stSDS.bDiffractometerLoadable := TRUE;
stSDS.bDiffractometerUnloadable := FALSE;
E_SampleSlotStates.FULL:
stSDS.bDiffractometerLoadable := FALSE;
stSDS.bDiffractometerUnloadable := TRUE;
E_SampleSlotStates.DISABLED:
stSDS.bDiffractometerLoadable := FALSE;
stSDS.bDiffractometerUnloadable := FALSE;
END_CASE
// Determine whether the garage is loadable or unloadable.
// If all slots in the garage are disabled, then nothing is allowed.
// If at least one slot in the garage is empty, it can be loaded.
// If at least one slot in the garage is full, it can be unloaded.
stSDS.bGarageLoadable := FALSE;
stSDS.bGarageUnloadable := FALSE;
FOR nIndex := 0 TO stSDS.nGarageSlots - 1 BY 1 DO
CASE stSDSP.astGarageSlot[nIndex].eState OF
E_SampleSlotStates.EMPTY:
stSDS.bGarageLoadable := TRUE;
E_SampleSlotStates.FULL:
stSDS.bGarageUnloadable := TRUE;
E_SampleSlotStates.DISABLED:
stSDS.bGarageLoadable := FALSE;
stSDS.bGarageUnloadable := FALSE;
END_CASE
END_FOR
END_METHOD
METHOD PRIVATE UpdatePositionStates
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
// Update parameters for transfer arm states.
// Horizontal axis states
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.RETRACTED], sName:='RETRACTED',
fPosition:=0,
fVelocity:=2,
fDelta := 2.5
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.GARAGE_SAMPLE_REMOVAL], sName:='GARAGE_SAMPLE_REMOVAL',
fPosition:=28,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.CONFIRM_GARAGE_EXTRACTION], sName:='CONFIRM_GARAGE_EXTRACTION',
fPosition:=23,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.GARAGE_SAMPLE_INSERTION], sName:='GARAGE_SAMPLE_INSERTION',
fPosition:=30,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.CONFIRM_GARAGE_INSERTION], sName:='CONFIRM_GARAGE_INSERTION',
fPosition:=25,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.DIFF_SAMPLE_INSERTION], sName:='DIFF_SAMPLE_INSERTION',
fPosition:=100,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.DIFF_SAMPLE_EXTRACTION], sName:='DIFF_SAMPLE_EXTRACTION',
fPosition:=100,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.CONFIRM_DIFF_INSERTION], sName:='CONFIRM_DIFF_INSERTION',
fPosition:=75,
fVelocity:=2
);
fbStateSetup(stPositionState:=stSDS.astTransferArmHPosState[E_SDSTransferArmHStates.CONFIRM_DIFF_EXTRACTION], sName:='CONFIRM_DIFF_EXTRACTION',
fPosition:=75,
fVelocity:=2
);
// Rotational axis states
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.FULLY_CCW], sName:='FULLY_CCW',
fPosition:=0,
fVelocity:=stSDS.fGarageRotationSpeed,
fDelta := 5.0
);
IF NOT stSDS.stTransferArmRStage.bLimitForwardEnable THEN
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.PREROTATE_SAMPLE_REMOVAL], sName:='PREROTATE_SAMPLE_REMOVAL',
fPosition:=stSDS.stTransferArmRStage.Axis.NcToPlc.ActPos,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
ELSE
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.PREROTATE_SAMPLE_REMOVAL], sName:='PREROTATE_SAMPLE_REMOVAL',
fPosition:=20000,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
END_IF
IF NOT stSDS.stTransferArmRStage.bLimitBackwardEnable THEN
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.PREROTATE_SAMPLE_INSERTION], sName:='PREROTATE_SAMPLE_INSERTION',
fPosition:=stSDS.stTransferArmRStage.Axis.NcToPlc.ActPos,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
ELSE
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.PREROTATE_SAMPLE_INSERTION], sName:='PREROTATE_SAMPLE_INSERTION',
fPosition:=-20000,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
END_IF
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.UNSCREW_FROM_GARAGE], sName:='UNSCREW_FROM_GARAGE',
fPosition:=0,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.UNSCREW_FROM_DIFF], sName:='UNSCREW_FROM_DIFF',
fPosition:=0,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.SCREW_INTO_GARAGE], sName:='SCREW_INTO_GARAGE',
fPosition:=1700,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.SCREW_INTO_DIFF], sName:='SCREW_INTO_DIFF',
fPosition:=1700,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
fbStateSetup(stPositionState:=stSDS.astTransferArmRPosState[E_SDSTransferArmRStates.SCREW_IN_HALF_TURN], sName:='SCREW_IN_HALF_TURN',
fPosition:=stSDS.stTransferArmRStage.Axis.NcToPlc.ActPos + 180,
fVelocity:=stSDS.fTransferArmRotationSpeed
);
// Update parameters for garage states.
// Y axis states
fbStateSetup(
stPositionState := stSDS.astGarageYPosState[E_SDSGarageYStates.RETRACTED], sName:='RETRACTED',
fPosition := 0.0,
fVelocity := 1.0,
fDelta := 10.0
);
FOR nIndex := E_SDSGarageYStates.T1 TO E_SDSGarageYStates.B4 BY 1 DO
fbStateSetup(
stPositionState := stSDS.astGarageYPosState[nIndex],
sName := stSDSP.astGarageSlot[nIndex].sTag,
fPosition := stSDSP.astGarageSlot[nIndex].fYPos,
fVelocity := 1.0,
);
END_FOR
// Rotational axis states
fbStateSetup(
stPositionState := stSDS.astGarageRPosState[E_SDSGarageRStates.PREDICTED_HOME], sName:='PREDICTED_HOME',
fPosition := 0.0,
fVelocity := 10.0,
fDelta := 1.0,
bLocked := TRUE
);
FOR nIndex := E_SDSGarageYStates.T1 TO E_SDSGarageYStates.B4 BY 1 DO
CASE stSDS.eFaceState OF
E_SDSFaceState.FACE_SIDE_PORT:
fbStateSetup(
stPositionState := stSDS.astGarageRPosState[nIndex],
sName := stSDSP.astGarageSlot[nIndex].sTag,
fPosition := stSDSP.astGarageSlot[nIndex].fDeg + stSDS.fFacePortOffset,
fVelocity := 10.0,
);
ELSE
fbStateSetup(
stPositionState := stSDS.astGarageRPosState[nIndex],
sName := stSDSP.astGarageSlot[nIndex].sTag,
fPosition := stSDSP.astGarageSlot[nIndex].fDeg,
fVelocity := 10.0,
);
END_CASE
END_FOR
END_METHOD
METHOD PRIVATE UpdateSampleLocationDatabaseDiffToArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
VAR
stTempSlot : ST_SampleSlot;
END_VAR
stSDS.bSuccess := FALSE;
stTempSlot := stSDSP.stTransferArmSlot;
stSDS.sStatusMsg := 'Updating the sample database.';
IF stSDS.eSelectedGarageSlot >= E_SDSGarageYStates.T1 AND stSDS.eSelectedGarageSlot <= E_SDSGarageYStates.B4 THEN
stSDSP.stTransferArmSlot.eState := stSDSP.stDiffractometerSlot.eState;
stSDSP.stTransferArmSlot.sDesc := stSDSP.stDiffractometerSlot.sDesc;
stSDSP.stDiffractometerSlot.eState := stTempSlot.eState;
stSDSP.stDiffractometerSlot.sDesc := stTempSlot.sDesc;
stSDS.bSuccess := TRUE;
ELSE
stSDS.sErrorMsg := 'Failed to automatically update sample position in database. Invalid slot selected. Sequence will continue, but please manually update the database from the interface. After manually updating the database, click "proceed" to continue.';
END_IF
END_METHOD
METHOD PRIVATE UpdateSampleLocationDatabaseGarageToArm
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
VAR
stTempSlot : ST_SampleSlot;
END_VAR
stSDS.bSuccess := FALSE;
stTempSlot := stSDSP.stTransferArmSlot;
stSDS.sStatusMsg := 'Updating the sample database.';
IF stSDS.eSelectedGarageSlot >= E_SDSGarageYStates.T1 AND stSDS.eSelectedGarageSlot <= E_SDSGarageYStates.B4 THEN
stSDSP.stTransferArmSlot.eState := stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].eState;
stSDSP.stTransferArmSlot.sDesc := stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].sDesc;
stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].eState := stTempSlot.eState;
stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].sDesc := stTempSlot.sDesc;
stSDS.bSuccess := TRUE;
ELSE
stSDS.sErrorMsg := 'Failed to automatically update sample position in database. Invalid slot selected. Sequence will continue, but please manually update the database from the interface. After manually updating the database, click "proceed" to continue.';
END_IF
END_METHOD
METHOD PRIVATE UpdateSampleLocationDatabaseManualExtraction
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Please extract the sample. Click "proceed" when you have finished.';
IF stSDS.eSelectedGarageSlot >= E_SDSGarageYStates.T1 AND stSDS.eSelectedGarageSlot <= E_SDSGarageYStates.B4 THEN
stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].eState := E_SampleSlotStates.EMPTY;
stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].sDesc := '';
ELSE
stSDS.sErrorMsg := 'Failed to update sample position in database. Invalid slot selected. Sequence will continue, but please manually update the database from the interface.';
END_IF
END_METHOD
METHOD PRIVATE UpdateSampleLocationDatabaseManualInsertion
VAR_IN_OUT
stSDS : ST_SDS;
stSDSP : ST_SDSPersistent;
END_VAR
stSDS.bSuccess := FALSE;
stSDS.sStatusMsg := 'Please insert the sample and write an identifying description for it in the text field. Click "proceed" when you have finished.';
IF stSDS.eSelectedGarageSlot >= E_SDSGarageYStates.T1 AND stSDS.eSelectedGarageSlot <= E_SDSGarageYStates.B4 THEN
stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].eState := E_SampleSlotStates.FULL;
stSDSP.astGarageSlot[stSDS.eSelectedGarageSlot].sDesc := stSDS.stUISelect.sTextInput;
ELSE
stSDS.sErrorMsg := 'Failed to update sample position in database. Invalid slot selected. Sequence will continue, but please manually update the database from the interface.';
END_IF
END_METHOD
FB_Slits
FUNCTION_BLOCK FB_Slits
VAR_IN_OUT
stTopBlade: ST_MotionStage;
stBottomBlade: ST_MotionStage;
stLeftBlade: ST_MotionStage; // Left Slit from upstream view
stRightBlade: ST_MotionStage; // Right Slit from upstream view
bExecuteMotion:BOOL ;
io_fbFFHWO : FB_HardwareFFOutput;
fbArbiter: FB_Arbiter();
END_VAR
VAR_INPUT
{attribute 'pytmc' := '
pv: PMPS_OK;
io: i;
field: ZNAM False
field: ONAM True
'}
bMoveOk:BOOL;
(*Offsets*)
{attribute 'pytmc' := '
pv: Offset_Top;
io: io;
'}
rEncoderOffsetTop: REAL;
{attribute 'pytmc' := '
pv: ZeroOffset_Bottom;
io: io;
'}
rEncoderOffsetBottom: REAL;
{attribute 'pytmc' := 'pv: ZeroOffset_Left; io: io;'}
rEncoderOffsetLeft: REAL;
{attribute 'pytmc' := '
pv: ZeroOffset_Right;
io: io;
'}
rEncoderOffsetRight: REAL;
i_DevName : STRING; //device name for FFO and PMPS diagnostics
{attribute 'pytmc' := '
pv: Home;
io: i;
field: ZNAM False
field: ONAM True
'}
bHome:BOOL:=FALSE;
END_VAR
VAR
fbTopBlade: FB_MotionStage;
fbBottomBlade: FB_MotionStage;
fbLeftBlade: FB_MotionStage;
fbRightBlade: FB_MotionStage;
fPosTopBlade: LREAL;
fPosBottomBlade: LREAL;
fPosLeftBlade: LREAL;
fPosRightBlade: LREAL;
bCollisionLimitationVert: BOOL;
bCollisionLimitationHorz: BOOL;
(*Motion Parameters*)
fSmallDelta: LREAL := 0.01;
fBigDelta: LREAL := 20;
fMaxVelocity: LREAL := 0.5;
fHighAccel: LREAL := 0.8;
fLowAccel: LREAL := 0.1;
stTop: ST_PositionState;
stBOTTOM: ST_PositionState;
stLeft: ST_PositionState;
stRight: ST_PositionState;
{attribute 'pytmc' := 'pv: TOP'}
fbTop: FB_StatePTPMove;
{attribute 'pytmc' := 'pv: BOTTOM'}
fbBottom: FB_StatePTPMove;
{attribute 'pytmc' := 'pv: LEFT'}
fbLeft: FB_StatePTPMove;
{attribute 'pytmc' := 'pv: RIGHT'}
fbRight: FB_StatePTPMove;
(*EPICS pvs*)
{attribute 'pytmc' := '
pv: XWID_REQ;
io: io;
'}
rReqApertureSizeX : REAL;
{attribute 'pytmc' := '
pv: YWID_REQ;
io: io;
'}
rReqApertureSizeY : REAL;
{attribute 'pytmc' := '
pv: XCEN_REQ;
io: io;
'}
rReqCenterX: REAL;
{attribute 'pytmc' := '
pv: YCEN_REQ;
io: io;
'}
rReqCenterY: REAL;
{attribute 'pytmc' := '
pv: ACTUAL_XWIDTH;
io: io;
'}
rActApertureSizeX : REAL;
{attribute 'pytmc' := '
pv: ACTUAL_YWIDTH;
io: io;
'}
rActApertureSizeY : REAL;
{attribute 'pytmc' := '
pv: ACTUAL_XCENTER;
io: io;
'}
rActCenterX: REAL;
{attribute 'pytmc' := '
pv: ACTUAL_YCENTER;
io: io;
'}
rActCenterY: REAL;
{attribute 'pytmc' := '
pv: XCEN_SETZERO;
io: io;
'}
rSetCenterX: BOOL;
{attribute 'pytmc' := '
pv: YCEN_SETZERO;
io: io;
'}
rSetCenterY: BOOL;
{attribute 'pytmc' := '
pv: OPEN;
io: io;
field: ZNAM False
field: ONAM True
'}
bOpen: BOOL;
{attribute 'pytmc' := '
pv: CLOSE;
io: io;
field: ZNAM False
field: ONAM True
'}
bClose: BOOL;
{attribute 'pytmc' := '
pv: BLOCK;
io: io;
field: ZNAM False
field: ONAM True
'}
bBlock: BOOL;
{attribute 'pytmc' := '
pv: HOME_READY;
io: i;
field: ZNAM False
field: ONAM True
'}
bHomeReady:BOOL:=FALSE;
//Local variables
bInit: BOOL :=true;
rTrig_Block: R_TRIG;
rTrig_Open: R_TRIG;
rTrig_Close: R_TRIG;
//old values
rOldReqApertureSizeX : REAL;
rOldReqApertureSizeY : REAL;
rOldReqCenterX: REAL;
rOldReqCenterY: REAL;
bExecuteMotionX: BOOL;
bExecuteMotionY: BOOL;
fPosBlock: LREAL;
fPosClose: LREAL;
fPosOpen: LREAL;
stSetPositionOptions:ST_SetPositionOptions;
fbSetPosition_TOP: MC_SetPosition;
fbSetPosition_Bottom: MC_SetPosition;
fbSetPosition_Left: MC_SetPosition;
fbSetPosition_Right: MC_SetPosition;
// For logging
fbLogger : FB_LogMessage := (eSubsystem:=E_SubSystem.MOTION);
tErrorPresent : R_TRIG;
tAction : R_TRIG;
tOverrideActivated : R_TRIG;
FFO : FB_FastFault :=(
i_DevName := 'Slits',
i_Desc := 'Fault occurs when center is greated than that requested',
i_TypeCode := 16#1010);
bTest: BOOL;
AptArrayStatus AT %Q* : ST_PMPS_Aperture_IO;
AptArrayReq AT %I* : ST_PMPS_Aperture_IO;
fbPower: MC_Power;
END_VAR
// Initialize
ACT_init();
// Instantiate Function block for all the blades
ACT_Motion();
//SET and GET the requested and Actual values
ACT_CalculatePositions();
// Software Limits to protect blades
ACT_VirtualLimitSW();
END_FUNCTION_BLOCK
ACTION ACT_BLOCK:
rTrig_Block (CLK:= bBlock);
rTrig_Open (CLK:= bOpen);
rTrig_Close (CLK:= bClose);
if (rTrig_Block.Q) THEN
//BLOCK
bBlock := false;
END_IF
if (rTrig_Open.Q) THEN
bOpen := false;
END_IF
if (rTrig_Close.Q) THEN
bClose := false;
END_IF
END_ACTION
ACTION ACT_CalculatePositions:
//check if requested center or gap has changed
//check that the requested values are within acceptable motion range
IF (rOldReqApertureSizeX <> rReqApertureSizeX) THEN
rOldReqApertureSizeX := rReqApertureSizeX;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new X gap.', eSevr:=TcEventSeverity.Verbose);
END_IF
IF (rOldReqCenterX <> rReqCenterX) THEN
rOldReqCenterX := rReqCenterX;
bExecuteMotionX := TRUE;
fbLogger(sMsg:='Requested new X center', eSevr:=TcEventSeverity.Verbose);
// ELSE
// rReqCenterX := rActCenterX;
END_IF
IF (rOldReqApertureSizeY <> rReqApertureSizeY) THEN
rOldReqApertureSizeY := rReqApertureSizeY;
bExecuteMotionY := TRUE;
fbLogger(sMsg:='Requested new Y gap.', eSevr:=TcEventSeverity.Verbose);
END_IF
IF (rOldReqCenterY <> rReqCenterY) THEN
rOldReqCenterY := rReqCenterY;
bExecuteMotionY := TRUE;
fbLogger(sMsg:='Requested new Y center.', eSevr:=TcEventSeverity.Verbose);
// ELSE
// rReqCenterY := rActCenterY;
END_IF
//Calculate requested target positions from requested gap and center
fPosTopBlade := (rReqApertureSizeY/2) + (rReqCenterY + rEncoderOffsetTop) ;
fPosBottomBlade := (-1*rReqApertureSizeY/2) + (rReqCenterY+rEncoderOffsetBottom);
fPosLeftBlade := (rReqApertureSizeX/2) + (rReqCenterX + rEncoderOffsetLeft);
fPosRightBlade := (-1*rReqApertureSizeX/2) + (rReqCenterX + rEncoderOffsetRight);
//Calculate actual gap and center from actual stages positions
rActApertureSizeX := LREAL_TO_REAL((stLeftBlade.stAxisStatus.fActPosition - rEncoderOffsetLeft) - (stRightBlade.stAxisStatus.fActPosition- rEncoderOffsetRight));
rActApertureSizeY := LREAL_TO_REAL((stTopBlade.stAxisStatus.fActPosition - rEncoderOffsetTop) - (stBottomBlade.stAxisStatus.fActPosition - rEncoderOffsetBottom));
rActCenterX := LREAL_TO_REAL((((stLeftBlade.stAxisStatus.fActPosition - rEncoderOffsetLeft) + (stRightBlade.stAxisStatus.fActPosition - rEncoderOffsetRight ))/2));
rActCenterY := LREAL_TO_REAL((((stTopBlade.stAxisStatus.fActPosition - rEncoderOffsetTop) + (stBottomBlade.stAxisStatus.fActPosition - rEncoderOffsetBottom))/2));
// Prevent collision
bCollisionLimitationHorz := (rActApertureSizeX > -0.1);
bCollisionLimitationVert := (rActApertureSizeY > -0.1);
END_ACTION
ACTION ACT_Init:
// init the motion stages parameters
IF ( bInit) THEN
stTopBlade.bHardwareEnable := TRUE;
stBottomBlade.bHardwareEnable := TRUE;
stLeftBlade.bHardwareEnable := TRUE;
stRightBlade.bHardwareEnable := TRUE;
stTopBlade.bPowerSelf :=FALSE;
stBottomBlade.bPowerSelf :=FALSE;
stLeftBlade.bPowerSelf :=FALSE;
stRightBlade.bPowerSelf :=FALSE;
stTopBlade.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
stBottomBlade.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
stLeftBlade.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
stRightBlade.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
FFO.i_DevName := i_DevName;
END_IF
END_ACTION
ACTION ACT_Motion:
// Instantiate Function block for all the blades
fbTopBlade(stMotionStage:=stTopBlade);
fbBottomBlade(stMotionStage:=stBottomBlade);
fbLeftBlade(stMotionStage:=stLeftBlade);
fbRightBlade(stMotionStage:=stRightBlade);
// PTP Motion for each blade
stTop.sName := 'Top';
stTop.fPosition := fPosTopBlade;
stTop.fDelta := fSmallDelta;
stTop.fVelocity := fMaxVelocity;
stTop.fAccel := fHighAccel;
stTop.fDecel := fHighAccel;
stBOTTOM.sName := 'Bottom';
stBOTTOM.fPosition := fPosBottomBlade;
stBOTTOM.fDelta := fSmallDelta;
stBOTTOM.fVelocity := fMaxVelocity;
stBOTTOM.fAccel := fHighAccel;
stBOTTOM.fDecel := fHighAccel;
stLeft.sName := 'Left';
stLeft.fPosition := fPosLeftBlade;
stLeft.fDelta := fSmallDelta;
stLeft.fVelocity := fMaxVelocity;
stLeft.fAccel := fHighAccel;
stLeft.fDecel := fHighAccel;
stRight.sName := 'Right';
stRight.fPosition := fPosRightBlade;
stRight.fDelta := fSmallDelta;
stRight.fVelocity := fMaxVelocity;
stRight.fAccel := fHighAccel;
stRight.fDecel := fHighAccel;
IF (bExecuteMotionY) THEN
fbTop.bExecute := fbBottom.bExecute := bExecuteMotionY;
bExecuteMotionY:= FALSE;
END_IF
IF (bExecuteMotionX) THEN
fbLeft.bExecute := fbRight.bExecute := bExecuteMotionX;
bExecuteMotionX:= FALSE;
END_IF
fbTop(
stPositionState:=stTop,
bMoveOk:=bMoveOk,
stMotionStage:=stTopBlade);
fbBottom(
stPositionState:=stBOTTOM,
bMoveOk:=bMoveOk,
stMotionStage:=stBottomBlade);
fbLeft(
stPositionState:=stLeft,
bMoveOk:=bMoveOk,
stMotionStage:=stLeftBlade);
fbRight(
stPositionState:=stRight,
bMoveOk:=bMoveOk,
stMotionStage:=stRightBlade);
END_ACTION
ACTION ACT_VirtualLimitSW:
// Force set to false
stLeftBlade.bPowerSelf := stRightBlade.bPowerSelf := stTopBlade.bPowerSelf := stBottomBlade.bPowerSelf := FALSE;
// Set SafetyReady flags manually
stTopBlade.bSafetyReady:= TRUE;
stBottomBlade.bSafetyReady:= TRUE;
stLeftBlade.bSafetyReady:= TRUE;
stRightBlade.bSafetyReady:= TRUE;
// Note: FB_MotionStage calls FB_SetEnables internally that overwrites .bAllForwardEnable/.bAllBackwardEnable flags
fbPower(
Axis := stTopBlade.Axis,
Enable := stTopBlade.bAllEnable,
Enable_Positive := stTopBlade.bAllForwardEnable,
Enable_Negative := stTopBlade.bAllBackwardEnable AND bCollisionLimitationVert,
Override := 100.000
);
fbPower(
Axis := stBottomBlade.Axis,
Enable := stBottomBlade.bAllEnable,
Enable_Positive := stBottomBlade.bAllForwardEnable AND bCollisionLimitationVert,
Enable_Negative := stBottomBlade.bAllBackwardEnable,
Override := 100.000
);
fbPower(
Axis := stLeftBlade.Axis,
Enable := stLeftBlade.bAllEnable,
Enable_Positive := stLeftBlade.bAllForwardEnable,
Enable_Negative := stLeftBlade.bAllBackwardEnable AND bCollisionLimitationHorz,
Override := 100.000
);
fbPower(
Axis := stRightBlade.Axis,
Enable := stRightBlade.bAllEnable,
Enable_Positive := stRightBlade.bAllForwardEnable AND bCollisionLimitationHorz,
Enable_Negative := stRightBlade.bAllBackwardEnable,
Override := 100.000
);
END_ACTION
PRG_1_PlcTask
PROGRAM PRG_1_PlcTask
VAR
END_VAR
PRG_2_PMPS();
PRG_3_LOG();
PRG_Diffractometer();
PRG_LAS();
PRG_SDS();
PRG_RotDet();
//PRG_Questar();
PRG_SpetrometerArm();
PRG_SSL();
PRG_Cryo();
END_PROGRAM
PRG_2_PMPS
PROGRAM PRG_2_PMPS
VAR
fbArbiterIO: FB_SubSysToArbiter_IO;
END_VAR
GVL_PMPS.fbFastFaultOutput1.Execute();
GVL_PMPS.fbFastFaultOutput2.Execute();
fbArbiterIO(
Arbiter:=GVL_PMPS.fbArbiter,
fbFFHWO:=GVL_PMPS.fbFastFaultOutput1);
END_PROGRAM
- Related:
PRG_2Theta
PROGRAM PRG_2Theta
VAR
bInit: BOOL := TRUE;
fbPower_AxisM2: MC_Power;
fb2ThetaStepper : FB_MotionStage;
bMoveOk: BOOL;
iEnableMotorState: UINT;
tonEnableMotorTimeout: TON;
tEnableMotorTimeoutTime: TIME := T#5s;
bMovePos: BOOL;
bMoveNeg: BOOL;
bMovePosAndOK: BOOL;
bMoveNegAndOK: BOOL;
rtExecuteMove: R_TRIG;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^Rack#1A-01 Coupler (EK1100)^Rack#1A-04 DR_2Th (EL7047)^STM Status^Status^Ready'}
b2ThetaDriveReady AT %I*: BOOL;
{attribute 'TcLinkTo' := 'TIPC^qrix_motion^qrix_motion Instance^PlcTask Outputs^PRG_SSL.EL7047_02_01_SSL_Status_to_NC'}
nSSLDriveStatus AT %I* : USINT;
bSSLDriveReady : BOOL;
END_VAR
VAR CONSTANT
fThresholdPS1: REAL := 0.15;
fThresholdPS2: REAL := 0.15;
fThresholdPS3: REAL := 0.15;
END_VAR
IF binit THEN
bInit := FALSE;
// Stepper
//Main.M2.nBrakeMode := ENUM_StageBrakeMode.IF_ENABLED;
// Disabled brake. It actually doesn't help with anything and we found that it
// is sometimes interfering with the movement of the motor. We think it might be
// some sort of lag in the brake disengaging that causes it to fight with the
// drive for a bit, which causes overcurrent faults.
Main.M2.nBrakeMode := ENUM_StageBrakeMode.IF_MOVING;
Main.M2.bBrakeRelease := TRUE;
Main.M2.bHardwareEnable := TRUE;
Main.M2.bPowerSelf := TRUE;
Main.M2.nEnableMode := ENUM_StageEnableMode.NEVER ;
END_IF;
bSSLDriveReady := nSSLDriveStatus.1; // Drive status ready bit after tying it to bFloating.
ACT_Stepper();
END_PROGRAM
ACTION ACT_Stepper:
// Simplified enabling logic by removing separate fbPower function and
// making the estop and other permissives drive the bPowerSelf bit.
// Now, the motor motion is handled entirely within the FB_MotionStage block
// which makes this usage more standard and easier to work with using our
// standard typhos widget.
Main.M2.bHardwareEnable := GVL_EPS.bESTOP // ESTOP
AND GVL_Sensor.bFloating
AND GVL_Sensor.stPS1.fValue > fThresholdPS1
AND GVL_Sensor.stPS2.fValue > fThresholdPS2
AND GVL_Sensor.stPS3.fValue > fThresholdPS3 // Compressed-air pressure
AND GVL_EPS.bOpenSV1
AND GVL_EPS.bOpenSV2;
IF NOT PRG_SSL.bRealignCouple THEN
CASE iEnableMotorState OF
0:
IF Main.M2.bExecute AND GVL_Sensor.bFloating THEN
rtExecuteMove(CLK:=FALSE);
Main.M2.bExecute := FALSE;
Main.M2.bEnable := TRUE;
Main.M1.bEnable := TRUE;
iEnableMotorState := iEnableMotorState + 1;
ELSIF Main.M2.bExecute AND NOT GVL_Sensor.bFloating THEN
Main.M2.bExecute := FALSE;
Main.M2.bError := TRUE;
Main.M1.bError := TRUE;
Main.M2.sCustomErrorMessage := 'Move failed. Arm must be levitating to move.';
Main.M1.sCustomErrorMessage := 'Move failed. Arm must be levitating to move.';
ELSE
Main.M2.bEnable := FALSE;
Main.M1.bEnable := FALSE;
END_IF
tonEnableMotorTimeout(IN:=FALSE);
1: // Wait until the logic confirms both motors are fully enabled
tonEnableMotorTimeout(IN:=NOT Main.M2.bAllEnable OR NOT Main.M1.bAllEnable,PT:=tEnableMotorTimeoutTime);
// Check which direction we are moving, and if the directional enables will allow that move.
bMovePos := Main.M2.stAxisStatus.fActPosition < Main.M2.fPosition;
bMoveNeg := Main.M2.stAxisStatus.fActPosition > Main.M2.fPosition;
bMovePosAndOK := bMovePos AND Main.M2.bAllForwardEnable AND Main.M1.bAllForwardEnable;
bMoveNegAndOK := bMoveNeg AND Main.M2.bAllBackwardEnable AND Main.M1.bAllBackwardEnable;
// Check for fully enabled
IF Main.M2.bAllEnable AND Main.M1.bAllEnable THEN
// Check direction of motion and if that direction is enabled
IF bMovePosAndOK OR bMoveNegAndOK THEN
iEnableMotorState := iEnableMotorState + 1;
tonEnableMotorTimeout(IN:=FALSE);
ELSE
IF NOT Main.M2.bError OR Main.M1.bError THEN
// Direction of motion is not enabled, so let's report the error and give a helpful error message.
Main.M2.bError := TRUE;
Main.M1.bError := TRUE;
IF bMovePos THEN
IF NOT Main.M2.bLimitForwardEnable THEN
Main.M2.sCustomErrorMessage := 'Move forward failed due to forward limit switch.';
Main.M1.sCustomErrorMessage := 'Move forward failed due to forward limit switch for 2 theta motor.';
ELSIF NOT Main.M1.bLimitForwardEnable THEN
Main.M1.sCustomErrorMessage := 'Move forward failed due to forward limit switch.';
Main.M2.sCustomErrorMessage := 'Move forward failed due to forward limit switch for SSL motor.';
ELSIF NOT Main.M2.bGantryForwardEnable AND Main.M2.bGantryAxis THEN
Main.M2.sCustomErrorMessage := 'Move forward failed due to gantry motion tolerance limit.';
Main.M1.sCustomErrorMessage := 'Move forward failed due to gantry motion tolerance limit for 2 theta motor.';
ELSIF NOT Main.M1.bGantryForwardEnable AND Main.M1.bGantryAxis THEN
Main.M1.sCustomErrorMessage := 'Move forward failed due to gantry motion tolerance limit.';
Main.M2.sCustomErrorMessage := 'Move forward failed due to gantry motion tolerance limit for SSL motor.';
ELSIF NOT Main.M2.stEPSForwardEnable.bEPS_OK THEN
Main.M2.sCustomErrorMessage := 'Move forward failed due to EPS condition.';
Main.M1.sCustomErrorMessage := 'Move forward failed due to EPS condition for 2 theta motor.';
ELSIF NOT Main.M1.stEPSForwardEnable.bEPS_OK THEN
Main.M1.sCustomErrorMessage := 'Move forward failed due to EPS condition.';
Main.M2.sCustomErrorMessage := 'Move forward failed due to EPS condition for SSL motor.';
ELSE
Main.M1.sCustomErrorMessage := 'Move forward failed for unspecified reason.';
Main.M2.sCustomErrorMessage := 'Move forward failed for unspecified reason.';
END_IF
ELSIF bMoveNeg THEN
IF NOT Main.M2.bLimitBackwardEnable THEN
Main.M2.sCustomErrorMessage := 'Move backward failed due to forward limit switch.';
Main.M1.sCustomErrorMessage := 'Move backward failed due to forward limit switch for 2 theta motor.';
ELSIF NOT Main.M1.bLimitBackwardEnable THEN
Main.M1.sCustomErrorMessage := 'Move backward failed due to forward limit switch.';
Main.M2.sCustomErrorMessage := 'Move backward failed due to forward limit switch for SSL motor.';
ELSIF NOT Main.M2.bGantryBackwardEnable AND Main.M2.bGantryAxis THEN
Main.M2.sCustomErrorMessage := 'Move backward failed due to gantry motion tolerance limit.';
Main.M1.sCustomErrorMessage := 'Move backward failed due to gantry motion tolerance limit for 2 theta motor.';
ELSIF NOT Main.M1.bGantryBackwardEnable AND Main.M1.bGantryAxis THEN
Main.M1.sCustomErrorMessage := 'Move backward failed due to gantry motion tolerance limit.';
Main.M2.sCustomErrorMessage := 'Move backward failed due to gantry motion tolerance limit for SSL motor.';
ELSIF NOT Main.M2.stEPSBackwardEnable.bEPS_OK THEN
Main.M2.sCustomErrorMessage := 'Move backward failed due to EPS condition.';
Main.M1.sCustomErrorMessage := 'Move backward failed due to EPS condition for 2 theta motor.';
ELSIF NOT Main.M1.stEPSBackwardEnable.bEPS_OK THEN
Main.M1.sCustomErrorMessage := 'Move backward failed due to EPS condition.';
Main.M2.sCustomErrorMessage := 'Move backward failed due to EPS condition for SSL motor.';
ELSE
Main.M2.sCustomErrorMessage := 'Move backward failed for unspecified reason.';
Main.M1.sCustomErrorMessage := 'Move backward failed for unspecified reason.';
END_IF
ELSE
Main.M2.sCustomErrorMessage := 'Move failed because it was not large enough to register.';
Main.M1.sCustomErrorMessage := 'Move failed because it was not large enough to register.';
END_IF
END_IF
iEnableMotorState := 0;
END_IF
ELSIF tonEnableMotorTimeout.Q THEN
IF NOT Main.M2.bError OR Main.M1.bError THEN
Main.M2.bError := TRUE;
Main.M1.bError := TRUE;
IF NOT Main.M2.bEnable THEN
Main.M2.sCustomErrorMessage := 'Move never enabled because bEnable never received.';
Main.M1.sCustomErrorMessage := 'Move never enabled because bEnable never received on 2 theta motor.';
ELSIF NOT Main.M1.bEnable THEN
Main.M1.sCustomErrorMessage := 'Move never enabled because bEnable never received.';
Main.M2.sCustomErrorMessage := 'Move never enabled because bEnable never received on SSL motor.';
ELSIF NOT Main.M2.bHardwareEnable THEN
Main.M2.sCustomErrorMessage := 'Move never enabled because bHardwareEnable set false.';
Main.M1.sCustomErrorMessage := 'Move never enabled because bHardwareEnable set false on 2 theta motor.';
ELSIF NOT Main.M1.bHardwareEnable THEN
Main.M1.sCustomErrorMessage := 'Move never enabled because bHardwareEnable set false.';
Main.M2.sCustomErrorMessage := 'Move never enabled because bHardwareEnable set false on SSL motor.';
ELSIF NOT Main.M2.stEPSPowerEnable.bEPS_OK THEN
Main.M2.sCustomErrorMessage := 'Move never enabled because of EPS condition.';
Main.M1.sCustomErrorMessage := 'Move never enabled because of EPS condition on 2 theta motor.';
ELSIF NOT Main.M1.stEPSPowerEnable.bEPS_OK THEN
Main.M1.sCustomErrorMessage := 'Move never enabled because of EPS condition.';
Main.M2.sCustomErrorMessage := 'Move never enabled because of EPS condition on SSL motor.';
ELSIF NOT Main.M2.bUserEnable THEN
Main.M2.sCustomErrorMessage := 'Move never enabled because user enable is set to false.';
Main.M1.sCustomErrorMessage := 'Move never enabled because user enable is set to false on 2 theta motor.';
ELSIF NOT Main.M1.bUserEnable THEN
Main.M1.sCustomErrorMessage := 'Move never enabled because user enable is set to false.';
Main.M2.sCustomErrorMessage := 'Move never enabled because user enable is set to false on SSL motor.';
ELSE
Main.M2.sCustomErrorMessage := 'Move never enabled in the timeout duration.';
Main.M1.sCustomErrorMessage := 'Move never enabled in the timeout duration.';
END_IF
END_IF
iEnableMotorState := 0;
END_IF
2: // Wait for the motor drives to switch to the ready state before commanding the move.
tonEnableMotorTimeout(IN:=NOT b2ThetaDriveReady OR NOT bSSLDriveReady,PT:=tEnableMotorTimeoutTime);
// Are both drives reporting that they are ready?
IF b2ThetaDriveReady AND bSSLDriveReady THEN
iEnableMotorState := iEnableMotorState + 1;
tonEnableMotorTimeout(IN:=FALSE);
ELSIF tonEnableMotorTimeout.Q THEN
IF NOT Main.M2.bError OR Main.M1.bError THEN
Main.M2.bError := TRUE;
Main.M1.bError := TRUE;
IF NOT b2ThetaDriveReady THEN
Main.M2.sCustomErrorMessage := 'Move failed because motor drive did not get ready status before timeout.';
Main.M1.sCustomErrorMessage := 'Move failed because 2 theta motor drive did not get ready status before timeout.';
ELSE
Main.M1.sCustomErrorMessage := 'Move failed because motor drive did not get ready status before timeout.';
Main.M2.sCustomErrorMessage := 'Move failed because SSL motor drive did not get ready status before timeout.';
END_IF
END_IF
iEnableMotorState := 0;
END_IF
3: // Allow the move execution to begin
rtExecuteMove(CLK:=TRUE);
IF rtExecuteMove.Q THEN
Main.M2.bExecute := TRUE;
END_IF
tonEnableMotorTimeout(IN:=NOT Main.M2.bBusy,PT:=T#100ms);
IF tonEnableMotorTimeout.Q THEN
iEnableMotorState := iEnableMotorState + 1;
END_IF
ELSE
iEnableMotorState := 0;
END_CASE
ELSE
Main.M2.bEnable := TRUE;
Main.M1.bEnable := TRUE;
END_IF
fb2ThetaStepper(stMotionStage:=Main.M2);
END_ACTION
- Related:
PRG_3_LOG
PROGRAM PRG_3_LOG
VAR
fbLogHandler: FB_LogHandler;
END_VAR
fbLogHandler();
END_PROGRAM
PRG_Cryo
PROGRAM PRG_Cryo
VAR
nCryoRotOutEncoderCounts AT %Q*: ULINT;
nCryoRotEncoderOffset: ULINT;
nDiffThetaOutEncoderCounts AT %Q*: ULINT;
nDiffThetaEncoderOffset: ULINT;
fb_Cryo_ROT : FB_MotionStage;
fb_Cryo_X : FB_MotionStage;
fb_Cryo_Y : FB_MotionStage;
fb_Cryo_Z : FB_MotionStage;
fbEPSCryoXBackward : FB_EPS;
fbEPSCryoXForward : FB_EPS;
fbEPSCryoYBackward : FB_EPS;
fbEPSCryoYForward : FB_EPS;
fbEPSCryoZBackward : FB_EPS;
fbEPSCryoZForward : FB_EPS;
fbEPSCryoThetaBackward : FB_EPS;
fbEPSCryoThetaForward : FB_EPS;
bInit : BOOl := TRUE;
nGantryTolerance: LINT;
{attribute 'pytmc' := '
pv: QRIX:CRYO:ROT:bExecuteCouple
io: io
field: DESC Write TRUE to couple cryo rotation axis with diffractometer theta axis.
'}
bCryoRotDiffThetaExecuteCouple: BOOL := FALSE;
{attribute 'pytmc' := '
pv: QRIX:CRYO:ROT:bExecuteDecouple
io: io
field: DESC Write FALSE to decouple cryo rotation axis from diffractometer theta axis.
'}
bCryoRotDiffThetaExecuteDecouple: BOOL := FALSE;
{attribute 'TcLinkTo' := '.Count := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-02^EL5042_03_17 - Diff - Theta - 2Theta^FB Inputs Channel 1^Position'}
stDiffThetaEnc: ST_RenishawAbsEnc;
{attribute 'TcLinkTo' := '.Count := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL5042_02_16^FB Inputs Channel 2^Position'}
stCryoRotEnc: ST_RenishawAbsEnc;
{attribute 'pytmc' := '
pv: QRIX:DIFF:THETACOUPLING
io: io
field: DESC Structure representing cryo and diffractometer theta coupling enablement coordinator.
'}
stCryoRotDiffThetaAutoRealignCouple: ST_AutoRealignCouple;
fbCryoRotDiffThetaAutoRealignCouple: FB_AutoRealignCouple;
fbCryoRotDiffThetaCoupling: FB_GantryAutoCoupling;
{attribute 'TcLinkTo' := '.bLeaderDriveReady := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-02^EL7047_03_16 - Diff - Theta^STM Status^Status^Ready;
.bFollowDriveReady := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_11^STM Status^Status^Ready
'}
{attribute 'pytmc' := '
pv: QRIX:DIFFCRYO:Coord
io: io
field: DESC Structure representing cryo and diffractometer theta coupling enablement coordinator.
'}
stCryoRotDiffThetaCoordinateGantryAxisEnable: ST_CoordinateGantryAxisEnable;
fbCryoRotDiffThetaCoordinateGantryAxisEnable: FB_CoordinateGantryAxisEnable;
{attribute 'TcLinkTo' := '.bFollowerDriveReady := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_12^STM Status^Status^Ready
'}
{attribute 'pytmc' := '
pv: QRIX:CRYO:MLC:X
io: io
field: DESC Structure representing Cryo X multi-leader coupling.
'}
stCryoXMultiLeader: ST_MultiLeaderMotionCoupling;
fbCryoXMultiLeader: FB_MultiLeaderMotionCoupling;
{attribute 'TcLinkTo' := '.bFollowerDriveReady := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_14^STM Status^Status^Ready
'}
{attribute 'pytmc' := '
pv: QRIX:CRYO:MLC:Y
io: io
field: DESC Structure representing Cryo Y multi-leader coupling.
'}
stCryoYMultiLeader: ST_MultiLeaderMotionCoupling;
fbCryoYMultiLeader: FB_MultiLeaderMotionCoupling;
{attribute 'TcLinkTo' := '.bFollowerDriveReady := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_15^STM Status^Status^Ready
'}
{attribute 'pytmc' := '
pv: QRIX:CRYO:MLC:Z
io: io
field: DESC Structure representing Cryo Z multi-leader coupling.
'}
stCryoZMultiLeader: ST_MultiLeaderMotionCoupling;
fbCryoZMultiLeader: FB_MultiLeaderMotionCoupling;
fCryoYNeutralPosition : LREAL;
{attribute 'TcLinkTo' := '.nMicrostepCounter := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_11^ENC Status compact^Counter value;
.bCounterOverflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_11^ENC Status compact^Status^Counter overflow;
.bCounterUnderflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_11^ENC Status compact^Status^Counter underflow'}
fbMeasureReferenceVelocityCryoROT: FB_MeasureReferenceVelocity;
{attribute 'TcLinkTo' := '.nMicrostepCounter := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_12^ENC Status compact^Counter value;
.bCounterOverflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_12^ENC Status compact^Status^Counter overflow;
.bCounterUnderflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_12^ENC Status compact^Status^Counter underflow'}
fbMeasureReferenceVelocityCryoX: FB_MeasureReferenceVelocity;
{attribute 'TcLinkTo' := '.nMicrostepCounter := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_14^ENC Status compact^Counter value;
.bCounterOverflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_14^ENC Status compact^Status^Counter overflow;
.bCounterUnderflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_14^ENC Status compact^Status^Counter underflow'}
fbMeasureReferenceVelocityCryoY: FB_MeasureReferenceVelocity;
{attribute 'TcLinkTo' := '.nMicrostepCounter := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_15^ENC Status compact^Counter value;
.bCounterOverflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_15^ENC Status compact^Status^Counter overflow;
.bCounterUnderflow := TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL7047_02_15^ENC Status compact^Status^Counter underflow'}
fbMeasureReferenceVelocityCryoZ: FB_MeasureReferenceVelocity;
END_VAR
IF bInit THEN
bInit := FALSE;
Main.M46.bHardwareEnable := TRUE;
Main.M46.bPowerSelf := TRUE;
Main.M46.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M46.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M46.fVelocity := 0.1;
Main.M47.bHardwareEnable := TRUE;
Main.M47.bPowerSelf := TRUE;
Main.M47.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M47.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M47.fVelocity := 0.1;
Main.M48.bHardwareEnable := TRUE;
Main.M48.bPowerSelf := TRUE;
Main.M48.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M48.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M48.fVelocity := 0.1;
Main.M49.bHardwareEnable := TRUE;
Main.M49.bPowerSelf := TRUE;
Main.M49.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M49.nEnableMode := ENUM_StageEnableMode.NEVER; // ENUM_StageEnableMode.DURING_MOTION;
Main.M49.fVelocity := 0.1;
Main.M49.bLimitBackwardEnable := TRUE;
Main.M49.bLimitForwardEnable := TRUE;
// Encoder count offset in order to shift the overflow point.
nCryoRotEncoderOffset := 1073741824;
// Encoder count offset in order to shift the overflow point.
nDiffThetaEncoderOffset := 1073741824;
// 0.000000083819 deg/inc => 11,930,469.225 inc/degree
// For 1 degree tolerance, need 11930469 inc
nGantryTolerance := 119304690000;
stCryoRotDiffThetaCoordinateGantryAxisEnable.bEnable := TRUE;
// Allow setting of the reference position for gantry diff limit calculation
stDiffThetaEnc.Ref := 121223792;
stCryoRotEnc.Ref := 0;
stCryoXMultiLeader.bEnable := TRUE;
stCryoXMultiLeader.bUseDynamicLimits := TRUE;
GVL_VAR.fCryoYNeutralPositionAtDiffY0 := 50.7;
stCryoYMultiLeader.bEnable := TRUE;
stCryoYMultiLeader.bUseDynamicLimits := TRUE;
GVL_VAR.fCryoZNeutralPositionAtDiffZ0 := 4.0;
stCryoZMultiLeader.bEnable := TRUE;
stCryoZMultiLeader.bUseDynamicLimits := TRUE;
END_IF
ACT_CryoThetaDiffThetaCoupling();
ACT_CryoXYZCoupling();
ACT_EPS();
fb_Cryo_X(stMotionStage:=Main.M46);
fb_Cryo_Y(stMotionStage:=Main.M47);
fb_Cryo_Z(stMotionStage:=Main.M48);
fb_Cryo_ROT(stMotionStage:=Main.M49);
fbMeasureReferenceVelocityCryoX(
stMotionStage:=Main.M46,
nMicrostepsPerStep:=64,
bBusy=>,
fMeasuredReferenceVelocity=>
);
fbMeasureReferenceVelocityCryoY(
stMotionStage:=Main.M47,
nMicrostepsPerStep:=64,
bBusy=>,
fMeasuredReferenceVelocity=>
);
fbMeasureReferenceVelocityCryoZ(
stMotionStage:=Main.M48,
nMicrostepsPerStep:=64,
bBusy=>,
fMeasuredReferenceVelocity=>
);
fbMeasureReferenceVelocityCryoROT(
stMotionStage:=Main.M49,
nMicrostepsPerStep:=64,
bBusy=>,
fMeasuredReferenceVelocity=>
);
END_PROGRAM
ACTION ACT_CryoThetaDiffThetaCoupling:
// Offset the raw value to ensure the rollover point is not in the path of motion.
// offset of 90 degrees negative for -90 to 270 degree range. 90 degrees because 90/360*2^32 = 1,073,741,824.
nCryoRotOutEncoderCounts := stCryoRotEnc.Count + nCryoRotEncoderOffset;
nDiffThetaOutEncoderCounts := UDINT_TO_ULINT(ULINT_TO_UDINT(stDiffThetaEnc.Count)) + nDiffThetaEncoderOffset;
stCryoRotEnc.Count := stCryoRotEnc.Count + nCryoRotEncoderOffset;
stDiffThetaEnc.Count := UDINT_TO_ULINT(ULINT_TO_UDINT(stDiffThetaEnc.Count)) + nDiffThetaEncoderOffset;
// Cryo Rotation and Diffractometer Theta Coupling
fbCryoRotDiffThetaCoupling(
nGantryTol := nGantryTolerance,
Master := Main.M34,
MasterEnc := stDiffThetaEnc,
Slave := Main.M49,
SlaveEnc := stCryoRotEnc,
bExecuteCouple := stCryoRotDiffThetaAutoRealignCouple.bExecuteCouple,
bExecuteDecouple := stCryoRotDiffThetaAutoRealignCouple.bExecuteDecouple
);
fbCryoRotDiffThetaAutoRealignCouple(
stMotionStageLeader:=Main.M34,
stMotionStageFollow:=Main.M49,
stAutoRealignCouple:=stCryoRotDiffThetaAutoRealignCouple
);
(*fbCryoRotDiffThetaCoordinateGantryAxisEnable(
stMotionStageLeader:=Main.M34,
stMotionStageFollow:=Main.M49,
stCoordinateGantryAxisEnable:=stCryoRotDiffThetaCoordinateGantryAxisEnable
);*)
IF stCryoRotDiffThetaAutoRealignCouple.bCoupled THEN
Main.M49.nEnableMode := E_StageEnableMode.NEVER;
Main.M49.bEnable := TRUE;
ELSE
Main.M49.nEnableMode := E_StageEnableMode.DURING_MOTION;
END_IF
END_ACTION
ACTION ACT_CryoXYZCoupling:
IF Main.M28.bBusy THEN
fbCryoXMultiLeader(
stMotionStageFollow := Main.M46,
stMultiLeaderMotionCoupling := stCryoXMultiLeader,
fTargetPos := Main.M28.Axis.NcToPlc.ActPos,
fTargetVel := Main.M28.Axis.NcToPlc.SetVelo,
fTargetAcc := Main.M28.Axis.NcToPlc.SetAcc,
bError := Main.M28.bError,
tTimeout := T#5s
);
ELSE
fbCryoXMultiLeader(
stMotionStageFollow := Main.M46,
stMultiLeaderMotionCoupling := stCryoXMultiLeader,
fTargetPos := Main.M28.Axis.NcToPlc.ActPos,
fTargetVel := 1.0,
fTargetAcc := 10.0,
bError := Main.M28.bError,
tTimeout := T#5s
);
END_IF
IF stCryoXMultiLeader.bEnable AND stCryoXMultiLeader.bError THEN
Main.M28.bError := TRUE;
END_IF
fCryoYNeutralPosition := GVL_VAR.fCryoYNeutralPositionAtDiffY0 + Main.M29.Axis.NcToPlc.ActPos;
fbCryoYMultiLeader(
stMotionStageFollow := Main.M47,
stMultiLeaderMotionCoupling := stCryoYMultiLeader,
fTargetPos := F_TargetCryoYPosition(
fNeutralPosition := GVL_VAR.fCryoYNeutralPositionAtDiffY0 + Main.M29.Axis.NcToPlc.ActPos,
fBraidDiameter := GVL_VAR.fBraidDiameter,
fPhiAngleDeg := Main.M32.Axis.NcToPlc.ActPos),
fTargetVel := 0.5,
fTargetAcc := 5,
bError := Main.M29.bError OR Main.M32.bError,
tTimeout := T#5s
);
IF stCryoYMultiLeader.bEnable AND stCryoYMultiLeader.bError THEN
Main.M29.bError := TRUE;
Main.M32.bError := TRUE;
END_IF
IF Main.M30.bBusy THEN
fbCryoZMultiLeader(
stMotionStageFollow:=Main.M48,
stMultiLeaderMotionCoupling:=stCryoZMultiLeader,
fTargetPos := fCryoZNeutralPositionAtDiffZ0 + Main.M30.Axis.NcToPlc.ActPos,
fTargetVel := Main.M30.Axis.NcToPlc.SetVelo,
fTargetAcc := Main.M30.Axis.NcToPlc.SetAcc,
bError := Main.M30.bError,
tTimeout := T#5s
);
ELSE
fbCryoZMultiLeader(
stMotionStageFollow:=Main.M48,
stMultiLeaderMotionCoupling:=stCryoZMultiLeader,
fTargetPos := fCryoZNeutralPositionAtDiffZ0 + Main.M30.Axis.NcToPlc.ActPos,
fTargetVel := 1.0,
fTargetAcc := 10.0,
bError := Main.M30.bError,
tTimeout := T#5s
);
END_IF
IF stCryoZMultiLeader.bEnable AND stCryoZMultiLeader.bError THEN
Main.M30.bError := TRUE;
END_IF
END_ACTION
ACTION ACT_EPS:
// Disable movement in the negative direction.
fbEPSCryoXBackward(eps:=Main.M46.stEPSBackwardEnable);
fbEPSCryoXBackward.setDescription('NEGATIVE MOTION');
fbEPSCryoXBackward.setBit(nBits := 0,
bValue := Main.M46.Axis.NcToPlc.ActPos - Main.M28.Axis.NcToPlc.ActPos > -1.0
);
// Disable movement in positive direction.
fbEPSCryoXForward(eps:=Main.M46.stEPSForwardEnable);
fbEPSCryoXForward.setDescription('POSITIVE MOTION');
fbEPSCryoXForward.setBit(nBits:= 0,
bValue := Main.M46.Axis.NcToPlc.ActPos - Main.M28.Axis.NcToPlc.ActPos < 1.0
);
// Disable movement in the negative direction.
fbEPSCryoYBackward(eps:=Main.M47.stEPSBackwardEnable);
fbEPSCryoYBackward.setDescription('NEGATIVE MOTION');
fbEPSCryoYBackward.setBit(nBits := 0,
bValue := Main.M47.Axis.NcToPlc.ActPos - F_TargetCryoYPosition(
fNeutralPosition := GVL_VAR.fCryoYNeutralPositionAtDiffY0 + Main.M29.Axis.NcToPlc.ActPos,
fBraidDiameter := GVL_VAR.fBraidDiameter,
fPhiAngleDeg := Main.M32.Axis.NcToPlc.ActPos) > -1.0
);
// Disable movement in positive direction.
fbEPSCryoYForward(eps:=Main.M47.stEPSForwardEnable);
fbEPSCryoYForward.setDescription('POSITIVE MOTION');
fbEPSCryoYForward.setBit(nBits:= 0,
bValue := Main.M47.Axis.NcToPlc.ActPos - F_TargetCryoYPosition(
fNeutralPosition := GVL_VAR.fCryoYNeutralPositionAtDiffY0 + Main.M29.Axis.NcToPlc.ActPos,
fBraidDiameter := GVL_VAR.fBraidDiameter,
fPhiAngleDeg := Main.M32.Axis.NcToPlc.ActPos) < 1.0
);
// Disable movement in the negative direction.
fbEPSCryoZBackward(eps:=Main.M48.stEPSBackwardEnable);
fbEPSCryoZBackward.setDescription('NEGATIVE MOTION');
fbEPSCryoZBackward.setBit(nBits := 0,
bValue := Main.M48.Axis.NcToPlc.ActPos - (GVL_VAR.fCryoZNeutralPositionAtDiffZ0 + Main.M30.Axis.NcToPlc.ActPos) > -1.0
);
// Disable movement in positive direction.
fbEPSCryoZForward(eps:=Main.M48.stEPSForwardEnable);
fbEPSCryoZForward.setDescription('POSITIVE MOTION');
fbEPSCryoZForward.setBit(nBits:= 0,
bValue := Main.M48.Axis.NcToPlc.ActPos - (GVL_VAR.fCryoZNeutralPositionAtDiffZ0 + Main.M30.Axis.NcToPlc.ActPos) < 1.0
);
// Disable movement in the negative direction.
fbEPSCryoThetaBackward(eps:=Main.M49.stEPSBackwardEnable);
fbEPSCryoThetaBackward.setDescription('NEGATIVE MOTION');
fbEPSCryoThetaBackward.setBit(nBits := 0,
bValue := Main.M49.Axis.NcToPlc.ActPos - Main.M34.Axis.NcToPlc.ActPos > -1.0
);
// Disable movement in positive direction.
fbEPSCryoThetaForward(eps:=Main.M49.stEPSForwardEnable);
fbEPSCryoThetaForward.setDescription('POSITIVE MOTION');
fbEPSCryoThetaForward.setBit(nBits:= 0,
bValue := Main.M49.Axis.NcToPlc.ActPos - Main.M34.Axis.NcToPlc.ActPos < 1.0
);
END_ACTION
PRG_DET_ARM
PROGRAM PRG_DET_ARM
VAR
bInit: BOOL := TRUE;
bExecuteCouple: BOOL := FALSE;
bExecuteDecouple: BOOL := FALSE;
fbMotionYF1: FB_MotionStage;
fbMotionYF2: FB_MotionStage;
fbMotionZF: FB_MotionStage;
fbAutoCoupling: FB_GantryAutoCoupling;
{attribute 'TcLinkTo' := ' .Count := TIIB[BOX-06 ENC_ZDC_YF1 (EL5042)]^FB Inputs Channel 2^Position'}
stRenishawAbsEncMaster: ST_RenishawAbsEnc;
{attribute 'TcLinkTo' := ' .Count := TIIB[BOX-07 ENC_YF2_ZF (EL5042)]^FB Inputs Channel 1^Position'}
stRenishawAbsEncSlave: ST_RenishawAbsEnc;
tonDelay: TON;
tonDelay2: TON;
rtDelay: R_TRIG;
bRunTimer: BOOL;
nState: INT := -1;
bDisableStateMachine: BOOL;
{attribute 'TcLinkTo' := '.nMicrostepCounter := TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-20 DR_YF1 (EL7047)^ENC Status compact^Counter value;
.bCounterOverflow := TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-20 DR_YF1 (EL7047)^ENC Status compact^Status^Counter overflow;
.bCounterUnderflow := TIID^Device 2 (EtherCAT)^BOX-01 Coupler (EK1100)^BOX-20 DR_YF1 (EL7047)^ENC Status compact^Status^Counter underflow'}
fbMeasureReferenceVelocity: FB_MeasureReferenceVelocity;
END_VAR
IF bInit THEN
bInit := FALSE;
bExecuteCouple := TRUE;
Main.M25.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M25.bHardwareEnable := TRUE;
Main.M25.bPowerSelf := TRUE;
Main.M25.fVelocity := 0.1;
Main.M26.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M26.bHardwareEnable := TRUE;
Main.M26.bPowerSelf := TRUE;
Main.M26.fVelocity := 0.1;
Main.M27.nBrakeMode := ENUM_StageBrakeMode.IF_ENABLED;
Main.M27.bHardwareEnable := TRUE;
Main.M27.bPowerSelf := TRUE;
Main.M27.fVelocity := 0.1;
// M25 and M26 are coupled with M25 as the leader.
// The motors cannot be set to always enabled, because they have brakes that
// only activate when the motors are not moving. Due to the weight
// of the frame, it appears that if the brake is not on, the motors
// are moving constantly to keep the frame from falling. So essentially,
// "is moving" is synonymous with "is enabled" for these motors which
// results in the brake only applying if the motors are disabled. Setting M25
// to only enable during motion, and M26 to NEVER means that
// M25 will be enabled when commanded to move from EPICS but
// for M26 the FB_MotionStage will not modify the bEnable boolean
// in the state machine. Therefore, we can set the M26 bEnable
// status based on the M25 bEnable status after the M25 FB_MotionStage
// runs. But this also allows the brake to still activate when M26 is
// not moving.
Main.M25.nEnableMode := ENUM_StageEnableMode.NEVER ;
// M26 bEnable is coupled to M25 bEnable below. Set M26 enable mode to NEVER
// so that the function block does not try to set the bEnable bit unless
// it detects an error. This allows the bEnable to be set outside of
// the function block.
Main.M26.nEnableMode := ENUM_StageEnableMode.NEVER ;
Main.M27.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
END_IF
stRenishawAbsEncMaster.Ref := 15370335;
stRenishawAbsEncSlave.Ref := 15347257;
// AutoCoupling
fbAutoCoupling( nGantryTol := 200000, // 50nm/count
Master := Main.M25,
MasterEnc := stRenishawAbsEncMaster,
Slave := Main.M26,
SlaveEnc := stRenishawAbsEncSlave,
bExecuteCouple := bExecuteCouple,
bExecuteDecouple := bExecuteDecouple,
);
// I don't think these 3 lines do anything but I am keeping for now.
// The FB_MotionStage overwrites the bAllEnable bit internally.
Main.M25.bAllEnable := Main.M25.bEnable AND GVL_EPS.bESTOP;
Main.M26.bAllEnable := Main.M26.bEnable AND GVL_EPS.bESTOP;
Main.M27.bAllEnable := Main.M27.bEnable AND GVL_EPS.bESTOP;
// Call Motion FB instance
IF NOT bDisableStateMachine THEN
CASE nState OF
-1:
IF Main.M25.bExecute AND NOT (Main.M25.bError OR Main.M26.bError) THEN
Main.M25.bExecute := FALSE;
Main.M25.bEnable := TRUE;
nState := 0;
ELSE
Main.M25.bEnable := FALSE;
Main.M25.bBrakeRelease := FALSE;
Main.M26.bBrakeRelease := FALSE;
END_IF
0:
tonDelay(IN:=TRUE, PT:=t#1s);
IF tonDelay.Q THEN
Main.M25.bBrakeRelease := TRUE;
Main.M26.bBrakeRelease := TRUE;
tonDelay(IN:=FALSE);
nState := 1;
ELSIF Main.M25.bError OR Main.M26.bError THEN
nState := -1;
END_IF
1:
tonDelay2(IN:=TRUE, PT:=t#1s);
IF tonDelay2.Q THEN
Main.M25.bExecute := TRUE;
tonDelay2(IN:=FALSE);
nState := 2;
ELSIF Main.M25.bError OR Main.M26.bError THEN
nState := -1;
END_IF
2:
IF NOT Main.M25.bBusy AND NOT Main.M26.bBusy THEN
nState := -1;
END_IF
100:
nState := -1;
END_CASE
END_IF
fbMotionYF1(stMotionStage:=Main.M25);
// Set the M26 bEnable equal to the M25 bEnable status to couple their enable bits together.
Main.M26.bEnable := Main.M25.bEnable;
fbMotionYF2(stMotionStage:=Main.M26);
fbMotionZF(stMotionStage:=Main.M27);
fbMeasureReferenceVelocity(
stMotionStage:=Main.M25,
nMicrostepsPerStep:=64,
bBusy=>,
fMeasuredReferenceVelocity=>
);
END_PROGRAM
- Related:
PRG_DET_CHAMBER
PROGRAM PRG_DET_CHAMBER
VAR
bInit: BOOL := TRUE;
fbMotionXDC: FB_MotionStage;
fbMotionRyDC: FB_MotionStage;
fbMotionZDC: FB_MotionStage;
END_VAR
// Det. Chmber
IF bInit THEN
bInit :=FALSE;
Main.M22.bHardwareEnable:= TRUE;
Main.M22.bPowerSelf:= TRUE;
Main.M22.nBrakeMode:= ENUM_StageBrakeMode.NO_BRAKE;
Main.M22.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M22.fVelocity := 0.1;
Main.M23.bHardwareEnable:= TRUE;
Main.M23.bPowerSelf:= TRUE;
Main.M23.nBrakeMode:= ENUM_StageBrakeMode.NO_BRAKE;
Main.M23.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M23.fVelocity := 0.1;
Main.M24.bHardwareEnable:= TRUE;
Main.M24.bPowerSelf:= TRUE;
Main.M24.nBrakeMode:= ENUM_StageBrakeMode.NO_BRAKE;
Main.M24.nEnableMode := ENUM_StageEnableMode.ALWAYS ; // This axis should always be enables due to belows
Main.M24.fVelocity := 0.1;
END_IF;
fbMotionXDC(stMotionStage:= Main.M22);
fbMotionRyDC(stMotionStage:= Main.M23);
fbMotionZDC(stMotionStage:= Main.M24);
END_PROGRAM
- Related:
PRG_DET_FRAME
PROGRAM PRG_DET_FRAME
VAR
bInit: BOOL := TRUE;
{attribute 'pytmc' := '
pv: QRIX:SA:bError
io: i
'}
bError: BOOL := FALSE;
{attribute 'pytmc' := '
pv: QRIX:SA:bBusy
io: i
'}
bBusy: BOOL;
{attribute 'pytmc' := '
pv: QRIX:SA:bMotorsBusy
io: i
'}
bMotorsBusy: BOOL;
{attribute 'pytmc' := 'pv: QRIX:SA:bResetError'}
bResetError: BOOL := FALSE;
{attribute 'pytmc' := 'pv: QRIX:SA:EnableJackOff'}
bEnableJackOff: BOOL;
{attribute 'pytmc' := 'pv: QRIX:SA:EnableLevitation'}
bEnableLevitation: BOOL;
{attribute 'pytmc' := 'pv: QRIX:SA:EnableLanding'}
bEnableLanding: BOOL;
{attribute 'pytmc' := 'pv: QRIX:DF:EnableAdjustingRoll'}
bEnableAdjustingRoll: BOOL;
{attribute 'pytmc' := 'pv: QRIX:DF:EnableAdjustingPitch'}
bEnableAdjustingPitch: BOOL;
{attribute 'pytmc' := 'pv: QRIX:DF:EnableAdjustingHeight'}
bEnableAdjustingHeight: BOOL;
tonWaitToStabilize: TON;
tonSV2Open: TON;
tonSV1Close: TON;
tonDelayTrig: TON;
iStepJackOff: INT := 1;
iStepAdjustingRoll : INT := 1;
iStepAdjustingPitch : INT := 1;
iStepAdjustingHeight : INT := 1;
trigDone: F_TRIG;
fDistanceX: LREAL := 2.114; // in m
fDistanceZ: LREAL := 2.142; // in m
fPositionThreshold: LREAL := 0.02; // mm
fPositionMaxDeviation: LREAL := 10.0; // mm
fPitchThreshold: LREAL := 0.02; // mrad
fRollThreshold: LREAL := 0.02; // mrad
{attribute 'pytmc' := '
pv: QRIX:SA:fRollTarget
io: io
'}
fRollTarget : LREAL;
{attribute 'pytmc' := 'pv: QRIX:F'}
fbYDFMotion: FB_3AxesJack;
fbReset_M15: MC_RESET;
fbReset_M16: MC_RESET;
fbReset_M17: MC_RESET;
nActiveSequenceIterationCount: UINT := 1;
nSequenceIterationCount: UINT := 3;
// EPS settings to prevent jacks from moving too far out of range of each other.
EPS_YDF1_Up : FB_EPS;
EPS_YDF1_Down : FB_EPS;
EPS_YDF2_Up : FB_EPS;
EPS_YDF2_Down : FB_EPS;
EPS_YDF3_Up : FB_EPS;
EPS_YDF3_Down : FB_EPS;
{attribute 'pytmc' := '
pv: QRIX:SA:rJackingSpeed
io: io
'}
rJackingSpeed: LREAL := 0.05;
{attribute 'pytmc' := '
pv: QRIX:SA:rJackingAccel
io: io
'}
rJackingAccel: LREAL := 0.005;
{attribute 'pytmc' := '
pv: QRIX:SA:bStopAxes
io: io
'}
bStopAxes: BOOL;
{attribute 'TcLinkTo' := 'TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(In) TxPDO-Map^Sensor Warning}
bLinearGaugeWarning AT %I* : BOOL;
{attribute 'TcLinkTo' := 'TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(In) TxPDO-Map^Sensor Error}
bLinearGaugeError AT %I* : BOOL;
{attribute 'TcLinkToOSO' := '<0,1,2>TINC^NC-Task 1 SAF^Axes^M15 - YDF1^Enc^Inputs^In^nState1'}
bLinearGaugeYDF1Ready AT %Q* : BOOL;
{attribute 'TcLinkToOSO' := '<0,1,2>TINC^NC-Task 1 SAF^Axes^M16 - YDF2^Enc^Inputs^In^nState1'}
bLinearGaugeYDF2Ready AT %Q* : BOOL;
{attribute 'TcLinkToOSO' := '<0,1,2>TINC^NC-Task 1 SAF^Axes^M17 - YDF3^Enc^Inputs^In^nState1'}
bLinearGaugeYDF3Ready AT %Q* : BOOL;
END_VAR
IF bInit THEN
bInit := FALSE;
Main.M15.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M15.bHardwareEnable := TRUE;
Main.M15.bPowerSelf := TRUE;
Main.M16.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M16.bHardwareEnable := TRUE;
Main.M16.bPowerSelf := TRUE;
Main.M17.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M17.bHardwareEnable := TRUE;
Main.M17.bPowerSelf := TRUE;
Main.M15.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M16.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M17.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
END_IF;
// FRAME JACKS EPS RESTRICTIONS
// YDF1 Disable movement in upward (positive/forward) direction.
EPS_YDF1_Up(eps:=Main.M15.stEPSForwardEnable);
EPS_YDF1_Up.setDescription('UPWARD MOVEMENT ALLOWED IF: YDF1 - YDF2 < 10mm AND YDF1 - YDF3 < 10mm |'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Prevent YDF1 from rising any more if it is more than fPositionMaxDeviation above YDF2
EPS_YDF1_Up.setBit(nBits:= 1, bValue:= Main.M15.stAxisStatus.fActPosition - Main.M16.stAxisStatus.fActPosition < fPositionMaxDeviation);
// Prevent YDF1 from rising any more if it is more than fPositionMaxDeviation above YDF3
EPS_YDF1_Up.setBit(nBits:= 0, bValue:= Main.M15.stAxisStatus.fActPosition - Main.M17.stAxisStatus.fActPosition < fPositionMaxDeviation);
// YDF1 Disable movement in downward (negative/downward) direction.
EPS_YDF1_Down(eps:=Main.M15.stEPSBackwardEnable);
EPS_YDF1_Down.setDescription('DOWNWARD MOVEMENT ALLOWED IF: YDF1 - YDF2 > -10mm AND YDF1 - YDF3 > -10mm |'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Prevent YDF1 from lowering any more if it is more than fPositionMaxDeviation below YDF2
EPS_YDF1_Down.setBit(nBits:= 1, bValue:= Main.M15.stAxisStatus.fActPosition - Main.M16.stAxisStatus.fActPosition > -fPositionMaxDeviation);
// Prevent YDF1 from lowering any more if it is more than fPositionMaxDeviation below YDF3
EPS_YDF1_Down.setBit(nBits:= 0, bValue:= Main.M15.stAxisStatus.fActPosition - Main.M17.stAxisStatus.fActPosition > -fPositionMaxDeviation);
// YDF2 Disable movement in upward (positive/forward) direction.
EPS_YDF2_Up(eps:=Main.M16.stEPSForwardEnable);
EPS_YDF2_Up.setDescription('UPWARD MOVEMENT ALLOWED IF: YDF2 - YDF1 < 10mm AND YDF2 - YDF3 < 10mm |'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Prevent YDF2 from rising any more if it is more than fPositionMaxDeviation above YDF1
EPS_YDF2_Up.setBit(nBits:= 1, bValue:= Main.M16.stAxisStatus.fActPosition - Main.M15.stAxisStatus.fActPosition < fPositionMaxDeviation);
// Prevent YDF2 from rising any more if it is more than fPositionMaxDeviation above YDF3
EPS_YDF2_Up.setBit(nBits:= 0, bValue:= Main.M16.stAxisStatus.fActPosition - Main.M17.stAxisStatus.fActPosition < fPositionMaxDeviation);
// YDF2 Disable movement in downward (negative/downward) direction.
EPS_YDF2_Down(eps:=Main.M16.stEPSBackwardEnable);
EPS_YDF2_Down.setDescription('DOWNWARD MOVEMENT ALLOWED IF: YDF2 - YDF1 > -10mm AND YDF2 - YDF3 > -10mm |'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Prevent YDF2 from lowering any more if it is more than fPositionMaxDeviation below YDF1
EPS_YDF2_Down.setBit(nBits:= 1, bValue:= Main.M16.stAxisStatus.fActPosition - Main.M15.stAxisStatus.fActPosition > -fPositionMaxDeviation);
// Prevent YDF2 from lowering any more if it is more than fPositionMaxDeviation below YDF3
EPS_YDF2_Down.setBit(nBits:= 0, bValue:= Main.M16.stAxisStatus.fActPosition - Main.M17.stAxisStatus.fActPosition > -fPositionMaxDeviation);
// YDF3 Disable movement in upward (positive/forward) direction.
EPS_YDF3_Up(eps:=Main.M17.stEPSForwardEnable);
EPS_YDF3_Up.setDescription('UPWARD MOVEMENT ALLOWED IF: YDF3 - YDF2 < 10mm AND YDF3 - YDF1 < 10mm |'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Prevent YDF3 from rising any more if it is more than fPositionMaxDeviation above YDF2
EPS_YDF3_Up.setBit(nBits:= 1, bValue:= Main.M17.stAxisStatus.fActPosition - Main.M16.stAxisStatus.fActPosition < fPositionMaxDeviation);
// Prevent YDF3 from rising any more if it is more than fPositionMaxDeviation above YDF1
EPS_YDF3_Up.setBit(nBits:= 0, bValue:= Main.M17.stAxisStatus.fActPosition - Main.M15.stAxisStatus.fActPosition < fPositionMaxDeviation);
// YDF3 Disable movement in downward (negative/downward) direction.
EPS_YDF3_Down(eps:=Main.M17.stEPSBackwardEnable);
EPS_YDF3_Down.setDescription('DOWNWARD MOVEMENT ALLOWED IF: YDF3 - YDF2 > -10mm AND YDF3 - YDF1 > -10mm |'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Prevent YDF3 from lowering any more if it is more than fPositionMaxDeviation below YDF2
EPS_YDF3_Down.setBit(nBits:= 1, bValue:= Main.M17.stAxisStatus.fActPosition - Main.M16.stAxisStatus.fActPosition > -fPositionMaxDeviation);
// Prevent YDF3 from lowering any more if it is more than fPositionMaxDeviation below YDF1
EPS_YDF3_Down.setBit(nBits:= 0, bValue:= Main.M17.stAxisStatus.fActPosition - Main.M15.stAxisStatus.fActPosition > -fPositionMaxDeviation);
GVL_Sensor.bFloating := GVL_Sensor.stPS1.fValue > 0.3 AND GVL_Sensor.stPS2.fValue > 0.18 AND GVL_Sensor.stPS3.fValue > 0.15;
bMotorsBusy := Main.M15.bBusy OR Main.M16.bBusy OR Main.M17.bBusy;
IF Main.M15.bError OR Main.M16.bError OR Main.M17.bError THEN
bError := TRUE;
iStepJackOff := -1;
iStepAdjustingRoll := -1;
iStepAdjustingPitch := -1;
iStepAdjustingHeight := -1;
IF NOT Main.M15.bError THEN
Main.M15.bReset := TRUE;
END_IF;
IF NOT Main.M16.bError THEN
Main.M16.bReset := TRUE;
END_IF;
IF NOT Main.M17.bError THEN
Main.M17.bReset := TRUE;
END_IF;
END_IF
ACT_1_JACK_OFF();
ACT_2_Levitation();
ACT_3_Landing();
ACT_4_AdjustingRoll();
ACT_5_AdjustingPitch();
ACT_6_AdjustingHeight();
ACT_ResetError();
bLinearGaugeYDF1Ready := NOT bLinearGaugeError;
bLinearGaugeYDF2Ready := NOT bLinearGaugeError;
bLinearGaugeYDF3Ready := NOT bLinearGaugeError;
fbYDFMotion(
bMoveOk := GVL_EPS.bESTOP,
stJack1Axis := Main.M16,
stJack2Axis := Main.M15,
stJack3Axis := Main.M17,
rDistanceX := fDistanceX,
rDistanceZ := fDistanceZ,
bJack3DoesPitch := TRUE,
rLimitPositionMax := 21.5,
rLimitPositionMin := -0.5,
rLimitPitchMin := -10,
rLimitPitchMax := 10,
rLimitRollMin := -10,
rLimitRollMax := 10,
fMaxVelocity := rJackingSpeed,
fHighAccel := rJackingAccel
);
IF bStopAxes THEN
Main.M1.bExecute := FALSE;
Main.M2.bExecute := FALSE;
Main.M7.bExecute := FALSE;
Main.M8.bExecute := FALSE;
Main.M9.bExecute := FALSE;
Main.M15.bExecute := FALSE;
Main.M16.bExecute := FALSE;
Main.M17.bExecute := FALSE;
bStopAxes := FALSE;
END_IF
END_PROGRAM
ACTION ACT_1_JACK_OFF:
CASE iStepJackOff OF
1:
IF bEnableJackOff AND NOT(bBusy AND bError) THEN
iStepJackOff := 2;
bDoneJackOff := FALSE;
bBusy := TRUE;
fbYDFMotion.bMoveToPosition := FALSE;
fbYDFMotion.bMoveToPitch := FALSE;
fbYDFMotion.bMoveToRoll := FALSE;
ELSE
bEnableJackOff := FALSE;
END_IF;
2:
// Step2 check if we are already at position or if we are in an error position.
IF abs(Main.M15.stAxisStatus.fActPosition) < fPositionThreshold AND
abs(Main.M16.stAxisStatus.fActPosition) < fPositionThreshold AND
abs(Main.M17.stAxisStatus.fActPosition) < fPositionThreshold THEN
// We dont need to move any more....
iStepJackOff := 4;
ELSE
iStepJackOff := 3;
END_IF;
3:
// Step3 set target position
fbYDFMotion.rReqPosition := 0.0;
fbYDFMotion.bMoveToPosition := TRUE;
fbYDFMotion.rReqRoll := 0.0;
fbYDFMotion.bMoveToRoll := TRUE;
fbYDFMotion.rReqPitch := 0.0;
fbYDFMotion.bMoveToPitch := TRUE;
iStepJackOff := 2;
4:
// End
bEnableJackOff:= FALSE;
bBusy := FALSE;
iStepJackOff:=1;
bDoneJackOff := TRUE;
fbYDFMotion.bMoveToPosition := FALSE;
fbYDFMotion.bMoveToPitch := FALSE;
fbYDFMotion.bMoveToRoll := FALSE;
-1:
// Error
bError:= TRUE;
bEnableJackOff:= FALSE;
bBusy := FALSE;
iStepJackOff:=1;
bDoneJackOff := FALSE;
fbYDFMotion.bMoveToPosition := FALSE;
fbYDFMotion.bMoveToPitch := FALSE;
fbYDFMotion.bMoveToRoll := FALSE;
END_CASE;
END_ACTION
ACTION ACT_2_Levitation:
IF bEnableLevitation AND NOT (bBusy OR bError) THEN
bBusy := TRUE;
IF GVL_Sensor.stPS1.fValue > 0.15 AND GVL_Sensor.stPS2.fValue > 0.15 THEN
GVL_EPS.bOpenSV1 := TRUE;
END_IF;
bDoneLevitation := FALSE;
END_IF;
// SV2 will open after 3 secounds from SV1 opened...
tonSV2Open(
IN := bEnableLevitation AND GVL_EPS.bOpenSV1,
PT := T#3s
);
IF tonSV2Open.Q THEN
GVL_EPS.bOpenSV2 := TRUE;
bEnableLevitation := FALSE;
bDoneLevitation := TRUE;
bBusy := FALSE;
END_IF;
END_ACTION
ACTION ACT_3_Landing:
IF bEnableLanding AND NOT (bBusy OR bError) THEN
GVL_EPS.bOpenSV2 := FALSE;
bBusy:= TRUE;
bDoneLanding := FALSE;
END_IF;
tonSV1Close(
IN := bEnableLanding AND NOT GVL_EPS.bOpenSV2,
PT := T#3s
);
IF tonSV1Close.Q THEN
GVL_EPS.bOpenSV1 := FALSE;
bEnableLanding := FALSE;
bBusy:= FALSE;
bDoneLanding := TRUE;
END_IF;
END_ACTION
ACTION ACT_4_AdjustingRoll:
CASE iStepAdjustingRoll OF
1:
IF bEnableAdjustingRoll AND NOT(bBusy AND bError) THEN
iStepAdjustingRoll := 2;
bDoneAdjustingRoll := FALSE;
bBusy := TRUE;
fbYDFMotion.bMoveToRoll := FALSE;
nActiveSequenceIterationCount := 0;
ELSE
bEnableAdjustingRoll := FALSE;
END_IF;
2:
// Step3 set target roll
IF abs(GVL_Sensor.stFrameR.fValue - fRollTarget) > fRollThreshold THEN
fbYDFMotion.bMoveToRoll := TRUE;
iStepAdjustingRoll := 3;
fbYDFMotion.rReqRoll := fbYDFMotion.rActRoll - (GVL_Sensor.stFrameR.fValue - fRollTarget) * 0.5;
ELSE
// Already at goal.
iStepAdjustingRoll := 4;
END_IF;
3:
// Wait until Done. and wait tilt sensor stabilization.
tonWaitToStabilize(IN:=NOT bMotorsBusy, PT:=T#5s);
IF tonWaitToStabilize.Q THEN
iStepAdjustingRoll := 4;
tonWaitToStabilize(IN:=FALSE);
END_IF;
4:
IF nActiveSequenceIterationCount > nSequenceIterationCount THEN
// Exceeded maximum allowable number of iterations. Something is wrong.
iStepAdjustingRoll := -1;
ELSIF abs(GVL_Sensor.stFrameR.fValue - fRollTarget) > fRollThreshold THEN
// Iteration
iStepAdjustingRoll := 2;
fbYDFMotion.bMoveToRoll := FALSE;
nActiveSequenceIterationCount := nActiveSequenceIterationCount + 1;
ELSE
// End
bEnableAdjustingRoll:= FALSE;
bBusy := FALSE;
iStepAdjustingRoll:=1;
bDoneAdjustingRoll := TRUE;
fbYDFMotion.bMoveToRoll := FALSE;
END_IF;
-1:
// Error
bError:= TRUE;
bBusy := FALSE;
bEnableAdjustingRoll:= FALSE;
iStepAdjustingRoll:=1;
bDoneAdjustingRoll := FALSE;
fbYDFMotion.bMoveToRoll := FALSE;
END_CASE;
END_ACTION
ACTION ACT_5_AdjustingPitch:
CASE iStepAdjustingPitch OF
1:
IF bEnableAdjustingPitch AND NOT(bBusy AND bError) THEN
iStepAdjustingPitch := 2;
bDoneAdjustingPitch := FALSE;
bBusy := TRUE;
fbYDFMotion.bMoveToPitch := FALSE;
nActiveSequenceIterationCount := 0;
ELSE
bEnableAdjustingPitch := FALSE;
END_IF;
2:
// Step2 set target pitch
IF abs(GVL_Sensor.stFrameP.fValue) > fPitchThreshold THEN
fbYDFMotion.bMoveToPitch := TRUE;
iStepAdjustingPitch := 3;
fbYDFMotion.rReqPitch := fbYDFMotion.rActPitch - GVL_Sensor.stFrameP.fValue * 0.5;
ELSE
// Already at goal.
iStepAdjustingPitch := 4;
END_IF;
3:
// Wait until Done. and wait tilt sensor stabilization.
tonWaitToStabilize(IN:=NOT bMotorsBusy, PT:=T#5s);
IF tonWaitToStabilize.Q THEN
iStepAdjustingPitch := 4;
tonWaitToStabilize(IN:=FALSE);
END_IF;
4:
IF nActiveSequenceIterationCount > nSequenceIterationCount THEN
// Exceeded maximum allowable number of iterations. Something is wrong.
iStepAdjustingPitch := -1;
ELSIF abs(GVL_Sensor.stFrameP.fValue) > fPitchThreshold THEN
// Iteration
iStepAdjustingPitch := 2;
fbYDFMotion.bMoveToPitch := FALSE;
nActiveSequenceIterationCount := nActiveSequenceIterationCount + 1;
ELSE
// End
bEnableAdjustingPitch:= FALSE;
bBusy := FALSE;
iStepAdjustingPitch:=1;
bDoneAdjustingPitch := TRUE;
fbYDFMotion.bMoveToPitch := FALSE;
END_IF;
-1:
// Error
bError:= TRUE;
bBusy := FALSE;
bEnableAdjustingPitch:= FALSE;
iStepAdjustingPitch:=1;
bDoneAdjustingPitch := FALSE;
fbYDFMotion.bMoveToPitch := FALSE;
END_CASE;
END_ACTION
ACTION ACT_6_AdjustingHeight:
CASE iStepAdjustingHeight OF
1:
IF bEnableAdjustingHeight AND NOT(bBusy AND bError) THEN
iStepAdjustingHeight := 2;
bDoneAdjustingHeight := FALSE;
bBusy := TRUE;
fbYDFMotion.bMoveToPosition := FALSE;
nActiveSequenceIterationCount := 0;
ELSE
bEnableAdjustingHeight := FALSE;
END_IF;
2:
// Step2 set target position
IF abs(GVL_Sensor.stHDF.fValue) > fPositionThreshold THEN
fbYDFMotion.bMoveToPosition := TRUE;
iStepAdjustingHeight := 3;
fbYDFMotion.rReqPosition := fbYDFMotion.rActPosition - GVL_Sensor.stHDF.fValue;
ELSE
// Already at goal.
iStepAdjustingHeight := 4;
END_IF;
3:
// Wait until Done. and wait tilt sensor stabilization.
tonWaitToStabilize(IN:=NOT bMotorsBusy, PT:=T#5s);
IF tonWaitToStabilize.Q THEN
iStepAdjustingHeight := 4;
tonWaitToStabilize(IN:=FALSE);
END_IF;
4:
IF nActiveSequenceIterationCount > nSequenceIterationCount THEN
// Exceeded maximum allowable number of iterations. Something is wrong.
iStepAdjustingHeight := -1;
ELSIF abs(GVL_Sensor.stHDF.fValue) > fPositionThreshold THEN
// Iteration
iStepAdjustingHeight := 2;
fbYDFMotion.bMoveToPosition := FALSE;
nActiveSequenceIterationCount := nActiveSequenceIterationCount + 1;
ELSE
// End
bEnableAdjustingHeight:= FALSE;
bBusy := FALSE;
iStepAdjustingHeight:=1;
bDoneAdjustingHeight := TRUE;
fbYDFMotion.bMoveToPosition := FALSE;
END_IF;
-1:
// Error
bError:= TRUE;
bBusy := FALSE;
bEnableAdjustingHeight:= FALSE;
iStepAdjustingHeight:=1;
bDoneAdjustingHeight := FALSE;
END_CASE;
END_ACTION
ACTION ACT_ResetError:
IF bResetError THEN
bResetError := FALSE;
bError:= FALSE;
bBusy:=FALSE;
bEnableJackOff := FALSE;
bEnableLevitation := FALSE;
bEnableLanding := FALSE;
bEnableAdjustingRoll := FALSE;
bEnableAdjustingPitch := FALSE;
bEnableAdjustingHeight := FALSE;
Main.M2.bReset := TRUE;
Main.M2.bError := FALSE;
(*Reset*)
fbReset_M15(
Execute:=Main.M15.Axis.Status.Error,
Axis:=Main.M15.Axis,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
fbReset_M16(
Execute:=Main.M16.Axis.Status.Error,
Axis:=Main.M16.Axis,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
fbReset_M17(
Execute:=Main.M17.Axis.Status.Error,
Axis:=Main.M17.Axis,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
iStepJackOff := 1;
iStepAdjustingRoll := 1;
iStepAdjustingPitch := 1;
iStepAdjustingHeight := 1;
fbYDFMotion.bMoveToPosition := FALSE;
fbYDFMotion.bMoveToPitch := FALSE;
fbYDFMotion.bMoveToRoll := FALSE;
fbYDFMotion.rReqPosition := fbYDFMotion.rActPosition;
fbYDFMotion.rReqPitch := fbYDFMotion.rActPitch;
fbYDFMotion.rReqRoll := fbYDFMotion.rActRoll;
bStopAxes := TRUE;
END_IF;
END_ACTION
- Related:
PRG_DET_SLIT
PROGRAM PRG_DET_SLIT
VAR
{attribute 'pytmc' := 'pv: QRIX:DETSL'}
fbSlits_Det: FB_Slits;
bExecuteMotion: BOOL := TRUE;
bMoveOk: BOOL :=TRUE;
bInit:BOOL:=TRUE;
END_VAR
// 4 jaws slit
IF bInit THEN
bInit := FALSE;
Main.M18.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M18.bHardwareEnable := TRUE;
Main.M18.bPowerSelf := TRUE;
Main.M18.fVelocity := 0.1;
Main.M19.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M19.bHardwareEnable := TRUE;
Main.M19.bPowerSelf := TRUE;
Main.M19.fVelocity := 0.1;
Main.M20.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M20.bHardwareEnable := TRUE;
Main.M20.bPowerSelf := TRUE;
Main.M20.fVelocity := 0.1;
Main.M21.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M21.bHardwareEnable := TRUE;
Main.M21.bPowerSelf := TRUE;
Main.M21.fVelocity := 0.1;
Main.M18.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M19.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M20.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M21.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
END_IF
fbSlits_Det( i_DevName:= 'DET_SLITS',
stTopBlade:= Main.M20,
stBottomBlade:= Main.M21,
stLeftBlade:= Main.M18,
stRightBlade:= Main.M19,
io_fbFFHWO:= GVL_PMPS.fbFastFaultOutput1,
fbArbiter:= GVL_PMPS.fbArbiter,
bExecuteMotion:= bExecuteMotion,
bMoveOk:= bMoveOk
);
END_PROGRAM
PRG_Diffractometer
PROGRAM PRG_Diffractometer
VAR
fb_Diff_Theta : FB_MotionStage;
fb_Diff_2Theta : FB_MotionStage;
fb_Diff_2ThetaY : FB_MotionStage;
fb_Diff_Chi : FB_MotionStage;
fb_Diff_Phi : FB_MotionStage;
fb_Diff_X : FB_MotionStage;
fb_Diff_Y : FB_MotionStage;
fb_Diff_Z : FB_MotionStage;
bInit : BOOl := TRUE;
//EPS
EPS_Diff_Chi_Backward : FB_EPS;
EPS_Diff_Phi_Forward : FB_EPS;
EPS_Diff_Theta_Backward : FB_EPS;
EPS_Diff_Theta_Forward : FB_EPS;
EPS_Diff_X_Forward : FB_EPS;
EPS_Diff_X_Backward : FB_EPS;
EPS_Diff_Y_Forward : FB_EPS;
EPS_Diff_Y_Backward : FB_EPS;
EPS_Diff_Z_Forward : FB_EPS;
EPS_Diff_Z_Backward : FB_EPS;
EPS_Diff_2Theta_Backward : FB_EPS;
EPS_Diff_2ThetaY : FB_EPS;
//Encoders for rotary stages
fbSetPosition: MC_SetPosition;
fb2ThetaAutoCoupling: FB_GantryAutoCoupling;
couple : MC_GEARIN;
decouple : MC_GEAROUT;
bExecuteCouple: BOOL := TRUE;
bExecuteDecouple: BOOL := FALSE;
n2Theta AT %Q* :ULINT;
r2Theta:LREAL;
temp:ULINT;
rtemp:LREAL;
GantryDiff:REAL;
fb_Diff_2Theta_Encoder : FB_MotionStage;
eDiffXStateGet: E_DiffState;
eDiffXStateSet: E_DiffState;
astDiffXPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiffXStage1D : FB_PositionState1D;
eDiffYStateGet: E_DiffState;
eDiffYStateSet: E_DiffState;
astDiffYPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiffYStage1D : FB_PositionState1D;
eDiffZStateGet: E_DiffState;
eDiffZStateSet: E_DiffState;
astDiffZPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiffZStage1D : FB_PositionState1D;
eDiff2ThetaYStateGet: E_DiffState;
eDiff2ThetaYStateSet: E_DiffState;
astDiff2ThetaYPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiff2ThetaYStage1D : FB_PositionState1D;
eDiffPhiStateGet: E_DiffState;
eDiffPhiStateSet: E_DiffState;
astDiffPhiPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiffPhiStage1D : FB_PositionState1D;
eDiffChiStateGet: E_DiffState;
eDiffChiStateSet: E_DiffState;
astDiffChiPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiffChiStage1D : FB_PositionState1D;
eDiff2ThetaStateGet: E_DiffState;
eDiff2ThetaStateSet: E_DiffState;
astDiff2ThetaPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiff2ThetaStage1D : FB_PositionState1D;
eDiffThetaStateGet: E_DiffState;
eDiffThetaStateSet: E_DiffState;
astDiffThetaPosState: ARRAY[1..GeneralConstants.MAX_STATES] OF ST_PositionState;
fbDiffThetaStage1D : FB_PositionState1D;
fbStateSetup: FB_StateSetupHelper;
stDefault: ST_PositionState := (
fDelta := 0.5,
fVelocity := 0.5,
bMoveOk := TRUE,
bValid := TRUE
);
END_VAR
IF (bInit) THEN
bInit := FALSE;
Main.M28.bHardwareEnable := TRUE;
Main.M28.bPowerSelf := TRUE;
Main.M28.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M28.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M28.fVelocity := 0.1;
Main.M29.bHardwareEnable := TRUE;
Main.M29.bPowerSelf := TRUE;
Main.M29.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M29.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M29.fVelocity := 0.1;
Main.M30.bHardwareEnable := TRUE;
Main.M30.bPowerSelf := TRUE;
Main.M30.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M30.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M30.fVelocity := 0.1;
Main.M31.bHardwareEnable := TRUE;
Main.M31.bPowerSelf := TRUE;
Main.M31.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M31.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M31.fVelocity := 0.1;
Main.M32.bHardwareEnable := TRUE;
Main.M32.bPowerSelf := TRUE;
Main.M32.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M32.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M32.fVelocity := 0.1;
Main.M33.bHardwareEnable := TRUE;
Main.M33.bPowerSelf := TRUE;
Main.M33.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M33.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M33.fVelocity := 0.1;
Main.M34.bHardwareEnable := TRUE;
Main.M34.bPowerSelf := TRUE;
Main.M34.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M34.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M34.fVelocity := 0.1;
Main.M35.bHardwareEnable := TRUE;
Main.M35.bPowerSelf := TRUE;
Main.M35.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M35.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M35.fVelocity := 0.1;
fbStateSetup(stPositionState:=stDefault, bSetDefault:=TRUE);
fbStateSetup(stPositionState:=astDiffXPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffYPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffZPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiff2ThetaYPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffPhiPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffChiPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiff2ThetaPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffThetaPosState[E_DiffState.TRANSFER_ARM_INTERFACE], sName:='TRANSFER_ARM_INTERFACE',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffXPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffYPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffZPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiff2ThetaYPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffPhiPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffChiPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiff2ThetaPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
fbStateSetup(stPositionState:=astDiffThetaPosState[E_DiffState.NOMINAL_POSITION], sName:='NOMINAL_POSITION',
fPosition := 0
);
END_IF
//EPS
// All bits must be TRUE to enable the specified direction (backward or forward)
// The bits are populated right to left in typhos screen. So Descriptions highest bit number is at the beginning of the description.
// The eps IN_OUT variable must be assigned before using the FB_EPS in any way.
// DIFFRACTOMETER THETA ANGLE RESTRICTIONS
// Disable movement in negative direction.
EPS_Diff_Theta_Backward(eps:=Main.M34.stEPSBackwardEnable);
EPS_Diff_Theta_Backward.setDescription('NEGATIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_Theta_Backward.setBit(nBits:= 3,
bValue := Main.M34.Axis.NcToPlc.ActPos - Main.M49.Axis.NcToPlc.ActPos > -1.0
);
// Diffractometer Theta cannot go less than 5 degrees if the Diffractometer Chi is less than -5 degrees
EPS_Diff_Theta_Backward.setBit(nBits:= 2, bValue:=F_CheckAxisGEQ(Main.M34,5) OR F_CheckAxisGEQ(Main.M33,-5));
// Diffractometer Theta cannot go less than 10 degrees if the MID IR (LAS VIS) is IN, which is defined as a position of 90 +/- 1.
EPS_Diff_Theta_Backward.setBit(nBits:= 1, bValue:= F_CheckAxisGRT(Main.M34,10) OR F_Limit(Main.M36.stAxisStatus.fActPosition, 91, 89, false));
// Diffractometer Theta cannot go less than 10 degrees if the Diffractometer phi is at 90 +/- 1.
EPS_Diff_Theta_Backward.setBit(nBits:= 0, bValue:=F_CheckAxisGRT(Main.M34,10) OR F_Limit(Main.M32.stAxisStatus.fActPosition, 91, 89, false));
// Disable movement in positive direction.
EPS_Diff_Theta_Forward(eps:=Main.M34.stEPSForwardEnable);
EPS_Diff_Theta_Forward.setDescription('POSITIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_Theta_Forward.setBit(nBits:= 1,
bValue := Main.M34.Axis.NcToPlc.ActPos - Main.M49.Axis.NcToPlc.ActPos < 1.0
);
// Diffractometer Theta cannot go more than Diffractometer 2Theta plus 21 degrees
EPS_Diff_Theta_Forward.setBit(nBits:= 0, bValue:=Main.M34.Axis.NcToPlc.ActPos < Main.M35.stAxisStatus.fActPosition+21);
// DIFFRACTOMETER CHI ANGLE RESTRICTIONS
// Disable movement in negative direction.
EPS_Diff_Chi_Backward(eps:=Main.M33.stEPSBackwardEnable);
EPS_Diff_Chi_Backward.setDescription('NEGATIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Diffractometer Chi cannot go less than -5 degrees if the Diffractometer Theta is less than 5 degrees
EPS_Diff_Chi_Backward.setBit(nBits:= 0, bValue:=F_CheckAxisGEQ(Main.M33,-5) OR F_CheckAxisGEQ(Main.M34,5));
// DIFFRACTOMETER PHI ANGLE RESTRICTIONS
// Disable movement in positive direction.
EPS_Diff_Phi_Forward(eps:=Main.M32.stEPSForwardEnable);
EPS_Diff_Phi_Forward.setDescription('POSITIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Diffractometer Phi cannot go greater than 45 degrees when the MID IR (LAS VIS) is greater than 45 degrees
EPS_Diff_Phi_Forward.setBit(nBits:= 1, bValue:=Main.M36.Axis.NcToPlc.ActPos <= 45 OR Main.M32.Axis.NcToPlc.ActPos <= 5);
// Diffractometer Phi cannot go greater than 45 degrees when theta is within 1 degree of 0 degrees.
EPS_Diff_Phi_Forward.setBit(nBits:= 0, bValue:=Main.M32.Axis.NcToPlc.ActPos <= 45 OR F_Limit(Main.M34.stAxisStatus.fActPosition, 1, -1, false));
// DIFFRACTOMETER Z POSITION RESTRICTIONS
// Disable movement in negative direction.
EPS_Diff_Z_Backward(eps:=Main.M30.stEPSBackwardEnable);
EPS_Diff_Z_Backward.setDescription('NEGATIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_Z_Backward.setBit(nBits:= 1,
bValue := Main.M30.Axis.NcToPlc.ActPos - (Main.M48.Axis.NcToPlc.ActPos - GVL_VAR.fCryoZNeutralPositionAtDiffZ0) > -1.0
);
// Diffractometer Z cannot go less than 5mm when the diagonal paddle horizontal (LAS_D_H) position is greater than 50 mm.
EPS_Diff_Z_Backward.setBit(nBits:= 0, bValue:=Main.M37.Axis.NcToPlc.ActPos <= 50 OR Main.M30.Axis.NcToPlc.ActPos >= 5);
// Disable movement in positive direction
EPS_Diff_Z_Forward(eps:=Main.M30.stEPSForwardEnable);
EPS_Diff_Z_Forward.setDescription('POSITIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_Z_Forward.setBit(nBits:= 0,
bValue := Main.M30.Axis.NcToPlc.ActPos - (Main.M48.Axis.NcToPlc.ActPos - GVL_VAR.fCryoZNeutralPositionAtDiffZ0) < 1.0
);
// Diff X
// Disable movement in negative direction.
EPS_Diff_X_Backward(eps:=Main.M28.stEPSBackwardEnable);
EPS_Diff_X_Backward.setDescription('NEGATIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_X_Backward.setBit(nBits:= 0,
bValue := Main.M28.Axis.NcToPlc.ActPos - Main.M46.Axis.NcToPlc.ActPos > -1.0
);
// Disable movement in positive direction
EPS_Diff_X_Forward(eps:=Main.M28.stEPSForwardEnable);
EPS_Diff_X_Forward.setDescription('POSITIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_X_Forward.setBit(nBits:= 0,
bValue := Main.M28.Axis.NcToPlc.ActPos - Main.M46.Axis.NcToPlc.ActPos < 1.0
);
// Diff Y
// Disable movement in negative direction.
EPS_Diff_Y_Backward(eps:=Main.M29.stEPSBackwardEnable);
EPS_Diff_Y_Backward.setDescription('NEGATIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_Y_Backward.setBit(nBits:= 0,
bValue := Main.M47.Axis.NcToPlc.ActPos - F_TargetCryoYPosition(
fNeutralPosition := GVL_VAR.fCryoYNeutralPositionAtDiffY0 + Main.M29.Axis.NcToPlc.ActPos,
fBraidDiameter := GVL_VAR.fBraidDiameter,
fPhiAngleDeg := Main.M32.Axis.NcToPlc.ActPos) < 1.0
);
// Disable movement in positive direction
EPS_Diff_Y_Forward(eps:=Main.M29.stEPSForwardEnable);
EPS_Diff_Y_Forward.setDescription('POSITIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_Diff_Y_Forward.setBit(nBits:= 0,
bValue := Main.M47.Axis.NcToPlc.ActPos - F_TargetCryoYPosition(
fNeutralPosition := GVL_VAR.fCryoYNeutralPositionAtDiffY0 + Main.M29.Axis.NcToPlc.ActPos,
fBraidDiameter := GVL_VAR.fBraidDiameter,
fPhiAngleDeg := Main.M32.Axis.NcToPlc.ActPos) > -1.0
);
// DIFFRACTOMETER 2-THETA ANGLE RESTRICTIONS
// Disable movement in negative direction.
EPS_Diff_2Theta_Backward(eps:=Main.M35.stEPSBackwardEnable);
EPS_Diff_2Theta_Backward.setDescription('NEGATIVE MOTION'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Diffractometer 2 Theta cannot go less than the diffractometer theta angle minus 21 degrees.
EPS_Diff_2Theta_Backward.setBit(nBits:= 0, bValue:=Main.M35.Axis.NcToPlc.ActPos > Main.M34.stAxisStatus.fActPosition - 21);
fb_Diff_X(stMotionStage:=Main.M28);
fb_Diff_Y(stMotionStage:=Main.M29);
fb_Diff_Z(stMotionStage:=Main.M30);
fb_Diff_2ThetaY(stMotionStage:=Main.M31);
fb_Diff_Phi(stMotionStage:=Main.M32);
fb_Diff_Chi(stMotionStage:=Main.M33);
fb_Diff_Theta(stMotionStage:=Main.M34);
fb_Diff_2Theta(stMotionStage:=Main.M35);
END_PROGRAM
PRG_LAS
PROGRAM PRG_LAS
VAR
fb_LAS_VIS : FB_MotionStage;
fb_LAS_D_H : FB_MotionStage;
fb_LAS_D_V : FB_MotionStage;
bInit : BOOl := TRUE;
//LAS_VIS BEAM OK/NOK
{attribute 'pytmc' := '
pv: QRIX:LAS:MMS:VIS:STATES:SET
io: io
'}
eLAS_VIS_StateSet: ENUM_LAS_VIS_States;
{attribute 'pytmc' := '
pv: QRIX:LAS:MMS:VIS:STATES:GET
io: i
'}
eLAS_VIS_StateGet: ENUM_LAS_VIS_States;
{attribute 'pytmc' := 'pv: QRIX:LAS:MMS:VIS'}
fbLAS_VIS_PositionState1D : FB_PositionState1D;
astLAS_VIS_PositionState : ARRAY [1..GeneralConstants.MAX_STATES] of ST_PositionState;
fbLAS_VIS_StateSetupHelper : FB_StateSetupHelper;
fbLAS_VIS_HardwareFFOutput : FB_HardwareFFOutput;
//EPS
stLAS_VIS : DUT_EPS;
fbLAS_VIS_EPS : FB_EPS;
stLAS_D_H : DUT_EPS;
fbLAS_D_H_EPS : FB_EPS;
END_VAR
IF (bInit) THEN
bInit := FALSE;
Main.M36.bHardwareEnable := TRUE;
Main.M36.bPowerSelf := TRUE;
Main.M36.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M36.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M36.fVelocity := 0.1;
Main.M37.bHardwareEnable := TRUE;
Main.M37.bPowerSelf := TRUE;
Main.M37.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M37.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M37.fVelocity := 0.1;
Main.M38.bHardwareEnable := TRUE;
Main.M38.bPowerSelf := TRUE;
Main.M38.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M38.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M38.fVelocity := 0.1;
// Initialize the M36 LAS VIS Axis States
// Beam OK if one of these two states is active. Otherwise, beam is not okay.
fbLAS_VIS_StateSetupHelper(stPositionState := astLAS_VIS_PositionState[1], sName := 'Position7to24mm', bValid := TRUE, fPosition := 15.5, fDelta := 8.5);
fbLAS_VIS_StateSetupHelper(stPositionState := astLAS_VIS_PositionState[2], sName := 'Position62to77mm', bValid := TRUE, fPosition := 69.5, fDelta := 7.5);
END_IF
//EPS
(*
fbLAS_D_H_EPS.setDescription('Less10mm;SampleZ;PHI'); // SET DESCRITION FOR BYTE INDICATOR LABELS
//Zero is fully retracted
fbLAS_D_H_EPS.setBit(nBits:= 0, bValue:=fb_LAS_D_H.stMotionStage.fPosition<=10);
fbLAS_D_H_EPS.setBit(nBits:= 1, bValue:=fb_LAS_D_H.stMotionStage.fPosition<=10 OR PRG_Diffractometer.fb_Diff_Z.stMotionStage.fPosition<=-20);
fbLAS_D_H_EPS.setBit(nBits:= 2, bValue:=fb_LAS_D_H.stMotionStage.fPosition<=10 OR PRG_Diffractometer.fb_Diff_Phi.stMotionStage.fPosition>0);
fbLAS_D_H_EPS(eps:=stLAS_D_H);
fb_LAS_D_H.stMotionStage.stEPSForwardEnable := stLAS_D_H; // Update Motion stage structure.
fbLAS_VIS_EPS.setDescription('IN90mm;PHIat90;Theta<10'); // SET DESCRITION FOR BYTE INDICATOR LABELS
//Zero is fully retracted
fbLAS_VIS_EPS.setBit(nBits:= 0, bValue:=fb_LAS_VIS.stMotionStage.fPosition<=5);
fbLAS_VIS_EPS.setBit(nBits:= 1, bValue:=fb_LAS_VIS.stMotionStage.fPosition<=5 OR PRG_Diffractometer.fb_Diff_Phi.stMotionStage.fPosition<>90);
fbLAS_VIS_EPS.setBit(nBits:= 2, bValue:=fb_LAS_VIS.stMotionStage.fPosition<=5 OR PRG_Diffractometer.fb_Diff_Theta.stMotionStage.fPosition>10);
fbLAS_VIS_EPS(eps:=fbLAS_VIS_EPS);
//fb_LAS_VIS.stMotionStage.stEPSForwardEnable := fbLAS_VIS_EPS;
*)
fbLAS_VIS_PositionState1D(
stMotionStage := Main.M36,
astPositionState := astLAS_VIS_PositionState,
eEnumSet := eLAS_VIS_StateSet,
eEnumGet := eLAS_VIS_StateGet,
bEnable := TRUE
);
IF eLAS_VIS_StateGet = ENUM_LAS_VIS_States.Position7to24mm OR eLAS_VIS_StateGet = ENUM_LAS_VIS_States.Position62to77mm THEN
// BEAM OKAY
ELSE
// BEAM NOT OKAY
END_IF
fb_LAS_VIS(stMotionStage:=Main.M36);
fb_LAS_D_H(stMotionStage:=Main.M37);
fb_LAS_D_V(stMotionStage:=Main.M38);
END_PROGRAM
- Related:
PRG_OPT
PROGRAM PRG_OPT
VAR
bInit: BOOL := TRUE;
fbMotionRxG: FB_MotionStage;
fbMotionXG: FB_MotionStage;
fbMotionRzPM: FB_MotionStage;
END_VAR
IF bInit THEN
Main.M10.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M10.bHardwareEnable := TRUE;
Main.M10.bPowerSelf := TRUE;
Main.M10.fVelocity := 0.1;
Main.M11.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M11.bHardwareEnable := TRUE;
Main.M11.bPowerSelf := TRUE;
Main.M11.fVelocity := 0.1;
Main.M14.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M14.bHardwareEnable := TRUE;
Main.M14.bPowerSelf := TRUE;
Main.M14.fVelocity := 0.1;
Main.M10.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M11.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M14.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
bInit := FALSE;
END_IF;
fbMotionRxG(stMotionStage:= Main.M10);
fbMotionXG(stMotionStage:= Main.M11);
fbMotionRzPM(stMotionStage:= Main.M14);
END_PROGRAM
- Related:
PRG_OPT_SLITS
PROGRAM PRG_OPT_SLITS
VAR
{attribute 'pytmc' := 'pv: QRIX:OPTSL'}
fbSlits_Opt: FB_Slits;
bExecuteMotion: BOOL := TRUE;
bMoveOk: BOOL :=TRUE;
bInit:BOOl:=TRUE;
END_VAR
// 4 jaws slit
IF bInit THEN
bInit := FALSE;
Main.M3.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M3.bHardwareEnable := TRUE;
Main.M3.bPowerSelf := TRUE;
Main.M3.fVelocity := 0.1;
Main.M4.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M4.bHardwareEnable := TRUE;
Main.M4.bPowerSelf := TRUE;
Main.M4.fVelocity := 0.1;
Main.M5.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M5.bHardwareEnable := TRUE;
Main.M5.bPowerSelf := TRUE;
Main.M5.fVelocity := 0.1;
Main.M6.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M6.bHardwareEnable := TRUE;
Main.M6.bPowerSelf := TRUE;
Main.M6.fVelocity := 0.1;
//
Main.M3.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M4.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M5.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M6.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
END_IF
fbSlits_Opt( i_DevName:= 'OPT_SLITS',
stTopBlade:= Main.M5,
stBottomBlade:= Main.M6,
stLeftBlade:= Main.M3,
stRightBlade:= Main.M4,
io_fbFFHWO:= GVL_PMPS.fbFastFaultOutput1,
fbArbiter:= GVL_PMPS.fbArbiter,
bExecuteMotion:= bExecuteMotion,
bMoveOk:= bMoveOk
);
END_PROGRAM
PRG_OPT_XPM
PROGRAM PRG_OPT_XPM
VAR
bInit: BOOL :=TRUE;
{attribute 'pytmc' := 'pv: QRIX:PM'}
fb2AxesXPM: FB_2AxesTrans;
bExecuteMotionXPM: BOOL :=TRUE;
bMoveOkXPM: BOOL :=TRUE;
END_VAR
VAR CONSTANT
rDistance :REAL := 0.4;
END_VAR
IF bINIT THEN
Main.M12.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M12.bHardwareEnable := TRUE;
Main.M12.bPowerSelf := TRUE;
Main.M12.fVelocity := 0.1;
Main.M13.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M13.bHardwareEnable := TRUE;
Main.M13.bPowerSelf := TRUE;
Main.M13.fVelocity := 0.1;
Main.M12.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M13.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
bINIT := FALSE;
END_IF;
fb2AxesXPM( i_DevName := 'OPT_XPM',
stFirstAxis := Main.M12,
stSecondAxis := Main.M13,
bExecuteMotion := bExecuteMotionXPM,
io_fbFFHWO := GVL_PMPS.fbFastFaultOutput1,
fbArbiter := GVL_PMPS.fbArbiter,
rDistance := rDistance,//actually 0.48m?? //0.4, // 0.4m
bMoveOk := bMoveOkXPM
);
END_PROGRAM
- Related:
PRG_OPT_YG
PROGRAM PRG_OPT_YG
VAR
bDebug: BOOL;
bExecuteMotion: BOOL := TRUE;
bMoveOk: BOOL :=TRUE;
{attribute 'pytmc' := 'pv: QRIX:OPT'}
fbYGMotion: FB_3AxesJack;
bInit: BOOL :=TRUE;
tonDelayUpdate: TON;
END_VAR
IF bInit THEN
bInit := FALSE;
Main.M7.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M7.bHardwareEnable := TRUE;
Main.M7.bPowerSelf := TRUE;
Main.M8.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M8.bHardwareEnable := TRUE;
Main.M8.bPowerSelf := TRUE;
Main.M9.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M9.bHardwareEnable := TRUE;
Main.M9.bPowerSelf := TRUE;
Main.M7.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M8.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
Main.M9.nEnableMode := ENUM_StageEnableMode.DURING_MOTION ;
END_IF
fbYGMotion(
bMoveOk := bMoveOk,
stJack1Axis := Main.M9,
stJack2Axis := Main.M7,
stJack3Axis := Main.M8,
rDistanceX := 0.24,
rDistanceZ := 0.88,
bJack3DoesPitch := FALSE,
rLimitPositionMax := 8.0,
rLimitPositionMin := -8.0,
rLimitPitchMin := -18,
rLimitPitchMax := 18,
rLimitRollMin := -18,
rLimitRollMax := 18,
fMaxVelocity := 0.1,
);
END_PROGRAM
- Related:
PRG_Questar
PROGRAM PRG_Questar
VAR
fb_Ques_01 : FB_MotionStage;
fb_Ques_02: FB_MotionStage;
bInit : BOOl := TRUE;
END_VAR
IF ( bInit) THEN
bInit := FALSE;
Main.M17.bHardwareEnable := TRUE;
Main.M17.bPowerSelf := TRUE;
Main.M17.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M17.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M17.fVelocity := 0.1;
Main.M18.bHardwareEnable := TRUE;
Main.M18.bPowerSelf := TRUE;
Main.M18.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M18.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M18.fVelocity := 0.1;
END_IF
//EPS??
fb_Ques_01(stMotionStage:=Main.M17);
fb_Ques_02(stMotionStage:=Main.M18);
END_PROGRAM
- Related:
PRG_RotDet
PROGRAM PRG_RotDet
VAR
fb_Rot_Det : FB_MotionStage;
bInit : BOOl := TRUE;
END_VAR
IF ( bInit) THEN
bInit := FALSE;
Main.M45.bHardwareEnable := TRUE;
Main.M45.bPowerSelf := TRUE;
Main.M45.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M45.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M45.bLimitForwardEnable := TRUE;
Main.M45.bLimitBackwardEnable := TRUE;
Main.M45.fVelocity := 0.1;
END_IF
fb_Rot_Det(stMotionStage:=Main.M45);
END_PROGRAM
- Related:
PRG_SDS
PROGRAM PRG_SDS
VAR
fb_SDS_ROT_V : FB_MotionStage;
fb_SDS_X : FB_MotionStage;
fb_SDS_Y : FB_MotionStage;
fb_SDS_Z : FB_MotionStage;
fb_SDS_ROT_H : FB_MotionStage;
fb_SDS_H : FB_MotionStage;
bInit : BOOl := TRUE;
EPS_SDS_H_Backward : FB_EPS;
EPS_SDS_H_Forward : FB_EPS;
fSDSYEPSOverride: REAL;
EPS_SDS_Y_Backward : FB_EPS;
EPS_SDS_Y_Forward : FB_EPS;
fMaxSampleDeliveryArmPositionWithValveClosed : REAL := 50;
{attribute 'pytmc' := '
pv: QRIX:SDS:NP
field: DESC Structure defining qrixs sample delivery system non-persistent data.
'}
stSDS: ST_SDS;
fbSDS: FB_SDS;
{attribute 'pytmc' := '
pv: QRIX:SDS:Y:EPS:BACK:OVRD
field: DESC Allow SDS Y backward limit to be user defined for 1h.
'}
bEPS_SDS_Y_Backward_Override : BOOL;
fbEPS_SDS_Y_Backward_Override_Start : R_TRIG;
tEPS_SDS_Y_Backward_Override_Time : TIME := T#1h;
fbEPS_SDS_Y_EPS_BACK_OVRD_TIMER : TON;
{attribute 'pytmc' := '
pv: QRIX:SDS:Y:EPS:BACK:OVRD_COUNTDOWN
field: DESC Time Remaining Untill Override resets.
'}
tSDS_Y_EPS_BACK_OVRD_TimerCountDown : STRING := TIME_TO_STRING(T#0s);
{attribute 'pytmc' := '
pv: QRIX:SDS:Y:EPS:BACK:USR:OVRD_LIM
field: DESC Allow SDS Y backward limit to be user defined for 1h.
io: io
autosave_pass1: VAL DESC
'}
fSDS_Y_NEGATIVE_LIM_USR : LREAL;
{attribute 'pytmc' := '
pv: QRIX:SDS:Y:EPS:BACK:OVRD_LIM
field: DESC Readback from Actual Soft Limit.
io: i
'}
fSDS_Y_NEGATIVE_LIM : LREAL;
fSDS_Y_NEGATIVE_LIM_DEFAULT : LREAL := -110;
END_VAR
VAR PERSISTENT
{attribute 'pytmc' := '
pv: QRIX:SDS:P
field: DESC Structure defining qrixs sample delivery system persistent data.
'}
stSDSPersistent: ST_SDSPersistent;
END_VAR
IF bInit THEN
bInit := FALSE;
Main.M39.bHardwareEnable := TRUE;
Main.M39.bPowerSelf := TRUE;
Main.M39.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M39.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M40.bHardwareEnable := TRUE;
Main.M40.bPowerSelf := TRUE;
Main.M40.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M40.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M41.bHardwareEnable := TRUE;
Main.M41.bPowerSelf := TRUE;
Main.M41.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M41.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M42.bHardwareEnable := TRUE;
Main.M42.bPowerSelf := TRUE;
Main.M42.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M42.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M42.nHomingMode := ENUM_EpicsHomeCmd.HOME_VIA_LOW;
Main.M42.bLimitBackwardEnable := TRUE;
Main.M42.bLimitForwardEnable := TRUE;
Main.M43.bHardwareEnable := TRUE;
Main.M43.bPowerSelf := TRUE;
Main.M43.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M43.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M43.nHomingMode := ENUM_EpicsHomeCmd.LOW_LIMIT;
Main.M43.fHomePosition := 0.0;
Main.M44.bHardwareEnable := TRUE;
Main.M44.bPowerSelf := TRUE;
Main.M44.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M44.nEnableMode := ENUM_StageEnableMode.DURING_MOTION;
Main.M44.nHomingMode := ENUM_EpicsHomeCmd.HOME_VIA_LOW;
Main.M44.fHomePosition := 0;//1.176; distance from home switch trigger to lower limit trigger
fSDSYEPSOverride := -40;
fSDS_Y_NEGATIVE_LIM := fSDS_Y_NEGATIVE_LIM_DEFAULT;
END_IF
GVL_Interface.QRIX_MOT_SDS_MMS_H_RAW_ENC_CNTS := Main.M44.nRawEncoderUINT;
GVL_Interface.QRIX_MOT_SDS_MMS_H_FORWARD_EN := Main.M44.bLimitForwardEnable;
GVL_Interface.QRIX_MOT_SDS_MMS_H_BACKWARD_EN := Main.M44.bLimitBackwardEnable;
//EPS
// Sample Delivery System
// Disable movement in negative direction.
EPS_SDS_H_Backward(eps:=Main.M44.stEPSBackwardEnable);
EPS_SDS_H_Backward.setDescription('NEGATIVE MOTION ALLOWED UNTIL LIMIT'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Disable movement in positive direction.
EPS_SDS_H_Forward(eps:=Main.M44.stEPSForwardEnable);
EPS_SDS_H_Forward.setDescription('POS MOTION IF VGC_03 closed: (cmd<50mm | pos<50mm | Homing Valid | SDS Y<-45mm)'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
// Sample Delivery Arm can Extend if the VGC valve is open and not closed, and it has been homed.
EPS_SDS_H_Forward.setBit(nBits:= 4, bValue:= NOT stSDS.tonForwardHomingLimit.Q);
EPS_SDS_H_Forward.setBit(nBits:= 3, bValue:=(NOT GVL_Interface.QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH AND
GVL_Interface.QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH) OR Main.M44.stAxisStatus.fPosition < fMaxSampleDeliveryArmPositionWithValveClosed);
EPS_SDS_H_Forward.setBit(nBits:= 2, bValue:=(NOT GVL_Interface.QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH AND
GVL_Interface.QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH) OR Main.M44.stAxisStatus.fActPosition < fMaxSampleDeliveryArmPositionWithValveClosed);
EPS_SDS_H_Forward.setBit(nBits:= 1, bValue:=(NOT GVL_Interface.QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH AND
GVL_Interface.QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH) OR NOT stSDS.bHomingRequired);
EPS_SDS_H_Forward.setBit(nBits:= 0, bValue:=(NOT GVL_Interface.QRIX_VAC_VGC_03_CLOSED_LIMIT_SWITCH AND
GVL_Interface.QRIX_VAC_VGC_03_OPEN_LIMIT_SWITCH) OR Main.M40.stAxisStatus.fActPosition < fSDSYEPSOverride);
// Disable movement in negative direction.
EPS_SDS_Y_Backward(eps:=Main.M40.stEPSBackwardEnable);
EPS_SDS_Y_Backward.setDescription('NEGATIVE MOTION ALLOWED UNTIL LIMIT'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
EPS_SDS_Y_Backward.setBit(nBits:=0, bValue:= Main.M40.stAxisStatus.fActPosition > fSDS_Y_NEGATIVE_LIM);
fbEPS_SDS_Y_EPS_BACK_OVRD_TIMER(IN:=bEPS_SDS_Y_Backward_Override, PT:= tEPS_SDS_Y_Backward_Override_Time);
tSDS_Y_EPS_BACK_OVRD_TimerCountDown := TIME_TO_STRING(tEPS_SDS_Y_Backward_Override_Time - fbEPS_SDS_Y_EPS_BACK_OVRD_TIMER.ET);
IF bEPS_SDS_Y_Backward_Override THEN
fSDS_Y_NEGATIVE_LIM := fSDS_Y_NEGATIVE_LIM_USR;
END_IF
// Resets timer when EPS timer has expired; 1hr default
IF fbEPS_SDS_Y_EPS_BACK_OVRD_TIMER.Q THEN
bEPS_SDS_Y_Backward_Override := FALSE;
fSDS_Y_NEGATIVE_LIM := fSDS_Y_NEGATIVE_LIM_DEFAULT;
END_IF
// Disable movement in positive direction.
EPS_SDS_Y_Forward(eps:=Main.M40.stEPSForwardEnable);
EPS_SDS_Y_Forward.setDescription('POSITIVE MOTION ALLOWED UNTIL LIMIT'); // SET DESCRIPTION FOR BYTE INDICATOR LABELS
fb_SDS_X(stMotionStage:=Main.M39);
fb_SDS_Y(stMotionStage:=Main.M40);
fb_SDS_Z(stMotionStage:=Main.M41);
Main.M42.bHome := NOT Main.M42.bHome;
fb_SDS_ROT_V(stMotionStage:=Main.M42);
fb_SDS_ROT_H(stMotionStage:=Main.M43);
fb_SDS_H(stMotionStage:=Main.M44);
stSDS.stGarageYStage REF= Main.M40;
stSDS.stGarageRStage REF= Main.M42;
stSDS.stTransferArmHStage REF= Main.M44;
stSDS.stTransferArmRStage REF= Main.M43;
stSDS.eDiffXStateGet REF= PRG_Diffractometer.eDiffXStateGet;
stSDS.eDiffXStateSet REF= PRG_Diffractometer.eDiffXStateSet;
stSDS.stDiffXStage REF= Main.M28;
stSDS.eDiffYStateGet REF= PRG_Diffractometer.eDiffYStateGet;
stSDS.eDiffYStateSet REF= PRG_Diffractometer.eDiffYStateSet;
stSDS.stDiffYStage REF= Main.M29;
stSDS.eDiffZStateGet REF= PRG_Diffractometer.eDiffZStateGet;
stSDS.eDiffZStateSet REF= PRG_Diffractometer.eDiffZStateSet;
stSDS.stDiffZStage REF= Main.M30;
stSDS.eDiff2ThetaYStateGet REF= PRG_Diffractometer.eDiff2ThetaYStateGet;
stSDS.eDiff2ThetaYStateSet REF= PRG_Diffractometer.eDiff2ThetaYStateSet;
stSDS.stDiff2ThetaYStage REF= Main.M31;
stSDS.eDiffPhiStateGet REF= PRG_Diffractometer.eDiffPhiStateGet;
stSDS.eDiffPhiStateSet REF= PRG_Diffractometer.eDiffPhiStateSet;
stSDS.stDiffPhiStage REF= Main.M32;
stSDS.eDiffChiStateGet REF= PRG_Diffractometer.eDiffChiStateGet;
stSDS.eDiffChiStateSet REF= PRG_Diffractometer.eDiffChiStateSet;
stSDS.stDiffChiStage REF= Main.M33;
stSDS.eDiff2ThetaStateGet REF= PRG_Diffractometer.eDiff2ThetaStateGet;
stSDS.eDiff2ThetaStateSet REF= PRG_Diffractometer.eDiff2ThetaStateSet;
stSDS.stDiff2ThetaStage REF= Main.M34;
stSDS.eDiffThetaStateGet REF= PRG_Diffractometer.eDiffThetaStateGet;
stSDS.eDiffThetaStateSet REF= PRG_Diffractometer.eDiffThetaStateSet;
stSDS.stDiffThetaStage REF= Main.M35;
// Function block containing logic for sample delivery system motion automation.
fbSDS(
stSDSin := stSDS,
stSDSPersistent := stSDSPersistent
);
END_PROGRAM
PRG_Sensor
PROGRAM PRG_Sensor
VAR
bInit : bool :=TRUE;
{attribute 'TcLinkTo' := 'TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(Out) RxPDO-Map^Head Value Mode'}
bSensorHeadValueMode AT %Q* : bool;
{attribute 'TcLinkTo' := 'TIIB[Rack#2D LinearGaugeAmp (SC-HG1-ETC)]^Process Data(Out) RxPDO-Map^Normal Value Mode'}
bSensorHeadNormalValueMode AT %Q* : bool;
bResetSensorMode AT %I* : bool;
uintTest AT %Q*: UINT;
END_VAR
If bInit Then
bSensorHeadNormalValueMode := True;
bInit := False;
End_If;
If(bResetSensorMode = True) Then
bResetSensorMode := False;
bSensorHeadValueMode := False;
bSensorHeadNormalValueMode := False;
bInit := True;
End_If;
GVL_Sensor.stYDF1.sName := 'YDF1 Panasonic HG-S';
GVL_Sensor.stYDF1.sEGU := 'mm';
GVL_Sensor.stYDF1.fCntsInEGU := 10000;
GVL_Sensor.nYDF1 := DINT_TO_UDINT(GVL_Sensor.stYDF1.iRawCnts + 10000);
GVL_Sensor.stYDF2.sName := 'YDF2 Panasonic HG-S';
GVL_Sensor.stYDF2.sEGU := 'mm';
GVL_Sensor.stYDF2.fCntsInEGU := 10000;
GVL_Sensor.nYDF2 := DINT_TO_UDINT(GVL_Sensor.stYDF2.iRawCnts + 10000);
GVL_Sensor.stYDF3.sName := 'YDF3 Panasonic HG-S';
GVL_Sensor.stYDF3.sEGU := 'mm';
GVL_Sensor.stYDF3.fCntsInEGU := 10000;
GVL_Sensor.nYDF3 := DINT_TO_UDINT(GVL_Sensor.stYDF3.iRawCnts + 10000);
GVL_Sensor.stHDF.sName := 'HDF Panasonic HG-S';
GVL_Sensor.stHDF.sEGU := 'mm';
GVL_Sensor.stHDF.fCntsInEGU := 10000;
GVL_Sensor.stGraniteP.sName := 'Granite Pitch';
GVL_Sensor.stGraniteP.sEGU := 'mrad';
GVL_Sensor.stGraniteP.fCntsInEGU:= 3276.7; // 1V = 1mrad in Low gain mode
GVL_Sensor.stGraniteP.iOffset := -1005;
GVL_Sensor.stGraniteR.sName := 'Granite Roll';
GVL_Sensor.stGraniteR.sEGU := 'mrad';
GVL_Sensor.stGraniteR.fCntsInEGU:= 3276.7;
GVL_Sensor.stGraniteR.iOffset := 3326;
GVL_Sensor.stFrameP.sName := 'Det. Frame Pitch';
GVL_Sensor.stFrameP.sEGU := 'mrad';
GVL_Sensor.stFrameP.fCntsInEGU := 3276.7;
GVL_Sensor.stFrameR.sName := 'Det. Frame Roll';
GVL_Sensor.stFrameR.sEGU := 'mrad';
GVL_Sensor.stFrameR.fCntsInEGU := 3276.7;
GVL_Sensor.stPS1.sName := 'PS1';
GVL_Sensor.stPS1.sEGU := 'MPa';
GVL_Sensor.stPS1.fCntsInEGU := 13106.8; // 0.25MPa/V
GVL_Sensor.stPS1.iOffset := 3277; // 0MPa@1V P=0.25(V-V0), V0 = 1.0
GVL_Sensor.stPS2.sName := 'PS2';
GVL_Sensor.stPS2.sEGU := 'MPa';
GVL_Sensor.stPS2.fCntsInEGU := 13106.8; // 0.25MPa/V
GVL_Sensor.stPS2.iOffset := 3277; // 0MPa@1V P=0.25(V-V0), V0 = 1.0
GVL_Sensor.stPS3.sName := 'PS3';
GVL_Sensor.stPS3.sEGU := 'MPa';
GVL_Sensor.stPS3.fCntsInEGU := 13106.8; // 0.25MPa/V
GVL_Sensor.stPS3.iOffset := 3277; // 0MPa@1V P=0.25(V-V0), V0 = 1.0
GVL_Sensor.stYDF1.fValue := DINT_TO_REAL(GVL_Sensor.stYDF1.iRawCnts - GVL_Sensor.stYDF1.iOffset) / GVL_Sensor.stYDF1.fCntsInEGU;
GVL_Sensor.stYDF2.fValue := DINT_TO_REAL(GVL_Sensor.stYDF2.iRawCnts - GVL_Sensor.stYDF2.iOffset) / GVL_Sensor.stYDF2.fCntsInEGU;
GVL_Sensor.stYDF3.fValue := DINT_TO_REAL(GVL_Sensor.stYDF3.iRawCnts - GVL_Sensor.stYDF3.iOffset) / GVL_Sensor.stYDF3.fCntsInEGU;
GVL_Sensor.stHDF.fValue := DINT_TO_REAL(GVL_Sensor.stHDF.iRawCnts - GVL_Sensor.stHDF.iOffset) / GVL_Sensor.stHDF.fCntsInEGU;
GVL_Sensor.stGraniteP.fValue := -1 * INT_TO_REAL(GVL_Sensor.stGraniteP.iRawCnts - GVL_Sensor.stGraniteP.iOffset) / GVL_Sensor.stGraniteP.fCntsInEGU;
GVL_Sensor.stGraniteR.fValue := INT_TO_REAL(GVL_Sensor.stGraniteR.iRawCnts - GVL_Sensor.stGraniteR.iOffset) / GVL_Sensor.stGraniteR.fCntsInEGU;
GVL_Sensor.stFrameP.fValue := INT_TO_REAL(GVL_Sensor.stFrameP.iRawCnts - GVL_Sensor.stFrameP.iOffset) / GVL_Sensor.stFrameP.fCntsInEGU;
GVL_Sensor.stFrameR.fValue := -1 * INT_TO_REAL(GVL_Sensor.stFrameR.iRawCnts - GVL_Sensor.stFrameR.iOffset) / GVL_Sensor.stFrameR.fCntsInEGU;
GVL_Sensor.stPS1.fValue := INT_TO_REAL(GVL_Sensor.stPS1.iRawCnts - GVL_Sensor.stPS1.iOffset) / GVL_Sensor.stPS1.fCntsInEGU;
GVL_Sensor.stPS2.fValue := INT_TO_REAL(GVL_Sensor.stPS2.iRawCnts - GVL_Sensor.stPS2.iOffset) / GVL_Sensor.stPS2.fCntsInEGU;
GVL_Sensor.stPS3.fValue := INT_TO_REAL(GVL_Sensor.stPS3.iRawCnts - GVL_Sensor.stPS3.iOffset) / GVL_Sensor.stPS3.fCntsInEGU;
//uintTest:= REAL_TO_UINT(GVL_Sensor.stYDF1.fValue);
END_PROGRAM
- Related:
PRG_SpetrometerArm
PROGRAM PRG_SpetrometerArm
VAR
{attribute 'TcLinkTo' := 'TIIB[Rack#1A-01 Coupler (EK1100)]^Rack#1A-02 SFIN (EL1918)^StandardOutputs^Standard In Var 2'}
bEnaIclk AT %Q*: BOOL := TRUE;
{attribute 'TcLinkTo' := 'TIIB[Rack#1A-01 Coupler (EK1100)]^Rack#1A-02 SFIN (EL1918)^StandardOutputs^Standard In Var 1'}
bEnaIclkErrAck AT %Q*: BOOL := FALSE;
{attribute 'TcLinkTo' := 'TIIB[Rack#1A-01 Coupler (EK1100)]^Rack#1A-02 SFIN (EL1918)^StandardOutputs^Standard In Var 9'}
bEnaIclkRestartESTOP AT %Q*: BOOL := FALSE;
fbPower_AxisM11: MC_Power;
fbPower_AxisM12: MC_Power;
fbPower_AxisM13: MC_Power;
fbPower_AxisM14: MC_Power;
rtESTOPDeactivated: R_TRIG;
tonTurnoffCBReset: TON;
END_VAR
ACT_ESTOP();
PRG_2Theta();
PRG_DET_ARM();
PRG_DET_CHAMBER();
PRG_DET_SLIT();
PRG_DET_FRAME();
PRG_OPT_SLITS();
PRG_OPT_YG();
PRG_OPT_XPM();
PRG_OPT();
PRG_Sensor();
tonTurnOffCBReset(IN:=
GVL_VAR.bCircuitBreakerB0108Reset OR
GVL_VAR.bCircuitBreakerB0113Reset OR
GVL_VAR.bCircuitBreakerB0118Reset OR
GVL_VAR.bCircuitBreakerR2A02Reset OR
GVL_VAR.bCircuitBreakerR2A07Reset OR
GVL_VAR.bCircuitBreakerR2A13Reset OR
GVL_VAR.bCircuitBreakerR2B14Reset OR
GVL_VAR.bCircuitBreakerR2B17Reset,
PT:=T#2s
);
IF tonTurnOffCBReset.Q THEN
GVL_VAR.bCircuitBreakerB0108Reset := FALSE;
GVL_VAR.bCircuitBreakerB0113Reset := FALSE;
GVL_VAR.bCircuitBreakerB0118Reset := FALSE;
GVL_VAR.bCircuitBreakerR2A02Reset := FALSE;
GVL_VAR.bCircuitBreakerR2A07Reset := FALSE;
GVL_VAR.bCircuitBreakerR2A13Reset := FALSE;
GVL_VAR.bCircuitBreakerR2B14Reset := FALSE;
GVL_VAR.bCircuitBreakerR2B17Reset := FALSE;
END_IF
END_PROGRAM
ACTION ACT_ESTOP:
IF bEnaIclkErrAck THEN
bEnaIclkErrAck := FALSE;
END_IF
IF bEnaIclkRestartESTOP THEN
bEnaIclkRestartESTOP := FALSE;
END_IF
// When user push the ESTOPs
// REVALUATE AFTER TESTING:
// Only specific motors are a part of the ESTOP at this point based on
// "Components" column on this page:
// https://confluence.slac.stanford.edu/display/L2SI/qRIXS+Spectrometer+Arm
// Items are commented out if not included in ESTOP to allow them to be added back easily.
IF NOT GVL_EPS.bESTOP THEN
GVL_EPS.bOpenSV1 := FALSE;
GVL_EPS.bOpenSV2 := FALSE;
Main.M1.bHardwareEnable := FALSE; // SSL Sliding Seal // QRIX:SSL:MMS
Main.M2.bHardwareEnable := FALSE; // 2Theta Arm // QRIX:SA:MMS:2Theta
//Main.M3.bHardwareEnable := FALSE; // Optics Slits XS1 // QRIX:OPTSL:MMS:NORTH
//Main.M4.bHardwareEnable := FALSE; // Optics Slits XS2 // QRIX:OPTSL:MMS:SOUTH
//Main.M5.bHardwareEnable := FALSE; // Optics Slits YS1 // QRIX:OPTSL:MMS:TOP
//Main.M6.bHardwareEnable := FALSE; // Optics Slits YS2 // QRIX:OPTSL:MMS:BOTTOM
Main.M7.bHardwareEnable := FALSE; // Optics Base YG1 // QRIX:OPT:MMS:Y1
Main.M8.bHardwareEnable := FALSE; // Optics Base YG2 // QRIX:OPT:MMS:Y2
Main.M9.bHardwareEnable := FALSE; // Optics Base YG3 // QRIX:OPT:MMS:Y3
//Main.M10.bHardwareEnable := FALSE; // Optics Grating RxG // QRIX:G:MMS:Rx
//Main.M11.bHardwareEnable := FALSE; // Optics Grating XG // QRIX:G:MMS:X
//Main.M12.bHardwareEnable := FALSE; // Optics Mirror XPM1 // QRIX:PM:MMS:X1
//Main.M13.bHardwareEnable := FALSE; // Optics Mirror XPM2 // QRIX:PM:MMS:X2
//Main.M14.bHardwareEnable := FALSE; // Optics Mirror RzPM // QRIX:PM:MMS:Rz
Main.M15.bHardwareEnable := FALSE; // Detector Frame YDF1 // QRIX:DF:MMS:Y1
Main.M16.bHardwareEnable := FALSE; // Detector Frame YDF2 // QRIX:DF:MMS:Y2
Main.M17.bHardwareEnable := FALSE; // Detector Frame YDF3 // QRIX:DF:MMS:Y3
//Main.M18.bHardwareEnable := FALSE; // Detector Slits XSDC1 // QRIX:DETSL:MMS:NORTH
//Main.M19.bHardwareEnable := FALSE; // Detector Slits XSDC2 // QRIX:DETSL:MMS:SOUTH
//Main.M20.bHardwareEnable := FALSE; // Detector Slits YSDC1 // QRIX:DETSL:MMS:TOP
//Main.M21.bHardwareEnable := FALSE; // Detector Slits YSDC2 // QRIX:DETSL:MMS:BOTTOM
//Main.M22.bHardwareEnable := FALSE; // Detector Chamber XDC // QRIX:DC:MMS:X
//Main.M23.bHardwareEnable := FALSE; // Detector Chamber RyDC // QRIX:DC:MMS:Ry
//Main.M24.bHardwareEnable := FALSE; // Detector Chamber ZDC // QRIX:DC:MMS:Z
Main.M25.bHardwareEnable := FALSE; // Detector Arm YF1 // QRIX:DA:MMS:Y1
Main.M26.bHardwareEnable := FALSE; // Detector Arm YF2 // QRIX:DA:MMS:Y2
Main.M27.bHardwareEnable := FALSE; // Detector Arm ZF // QRIX:DA:MMS:Z
bDoneJackOff := FALSE;
bDoneLevitation := FALSE;
bDoneLanding := FALSE;
bDoneAdjustingRoll := FALSE;
bDoneAdjustingPitch := FALSE;
bDoneAdjustingHeight := FALSE;
END_IF
rtESTOPDeactivated(CLK:=GVL_EPS.bESTOP);
IF rtESTOPDeactivated.Q THEN
Main.M1.bHardwareEnable := TRUE; // SSL Sliding Seal // QRIX:SSL:MMS
Main.M2.bHardwareEnable := TRUE; // 2Theta Arm // QRIX:SA:MMS:2Theta
//Main.M3.bHardwareEnable := TRUE; // Optics Slits XS1 // QRIX:OPTSL:MMS:NORTH
//Main.M4.bHardwareEnable := TRUE; // Optics Slits XS2 // QRIX:OPTSL:MMS:SOUTH
//Main.M5.bHardwareEnable := TRUE; // Optics Slits YS1 // QRIX:OPTSL:MMS:TOP
//Main.M6.bHardwareEnable := TRUE; // Optics Slits YS2 // QRIX:OPTSL:MMS:BOTTOM
Main.M7.bHardwareEnable := TRUE; // Optics Base YG1 // QRIX:OPT:MMS:Y1
Main.M8.bHardwareEnable := TRUE; // Optics Base YG2 // QRIX:OPT:MMS:Y2
Main.M9.bHardwareEnable := TRUE; // Optics Base YG3 // QRIX:OPT:MMS:Y3
//Main.M10.bHardwareEnable := TRUE; // Optics Grating RxG // QRIX:G:MMS:Rx
//Main.M11.bHardwareEnable := TRUE; // Optics Grating XG // QRIX:G:MMS:X
//Main.M12.bHardwareEnable := TRUE; // Optics Mirror XPM1 // QRIX:PM:MMS:X1
//Main.M13.bHardwareEnable := TRUE; // Optics Mirror XPM2 // QRIX:PM:MMS:X2
//Main.M14.bHardwareEnable := TRUE; // Optics Mirror RzPM // QRIX:PM:MMS:Rz
Main.M15.bHardwareEnable := TRUE; // Detector Frame YDF1 // QRIX:DF:MMS:Y1
Main.M16.bHardwareEnable := TRUE; // Detector Frame YDF2 // QRIX:DF:MMS:Y2
Main.M17.bHardwareEnable := TRUE; // Detector Frame YDF3 // QRIX:DF:MMS:Y3
//Main.M18.bHardwareEnable := TRUE; // Detector Slits XSDC1 // QRIX:DETSL:MMS:NORTH
//Main.M19.bHardwareEnable := TRUE; // Detector Slits XSDC2 // QRIX:DETSL:MMS:SOUTH
//Main.M20.bHardwareEnable := TRUE; // Detector Slits YSDC1 // QRIX:DETSL:MMS:TOP
//Main.M21.bHardwareEnable := TRUE; // Detector Slits YSDC2 // QRIX:DETSL:MMS:BOTTOM
//Main.M22.bHardwareEnable := TRUE; // Detector Chamber XDC // QRIX:DC:MMS:X
//Main.M23.bHardwareEnable := TRUE; // Detector Chamber RyDC // QRIX:DC:MMS:Ry
//Main.M24.bHardwareEnable := TRUE; // Detector Chamber ZDC // QRIX:DC:MMS:Z
Main.M25.bHardwareEnable := TRUE; // Detector Arm YF1 // QRIX:DA:MMS:Y1
Main.M26.bHardwareEnable := TRUE; // Detector Arm YF2 // QRIX:DA:MMS:Y2
Main.M27.bHardwareEnable := TRUE; // Detector Arm ZF // QRIX:DA:MMS:Z
END_IF
// Disable M1(Servo) and M2(Stepper) if the frame is landing...
IF NOT GVL_Sensor.bFloating THEN
//Main.M1.bEnable := FALSE;
//Main.M2.bEnable := FALSE;
END_IF
// When user requests to reset ESTOP.
IF GVL_EPS.bResetIclk THEN
GVL_EPS.bResetIclk := FALSE;
bEnaIclkErrAck := TRUE;
bEnaIclkRestartESTOP := TRUE;
END_IF
END_ACTION
PRG_SSL
PROGRAM PRG_SSL
VAR
fb_SSL_Motor : FB_MotionStage;
(*EPS Limit switches*)
{attribute 'TcLinkTo' := 'TIIB[EL1004_02_03]^Channel 2^Input'}
bLimitBackwardEnable AT %I* :BOOL;
{attribute 'TcLinkTo' := 'TIIB[EL1004_02_03]^Channel 3^Input'}
bLimitForwardEnable AT %I* :BOOL;
{attribute 'pytmc' := 'pv: QRIX:SPCA:COUPLED'}
bExecuteCouple: BOOL := FALSE;
bExecuteDecouple: BOOL := FALSE;
bInit : BOOl := TRUE;
// Function block to couple sliding seal motor motion to 2Theta motion
fbSSLTo2ThetaCoupling: FB_GantryAutoCoupling;
{attribute 'TcLinkTo' := ' .Count := TIID^Device 2 (EtherCAT)^Rack#1A-01 Coupler (EK1100)^Rack#1A-06 ENC_2Th (EL5042)^FB Inputs Channel 1^Position'}
stRenishawAbsEncMaster: ST_RenishawAbsEnc;
{attribute 'TcLinkTo' := 'TIID^Device 2 (EtherCAT)^B950-233-WALL-S-R01-DRL-01^EL5042_02_02^FB Inputs Channel 1^Position'}
nRenishawAbsEncSlaveCounts AT %I*: ULINT; // Connect to encoder "Position" inputULINT
fSlaveCountsToMasterCountsScalingFactor: LREAL;
fRenishawAbsEncSlaveCountsInMasterScale: LREAL;
nRenishawAbsEncSlaveCountsInMasterScale: ULINT;
stRenishawAbsEncSlave: ST_RenishawAbsEnc;
nGantryTolerance: LINT;
fAngleOffset: LREAL;
{attribute 'pytmc' := '
pv: QRIX:SSL2ThetaDegTol
io: io
'}
fAngleTolerance: REAL := 0.2;
{attribute 'pytmc' := '
pv: QRIX:SSL2ThetaPosDiff
io: i
'}
fPosDiff: REAL;
fbExtSetPointGenEnable: MC_ExtSetPointGenEnable;
fbExtSetPointGenDisable: MC_ExtSetPointGenDisable;
{attribute 'pytmc' := '
pv: QRIX:bRealignCouple
io: io
'}
bRealignCouple: BOOL;
{attribute 'pytmc' := '
pv: QRIX:iRealignStep
io: i
'}
iRealignStep: UINT := 0;
{attribute 'pytmc' := '
pv: QRIX:bRealignSpeed
io: io
'}
fRealignSpeed: LREAL := 0.05;
rtStopSSL: R_TRIG;
tonRealignCoupleTimeout: TON;
timeRealignTimeout: TIME;
bFullyDecouple: BOOL;
EL7047_02_01_SSL_Status AT %I*: USINT;
EL7047_02_01_SSL_Status_to_NC AT %Q*: USINT;
END_VAR
IF bInit THEN
bInit := FALSE;
Main.M1.bPowerSelf := TRUE;
Main.M1.bHardwareEnable := TRUE;
Main.M1.nBrakeMode := ENUM_StageBrakeMode.NO_BRAKE;
Main.M1.nEnableMode := ENUM_StageEnableMode.NEVER;
END_IF
// Due to bug in SSL drive, ready bit is not being reported properly. This will tie the ready bit to the floating status.
EL7047_02_01_SSL_Status.1 := GVL_Sensor.bFloating;
EL7047_02_01_SSL_Status_to_NC := EL7047_02_01_SSL_Status;
// Set the encoder counts at the zero position
// CHOOSING TO SET THE COUNTS IN UNITS OF THE SCALE OF THE PRIMARY AXIS ENCODER.
// THE VIRTUAL LIMIT SWITCH FUNCTION BLOCK IN THE GANTRY AUTOCOUPLING FB ASSUMES
// THAT THE ENCODERS HAVE THE SAME SCALE. HOWEVER IN THIS CASE, THEY DO NOT HAVE
// THE SAME SCALE. IN ORDER TO COMPARE THEM USING THE GANTRY FB, THE SECONDARY
// AXIS ENCODER COUNTS HAVE TO BE SCALED TO MATCH THE PRIMARY AXIS ENCODER COUNTS.
fSlaveCountsToMasterCountsScalingFactor := Main.M1.stAxisParameters.fEncScaleFactorInternal / Main.M2.stAxisParameters.fEncScaleFactorInternal;
fRenishawAbsEncSlaveCountsInMasterScale := ULINT_TO_LREAL(nRenishawAbsEncSlaveCounts) * fSlaveCountsToMasterCountsScalingFactor;
nRenishawAbsEncSlaveCountsInMasterScale := LREAL_TO_ULINT(fRenishawAbsEncSlaveCountsInMasterScale);
stRenishawAbsEncSlave.Count := nRenishawAbsEncSlaveCountsInMasterScale;
// Going to use 90 degree position as the reference position because the Ref value is an unsigned value. This makes it so I
// can't set the reference to actual 0 because that requires a negative number.
stRenishawAbsEncMaster.Ref := LREAL_TO_ULINT((90.0 - Main.M2.stAxisParameters.fEncOffset) / Main.M2.stAxisParameters.fEncScaleFactorInternal);
stRenishawAbsEncSlave.Ref := LREAL_TO_ULINT((90.0 - Main.M1.stAxisParameters.fEncOffset) / Main.M1.stAxisParameters.fEncScaleFactorInternal * fSlaveCountsToMasterCountsScalingFactor);
// Compute the gantry tolerance
// Tolerance = X degrees
// 0.000003001664895 deg/count => For 0.1 deg tolerance then 0.1 deg / 0.000003001664895 deg/count = 33315 counts for 0.1 deg
nGantryTolerance := LREAL_TO_LINT(fAngleTolerance / Main.M2.stAxisParameters.fEncScaleFactorInternal);
fAngleOffset := LINT_TO_LREAL(
( ULINT_TO_LINT(stRenishawAbsEncMaster.Count) - ULINT_TO_LINT(stRenishawAbsEncMaster.Ref) ) -
( ULINT_TO_LINT(stRenishawAbsEncSlave.Count) - ULINT_TO_LINT(stRenishawAbsEncSlave.Ref) )
) * Main.M2.stAxisParameters.fEncScaleFactorInternal;
// AutoCoupling
fbSSLTo2ThetaCoupling( nGantryTol := nGantryTolerance,
Master := Main.M2,
MasterEnc := stRenishawAbsEncMaster,
Slave := Main.M1,
SlaveEnc := stRenishawAbsEncSlave,
bExecuteCouple := bExecuteCouple,
bExecuteDecouple := bExecuteDecouple,
);
// Calculate twice the expected amount of time to make the realignment move.
timeRealignTimeout := LREAL_TO_TIME(abs(Main.M1.stAxisStatus.fActPosition - Main.M2.stAxisStatus.fActPosition) / fRealignSpeed * 3.0 * 1000.0);
tonRealignCoupleTimeout(IN:=bRealignCouple,PT:=timeRealignTimeout);
CASE iRealignStep OF
0:
IF bRealignCouple THEN
iRealignStep := 1;
END_IF
1:
// Step 1: If the gantry is already coupled, the motor is not busy, and it is enabled, then decouple the axes.
IF fbSSLTo2ThetaCoupling.bGantryAlreadyCoupled AND NOT Main.M1.bBusy AND NOT Main.M1.bError THEN
bExecuteDecouple := TRUE;
iRealignStep := 2;
ELSIF NOT fbSSLTo2ThetaCoupling.bGantryAlreadyCoupled AND NOT Main.M1.bBusy AND NOT Main.M1.bError THEN
iRealignStep := 2;
END_IF
2:
// Step 2: If the gantry is no longer coupled, the motor is not busy, and it is enabled, then reset the decouple command and
// command the axis to align the SSL motor to the 2Theta motor. Set the bTriedToRealign bit to true to record that
// the realign sequence is what commanded the move.
IF NOT fbSSLTo2ThetaCoupling.bGantryAlreadyCoupled AND NOT Main.M1.bBusy AND NOT Main.M1.bError THEN
bExecuteDecouple := FALSE;
Main.M1.fPosition := Main.M2.stAxisStatus.fActPosition;
Main.M1.fVelocity := fRealignSpeed;
Main.M1.bMoveCmd := TRUE;
iRealignStep := 3;
END_IF
3:
// Step 3: Wait for the SSL motor to reach the busy status, then proceed.
IF Main.M1.bBusy THEN
iRealignStep := 4;
END_IF
4:
// Step 4: If the SSL motor is no longer busy (finished or errored or stopped) then recouple the SSL motor to
// the 2Theta motor.
IF NOT Main.M1.bBusy AND NOT Main.M1.bError THEN
bExecuteCouple := TRUE;
iRealignStep := 5;
END_IF
5:
// Step 5: If the motors are coupled again then reinitialize the bits to
// get them ready for another realignment attempt in the future.
IF fbSSLTo2ThetaCoupling.bGantryAlreadyCoupled THEN
iRealignStep := 0;
bRealignCouple := FALSE;
bExecuteDecouple := FALSE;
bExecuteCouple := FALSE;
tonRealignCoupleTimeout(IN:=FALSE);
ELSE
iRealignStep := 4;
END_IF
END_CASE
// Timeout: In case something bad happens, have the timeout timer in place to reset the bits to guarantee we do not get
// stuck in this recoupling and alignment sequence.
// If coupled axis fails to move due to axis not ready, may need to force the SSL motor ready status to TRUE. Then is allows it to move.
// seems like some sort of bug with decoupling/coupling the axes too much.
IF bRealignCouple AND tonRealignCoupleTimeout.Q THEN
IF fbSSLTo2ThetaCoupling.bGantryAlreadyCoupled OR Main.M1.bError THEN
iRealignStep := 0;
bRealignCouple := FALSE;
bExecuteDecouple := FALSE;
bExecuteCouple := FALSE;
tonRealignCoupleTimeout(IN:=FALSE);
ELSIF NOT Main.M1.bBusy THEN
bExecuteCouple := TRUE;
END_IF
END_IF
fPosDiff := Main.M1.stAxisStatus.fActPosition - Main.M2.stAxisStatus.fActPosition;
fb_SSL_Motor(stMotionStage:=Main.M1);
END_PROGRAM
- Related: