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