m.heisig
c1850f780b
- Aligned X and Y NC Axis with aligner camera coordination system - Added robot plc jobs and feedback - Began Meca500 robot interface - Changed hotplate control to slow PWM - PackML statemachine now starts in aborted state - Fixed StateML start method
552 lines
13 KiB
XML
552 lines
13 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1">
|
|
<POU Name="FB_TFProtocol" Id="{f0c44f0b-70b8-45d0-8720-9bd7e08b6aa6}" SpecialFunc="None">
|
|
<Declaration><![CDATA[FUNCTION_BLOCK FB_TFProtocol
|
|
VAR_INPUT
|
|
sIpAddr : STRING;
|
|
udiPort : UDINT;
|
|
|
|
xConfirmAlarms : BOOL;
|
|
END_VAR
|
|
VAR_OUTPUT
|
|
xConnected : BOOL;
|
|
|
|
xNewResponseReady : BOOL;
|
|
xBusy : BOOL;
|
|
xError : BOOL;
|
|
END_VAR
|
|
VAR
|
|
// Connection settings
|
|
_fbTcpConnection : FB_ClientServerConnection;
|
|
_sIpAddr : STRING;
|
|
_udiPort : UDINT;
|
|
_hSocket : T_HSOCKET;
|
|
_xConnect : BOOL := TRUE;
|
|
_xConnected : BOOL;
|
|
|
|
// Socket send
|
|
_fbSocketSend : FB_SocketSend;
|
|
_timSendTimeout : TIME := T#5S;
|
|
|
|
// Socket receive
|
|
_fbSocketReceive : FB_SocketReceive;
|
|
_timReceiveTimeout : TIME := T#5S;
|
|
_abReceivedBuffer : ARRAY[0..100] OF BYTE;
|
|
_asReceivedResponse : ARRAY[0..9] OF STRING;
|
|
_uiBufferPos : UINT;
|
|
_udiCounterReceive : UDINT;
|
|
_udiReceivedBytes : UDINT;
|
|
_timPollingTime : TIME := T#1S;
|
|
_tonPollTimer : TON;
|
|
_xEnableReceiveTimeout : BOOL;
|
|
_timReceiveTimeoutTime : TIME := T#5S;
|
|
_tonReceiveTimeout : TON;
|
|
_uiLastReceivedResponseId : UINT := 0;
|
|
|
|
// Receive state machine
|
|
_iStateReceive : INT;
|
|
|
|
// Command data
|
|
_uiCmdId : UINT := 1;
|
|
_sCmd : STRING(255);
|
|
_sAck : STRING(255);
|
|
|
|
// Main state machine
|
|
_iState : INT := 0;
|
|
_diCounter : DINT;
|
|
_xSendCmd : BOOL;
|
|
_uiRetries : UINT;
|
|
_xCmdReceived : BOOL;
|
|
_iPlaceCmdSeperator : INT;
|
|
_sTmp : STRING;
|
|
_xReceivedResponseOk : BOOL;
|
|
_uiReceivedCommandId : UINT;
|
|
|
|
// Buffer before output
|
|
_xBusy : BOOL;
|
|
_xError : BOOL;
|
|
END_VAR
|
|
VAR CONSTANT
|
|
MAX_RETRIES : UINT := 3;
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// Call client server connection fb
|
|
_fbTcpConnection(
|
|
sSrvNetID:= '',
|
|
nMode:= ,
|
|
sRemoteHost:= sIpAddr,
|
|
nRemotePort:= udiPort,
|
|
bEnable:= _xConnect,
|
|
tReconnect:= T#2S,
|
|
bBusy=> ,
|
|
bError=> ,
|
|
nErrId=> ,
|
|
hSocket=> _hSocket,
|
|
eState=> );
|
|
|
|
// Get connected state
|
|
_xConnected := (_fbTcpConnection.eState = E_SocketConnectionState.eSOCKET_CONNECTED);
|
|
|
|
// Receive timeout after sending a command
|
|
_tonReceiveTimeout(IN := _xEnableReceiveTimeout, PT := _timReceiveTimeoutTime);
|
|
|
|
// Receive state machine
|
|
CASE _iStateReceive OF
|
|
// Main socket not connected
|
|
0:
|
|
IF _xConnected THEN
|
|
_iStateReceive := 10;
|
|
END_IF
|
|
|
|
// Call receive
|
|
10:
|
|
_fbSocketReceive(
|
|
sSrvNetId:= '',
|
|
hSocket:= _hSocket,
|
|
cbLen:= SIZEOF(_abReceivedBuffer) - _udiReceivedBytes,
|
|
pDest:= ADR(_abReceivedBuffer) + _udiReceivedBytes,
|
|
bExecute:= TRUE,
|
|
tTimeout:= _timReceiveTimeout,
|
|
bBusy=> ,
|
|
bError=> ,
|
|
nErrId=> ,
|
|
nRecBytes=> );
|
|
|
|
IF (NOT _fbSocketReceive.bBusy) AND (NOT _fbSocketReceive.bError) THEN
|
|
_fbSocketReceive(bExecute := FALSE);
|
|
IF _fbSocketReceive.nRecBytes > 0 THEN
|
|
_udiReceivedBytes := _udiReceivedBytes + _fbSocketReceive.nRecBytes;
|
|
_iStateReceive := 20;
|
|
ELSE
|
|
_iStateReceive := 15;
|
|
END_IF
|
|
|
|
END_IF
|
|
|
|
// If we have an error, check if we are connected
|
|
IF _fbSocketReceive.bError THEN
|
|
_fbSocketReceive(bExecute := FALSE);
|
|
_iStateReceive := 0;
|
|
END_IF
|
|
|
|
// Wait some time before rechecking
|
|
15:
|
|
_tonPollTimer(IN := TRUE, PT := _timPollingTime);
|
|
|
|
IF _tonPollTimer.Q THEN
|
|
_tonPollTimer(IN := FALSE);
|
|
_iStateReceive := 10;
|
|
END_IF
|
|
|
|
// Check if we are still connected
|
|
IF (NOT _xConnected) THEN
|
|
_tonPollTimer(IN := FALSE);
|
|
//_asReceivedResponse[] := '';
|
|
_iStateReceive := 0;
|
|
END_IF
|
|
|
|
// Check received data
|
|
20:
|
|
// Check if received command is complete
|
|
MEMCPY(destAddr := ADR(_asReceivedResponse[_uiBufferPos]), srcAddr := ADR(_abReceivedBuffer), n := _udiReceivedBytes);
|
|
_uiBufferPos := _uiBufferPos + 1;
|
|
IF _uiBufferPos > 9 THEN
|
|
_uiBufferPos := 0;
|
|
END_IF
|
|
_udiReceivedBytes := 0;
|
|
_xCmdReceived := TRUE;
|
|
|
|
// Go back to polling wait state
|
|
_iStateReceive := 15;
|
|
END_CASE
|
|
|
|
// CASE _iState OF
|
|
// // Wait for active connection to tray feeder
|
|
// 0:
|
|
// IF _xConnected THEN
|
|
// _iState := 10;
|
|
// END_IF
|
|
//
|
|
// // Connected and idle
|
|
// 10:
|
|
// // Got to disconnected state if connection is lost
|
|
// IF (NOT _xConnected) THEN
|
|
// _iState := 0;
|
|
// END_IF
|
|
//
|
|
// IF _xSendCmd THEN
|
|
// _xSendCmd := FALSE;
|
|
// _xBusy := TRUE;
|
|
// _iState := 20;
|
|
// END_IF
|
|
//
|
|
// // Check if we received a response without sending a command
|
|
// IF _xCmdReceived THEN
|
|
// _xCmdReceived := FALSE;
|
|
// _iState := 50;
|
|
// END_IF
|
|
//
|
|
// // Send command
|
|
// 20:
|
|
// _fbSocketSend(
|
|
// sSrvNetId:= '',
|
|
// hSocket:= _hSocket,
|
|
// cbLen:= SIZEOF(_sCmd),
|
|
// pSrc:= ADR(_sCmd),
|
|
// bExecute:= TRUE,
|
|
// tTimeout:= _timSendTimeout,
|
|
// bBusy=> ,
|
|
// bError=> ,
|
|
// nErrId=> );
|
|
//
|
|
// IF (NOT _fbSocketSend.bBusy) AND (NOT _fbSocketSend.bError) THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _xEnableReceiveTimeout := TRUE;
|
|
// _iState := 30;
|
|
// END_IF
|
|
//
|
|
// IF _fbSocketSend.bError THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _iState := 90;
|
|
// END_IF
|
|
//
|
|
// // Wait for response
|
|
// 30:
|
|
// // Received a response
|
|
// IF _xCmdReceived THEN
|
|
// _xCmdReceived := FALSE;
|
|
// _xEnableReceiveTimeout := FALSE;
|
|
// _uiRetries := 0;
|
|
// _iState := 40;
|
|
// END_IF
|
|
//
|
|
// // Didnt receive command ack in time
|
|
// // So resend the command if max retries are not reached
|
|
// IF _tonReceiveTimeout.Q THEN
|
|
// _xEnableReceiveTimeout := FALSE;
|
|
// _uiRetries := _uiRetries + 1;
|
|
//
|
|
// // Check if we reached the max number of retries
|
|
// IF _uiRetries > MAX_RETRIES THEN
|
|
// _iState := 90;
|
|
// ELSE
|
|
// // Retry by sendind command again
|
|
// _iState := 20;
|
|
// END_IF
|
|
// END_IF
|
|
//
|
|
// // Check response
|
|
// 40:
|
|
// // Check for cmd id
|
|
// IF (TO_STRING(_sReceivedResponse[0]) <> UINT_TO_STRING(_uiCmdId)) THEN
|
|
// // Wrong command id received
|
|
// _iState := 900;
|
|
// END_IF
|
|
//
|
|
// // For for response indicator
|
|
// IF _sReceivedResponse[1] <> F_ToASC('<') THEN
|
|
// // Wrong response indicator
|
|
// _iState := 901;
|
|
// END_IF
|
|
//
|
|
// // Check for correct cmd
|
|
// _xReceivedResponseOk := TRUE;
|
|
// FOR _diCounter := 2 TO 10 DO
|
|
// IF _sReceivedResponse[_diCounter] <> _sCmd[_diCounter] THEN
|
|
// _xReceivedResponseOk := FALSE;
|
|
// EXIT;
|
|
// END_IF
|
|
// END_FOR
|
|
//
|
|
// IF _xReceivedResponseOk THEN
|
|
// _xBusy := FALSE;
|
|
// _iState := 10;
|
|
// ELSE
|
|
// _iState := 902;
|
|
// END_IF
|
|
//
|
|
// 50:
|
|
// // Get command id
|
|
// _uiReceivedCommandId := _sReceivedResponse[0] - 16#30;
|
|
//
|
|
// // Check if it is a new command
|
|
// IF (_uiReceivedCommandId <> _uiLastReceivedResponseId) OR (_uiReceivedCommandId = 0) THEN
|
|
// _uiLastReceivedResponseId := _uiReceivedCommandId;
|
|
// xNewResponseReady := FALSE;
|
|
//
|
|
// // Send acknowledgement
|
|
// _iState := 60;
|
|
// END_IF
|
|
//
|
|
// // Prepare ack response
|
|
// 60:
|
|
// FOR _diCounter := 0 TO 10 DO
|
|
// _sAck[_diCounter] := _sReceivedResponse[_diCounter];
|
|
// END_FOR
|
|
// _sAck[11] := 0;
|
|
//
|
|
// IF _sReceivedResponse[1] = F_ToAsc('>') THEN
|
|
// _iState := 70;
|
|
// ELSE
|
|
// _iState := 10;
|
|
// END_IF
|
|
//
|
|
//
|
|
// // Send ack response
|
|
// 70:
|
|
// _fbSocketSend(
|
|
// sSrvNetId:= '',
|
|
// hSocket:= _hSocket,
|
|
// cbLen:= SIZEOF(_sAck),
|
|
// pSrc:= ADR(_sAck),
|
|
// bExecute:= TRUE,
|
|
// tTimeout:= _timSendTimeout,
|
|
// bBusy=> ,
|
|
// bError=> ,
|
|
// nErrId=> );
|
|
//
|
|
// IF (NOT _fbSocketSend.bBusy) AND (NOT _fbSocketSend.bError) THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _xEnableReceiveTimeout := TRUE;
|
|
// _xBusy := FALSE;
|
|
// _iState := 10;
|
|
// END_IF
|
|
//
|
|
// IF _fbSocketSend.bError THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _iState := 90;
|
|
// END_IF
|
|
//
|
|
// // Error
|
|
// 90:
|
|
// _xError := TRUE;
|
|
// _xBusy := FALSE;
|
|
//
|
|
// IF xConfirmAlarms THEN
|
|
// _xError := FALSE;
|
|
// _uiRetries := 0;
|
|
// _iState := 0;
|
|
// END_IF
|
|
// END_CASE
|
|
|
|
// Copy output buffers to outputs
|
|
xConnected := _xConnected;
|
|
xBusy := _xBusy;
|
|
xError := _xError;]]></ST>
|
|
</Implementation>
|
|
<Method Name="M_Debug" Id="{24e700f8-310e-46c8-98c0-8a10312afeda}">
|
|
<Declaration><![CDATA[METHOD M_Debug
|
|
VAR_INPUT
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// CASE _iState OF
|
|
// // Wait for active connection to tray feeder
|
|
// 0:
|
|
// IF _xConnected THEN
|
|
// _iState := 10;
|
|
// END_IF
|
|
//
|
|
// // Connected and idle
|
|
// 10:
|
|
// // Got to disconnected state if connection is lost
|
|
// IF (NOT _xConnected) THEN
|
|
// _iState := 0;
|
|
// END_IF
|
|
//
|
|
// IF _xSendCmd THEN
|
|
// _xSendCmd := FALSE;
|
|
// _xBusy := TRUE;
|
|
// _iState := 20;
|
|
// END_IF
|
|
//
|
|
// // Send command
|
|
// 20:
|
|
// _fbSocketSend(
|
|
// sSrvNetId:= '',
|
|
// hSocket:= _hSocket,
|
|
// cbLen:= SIZEOF(_sCmd),
|
|
// pSrc:= ADR(_sCmd),
|
|
// bExecute:= TRUE,
|
|
// tTimeout:= _timSendTimeout,
|
|
// bBusy=> ,
|
|
// bError=> ,
|
|
// nErrId=> );
|
|
//
|
|
// IF (NOT _fbSocketSend.bBusy) AND (NOT _fbSocketSend.bError) THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _xEnableReceiveTimeout := TRUE;
|
|
// _iState := 30;
|
|
// END_IF
|
|
//
|
|
// IF _fbSocketSend.bError THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _iState := 90;
|
|
// END_IF
|
|
//
|
|
// // Wait for response
|
|
// 30:
|
|
// // Received a response
|
|
// IF _xCmdReceived THEN
|
|
// _xCmdReceived := FALSE;
|
|
// _xEnableReceiveTimeout := FALSE;
|
|
// _uiRetries := 0;
|
|
// _iState := 40;
|
|
// END_IF
|
|
//
|
|
// // Didnt receive command ack in time
|
|
// // So resend the command if max retries are not reached
|
|
// IF _tonReceiveTimeout.Q THEN
|
|
// _xEnableReceiveTimeout := FALSE;
|
|
// _uiRetries := _uiRetries + 1;
|
|
//
|
|
// // Check if we reached the max number of retries
|
|
// IF _uiRetries > MAX_RETRIES THEN
|
|
// _iState := 90;
|
|
// ELSE
|
|
// // Retry by sendind command again
|
|
// _iState := 20;
|
|
// END_IF
|
|
// END_IF
|
|
//
|
|
// // Check response
|
|
// 40:
|
|
// // Check for cmd id
|
|
// IF (TO_STRING(_asReceivedResponse[0]) <> UINT_TO_STRING(_uiCmdId)) THEN
|
|
// // Wrong command id received
|
|
// _iState := 900;
|
|
// END_IF
|
|
//
|
|
// // For for response indicator
|
|
// IF _asReceivedResponse[1] <> F_ToASC('<') THEN
|
|
// // Wrong response indicator
|
|
// _iState := 901;
|
|
// END_IF
|
|
//
|
|
// // Check for correct cmd
|
|
// _xReceivedResponseOk := TRUE;
|
|
// FOR _diCounter := 2 TO 10 DO
|
|
// IF _asReceivedResponse[_diCounter] <> _sCmd[_diCounter] THEN
|
|
// _xReceivedResponseOk := FALSE;
|
|
// EXIT;
|
|
// END_IF
|
|
// END_FOR
|
|
//
|
|
// IF _xReceivedResponseOk THEN
|
|
// _xBusy := FALSE;
|
|
// _iState := 10;
|
|
// ELSE
|
|
// _iState := 902;
|
|
// END_IF
|
|
//
|
|
// 50:
|
|
// // Get command id
|
|
// _uiReceivedCommandId := _asReceivedResponse[0] - 16#30;
|
|
//
|
|
// // Check if it is a new command
|
|
// IF (_uiReceivedCommandId <> _uiLastReceivedResponseId) OR (_uiReceivedCommandId = 0) THEN
|
|
// _uiLastReceivedResponseId := _uiReceivedCommandId;
|
|
// xNewResponseReady := FALSE;
|
|
//
|
|
// // Send acknowledgement
|
|
// _iState := 60;
|
|
// END_IF
|
|
//
|
|
// // Prepare ack response
|
|
// 60:
|
|
// FOR _diCounter := 0 TO 10 DO
|
|
// _sAck[_diCounter] := _asReceivedResponse[_diCounter];
|
|
// END_FOR
|
|
// _sAck[11] := 0;
|
|
//
|
|
// IF _asReceivedResponse[1] = F_ToAsc('>') THEN
|
|
// _iState := 70;
|
|
// ELSE
|
|
// _iState := 10;
|
|
// END_IF
|
|
//
|
|
//
|
|
// // Send ack response
|
|
// 70:
|
|
// _fbSocketSend(
|
|
// sSrvNetId:= '',
|
|
// hSocket:= _hSocket,
|
|
// cbLen:= SIZEOF(_sAck),
|
|
// pSrc:= ADR(_sAck),
|
|
// bExecute:= TRUE,
|
|
// tTimeout:= _timSendTimeout,
|
|
// bBusy=> ,
|
|
// bError=> ,
|
|
// nErrId=> );
|
|
//
|
|
// IF (NOT _fbSocketSend.bBusy) AND (NOT _fbSocketSend.bError) THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _xEnableReceiveTimeout := TRUE;
|
|
// _xBusy := FALSE;
|
|
// _iState := 10;
|
|
// END_IF
|
|
//
|
|
// IF _fbSocketSend.bError THEN
|
|
// _fbSocketSend(bExecute := FALSE);
|
|
// _iState := 90;
|
|
// END_IF
|
|
//
|
|
// // Error
|
|
// 90:
|
|
// _xError := TRUE;
|
|
// _xBusy := FALSE;
|
|
//
|
|
// IF xConfirmAlarms THEN
|
|
// _xError := FALSE;
|
|
// _uiRetries := 0;
|
|
// _iState := 0;
|
|
// END_IF
|
|
// END_CASE]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_GetResponse" Id="{48f8719d-7ed0-4fb1-824a-0bf475fcfc2c}">
|
|
<Declaration><![CDATA[METHOD M_GetResponse : STRING
|
|
VAR
|
|
_sTmp : STRING(255);
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[xNewResponseReady := FALSE;
|
|
//MEMCPY(destAddr := ADR(_sTmp), ADR(_asReceivedResponse) + 2, n := INT_TO_UDINT(LEN(_asReceivedResponse) - 2));
|
|
M_GetResponse := _sTmp;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_SendCmd" Id="{d00a8c1a-c183-4659-9c9e-be5b30566b7c}">
|
|
<Declaration><![CDATA[METHOD M_SendCmd : BOOL
|
|
VAR_INPUT
|
|
sCmd : STRING(80);
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// Check if we are already sending a command
|
|
IF _xBusy OR _xError THEN
|
|
M_SendCmd := FALSE;
|
|
RETURN;
|
|
END_IF
|
|
|
|
// Increment command id
|
|
_uiCmdId := _uiCmdId + 1;
|
|
|
|
// Overflows at 9 and is reset to 1
|
|
IF _uiCmdId > 9 THEN
|
|
_uiCmdId := 1;
|
|
END_IF
|
|
|
|
// Create command with id
|
|
_sCmd := CONCAT(UINT_TO_STRING(_uiCmdId), '>');
|
|
_sCmd := CONCAT(_sCmd, sCmd);
|
|
|
|
// start sendind command state machine
|
|
_xSendCmd := TRUE;
|
|
|
|
M_SendCmd := TRUE;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
</POU>
|
|
</TcPlcObject> |