First bigger step to automation
- 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
This commit is contained in:
@@ -32,13 +32,14 @@ VAR
|
||||
_fbSocketReceive : FB_SocketReceive;
|
||||
_timReceiveTimeout : TIME := T#5S;
|
||||
_abReceivedBuffer : ARRAY[0..100] OF BYTE;
|
||||
_sReceivedResponse : STRING;
|
||||
_asReceivedResponse : ARRAY[0..9] OF STRING;
|
||||
_uiBufferPos : UINT;
|
||||
_udiCounterReceive : UDINT;
|
||||
_udiReceivedBytes : UDINT;
|
||||
_timPollingTime : TIME := T#50MS;
|
||||
_timPollingTime : TIME := T#1S;
|
||||
_tonPollTimer : TON;
|
||||
_xEnableReceiveTimeout : BOOL;
|
||||
_timReceiveTimeoutTime : TIME := T#500MS;
|
||||
_timReceiveTimeoutTime : TIME := T#5S;
|
||||
_tonReceiveTimeout : TON;
|
||||
_uiLastReceivedResponseId : UINT := 0;
|
||||
|
||||
@@ -141,194 +142,369 @@ CASE _iStateReceive OF
|
||||
// Check if we are still connected
|
||||
IF (NOT _xConnected) THEN
|
||||
_tonPollTimer(IN := FALSE);
|
||||
_sReceivedResponse := '';
|
||||
//_asReceivedResponse[] := '';
|
||||
_iStateReceive := 0;
|
||||
END_IF
|
||||
|
||||
// Check received data
|
||||
20:
|
||||
// Check if received command is complete
|
||||
MEMCPY(destAddr := ADR(_sReceivedResponse), srcAddr := ADR(_abReceivedBuffer), n := _udiReceivedBytes);
|
||||
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;
|
||||
_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
|
||||
// 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 := 90;
|
||||
END_IF
|
||||
|
||||
// For for response indicator
|
||||
IF _sReceivedResponse[1] <> F_ToASC('<') THEN
|
||||
// Wrong response indicator
|
||||
_iState := 90;
|
||||
END_IF
|
||||
|
||||
// Check for correct cmd
|
||||
_xReceivedResponseOk := TRUE;
|
||||
FOR _diCounter := 2 TO (LEN(_sReceivedResponse) - 1) DO
|
||||
IF _sReceivedResponse[_diCounter] = F_ToASC(':') THEN
|
||||
EXIT;
|
||||
ELSIF _sReceivedResponse[_diCounter] <> _sCmd[_diCounter] THEN
|
||||
_xReceivedResponseOk := FALSE;
|
||||
EXIT;
|
||||
END_IF
|
||||
END_FOR
|
||||
|
||||
IF _xReceivedResponseOk THEN
|
||||
_iState := 10;
|
||||
ELSE
|
||||
_iState := 90;
|
||||
END_IF
|
||||
|
||||
50:
|
||||
// Get command id
|
||||
_uiReceivedCommandId := STRING_TO_UINT(TO_STRING(_sReceivedResponse[0]));
|
||||
|
||||
// Check if it is a new command
|
||||
IF (_uiReceivedCommandId <> _uiLastReceivedResponseId) OR (_uiReceivedCommandId = 0) THEN
|
||||
_uiLastReceivedResponseId := _uiReceivedCommandId;
|
||||
xNewResponseReady := TRUE;
|
||||
|
||||
// Send acknowledgement
|
||||
_iState := 60;
|
||||
END_IF
|
||||
|
||||
// Prepare ack response
|
||||
60:
|
||||
_sAck := CONCAT(UINT_TO_STRING(_uiLastReceivedResponseId), '<');
|
||||
|
||||
FOR _diCounter := 2 TO (LEN(_sReceivedResponse) - 1) DO
|
||||
IF _sReceivedResponse[_diCounter] <> F_ToASC(':') THEN
|
||||
_sAck[_diCounter] := _sReceivedResponse[_diCounter];
|
||||
ELSE
|
||||
_sAck[_diCounter] := F_ToASC('\0');
|
||||
EXIT;
|
||||
END_IF
|
||||
END_FOR
|
||||
|
||||
_iState := 70;
|
||||
|
||||
// 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;
|
||||
_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;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
END_CASE
|
||||
//
|
||||
// // 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
|
||||
@@ -337,7 +513,7 @@ END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[xNewResponseReady := FALSE;
|
||||
MEMCPY(destAddr := ADR(_sTmp), ADR(_sReceivedResponse) + 2, n := INT_TO_UDINT(LEN(_sReceivedResponse) - 2));
|
||||
//MEMCPY(destAddr := ADR(_sTmp), ADR(_asReceivedResponse) + 2, n := INT_TO_UDINT(LEN(_asReceivedResponse) - 2));
|
||||
M_GetResponse := _sTmp;]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
|
||||
@@ -15,7 +15,7 @@ VAR
|
||||
|
||||
_xTest : BOOL;
|
||||
_xSendResult : BOOL;
|
||||
_sCmd : STRING := 'STAT-FEED:';
|
||||
_sCmd : STRING := 'STAT-FEED';
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
|
||||
Reference in New Issue
Block a user