Started Modbus Master for Levitronix Pumps and Flow sensors

PLC/LibraryCandidates/ModbusMaster/FB_ModbusMaster.TcPOU

@@ -0,0 +1,147 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_ModbusMaster" Id="{90292ada-f5b3-4b38-a85e-d6dc52e68612}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_ModbusMaster
+VAR_INPUT
+	timGapTime : TIME := T#20MS;
+END_VAR
+VAR_OUTPUT
+	xBusy : BOOL;
+END_VAR
+VAR
+	// Modbus master comm fb
+	_fbMaster : ModbusRtuMasterV2_KL6x22B;
+	
+	// Request queue
+	_astReqQueue : ARRAY[0..(GVL_ModbusMaster.QUEUE_SIZE - 1)] OF ST_Modbus_Req;
+	_iHead : INT := 0;
+	_iTail : INT := 0;
+	
+	// Current request from queue
+	_stCurrReq : ST_Modbus_Req;
+	
+	// Gap timer so that we dont send requests back to back
+	_tonGapTimer : TON;
+	
+	// State machine state
+	_iState : INT := 0;
+	
+	// Rerun flag for faster state machine
+	_xSMCycleDone : BOOL := TRUE;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[// Main state machine
+REPEAT
+	// Safety reset
+	_xSMCycleDone := TRUE;
+	
+	CASE _iState OF
+		// Wait for gap timer to be expired
+		0:
+			_tonGapTimer(IN := TRUE);
+			
+			IF _tonGapTimer.Q THEN
+				_tonGapTimer(IN := FALSE);
+				_xSMCycleDone := FALSE;
+				_iState := 1;
+			END_IF
+			
+		// Get next request if there is one in the queue
+		1:
+			// Get next request from queue if there is one
+			IF M_Dequeue() THEN
+				// Set fb data
+				_fbMaster.UnitID := _stCurrReq.bySlaveAddr;
+				_fbMaster.MBAddr := _stCurrReq.wStartAddr;
+				_fbMaster.Quantity := _stCurrReq.wQuantity;
+				_fbMaster.cbLength := _stCurrReq.wDataSize;
+				_fbMaster.pMemoryAddr := _stCurrReq.pvData;
+				
+				// Rerun statemachine this cycle
+				_xSMCycleDone := FALSE;
+				_iState := 10;
+			END_IF
+			
+		// Execute dequeued command
+		10:
+			// Call modbus master corresponding function
+			CASE _stCurrReq.eCmd OF
+				E_Modbus_Cmd.READ_HOLDING:
+					_fbMaster.ReadRegs();
+				
+				E_Modbus_Cmd.READ_INPUTS:
+					_fbMaster.ReadInputRegs();
+					
+				E_Modbus_Cmd.WRITE_HOLDING:
+					_fbMaster.WriteRegs();
+					
+				// Reject unknown commands
+				ELSE
+					_stCurrReq.pxError^ := TRUE;
+					_iState := 0;
+			END_CASE
+			
+			// Wait for fb to be done
+			IF (NOT _fbMaster.BUSY) THEN
+				_stCurrReq.pxDone^ := TRUE;
+				// Rerun statemachine this cycle
+				_xSMCycleDone := FALSE;
+				_iState := 0;
+			END_IF
+			
+			IF _fbMaster.Error THEN
+				_stCurrReq.pxError^ := TRUE;
+				// Rerun statemachine this cycle
+				_xSMCycleDone := FALSE;
+				_iState := 0;
+			END_IF
+	END_CASE
+
+// Check if we want to rerun the state machine
+UNTIL _xSMCycleDone
+END_REPEAT
+]]></ST>
+    </Implementation>
+    <Method Name="M_Dequeue" Id="{083aa9a5-ac23-4366-b080-ab00cfa6b4ce}">
+      <Declaration><![CDATA[METHOD PRIVATE M_Dequeue : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[// No entries in queue
+IF _iHead = _iTail THEN
+	M_Dequeue := FALSE;
+ELSE
+	// Get next entry
+	_stCurrReq := _astReqQueue;
+	_iHead := (_iHead + 1) MOD GVL_ModbusMaster.QUEUE_SIZE;
+	M_Dequeue := TRUE;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="M_Enqueue" Id="{37963eb3-9b4c-4945-ba35-cfefd78cb427}">
+      <Declaration><![CDATA[METHOD M_Enqueue : BOOL
+VAR_INPUT
+	stReq : ST_Modbus_Req;
+END_VAR
+VAR
+	_iNextTail : INT;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[// Calculate next entry 
+_iNextTail := (_iTail + 1) MOD GVL_ModbusMaster.QUEUE_SIZE;
+
+// If next tail euqals head, the buffer is full
+// so disgard this request
+IF _iNextTail = _iHead THEN
+	M_Enqueue := FALSE;
+ELSE
+	_astReqQueue[_iNextTail] := stReq;
+	_iTail := _iNextTail;
+	M_Enqueue := TRUE;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>