Merge pull request #1 from ZLLentz/master

ENH: Make the library useful
pcdshub · Jun 18, 2019 · eee05e2 · eee05e2
2 parents 0df5153 + b5cb92f
commit eee05e2
Show file tree

Hide file tree

Showing 36 changed files with 2,919 additions and 0 deletions.
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1,27 @@
+# Twincat ignores
+_Boot
+_Libraries
+_CompileInfo
+*.sln
+*.suo
+*~
+*.tpy
+*.bak
+*.~u
+TrialLicense.tclrs
+*.library
+
+# EPICS ignores
+O.*
+archive
+autosave
+bin
+build
+db
+dbd
+
+# Vim ignores
+*.swp
+
+# Output from scope ignores
+*.csv
diff --git a/Config/FB_AxisConfig.TcPOU b/Config/FB_AxisConfig.TcPOU
@@ -0,0 +1,118 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4020.14">
+  <POU Name="FB_AxisConfig" Id="{fcb6a2ad-e399-41e9-a4d8-8a9b39418313}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_AxisConfig
+VAR_INPUT
+	bExecute: BOOL; // Set, then get
+	bExecGet: BOOL; // Only get
+	bReset: BOOL;
+	stConfigSet: ST_AxisParams;
+	stHardware: ST_AxisHardware;
+END_VAR
+VAR_IN_OUT
+	Axis: AXIS_REF;
+END_VAR
+VAR_OUTPUT
+	bDone: BOOL;
+	bValid: BOOL;
+	bBusy: BOOL;
+	bError: BOOL;
+	nErrorID: UDINT;
+	bInitDone: BOOL;
+	bInitPreOp: BOOL;
+	stConfigGet: ST_AxisParams;
+END_VAR
+VAR
+	fbRead: FB_AxisReadParams;
+	fbWrite: FB_AxisWriteParams;
+	bDoRead: BOOL;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[// Write first
+fbWrite(bExecute:=bExecute,
+	bReset:=bReset,
+	stConfig:=stConfigSet,
+	stHardware:=stHardware,
+	Axis:=Axis);
+// Can only be turned on! Not back off!
+IF fbWrite.bInitPreOp THEN
+	bInitPreOP := TRUE;
+END_IF
+// Reset bDoRead if both execs are false
+IF NOT bExecute AND NOT bExecGet THEN
+	bDoRead := FALSE;
+END_IF
+// If bExecute is TRUE, wait untill after the write to read
+IF bExecute THEN
+	bDoRead := fbWrite.bDone;
+ELSE
+	bDoRead := bExecGet;
+END_IF
+// Notify that its ok to set master to OP.
+// This block is really only used if we skip the write step.
+IF bDoRead THEN
+	bInitPreOP := TRUE;
+END_IF
+// Read now
+fbRead(bExecute:=bDoRead,
+	bReset:=bReset,
+	stHardware:=stHardware,
+	Axis:=Axis,
+	stConfig=>stConfigGet,
+	bValid=>bValid);
+// Set the status outputs
+IF bExecute THEN
+	bDone := fbWrite.bDone AND NOT fbRead.bBusy;
+ELSIF bExecGet THEN
+	bDone := NOT fbRead.bBusy;
+ELSE
+	bDone := FALSE;
+END_IF
+bBusy := fbWrite.bBusy OR fbRead.bBusy;
+bError := fbWrite.bError OR fbRead.bError;
+IF fbWrite.nErrorID > 0 THEN
+	nErrorID := fbWrite.nErrorID;
+ELSE
+	nErrorID := fbRead.nErrorID;
+END_IF
+IF bDone THEN
+	bInitDone := TRUE;
+END_IF
+IF bError THEN
+	bInitDone := FALSE;
+END_IF
+IF bReset THEN
+	bReset := FALSE;
+END_IF]]></ST>
+    </Implementation>
+    <LineIds Name="FB_AxisConfig">
+      <LineId Id="57" Count="2" />
+      <LineId Id="112" Count="0" />
+      <LineId Id="60" Count="0" />
+      <LineId Id="249" Count="0" />
+      <LineId Id="246" Count="2" />
+      <LineId Id="78" Count="4" />
+      <LineId Id="72" Count="0" />
+      <LineId Id="74" Count="0" />
+      <LineId Id="76" Count="1" />
+      <LineId Id="75" Count="0" />
+      <LineId Id="181" Count="1" />
+      <LineId Id="177" Count="2" />
+      <LineId Id="30" Count="1" />
+      <LineId Id="9" Count="0" />
+      <LineId Id="113" Count="0" />
+      <LineId Id="19" Count="2" />
+      <LineId Id="61" Count="0" />
+      <LineId Id="210" Count="1" />
+      <LineId Id="213" Count="3" />
+      <LineId Id="212" Count="0" />
+      <LineId Id="63" Count="3" />
+      <LineId Id="68" Count="1" />
+      <LineId Id="67" Count="0" />
+      <LineId Id="144" Count="5" />
+      <LineId Id="243" Count="2" />
+      <LineId Id="29" Count="0" />
+    </LineIds>
+  </POU>
+</TcPlcObject>
diff --git a/Config/FB_AxisEthMaster.TcPOU b/Config/FB_AxisEthMaster.TcPOU
@@ -0,0 +1,29 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4020.14">
+  <POU Name="FB_AxisEthMaster" Id="{5752344c-db01-4f54-8215-8c3ef3b44e9a}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_AxisEthMaster
+VAR_INPUT
+	bExecute: BOOL := TRUE;
+END_VAR
+VAR_OUTPUT
+	bCfgReady: BOOL;
+END_VAR
+VAR
+	amsNetID AT %I*: AMSNETID;
+	fbMasterOp: FB_EcSetMasterState;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[// Set master to OP
+// This should wait until all relevant slaves are set to PREOP or they will be prematurely set to OP
+fbMasterOp(sNetId:=F_CREATEAMSNETID(amsNetID),
+	bExecute:=bExecute,
+	reqState:=EC_DEVICE_STATE_OP);]]></ST>
+    </Implementation>
+    <LineIds Name="FB_AxisEthMaster">
+      <LineId Id="29" Count="1" />
+      <LineId Id="33" Count="1" />
+      <LineId Id="9" Count="0" />
+    </LineIds>
+  </POU>
+</TcPlcObject>
diff --git a/Config/FB_AxisHardware.TcPOU b/Config/FB_AxisHardware.TcPOU
@@ -0,0 +1,103 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4020.14">
+  <POU Name="FB_AxisHardware" Id="{1564163d-e493-4d9a-b680-85c98f09b3c4}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_AxisHardware
+VAR_INPUT
+	bExecute: BOOL;
+	amsEncAddr: AMSADDR;
+	nEncCh: USINT;
+END_VAR
+VAR_IN_OUT
+	Axis: AXIS_REF;
+END_VAR
+VAR_OUTPUT
+	bDone: BOOL;
+	bBusy: BOOL;
+	bError: BOOL;
+	nErrorID: UDINT;
+	stHardware: ST_AxisHardware;
+END_VAR
+VAR
+	fbReadDriveAddress: MC_ReadDriveAddress;
+	fbDriveId: FB_EcGetSlaveIdentity;
+	fbEncId: FB_EcGetSlaveIdentity;
+	fbADSRead: ADSREADEX;
+	stDriveAddr: ST_DriveAddress;
+	stDriveRef: ST_DriveRef;
+	stEncRef: ST_DriveRef;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[// Get drive ref using AXIS_REF
+fbReadDriveAddress(Execute:=bExecute,
+	Axis:=Axis,
+	DriveAddress=>stDriveAddr);
+stDriveRef.sNetId := stDriveAddr.NetID;
+stDriveRef.nSlaveAddr := stDriveAddr.SlaveAddress;
+stDriveRef.nDriveNo := stDriveAddr.Channel;
+stHardware.drive_ref := stDriveRef;
+
+// Get encoder ref using input args (cannot be derived from AXIS_REF)
+stEncRef.sNetId := F_CreateAmsNetId(amsEncAddr.netId);
+stEncRef.nSlaveAddr := amsEncAddr.port;
+stEncRef.nDriveNo := nEncCh;
+stHardware.enc_ref := stEncRef;
+
+// Get the drive model (e.g. 7031)
+IF stDriveRef.nSlaveAddr > 0 THEN
+	fbDriveId(bExecute:=bExecute AND fbReadDriveAddress.Done,
+		sNetId:=stDriveRef.sNetId,
+		nSlaveAddr:=stDriveRef.nSlaveAddr);
+	stHardware.drive_type := UDINT_TO_UINT(SHR(fbDriveID.identity.productCode, 16));
+END_IF
+
+// Get the encoder model (e.g. 5042)
+IF stEncRef.nSlaveAddr > 0 THEN
+	fbEncId(bExecute:=bExecute AND fbReadDriveAddress.Done,
+		sNetId:=stEncRef.sNetId,
+		nSlaveAddr:=stEncRef.nSlaveAddr);
+	stHardware.enc_type := UDINT_TO_UINT(SHR(fbEncID.identity.productCode, 16));
+END_IF
+
+// Summarize outputs
+bDone := fbReadDriveAddress.Done AND NOT fbDriveId.bBusy AND NOT fbEncId.bBusy;
+bBusy := fbReadDriveAddress.Busy OR fbDriveId.bBusy OR fbEncId.bBusy;
+bError := fbReadDriveAddress.Error OR fbDriveId.bError OR fbEncId.bError;
+IF fbReadDriveAddress.ErrorID > 0 THEN
+	nErrorID := fbReadDriveAddress.ErrorID;
+ELSIF fbDriveId.nErrId >0 THEN
+	nErrorID := fbDriveId.nErrId;
+ELSE
+	nErrorID := fbEncId.nErrId;
+END_IF]]></ST>
+    </Implementation>
+    <LineIds Name="FB_AxisHardware">
+      <LineId Id="18" Count="1" />
+      <LineId Id="38" Count="1" />
+      <LineId Id="44" Count="2" />
+      <LineId Id="79" Count="0" />
+      <LineId Id="83" Count="0" />
+      <LineId Id="85" Count="0" />
+      <LineId Id="90" Count="2" />
+      <LineId Id="84" Count="0" />
+      <LineId Id="80" Count="0" />
+      <LineId Id="100" Count="0" />
+      <LineId Id="20" Count="2" />
+      <LineId Id="27" Count="0" />
+      <LineId Id="81" Count="0" />
+      <LineId Id="101" Count="0" />
+      <LineId Id="86" Count="0" />
+      <LineId Id="102" Count="0" />
+      <LineId Id="87" Count="0" />
+      <LineId Id="95" Count="2" />
+      <LineId Id="94" Count="0" />
+      <LineId Id="103" Count="0" />
+      <LineId Id="82" Count="0" />
+      <LineId Id="47" Count="4" />
+      <LineId Id="53" Count="1" />
+      <LineId Id="98" Count="1" />
+      <LineId Id="52" Count="0" />
+      <LineId Id="9" Count="0" />
+    </LineIds>
+  </POU>
+</TcPlcObject>