DUTs¶
DUT_HOMS¶
TYPE DUT_HOMS :
STRUCT
// System initializiation
fbRunHOMS : FB_RunHOMS;
// Couple/Decouple motors
{attribute 'pytmc' := '
pv: COUPLE_Y
io: o
'}
bExecuteCoupleY : BOOL;
{attribute 'pytmc' := '
pv: DECOUPLE_Y
io: o
'}
bExecuteDecoupleY : BOOL;
{attribute 'pytmc' := '
pv: COUPLE_X
io: o
'}
bExecuteCoupleX : BOOL;
{attribute 'pytmc' := '
pv: DECOUPLE_X
io: o
'}
bExecuteDecoupleX : BOOL;
// Coupling status
{attribute 'pytmc' := '
pv: ALREADY_COUPLED_Y
io: i
'}
bGantryAlreadyCoupledY : BOOL;
{attribute 'pytmc' := '
pv: ALREADY_COUPLED_X
io: i
'}
bGantryAlreadyCoupledX : BOOL;
// Current gantry differences
nCurrGantryY : LINT; // encoder counts = nm
nCurrGantryX : LINT; // encoder counts = nm
// Convert gantry differences to um (smaller number) to readout in epics
{attribute 'pytmc' := '
pv: GANTRY_Y
field: EGU um
io: i
'}
fCurrGantryY_um : REAL; // Y Gantry difference in um
{attribute 'pytmc' := '
pv: GANTRY_X
field: EGU um
io: i
'}
fCurrGantryX_um : REAL; // X Gantry difference in um
END_STRUCT
END_TYPE
E_PiezoControl¶
TYPE E_PiezoControl :
(
//Piezo Control Machine
EPC_Idle := 0,
EPC_Init := 10,
EPC_MoveRequested := 50,
EPC_MovingPositive := 100,
EPC_MovingNegative := 200,
EPC_MoveCompleted := 350,
EPC_Error := 500
);
END_TYPE
E_PitchControl¶
TYPE E_PitchControl :
(
//Pitch Control Machine
PCM_Init := 0,
PCM_Standby := 1,
PCM_MoveRequested := 10,
PCM_Coarse50Piezo := 20,
PCM_CoarseMove := 21,
PCM_CoarseMoveCleanup := 22,
PCM_FineMove := 30,
PCM_Halt := 50,
PCM_Done := 8000, //why is 8000 done? Where did this come from??
PCM_Error := 9000, //Anything above 9000 is considered an error
PCM_StepperError := 9100,
PCM_PiezoError := 9200,
PCM_OtherError := 9300,
PCM_STOHit := 9400
);
END_TYPE
HOMS_PitchMechanism¶
TYPE HOMS_PitchMechanism :
STRUCT
Piezo : ST_PiezoAxis; //Piezo
(* Soft limits, egu urad *)
ReqPosLimHi : REAL;
ReqPosLimLo : REAL;
(* Hard limits, egu INC *)
(* These are discovered during installation, and represent encoder ticks, unbiased *)
(* We changed to these when our pitch mechanism limit switches were insufficient for
good control. They had too much hysteresis/ lack of precision. At this point the limit
switches are ignored, and instead their function is carried out by these. *)
diEncPosLimHi : LINT;
diEncPosLimLo : LINT;
//Raw encoder count
diEncCnt AT %I* : LINT;
END_STRUCT
END_TYPE
ST_PiezoAxis¶
TYPE ST_PiezoAxis :
STRUCT
(* IO *)
//Readback
sIdn : STRING; //Identity
iCurError : INT; //Current error code, should be 0 most of the time
iLastError : INT; //Last error code, for history
rActVoltage : REAL; //Actual voltage
rLastReqVoltage : REAL; //Last requested piezo voltage
//Control
rSetVoltage : REAL; //this parameter is set by the control loop/ voltage mode
sAxis : STRING :='A'; //Axis, e.g. A, B, C...A if single unit
//Summaries
xTimeout : BOOL;
xDriverError : BOOL; //Summary of any driver errors
(* Operation *)
xEnable : BOOL; //Enable control.
(* Note: Voltage mode and Idle mode overrides "DirectPiezoMode" of FB_PitchControl *)
xVoltageMode : BOOL; //Voltage mode gives direct access to piezo voltage, false means closed loop position acquisition (see FB_PitchControl for piezo and stepper separation)
xIdleMode : BOOL; //Use to put the piezo at half-stroke
rReqVoltage : REAL; //Requested piezo voltage in voltage mode
rReqAbsPos : LREAL; //Requested Position, latched at rising StartAbsMov
xStop : BOOL; //Stops piezo and holds position
(* Control Parameters *)
rActPos : LREAL; //Encoder Readback
//Pitch piezo dmove range (urad)
rPiezoDmovRange : REAL := 1.0;
stPIParams : ST_CTRL_PI_PARAMS := (
tCtrlCycleTime := T#0MS,
tTaskCycleTime := T#0MS,
tTn := T#200MS,
fKp := 0.0005,
fOutMaxLimit := 1,
fOutMinLimit := -1,
bARWOnIPartOnly := FALSE);
(* Voltage ranges, come from specifications of the driver *)
UpperVoltage : REAL := GVL_Constants.cPiezoMaxVoltage; // E-816 has no software limits
LowerVoltage : REAL := GVL_Constants.cPiezoMinVoltage; // E-816 has no software limits
END_STRUCT
END_TYPE
GVLs¶
Global_Version¶
{attribute 'TcGenerated'}
// This function has been automatically generated from the project information.
VAR_GLOBAL CONSTANT
{attribute 'const_non_replaced'}
{attribute 'linkalways'}
stLibVersion_lcls_twincat_optics : ST_LibVersion := (iMajor := 0, iMinor := 2, iBuild := 1, iRevision := 0, sVersion := '0.2.1');
END_VAR
GVL_Constants¶
{attribute 'qualified_only'}
VAR_GLOBAL CONSTANT
nGANTRY_TOLERANCE_NM_DEFAULT : LINT := 50000; // default gantry tolerance in encoder counts = nm
cPiezoMaxVoltage : LREAL := 120; // in Volts
cPiezoMinVoltage : LREAL := -10; // in Volts
cPiezoRange : REAL := 60.0; // From Old HOMS_FEE Project, 90 um of piezo stroke, unsure what these units are
END_VAR
GVL_TestStructs¶
{attribute 'qualified_only'}
VAR_GLOBAL
TestPitch_LimitSwitches : HOMS_PitchMechanism := (ReqPosLimHi:=2000,
ReqPosLimLo:=-2000,
diEncPosLimHi:=10768330,
diEncPosLimLo:=8141680);
END_VAR
POUs¶
FB_Bender¶
FUNCTION_BLOCK FB_Bender
VAR_IN_OUT
stBender : DUT_MotionStage;
bSTOEnable1 : BOOL;
bSTOEnable2 : BOOL;
END_VAR
VAR_INPUT
END_VAR
VAR_OUTPUT
END_VAR
VAR
END_VAR
// Simple FB to tie stBender.bHardwareEnable to STO
// Originally part of FB_RunHOMS, but want to use this for all systems, not all of which have a bender motor
stBender.bHardwareEnable := bSTOEnable1 AND bSTOEnable2;
END_FUNCTION_BLOCK
FB_MirrorTwoCoatingProtection¶
FUNCTION_BLOCK FB_MirrorTwoCoatingProtection
VAR_INPUT
nCurrentEncoderCount : UDINT; // Current encoder count
neVRange : DWORD; // Current ev range from stCurrentBeamParams
sDevName : STRING := ''; // Device name
nUpperCoatingBoundary : UDINT; // Encoder count for upper boundary
nUpperCoatingBitmask : DWORD; // Bitmask for upper coating
sUpperCoatingType : STRING := ''; // Type of coating
nLowerCoatingBoundary : UDINT; // Encoder count for lower boundary
nLowerCoatingBitMask : DWORD;
sLowerCoatingType : STRING := ''; // Type of coating
bAutoClear : BOOL := TRUE; // Auto-clear these fast faults
END_VAR
VAR_OUTPUT
END_VAR
VAR_IN_OUT
FFO : FB_HardwareFFOutput;
END_VAR
VAR
ffUpperCoating: FB_FastFault := (
i_xAutoReset := FALSE,
i_TypeCode := 16#401);
ffLowerCoating : FB_FastFault := (
i_xAutoReset := FALSE,
i_TypeCode := 16#401);
bInit : BOOL;
END_VAR
IF NOT bInit THEN
ffUpperCoating.i_Desc := CONCAT(sUpperCoatingType, ' mirror coating incompatible with beam photon energy');
ffUpperCoating.i_DevName := sDevName;
ffLowerCoating.i_Desc := CONCAT(sLowerCoatingType, ' mirror coating incompatible with beam photon energy');
ffLowerCoating.i_DevName := sDevName;
bInit := TRUE;
END_IF
IF nCurrentEncoderCount <= nLowerCoatingBoundary THEN
ffLowerCoating.i_xOK := (neVRange AND nLowerCoatingBitMask) = neVRange;
ffUpperCoating.i_xOK := TRUE;
ELSIF nCurrentEncoderCount >= nUpperCoatingBoundary THEN
ffUpperCoating.i_xOK := (neVRange AND nUpperCoatingBitmask) = neVRange;
ffLowerCoating.i_xOK := TRUE;
ELSE
ffLowerCoating.i_xOK := FALSE;
ffUpperCoating.i_xOK := FALSE;
END_IF
ffUpperCoating(io_fbFFHWO:=FFO, i_xAutoReset := bAutoClear);
ffLowerCoating(io_fbFFHWO:=FFO, i_xAutoReset := bAutoClear);
END_FUNCTION_BLOCK
FB_PI_E621_SerialDriver¶
FUNCTION_BLOCK FB_PI_E621_SerialDriver
VAR_INPUT
/// rising edge execute
i_xExecute: BOOL;
/// Maximum wait time for reply
i_tTimeOut: TIME := TIME#1S0MS;
// i_xReset : BOOL := FALSE; //reset function, for timeout etc
END_VAR
VAR_OUTPUT
q_xDone: BOOL;
q_xError: BOOL;
q_sResult: STRING(255);
/// Last Strings Sent to Serial Device - for debugging
q_asLastSentStrings: ARRAY[1..50] OF STRING;
/// Last Strings Received from Serial Device - for debugging
q_asLastReceivedStrings: ARRAY[1..50] OF STRING;
END_VAR
VAR_IN_OUT
iq_stPiezoAxis : ST_PiezoAxis;
iq_stSerialRXBuffer: ComBuffer;
iq_stSerialTXBuffer: ComBuffer;
END_VAR
VAR
rtExecute : R_TRIG;
rtTransDone : R_TRIG;
iStep: INT;
sSendData: STRING;
fbPITransaction: FB_PI_E621_SerialTransaction;
fbFormatString: FB_FormatString;
sErrMesg : STRING := 'In step %d fbPITransaction failed with message: %s';
i : INT := 1;
END_VAR
(* S. Stubbs, 2-23-2017 *)
(* This function block drives serial communication with a PI E-816 or compatible comm module.
Note this needs to be re-jiggered if the E-517 is used, uses number rather than letter for axis *)
(* RS232 default settings: 115200 8N1, RTS/CTS
All commands follow format:
CMD X sV.V(Line feed)
Where CMD is the command, X is axis, and sV.V is sign and number (float or int).
Not all commands use axis and parameter, for example ERR?
*)
(* rising edge trigger *)
rtExecute(CLK:= i_xExecute);
IF rtExecute.Q THEN
q_xDone := FALSE;
q_xError := FALSE;
q_sResult:= '';
iq_stPiezoAxis.xDriverError := FALSE;
// i_xReset := FALSE;
a_ClearTrans(); (* to provide rising edge for execute *)
IF iq_stPiezoAxis.sIdn= '' THEN (* Should only need to check identity once *)
iStep := 5;
ELSE
iStep := 10;
END_IF
END_IF
CASE iStep OF
0: (* idle *)
;
(* Commands *)
5: (* Get Identity *)
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= '*IDN?';
IF fbPITransaction.q_xDone THEN
iq_stPiezoAxis.sIdn := fbPITransaction.q_sResponseData; //Hello I am a piezo
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := 10;
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
10: (* Check Servo Mode
To use manual voltage servo mode must be off *)
(* Response: 0$L or 1$L *)
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= 'SVO?';
fbPITransaction.i_sAxis:= iq_stPiezoAxis.sAxis;
IF fbPITransaction.q_xDone THEN
IF FIND('1',fbPITransaction.q_sResponseData) <> 0 THEN //Iff in servo mode, turn it off
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := iStep + 10;
ELSE
a_ClearTrans();
iStep := iStep + 20; //Skip setting servo mode
END_IF
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
20: (* Set Servo Mode *)
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= 'SVO';
fbPITransaction.i_sAxis:= iq_stPiezoAxis.sAxis;
fbPITransaction.i_sParam:= '0';
fbPITransaction.i_xExpectReply:= FALSE;
IF fbPITransaction.q_xDone THEN
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := iStep + 10;
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
30: (* Set Voltage, only if needed *)
IF iq_stPiezoAxis.rSetVoltage <> iq_stPiezoAxis.rLastReqVoltage THEN
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= 'SVA';
fbPITransaction.i_sAxis:= iq_stPiezoAxis.sAxis;
fbPITransaction.i_sParam:=REAL_TO_STRING(iq_stPiezoAxis.rSetVoltage);
fbPITransaction.i_xExpectReply:= FALSE;
IF fbPITransaction.q_xDone THEN
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := iStep + 10;
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
ELSE
iStep := iStep + 30; //Should only need to check error and setpoint if setting voltage
END_IF
40: (* Get Error Code, also resets current error *)
(* Response: integer error code *)
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= 'ERR?';
IF fbPITransaction.q_xDone THEN
iq_stPiezoAxis.iCurError := STRING_TO_INT(fbPITransaction.q_sResponseData);
IF iq_stPiezoAxis.iCurError <> 0 THEN
iq_stPiezoAxis.iLastError:= iq_stPiezoAxis.iCurError;
END_IF
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := iStep + 10;
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
50: (* Get Last Requested Piezo Voltage *)
(* Response: (float)$L *)
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= 'SVA?';
fbPITransaction.i_sAxis:= iq_stPiezoAxis.sAxis;
IF fbPITransaction.q_xDone THEN
iq_stPiezoAxis.rLastReqVoltage := STRING_TO_REAL(fbPITransaction.q_sResponseData);
//Check and reset attempts if it went through
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := iStep + 10;
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
60: (* Get Actual Piezo Voltage *)
(* Response: (float)$L *)
fbPITransaction.i_xExecute:= TRUE;
fbPITransaction.i_sCmd:= 'VOL?';
// E-517 works differently, uses number rather than letter for axis
fbPITransaction.i_sAxis:= iq_stPiezoAxis.sAxis;
IF fbPITransaction.q_xDone THEN
iq_stPiezoAxis.rActVoltage := STRING_TO_REAL(fbPITransaction.q_sResponseData);
a_ClearTrans(); (* to provide rising edge for execute *)
iStep := 8000; (* Done *)
ELSIF fbPITransaction.q_xError THEN
a_ErrorMesg();
iStep := 9000;
END_IF
8000: (* done *)
q_xDone := TRUE;
IF i_xExecute = FALSE THEN
q_xDone:= FALSE;
iStep := 0;
END_IF
9000: (* Error *)
a_ClearTrans(); (* to provide rising edge for execute *)
IF fbPITransaction.q_xTimeout THEN
iStep:=10;//start over
ELSE
q_xError := TRUE;
iq_stPiezoAxis.xDriverError := TRUE;
END_IF
END_CASE
//call transaction
fbPITransaction(
iq_stSerialRXBuffer:= iq_stSerialRXBuffer,
iq_stSerialTXBuffer:= iq_stSerialTXBuffer);
iq_stPiezoAxis.xTimeout:=fbPITransaction.q_xTimeout;
(* Rising edge trigger to take care of debugging history *)
rtTransDone(CLK:= fbPITransaction.q_xDone);
IF rtTransDone.Q THEN
q_asLastSentStrings[i] := fbPITransaction.q_sLastSentString;
q_asLastReceivedStrings[i] := fbPITransaction.q_sLastReceivedString;
i := i + 1;
END_IF
IF i = 51 THEN i := 1; END_IF
END_FUNCTION_BLOCK
ACTION a_ClearTrans:
(* Refactor this action to match your transaction *)
fbPITransaction.i_xExecute := TRUE;
fbPITransaction.i_sCmd:= ''; //Input args are Cmd, Axis and Param
fbPITransaction.i_sAxis:= '';
fbPITransaction.i_sParam:= '';
fbPITransaction(
i_tTimeOut:= i_tTimeOut,
iq_stSerialRXBuffer:= iq_stSerialRXBuffer,
iq_stSerialTXBuffer:= iq_stSerialTXBuffer );
fbPITransaction.i_xExecute := FALSE;
fbPITransaction(
i_tTimeOut:= i_tTimeOut,
iq_stSerialRXBuffer:= iq_stSerialRXBuffer,
iq_stSerialTXBuffer:= iq_stSerialTXBuffer );
fbPITransaction.i_xExpectReply:=TRUE;
END_ACTION
ACTION a_ErrorMesg:
fbFormatString( sformat:=sErrMesg,
arg1:=F_INT(iStep),
arg2:=F_STRING(fbPITransaction.q_sResult),
sOut => q_sResult);
END_ACTION
ACTION a_UnknownError:
q_sResult:= 'Unknown error';
fbFormatString( sformat:=sErrMesg,
arg1:=F_INT(iStep),
arg2:=F_STRING(q_sResult), //Little silly, but have to do this because F_STRING requires read/write access
sOut => q_sResult);
END_ACTION
FB_PI_E621_SerialTransaction¶
FUNCTION_BLOCK FB_PI_E621_SerialTransaction
VAR_INPUT
/// rising edge execute
i_xExecute: BOOL;
/// Maximum wait time for reply
i_tTimeOut: TIME := TIME#1s0ms;
// Command field
i_sCmd: STRING;
// Axis field
i_sAxis: STRING;
// Parameter field
i_sParam: STRING;
// Does command have a reply? Default behavior is the same as the other drivers.
i_xExpectReply: BOOL := TRUE;
END_VAR
VAR_OUTPUT
q_xDone: BOOL;
q_sResponseData: STRING;
q_xError: BOOL;
q_xTimeout: BOOL;
q_sResult: STRING;
/// Last String Sent to Serial Device - for debugging
q_sLastSentString: STRING;
/// Last String Received from Serial Device - for debugging
q_sLastReceivedString: STRING;
END_VAR
VAR_IN_OUT
iq_stSerialRXBuffer: ComBuffer;
iq_stSerialTXBuffer: ComBuffer;
END_VAR
VAR
rtExecute: R_TRIG;
iStep: INT;
fbClearComBuffer: ClearComBuffer;
sSendString: STRING;
fbFormatString: FB_FormatString;
iChecksum: INT;
fbSendString: SendString;
fbReceiveString: ReceiveString;
sReceivedString: STRING;
tonTimeout: TON;
sRXStringForChecksum: STRING;
sReceiveStringWOChecksum: STRING;
sRXCheckSum: STRING;
sRXAddress: STRING;
sRXParmNum: STRING;
END_VAR
(* This function block performs serial transactions with a PI E-816 or compatible comm module *)
(* rising edge trigger *)
rtExecute(CLK:= i_xExecute);
IF rtExecute.Q THEN
q_xDone := FALSE;
q_sResponseData := '';
q_xError := FALSE;
q_sResult:= '';
q_sLastSentString := '';
q_sLastReceivedString:= '';
iStep := 10;
END_IF
CASE iStep OF
0:
; (* idle *)
10: (* clear com buffers *)
fbClearComBuffer(Buffer:= iq_stSerialRXBuffer);
fbClearComBuffer(Buffer:= iq_stSerialTXBuffer);
(* build the send string *)
IF i_sParam = '' AND i_sAxis <> '' THEN //Axis but no parameter
fbFormatString( sFormat:= '%s %s$L',
arg1:= F_STRING(i_sCmd),
arg2:= F_STRING(i_sAxis),
sOut=> sSendString);
ELSIF i_sParam <> '' AND i_sAxis = '' THEN //Parameter but no axis, global command
fbFormatString( sFormat:= '%s %s$L',
arg1:= F_STRING(i_sCmd),
arg2:= F_STRING(i_sParam), //May not work for all commands, good enough for now
sOut=> sSendString);
ELSIF i_sParam = '' AND i_sAxis = '' THEN //Global Query/Command
fbFormatString( sFormat:= '%s$L',
arg1:= F_STRING(i_sCmd),
sOut=> sSendString);
ELSE
fbFormatString( sFormat:= '%s %s %s$L',
arg1:= F_STRING(i_sCmd),
arg2:= F_STRING(i_sAxis),
arg3:= F_STRING(i_sParam), //May not work for all commands, good enough for now
sOut=> sSendString);
END_IF
(* send it *)
fbSendString( SendString:= sSendString, TXbuffer:= iq_stSerialTXBuffer );
q_sLastSentString := sSendString;
iStep := iStep + 10;
20: (* Finish sending the String *)
IF fbSendString.Busy THEN
fbSendString( SendString:= sSendString, TXbuffer:= iq_stSerialTXBuffer );
ELSIF fbSendString.Error <> 0 THEN
q_sResult := CONCAT('In step 20 fbSendString resulted in error: ', INT_TO_STRING(fbSendString.Error));
iStep := 9000;
ELSIF NOT fbSendString.Busy THEN
IF i_xExpectReply THEN
iStep := iStep + 10;
ELSE //No reply expected, transaction complete
q_xDone:= TRUE;
q_sResult := 'Success.';
q_xTimeout := FALSE; //no timeout
iStep := 100;
END_IF
END_IF
(* Reset receive *)
fbReceiveString(
Reset:= TRUE,
ReceivedString:= sReceivedString,
RXbuffer:= iq_stSerialRXBuffer );
tonTimeout(IN:= FALSE);
30: (* Get reply, if there is one *)
fbReceiveString(
Prefix:= ,
Suffix:= '$L',
Timeout:= i_tTimeOut,
Reset:= FALSE,
ReceivedString:= sReceivedString,
RXbuffer:= iq_stSerialRXBuffer );
tonTimeout(IN:= TRUE, PT:= i_tTimeOut);
IF fbReceiveString.Error <> 0 AND fbReceiveString.Error <> 16#1008 THEN //16#1008 is timeout error
q_sResult := CONCAT('In step 30 fbReceiveString resulted in error: ', INT_TO_STRING(fbReceiveString.Error));
iStep := 9000;
ELSIF fbReceiveString.RxTimeout OR tonTimeout.Q THEN
q_sResult := 'Device failed to reply within timeout period';
q_xTimeout := TRUE;
iStep := 9000;
ELSIF fbReceiveString.StringReceived THEN
q_xTimeout := FALSE; //no timeout
q_sLastReceivedString := sReceivedString;
q_sResponseData := sReceivedString;
q_sResult := 'Success.';
q_xDone:= TRUE;
iStep := 100;
END_IF
100: (* done *)
IF i_xExecute = FALSE THEN
q_xDone:= FALSE;
iStep := 0;
END_IF
9000:
q_xError := TRUE;
END_CASE
END_FUNCTION_BLOCK
FB_PiezoControl¶
FUNCTION_BLOCK FB_PiezoControl
VAR_IN_OUT
iq_Piezo : ST_PiezoAxis;
END_VAR
VAR_INPUT
xExecute : BOOL; //Rising edge being piezo motion
xReset : BOOL;
Enable_Positive : BOOL; //Reverse of Positive Limit Switch
Enable_Negative : BOOL; //Reverse of Negative Limit Switch
END_VAR
VAR_OUTPUT
xBusy : BOOL; //Busy remains true while piezo position is being adjusted
xDone : BOOL; //Reached target position
xError : BOOL; //General error
xLimited: BOOL; //Piezo move was limited
END_VAR
VAR
E_State : E_PiezoControl; //ENUM for Piezo Control State
rtStartMove : R_TRIG; //Rising Trigger for Execution
rtReset : R_TRIG; //Rising Trigger for Error reset
rSetpoint : REAL; //Internal Storage of Setpoint
rReqVoltage : REAL; //requested voltage
rLLSV: REAL := 0;
rHLSV: REAL := 120;
fbPI: FB_CTRL_PI;
fbRamp: FB_CTRL_RAMP_GENERATOR_EXT;
// FB initialized flag
bInitialized: BOOL;
//Get cycle time for control FBs
fbGetCycleTime : FB_CTRL_GET_TASK_CYCLETIME;
tTaskCycleTime: TIME;
bCycleTimeValid: BOOL;
rtVoltMode: R_TRIG;
fOut: LREAL;
fPiezoBias: LREAL := 60;
fScale: REAL := -60;
tonPiezoDone: TON := (PT:=T#2S);
tonPiezoLimited: TON := (PT:=T#500MS);
xVoltageLimited: BOOL;
ftEnPos : F_TRIG;
ftEnNeg : F_TRIG;
rtEnPos : R_TRIG;
rtEnNeg : R_TRIG;
fOutLimitHolder : LREAL; //holds the limit value until restored
fOutHiLimHolder : LREAL; //holds the limit value until restored
fOutLoLimHolder : LREAL; //holds the limit value until restored
xFirstPass : BOOL := TRUE;
END_VAR
// FB Piezo Control
//Triggers
///////////////////////////////
rtStartMove(CLK:=xExecute);
rtReset(CLK:=iq_Piezo.xEnable);
rtVoltMode(CLK:=iq_Piezo.xVoltageMode);
//Status bits
///////////////////////////
xBusy S= rtStartMove.Q;
xDone R= rtStartMove.Q;
//Keep requested voltage to within limits
iq_Piezo.rReqVoltage := LIMIT(iq_Piezo.LowerVoltage, iq_Piezo.rReqVoltage, iq_Piezo.UpperVoltage);
//Limits
(* These appear flipped, but in-fact are not *)
ftEnPos(CLK:=Enable_Positive);
ftEnNeg(CLK:=Enable_Negative);
rtEnPos(CLK:=Enable_Positive);
rtEnNeg(CLK:=Enable_Negative);
IF xFirstPass THEN
//Want to hold the limits on first pass if a switch is hit.
(* When we move off the limit, we'll restore the init value (usually 1). This will be reset
to something less than 1 when the limit gets tripped again, because presumably the actual limit
would have been set at a value < 1 if the system had been runing.
We just need to hold the init value to make it past this edge case that is present at startup. *)
IF NOT Enable_Positive THEN fOutHiLimHolder := iq_Piezo.stPIParams.fOutMaxLimit; END_IF
IF NOT Enable_Negative THEN fOutLoLimHolder := iq_Piezo.stPIParams.fOutMinLimit; END_IF
ELSE
IF ftEnPos.Q THEN
rLLSV := iq_Piezo.rSetVoltage;
fOutHiLimHolder := iq_Piezo.stPIParams.fOutMaxLimit;
iq_Piezo.stPIParams.fOutMaxLimit := fbPI.fOut;
ELSIF rtEnPos.Q THEN
rLLSV := iq_Piezo.LowerVoltage;
iq_Piezo.stPIParams.fOutMaxLimit := fOutHiLimHolder;
END_IF
IF ftEnNeg.Q THEN
rHLSV := iq_Piezo.rSetVoltage;
fOutLoLimHolder := iq_Piezo.stPIParams.fOutMinLimit;
iq_Piezo.stPIParams.fOutMinLimit := fbPI.fOut;
ELSIF rtEnNeg.Q THEN
rHLSV := iq_Piezo.UpperVoltage;
iq_Piezo.stPIParams.fOutMinLimit := fOutLoLimHolder;
END_IF
END_IF
// Don't do anything until we're ready
IF bInitialized THEN
// While the block is working, a new position may be requested, this is OK
IF xBusy THEN
fbPI.fSetpointValue := iq_Piezo.rReqAbsPos;
END_IF
(* The next chunk of code prevents the PI block from winding up.
First, when the PI block begins to request a voltage that is
beyond the permitted range (this range is affected by the state
of limit switches/ or enable fwd/bwd), we latch the requested position.
Presumeably this position request *)
//Select the PI block control mode
////////////////////////////////////////
IF iq_Piezo.xVoltageMode THEN
//Set PI block to idle
fbPI.eMode := eCTRL_MODE_PASSIVE;
rReqVoltage := iq_Piezo.rReqVoltage; //TODO add a ramp
ELSE
IF iq_Piezo.xIdleMode THEN
rReqVoltage := fScale * 0 + fPiezoBias;
fbPI.eMode := eCTRL_MODE_MANUAL;
ACT_Controller();
fbPI.bHold := TRUE;
ELSE
//Fout is connected to the piezo voltage control
rReqVoltage := fScale * fbPI.fOut + fPiezoBias;
fbPI.bHold := FALSE;
//Control mode is always active, so compensation takes over more smoothly
fbPI.eMode := eCTRL_MODE_ACTIVE;
END_IF
END_IF
ACT_Controller();
xVoltageLimited := rLLSV > rReqVoltage OR rHLSV < rReqVoltage;
//This is where the voltage request gets sent to the piezo driver
iq_Piezo.rSetVoltage := LIMIT(rLLSV, rReqVoltage, rHLSV);
//Initialization
ELSE
fbGetCycleTime( eMode := eCTRL_MODE_ACTIVE,
tTaskCycleTime => tTaskCycleTime,
bCycleTimeValid => bCycleTimeValid);
IF bCycleTimeValid THEN
iq_Piezo.stPIParams.tTaskCycleTime := tTaskCycleTime;
iq_Piezo.stPIParams.tCtrlCycleTime := tTaskCycleTime;
bInitialized := TRUE;
END_IF
END_IF
tonPiezoDone.IN := WithinRange(ValA:=iq_Piezo.rActPos, Center:=iq_Piezo.rReqAbsPos, Range:=iq_Piezo.rPiezoDmovRange, Offset:=0)
AND NOT rtStartMove.Q; //rtStartMove interrupts the timer, resetting it
tonPiezoDone();
tonPiezoLimited.IN := (fbPI.bARWactive OR xVoltageLimited) AND NOT rtStartMove.Q;
tonPiezoLimited();
xDone S= xBusy AND (tonPiezoDone.Q OR tonPiezoLimited.Q);
xLimited := tonPiezoLimited.Q;
xBusy R= xDone;
xFirstPass := FALSE;
END_FUNCTION_BLOCK
ACTION ACT_CheckLimits:
END_ACTION
ACTION ACT_Controller:
END_ACTION
FB_PitchControl¶
FUNCTION_BLOCK FB_PitchControl
VAR_IN_OUT
Pitch : HOMS_PitchMechanism;
Stepper : DUT_MotionStage;
END_VAR
VAR_INPUT
lrCurrentSetpoint : LREAL; // Setpoint: Epics writes to DUT_MotionStage which gets fed into this
END_VAR
VAR_OUTPUT
q_bError : BOOL;
q_bDone : BOOL;
q_bBusy : BOOL;
END_VAR
VAR
// Logging
stDiag : ST_fbDiagnostics;
fbFormatString : FB_FormatString;
{attribute 'instance-path'}
{attribute 'no_init'}
POUName : STRING; // Name of the POU for logging/error reporting
// Stepper Motion
lrActPos : LREAL; // Actual Position of piezo mechanism
lrPrevStepperPos : LREAL; // Previous successfully achieved stepper position
ftLimitSwitch : F_TRIG;
lrOriginalPosRequest : LREAL; // Used for logging
lrLastSetpoint : LREAL; // Previous successfully achieved setpoint
fbMotionRequest : FB_MotionRequest;
fbMotionStage : FB_MotionStage;
bLimitHit : BOOL;
tonStepperHold : TON := (PT:=T#100MS); // Timer to hold stepper position while the system relaxes
rSettledRange : REAL := 5.0; // Units = urad
bResetStepper : BOOL;
bExecuteStepper : BOOL;
enumMotionRequest : ENUM_MotionRequest := ENUM_MotionRequest.WAIT; // Wait for move to complete before taking another request
// Piezo
tonPiezoSettled : TON := (PT:=T#2S);
fbPiezoControl : FB_PiezoControl;
rtPiezoMoveDone : R_TRIG;
// State Machine
PC_State : E_PitchControl := PCM_Init;
bCoarse50PiezoMove : BOOL;
END_VAR
(* HOMS Pitch Control
A. Wallace
J. Sheppard - Updating to new lcls-twincat-motion API
The HOMS Pitch mechanism consists of a stepper and piezo that work together to adjust
the pitch of the mirror assembly.
Pitch control state machine
If the target position is beyond the range of the piezo mechanism,
execute a coarse pitch move with the stepper.
The target of the coarse move shall be set to the requested position.
Once coarse motion has completed the coarse motion drive position
correction output shall be set to zero.
Fine pitch motion with the piezo will be initiated to finish closing the loop.
The piezo mechanism can actuate ~ 180urad or 90um.
*)
lrActPos := Stepper.stAxisStatus.fActPosition;
// If we hit a limit during a move, we need to change the setpoint
ftLimitSwitch(CLK:=Stepper.bAllForwardEnable AND Stepper.bAllBackwardEnable);
IF ftLimitSwitch.Q THEN
bExecuteStepper := FALSE;
bLimitHit := TRUE;
lrCurrentSetpoint := lrActPos;
END_IF
// Left out Manual Mode Switch and Tweak FBs
// State Machine
CASE PC_State OF
PCM_Init:
lrCurrentSetpoint := lrActPos;
lrLastSetpoint := lrCurrentSetpoint;
lrPrevStepperPos := lrCurrentSetpoint;
PC_State := PCM_Standby;
PCM_Standby:
// Waits for move requests and determines if they are valid
IF (lrLastSetpoint <> lrCurrentSetpoint) THEN // lrLastSetpoint initially set in PCM_Done
// Check for bad setpoints -> revert to previous setpoint
IF (lrCurrentSetpoint > Pitch.ReqPosLimHi) OR (lrCurrentSetpoint < Pitch.ReqPosLimLo) OR NOT Stepper.bHardwareEnable THEN
// Outside range of limit switches or bHardwareEnable is FALSE
ACT_ResetSetpoint();
ELSIF lrCurrentSetpoint > lrLastSetpoint AND NOT Stepper.bAllForwardEnable THEN
// Forward move when on HL
ACT_ResetSetpoint();
ELSIF lrCurrentSetpoint < lrLastSetpoint AND NOT Stepper.bAllBackwardEnable THEN
// Backward move when on LL
ACT_ResetSetpoint();
END_IF
// If the current setpoint still differs from the prvious, we know the move is safe and OK to proceed
IF lrLastSetpoint <> lrCurrentSetpoint THEN
q_bDone := FALSE;
PC_State := PCM_MoveRequested;
END_IF
END_IF
PCM_MoveRequested:
// A move has been requested, is it within range of the piezo?
IF WithinRange(ValA:=lrCurrentSetpoint, Center:=lrPrevStepperPos, Range:=GVL_Constants.cPiezoRange, Offset:=0) THEN
// Move is within the nominal range of the piezo
fbFormatString.sFormat := 'Within range, fine move %f';
fbFormatString.arg1 := F_LREAL(lrCurrentSetpoint);
fbFormatString(sOut=>stDiag.asResults[stDiag.resultIdx.IncVal()]);
PC_State := PCM_FineMove;
ELSE
// Out of range, head to coarse move
fbFormatString.arg1 := F_LREAL(lrCurrentSetpoint);
fbFormatString.sFormat := 'OoR, using stepper %f';
fbFormatString(sOut=>stDiag.asResults[stDiag.resultIdx.IncVal()]);
PC_State := PCM_Coarse50Piezo;
END_IF
PCM_Coarse50Piezo:
// A coarse move uses the stepper to do a best-effort position
// First set the piezo to nominal 50% extension using idle mode
//////////////////////////////////////////////////////////////////////////////
Pitch.Piezo.xIdleMode := TRUE;
// Indicate we are doing the coarse 50% piezo move
bCoarse50PiezoMove := TRUE;
// Wait for piezo to settle
tonPiezoSettled.IN := TRUE;
bCoarse50PiezoMove R= tonPiezoSettled.Q;
IF tonPiezoSettled.Q THEN
//Piezo has moved to 50% position, finish with the stepper
PC_State := PCM_CoarseMove;
tonPiezoSettled.IN := FALSE;
END_IF
PCM_CoarseMove:
// With the piezo at a nominal 50% extension, move the stepper to requested position
bExecuteStepper := TRUE;
// Timer that waits to start until stepper is within range of the setpoint
tonStepperHold.IN := WithinRange(ValA:=LREAL_TO_REAL(lrActPos), Center:=lrCurrentSetpoint, Range:=rSettledRange, Offset:=0);
tonStepperHold(); // call this here to reset Q just below on first cycle
// If the coarse move is complete, finish position correction with the piezo
IF tonStepperHold.Q OR ftLimitSwitch.Q THEN
PC_State := PCM_CoarseMoveCleanup;
lrPrevStepperPos := lrActPos;
ELSIF Stepper.bError THEN
bExecuteStepper := FALSE;
PC_State := PCM_StepperError;
// Left out logging
END_IF
PCM_CoarseMoveCleanup:
bExecuteStepper := FALSE;
PC_State := PCM_FineMove;
PCM_FineMove:
Pitch.Piezo.xIdleMode := FALSE;
fbPiezoControl.xExecute := TRUE;
IF bLimitHit THEN
Pitch.Piezo.rReqAbsPos := lrActPos;
ELSE
Pitch.Piezo.rReqAbsPos := lrCurrentSetpoint;
END_IF
rtPiezoMoveDone(CLK:=fbPiezoControl.xDone);
IF rtPiezoMoveDone.Q THEN
fbPiezoControl.xExecute := FALSE;
PC_State := PCM_Done;
END_IF
PCM_Done:
// Set the previously requested position here
lrLastSetpoint := lrCurrentSetpoint;
bLimitHit := FALSE;
// Indicate we're done
q_bDone := TRUE;
// Move back to standby
PC_State := PCM_Standby;
PCM_StepperError:
PC_State := PCM_Init;
PCM_PiezoError:
PC_State := PCM_Init;
PCM_OtherError:
PC_State := PCM_Init;
END_CASE
fbMotionStage(stMotionStage:=Stepper);
// Transfer to the Piezo
Pitch.Piezo.rActPos := lrActPos;
tonPiezoSettled();
tonStepperHold();
fbPiezoControl(iq_Piezo:=Pitch.Piezo,
Enable_Positive:=Stepper.bLimitForwardEnable,
Enable_Negative:=Stepper.bLimitBackwardEnable);
END_FUNCTION_BLOCK
ACTION ACT_ResetSetpoint:
// Action to reset the Setpoint to the previous value when:
// - New setpoint outside range of soft limits
// - bHardwareEnable is FALSE
// - Limit switches are hit and new setpoint the direction of the hit switch
lrOriginalPosRequest := lrCurrentSetpoint;
lrCurrentSetpoint := lrLastSetpoint;
// Only want to log one warning about a bad position request
IF lrOriginalPosRequest <> lrCurrentSetpoint THEN
// Log a warning
fbFormatString.sFormat := 'Pitch req OoR fb (%s), reset within limits, %f';
fbFormatString.arg1 := F_STRING(POUName);
fbFormatString.arg2 := F_LREAL(lrOriginalPosRequest);
fbFormatString(sOut=>stDiag.asResults[stDiag.resultIdx.IncVal()]);
PC_State := PCM_Standby;
END_IF
END_ACTION
FB_RMSWatch¶
FUNCTION_BLOCK FB_RMSWatch
VAR_INPUT
END_VAR
VAR_OUTPUT
// RMS Error
fMaxRMSError : LREAL := 0;
fMinRMSError : LREAL := 1000; // start at something huge, FB will update with any smaller measured value
END_VAR
VAR_IN_OUT
stMotionStage : DUT_MotionStage;
END_VAR
VAR
fbDataActPos : FB_LREALBuffer; // ActPos Data Acquisition FB
fbDataSetPos : FB_LREALBuffer; // SetPos Data Acquisition FB
bExecuteDataStorage : BOOL := TRUE; // Take data of both ActPos and SetPos
bNewEncArray : BOOL;
fbStats : FB_BasicStats; // Calculate mean/standard deviation of ActPos
{attribute 'pytmc' := '
pv: MEAN
io: i
'}
fEncMean : LREAL;
{attribute 'pytmc' := '
pv: STDEV
io: i
'}
fEncStDev : LREAL;
{attribute 'pytmc' := '
pv: RMS
io: i
'}
fCurrRMSError : LREAL := 0;
nIndex : DINT;
fSum : LREAL := 0; // Just for calculating rms
fDiff : LREAL := 0;
{attribute 'pytmc' := '
pv: ACTPOSARRAY
io: i
'}
aEncActPos : ARRAY [1..1000] OF LREAL;
{attribute 'pytmc' := '
pv: SETPOSARRAY
io: i
'}
aEncSetPos : ARRAY [1..1000] OF LREAL;
END_VAR
// FB to store encoder positions in 1000 element arrays, compute RMS errors, and watch for min/max
// Encoder Readback/Storage
fbDataActPos(bExecute:=bExecuteDataStorage,
fInput:=stMotionStage.stAxisStatus.fActPosition,
arrOutput=>aEncActPos,
bNewArray=>bNewEncArray);
fbDataSetPos(bExecute:=bExecuteDataStorage,
fInput:=stMotionStage.Axis.NcToPlc.SetPos,
arrOutput=>aEncSetPos);
fbStats(aSignal:=aEncActPos,
bAlwaysCalc:=TRUE,
fMean=>fEncMean,
fStDev=>fEncStDev);
// Calculate RMS Error:
If bNewEncArray THEN
fCurrRMSError := 0;
FOR nIndex := 2 TO 1000 DO
// First point in array stuck as 0 for some reason...
fDiff := aEncActPos[nIndex] - aEncSetPos[nIndex];
fSum := EXPT(fDiff, 2);
fCurrRMSError := fCurrRMSError + fSum;
END_FOR;
fCurrRMSError := fCurrRMSError / 999.0; // 1000 element array but ditched the first point
fCurrRMSError := SQRT(fCurrRMSError);
// Watch for max:
IF fCurrRMSError > fMaxRMSError THEN
fMaxRMSError := fCurrRMSError;
END_IF
// Watch for min:
IF fCurrRMSError < fMinRMSError THEN
fMinRMSError := fCurrRMSError;
END_IF
END_IF
END_FUNCTION_BLOCK
FB_RunHOMS¶
FUNCTION_BLOCK FB_RunHOMS
VAR_INPUT
// Encoder Reference Values
nYupEncRef : ULINT;
nYdwnEncRef : ULINT;
nXupEncRef : ULINT;
nXdwnEncRef : ULINT;
// Gantry Tolerances
nGantryTolY : LINT := GVL_Constants.nGANTRY_TOLERANCE_NM_DEFAULT; // Encoder counts = nm
nGantryTolX : LINT := GVL_Constants.nGANTRY_TOLERANCE_NM_DEFAULT; // Encoder counts = nm
END_VAR
VAR_OUTPUT
// Gantry coupling status
bGantryAlreadyCoupledY : BOOL;
bGantryAlreadyCoupledX : BOOL;
// Current gantry difference
nCurrGantryY : LINT;
nCurrGantryX : LINT;
END_VAR
VAR_IN_OUT
// Motor Structs
stYup : DUT_MotionStage;
stYdwn : DUT_MotionStage;
stXup : DUT_MotionStage;
stXdwn : DUT_MotionStage;
stPitch : DUT_MotionStage;
// Manual coupling Gantried Axes
bExecuteCoupleY : BOOL;
bExecuteCoupleX : BOOL;
bExecuteDecoupleY : BOOL;
bExecuteDecoupleX : BOOL;
END_VAR
VAR
// STO Button
bSTOEnable1 AT %I* : BOOL;
bSTOEnable2 AT %I* : BOOL;
// Encoders
stYupEnc AT %I* : ST_RenishawAbsEnc;
stYdwnEnc AT %I* : ST_RenishawAbsEnc;
stXupEnc AT %I* : ST_RenishawAbsEnc;
stXdwnEnc AT %I* : ST_RenishawAbsEnc;
// Autocoupling Gantried Axes
fbAutoCoupleY : FB_GantryAutoCoupling;
fbAutoCoupleX : FB_GantryAutoCoupling;
END_VAR
// Encoder Reference Values
stYupEnc.Ref := nYupEncRef;
stYdwnEnc.Ref := nYdwnEncRef;
stXupEnc.Ref := nXupEncRef;
stXdwnEnc.Ref := nXdwnEncRef;
// Gantry Differences to monitor
nCurrGantryY := ((ULINT_TO_LINT(stYupEnc.Count) - ULINT_TO_LINT(stYupEnc.Ref)) - (ULINT_TO_LINT(stYdwnEnc.Count) - ULINT_TO_LINT(stYdwnEnc.Ref)));
nCurrGantryX := ((ULINT_TO_LINT(stXupEnc.Count) - ULINT_TO_LINT(stXupEnc.Ref)) - (ULINT_TO_LINT(stXdwnEnc.Count) - ULINT_TO_LINT(stXdwnEnc.Ref)));
// Release the hounds!
stYup.bHardwareEnable := bSTOEnable1 AND bSTOEnable2;
stYdwn.bHardwareEnable := bSTOEnable1 AND bSTOEnable2;
stXup.bHardwareEnable := bSTOEnable1 AND bSTOEnable2;
stXdwn.bHardwareEnable := bSTOEnable1 AND bSTOEnable2;
stPitch.bHardwareEnable := bSTOEnable1 AND bSTOEnable2;
// Start Autocoupling
fbAutoCoupleY(nGantryTol:=nGantryTolY,
Master:=stYup,
MasterEnc:= stYupEnc,
Slave:=stYdwn,
SlaveEnc:=stYdwnEnc,
bExecuteCouple:=bExecuteCoupleY,
bExecuteDecouple:=bExecuteDecoupleY,
bGantryAlreadyCoupled=>bGantryAlreadyCoupledY);
fbAutoCoupleX(nGantryTol:=nGantryTolX,
Master:=stXup,
MasterEnc:= stXupEnc,
Slave:=stXdwn,
SlaveEnc:=stXdwnEnc,
bExecuteCouple:=bExecuteCoupleX,
bExecuteDecouple:=bExecuteDecoupleX,
bGantryAlreadyCoupled=>bGantryAlreadyCoupledX);
END_FUNCTION_BLOCK
Main¶
PROGRAM Main
VAR
(*
// Test Pitch Control
fbPitchControl : FB_PitchControl;
TestPitch : HOMS_PitchMechanism := (ReqPosLimHi:=2000,
ReqPosLimLo:=-2000,
diEncPosLimHi:=10768330,
diEncPosLimLo:=8141680);
M1 : DUT_MotionStage;
bPitchDone : BOOL;
*)
// Test Bender vs No Bender
TESTWithBender : DUT_HOMS;
M1 : DUT_MotionStage := (nEnableMode:=ENUM_StageEnableMode.ALWAYS);
M2 : DUT_MotionStage := (nEnableMode:=ENUM_StageEnableMode.ALWAYS);
M3 : DUT_MotionStage := (nEnableMode:=ENUM_StageEnableMode.ALWAYS);
M4 : DUT_MotionStage := (nEnableMode:=ENUM_StageEnableMode.ALWAYS);
M5 : DUT_MotionStage := (nEnableMode:=ENUM_StageEnableMode.ALWAYS);
M6 : DUT_MotionStage := (nEnableMode:=ENUM_StageEnableMode.ALWAYS);
fbBender : FB_Bender;
fbMotionStage_m1 : FB_MotionStage;
fbMotionStage_m2 : FB_MotionStage;
fbMotionStage_m3 : FB_MotionStage;
fbMotionStage_m4 : FB_MotionStage;
fbMotionStage_m6 : FB_MotionStage;
END_VAR
(*
// Test Pitch Control
M1.bLimitBackwardEnable;
M1.bLimitForwardEnable;
M1.bHardwareEnable;
M1.fVelocity := 150.0;
fbPitchControl(Pitch:=TestPitch,
Stepper:=M1,
lrCurrentSetpoint:=M1.fPosition,
q_bDone=>bPitchDone,
q_bBusy=>);
IF NOT M1.bHardwareEnable THEN
M1.fPosition := M1.stAxisStatus.fActPosition;
END_IF
*)
// Test Bender vs. No Bender:
// M1L0
M1.bLimitForwardEnable := TRUE;
M1.bLimitBackwardEnable := TRUE;
M1.bPowerSelf := TRUE;
M2.bLimitForwardEnable := TRUE;
M2.bLimitBackwardEnable := TRUE;
M2.bPowerSelf := TRUE;
M3.bLimitForwardEnable := TRUE;
M3.bLimitBackwardEnable := TRUE;
M3.bPowerSelf := TRUE;
M4.bLimitForwardEnable := TRUE;
M4.bLimitBackwardEnable := TRUE;
M4.bPowerSelf := TRUE;
M5.bLimitForwardEnable := TRUE;
M5.bLimitBackwardEnable := TRUE;
M5.bPowerSelf := TRUE;
M6.bLimitForwardEnable := TRUE;
M6.bLimitBackwardEnable := TRUE;
M6.bPowerSelf := TRUE;
TESTWithBender.fbRunHOMS(stYup:=M1,
stYdwn:=M2,
stXup:=M3,
stXdwn:=M4,
stPitch:=M5,
nYupEncRef:=0,
nYdwnEncRef:=0,
nXupEncRef:=0,
nXdwnEncRef:=0,
bExecuteCoupleY:=TESTWithBender.bExecuteCoupleY,
bExecuteCoupleX:=TESTWithBender.bExecuteCoupleX,
bExecuteDecoupleY:=TESTWithBender.bExecuteDecoupleY,
bExecuteDecoupleX:=TESTWithBender.bExecuteDecoupleX,
bGantryAlreadyCoupledY=>TESTWithBender.bGantryAlreadyCoupledY,
bGantryAlreadyCoupledX=>TESTWithBender.bGantryAlreadyCoupledX,
nCurrGantryY=>TESTWithBender.nCurrGantryY,
nCurrGantryX=>TESTWithBender.nCurrGantryX);
fbBender(stBender:=M6,
bSTOEnable1:=TESTWithBender.fbRunHOMS.bSTOEnable1,
bSTOEnable2:=TESTWithBender.fbRunHOMS.bSTOEnable2);
fbMotionStage_m1(stMotionStage:=M1);
fbMotionStage_m2(stMotionStage:=M2);
fbMotionStage_m3(stMotionStage:=M3);
fbMotionStage_m4(stMotionStage:=M4);
fbMotionStage_m6(stMotionStage:=M6);
END_PROGRAM
MC_SmoothMover¶
FUNCTION_BLOCK MC_SmoothMover
VAR_IN_OUT
Axis : AXIS_REF;
END_VAR
VAR_INPUT
Velocity : LREAL;
ReqAbsPos : LREAL; //New requested position
Enable : BOOL; //While true the block will accept new positions and attempt to move to them if they are different
Execute : BOOL; //Will retry a move if the target position is the same
END_VAR
VAR_OUTPUT
Done : BOOL;
Busy : BOOL;
Error : BOOL;
END_VAR
VAR
mcMoveAbsolute : ARRAY[1..2] OF MC_MoveAbsolute;
iI: INT;
imcBlockIndex: INT;
ReqAbsPosPrevious : LREAL;
rtExecute: R_TRIG;
END_VAR
(* Smooth Mover
2017-8-30
A. Wallace
Enable means the block will always aquire new positions as they are updated. Execute
can be used to retry a move. Axis must be enabled by a power block.
*)
rtExecute(CLK:=Execute);
IF ( (ReqAbsPos <> ReqAbsPosPrevious AND Enable) OR rtExecute.Q) THEN
mcMoveAbsolute[imcBlockIndex].Execute := FALSE;
imcBlockIndex := imcBlockIndex + 1;
IF imcBlockIndex >2 THEN imcBlockIndex := 1; END_IF
mcMoveAbsolute[imcBlockIndex].Position := ReqAbsPos;
mcMoveAbsolute[imcBlockIndex].Execute := TRUE;
ReqAbsPosPrevious := ReqAbsPos;
ELSIF mcMoveAbsolute[imcBlockIndex].Done OR
mcMoveAbsolute[imcBlockIndex].CommandAborted OR
mcMoveAbsolute[imcBlockIndex].Busy OR
mcMoveAbsolute[imcBlockIndex].Error THEN
mcMoveAbsolute[imcBlockIndex].Execute := FALSE;
END_IF
FOR iI := 1 TO 2 DO
mcMoveAbsolute[iI](Axis := Axis, Velocity:=Velocity, BufferMode:=MC_Aborting);
END_FOR
Error := mcMoveAbsolute[1].Error OR mcMoveAbsolute[2].Error;
Done S= mcMoveAbsolute[1].Done OR mcMoveAbsolute[2].Done;
Busy := mcMoveAbsolute[1].Busy OR mcMoveAbsolute[2].Busy;
Done R= Busy OR Error;
END_FUNCTION_BLOCK
TEST_PitchControl¶
{attribute 'call_after_init'}
FUNCTION_BLOCK TEST_PitchControl EXTENDS TcUnit.FB_TestSuite
VAR_INPUT
END_VAR
VAR_OUTPUT
END_VAR
VAR
END_VAR
LimitSwitches();
END_FUNCTION_BLOCK
WithinRange¶
FUNCTION WithinRange : BOOL
VAR_INPUT
ValA : REAL; //New position to evaluate
Center : REAL; //Current position
Range : REAL; //Span of the range
Offset : REAL := 0; //Offset from center if the range is non-symetric
END_VAR
VAR
END_VAR
IF ValA < (Center + Offset - (Range/2) ) THEN
WithinRange := FALSE;
ELSIF ValA > (Center + Offset + (Range/2) ) THEN
WithinRange := FALSE;
ELSE
WithinRange := TRUE;
END_IF
END_FUNCTION