Initial
This commit is contained in:
368
PLC/0 General/POUs/KeyenceCamera_FB.TcPOU
Normal file
368
PLC/0 General/POUs/KeyenceCamera_FB.TcPOU
Normal file
@@ -0,0 +1,368 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4026.13">
|
||||
<POU Name="KeyenceCamera_FB" Id="{e04efa2a-35e2-41b0-8dcf-22b085953a3c}" SpecialFunc="None">
|
||||
<Declaration><![CDATA[FUNCTION_BLOCK KeyenceCamera_FB EXTENDS Peripherie_FB
|
||||
VAR_INPUT
|
||||
Trigger AT %Q* : BOOL;
|
||||
Result_Ack AT %Q* : BOOL;
|
||||
|
||||
Go_to_Run AT %Q* : BOOL;
|
||||
Go_to_Setting AT %Q* : BOOL;
|
||||
|
||||
rOutProgramNumber AT %Q* : UDINT;
|
||||
bOutProgramLoadExecute AT %Q* : BOOL;
|
||||
END_VAR
|
||||
VAR_OUTPUT
|
||||
CameraInputData : ARRAY [0..288] OF BYTE;
|
||||
|
||||
ResultData128 AT %I* : ARRAY [0..127] OF BYTE;
|
||||
ResultData32 AT %I* : ARRAY [1..5] OF ARRAY [0..31] OF BYTE;
|
||||
|
||||
ToolJudge AT %I* : ARRAY[0..3] OF BYTE;
|
||||
ToolJudgeArray AT %I* : ARRAY[0..3] OF ARRAY[0..7] OF BOOL;
|
||||
|
||||
Trigger_Ready AT %I* : BOOL;
|
||||
Trigger_Ack AT %I* : BOOL;
|
||||
Run_Status AT %I* : BOOL;
|
||||
Result_Ready AT %I* : BOOL;
|
||||
|
||||
rInProgramNumber AT %I* : UDINT;
|
||||
bOutProgramLoadComplete AT %I* : BOOL;
|
||||
|
||||
bOutBusy : BOOL;
|
||||
END_VAR
|
||||
VAR
|
||||
i : LWORD;
|
||||
END_VAR
|
||||
VAR CONSTANT
|
||||
SizeDetectionDataSize : USINT := 15;
|
||||
MappingDataSize : SINT := 79;
|
||||
END_VAR]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[MEMMOVE(ADR(CameraInputData), ADR(ResultData128), SIZEOF(ResultData128));
|
||||
|
||||
FOR i := 1 TO 5 DO
|
||||
MEMMOVE(ADR(CameraInputData)+128+(i-1)*32, ADR(ResultData32[i]), SIZEOF(ResultData32[i]));
|
||||
END_FOR]]></ST>
|
||||
</Implementation>
|
||||
<Method Name="DMC" Id="{823d2044-934c-4f9a-bd4c-2fe6af4ff611}">
|
||||
<Declaration><![CDATA[METHOD DMC : BOOL
|
||||
VAR_INPUT
|
||||
Size : USINT;
|
||||
Data : REFERENCE TO T_MaxString;
|
||||
END_VAR
|
||||
VAR_INST
|
||||
Step : INT;
|
||||
Counter : INT;
|
||||
|
||||
ManualData : T_MaxString;
|
||||
END_VAR
|
||||
VAR CONSTANT
|
||||
cProgram : UDINT := 400;
|
||||
END_VAR]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[CASE Step OF
|
||||
0:
|
||||
IF bOutBusy THEN
|
||||
RETURN;
|
||||
END_IF
|
||||
|
||||
bOutBusy := TRUE;
|
||||
Step := 10;
|
||||
Counter := 0;
|
||||
10:
|
||||
IF RunProgram(cProgram+Size) THEN
|
||||
Step := 20;
|
||||
END_IF
|
||||
20:
|
||||
CASE CameraInputData[28] OF
|
||||
0:
|
||||
Counter := Counter +1;
|
||||
IF Counter > 3 THEN
|
||||
DMC := TRUE;
|
||||
Step := 0;
|
||||
bOutBusy := FALSE;
|
||||
//Step := 110;
|
||||
ELSE
|
||||
Step := 10;
|
||||
END_IF
|
||||
1:
|
||||
Step := 30;
|
||||
END_CASE
|
||||
30:
|
||||
Data := '';
|
||||
MEMMOVE(ADR(Data), ADR(CameraInputData[30]), CameraInputData[29]);
|
||||
DMC := TRUE;
|
||||
Step := 0;
|
||||
bOutBusy := FALSE;
|
||||
40:
|
||||
Data := ManualData;
|
||||
DMC := TRUE;
|
||||
bOutBusy := FALSE;
|
||||
Step := 0;
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="GetCurrentCameraPosition" Id="{7a36b66a-c0d0-4d66-a705-d59bc7557dd3}">
|
||||
<Declaration><![CDATA[METHOD GetCurrentCameraPosition : ARRAY[0..MaxNumberOfSubstratesPerTray-1] OF ARRAY[0..1] OF UINT;
|
||||
VAR_INPUT
|
||||
END_VAR
|
||||
VAR
|
||||
i : INT;
|
||||
DummyData : ARRAY[0..1] OF BYTE;
|
||||
END_VAR
|
||||
VAR_INST
|
||||
Data : ARRAY[0..MaxNumberOfSubstratesPerTray*4-1] OF BYTE;
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[MEMMOVE(ADR(Data), ADR(CameraInputData[29]), SIZEOF(Data));
|
||||
|
||||
FOR i := 0 TO MaxNumberOfSubstratesPerTray-1 DO
|
||||
DummyData[0] := Data[(i)*4+1];
|
||||
DummyData[1] := Data[(i)*4];
|
||||
|
||||
MEMMOVE(ADR(GetCurrentCameraPosition[i][0]), ADR(DummyData), SIZEOF(DummyData));
|
||||
|
||||
DummyData[0] := Data[(i)*4+3];
|
||||
DummyData[1] := Data[(i)*4+2];
|
||||
|
||||
MEMMOVE(ADR(GetCurrentCameraPosition[i][1]), ADR(DummyData), SIZEOF(DummyData));
|
||||
END_FOR]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="Mapping" Id="{e769376f-5b47-48bb-b89d-28e7c6e5ce78}">
|
||||
<Declaration><![CDATA[METHOD Mapping : BOOL
|
||||
VAR_INPUT
|
||||
Size : USINT;
|
||||
Out : REFERENCE TO ARRAY[1..MaxNumberOfSubstratesPerTray] OF BYTE;
|
||||
END_VAR
|
||||
|
||||
VAR_INST
|
||||
Step : INT;
|
||||
Data : ARRAY[0..MaxNumberOfSubstratesPerTray*4-1] OF BYTE;
|
||||
NumberOfSubstrateDetected : SINT;
|
||||
|
||||
MappingArrayPosition : ARRAY[1..MaxNumberOfSubstratesPerTray] OF ARRAY[0..1] OF UINT;
|
||||
|
||||
MaxNumberOfWafer : INT;
|
||||
|
||||
_Size : USINT;
|
||||
END_VAR
|
||||
VAR
|
||||
i,j : INT;
|
||||
DummyData : ARRAY[0..1] OF BYTE;
|
||||
|
||||
X, Y : DINT;
|
||||
END_VAR
|
||||
VAR CONSTANT
|
||||
cProgram : UDINT := 100;
|
||||
END_VAR]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[CASE Step OF
|
||||
0:
|
||||
_Size := Size;
|
||||
IF bOutBusy THEN
|
||||
RETURN;
|
||||
END_IF
|
||||
bOutBusy := TRUE;
|
||||
Step := 20;
|
||||
20:
|
||||
IF _Size > 0 THEN
|
||||
IF RunProgram(cProgram+_Size) THEN
|
||||
Step := 30;
|
||||
END_IF
|
||||
END_IF
|
||||
30:
|
||||
FOR i := 1 TO MaxNumberOfSubstratesPerTray DO
|
||||
Out[i] := 0;
|
||||
END_FOR
|
||||
|
||||
NumberOfSubstrateDetected := BYTE_TO_SINT(CameraInputData[28]);
|
||||
MaxNumberOfWafer := ROBOT_MAIN.Config.SubstrateInformation[_Size].MaxNumber;
|
||||
|
||||
IF NumberOfSubstrateDetected = MaxNumberOfWafer THEN
|
||||
FOR i := 1 TO NumberOfSubstrateDetected DO
|
||||
Out[i] := 1;
|
||||
END_FOR
|
||||
Step := 100;
|
||||
ELSIF NumberOfSubstrateDetected > 0 THEN
|
||||
Step := 40;
|
||||
ELSE
|
||||
Step := 100;
|
||||
END_IF
|
||||
40:
|
||||
MappingArrayPosition := GetCurrentCameraPosition();
|
||||
MaxNumberOfWafer := ROBOT_MAIN.Config.SubstrateInformation[_Size].MaxNumber;
|
||||
i := 1;
|
||||
Step := 50;
|
||||
50:
|
||||
IF i > MaxNumberOfWafer THEN
|
||||
Step := 100;
|
||||
ELSE
|
||||
Step := 60;
|
||||
END_IF
|
||||
60:
|
||||
FOR j := 1 TO NumberOfSubstrateDetected DO
|
||||
X := UINT_TO_DINT(CASSETTE_MAIN[FirstCassette].Config.TeachPosition[_Size].Position[i].Camera[0]) - UINT_TO_DINT(MappingArrayPosition[j][0]);
|
||||
Y := UINT_TO_DINT(CASSETTE_MAIN[FirstCassette].Config.TeachPosition[_Size].Position[i].Camera[1]) - UINT_TO_DINT(MappingArrayPosition[j][1]);
|
||||
IF ABS(X) < 50 AND ABS(Y) < 50 THEN
|
||||
Out[i] := 1;
|
||||
EXIT;
|
||||
END_IF
|
||||
END_FOR
|
||||
i := i +1;
|
||||
Step := 50;
|
||||
100:
|
||||
Mapping := TRUE;
|
||||
Step := 0;
|
||||
bOutBusy := FALSE;
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="RunProgram" Id="{cd0e6869-d425-4e3a-8113-1920b8b06cec}">
|
||||
<Declaration><![CDATA[METHOD RunProgram : BOOL
|
||||
VAR_INPUT
|
||||
In : UDINT;
|
||||
END_VAR
|
||||
VAR_INST
|
||||
Step : INT;
|
||||
END_VAR]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[CASE Step OF
|
||||
0:
|
||||
Go_to_Run := TRUE;
|
||||
IF Run_Status THEN
|
||||
Go_to_Run := FALSE;
|
||||
Step := 10;
|
||||
END_IF
|
||||
10:
|
||||
rOutProgramNumber := In;
|
||||
IF rInProgramNumber = rOutProgramNumber THEN
|
||||
Step := 20;
|
||||
ELSE
|
||||
Step := Step +1;
|
||||
END_IF
|
||||
11:
|
||||
bOutProgramLoadExecute := TRUE;
|
||||
IF bOutProgramLoadComplete THEN
|
||||
bOutProgramLoadExecute := FALSE;
|
||||
Step := 10;
|
||||
END_IF
|
||||
20:
|
||||
IF Trigger_Ready THEN
|
||||
Trigger := TRUE;
|
||||
Step := Step +1;
|
||||
END_IF
|
||||
21:
|
||||
IF Trigger_Ack THEN
|
||||
Trigger := FALSE;
|
||||
Step := Step +1;
|
||||
END_IF
|
||||
22:
|
||||
IF Result_Ready THEN
|
||||
Result_Ack := TRUE;
|
||||
Step := Step +1;
|
||||
END_IF
|
||||
23:
|
||||
IF NOT Result_Ready THEN
|
||||
Result_Ack := FALSE;
|
||||
Step := 30;
|
||||
END_IF
|
||||
30:
|
||||
RunProgram := TRUE;
|
||||
Step := 0;
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="SizeDetection" Id="{ef8e9d1a-4440-4fe2-97d1-cebcaf436cc6}">
|
||||
<Declaration><![CDATA[METHOD SizeDetection : BOOL
|
||||
VAR CONSTANT
|
||||
cProgram : UDINT := 1;
|
||||
END_VAR
|
||||
VAR_INPUT
|
||||
Stop : BOOL := FALSE;
|
||||
END_VAR
|
||||
VAR_OUTPUT
|
||||
Out : USINT;
|
||||
END_VAR
|
||||
VAR_INST
|
||||
Step : INT;
|
||||
Data : ARRAY[0..SizeDetectionDataSize] OF BYTE;
|
||||
Found : BOOL;
|
||||
Counter : INT;
|
||||
|
||||
SizeFound : USINT;
|
||||
END_VAR
|
||||
VAR
|
||||
i : USINT;
|
||||
j : SINT;
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[IF Stop THEN
|
||||
Step := 0;
|
||||
RETURN;
|
||||
END_IF
|
||||
|
||||
CASE Step OF
|
||||
0:
|
||||
IF bOutBusy THEN
|
||||
RETURN;
|
||||
END_IF
|
||||
bOutBusy := TRUE;
|
||||
|
||||
Step := 10;
|
||||
SizeFound := 0;
|
||||
Found := FALSE;
|
||||
Counter := 0;
|
||||
10:
|
||||
IF Counter < 3 THEN
|
||||
Step := 20;
|
||||
ELSE
|
||||
Step := 110;
|
||||
END_IF
|
||||
20:
|
||||
IF RunProgram(cProgram) THEN
|
||||
Step := 30;
|
||||
END_IF
|
||||
30:
|
||||
MEMMOVE(ADR(Data), ADR(CameraInputData[28]), SIZEOF(Data));
|
||||
|
||||
FOR i := 0 TO SizeDetectionDataSize DO
|
||||
FOR j := 0 TO 7 DO
|
||||
IF GETBIT32(Data[i], j) AND NOT Found THEN
|
||||
SizeFound := i * 8 + SINT_TO_USINT(j) + 1;
|
||||
Found := TRUE;
|
||||
ELSIF GETBIT32(Data[i], j) AND Found THEN
|
||||
SizeFound := 0;
|
||||
Counter := Counter +1;
|
||||
Step := 10;
|
||||
RETURN;
|
||||
END_IF
|
||||
END_FOR
|
||||
END_FOR
|
||||
|
||||
IF Found THEN
|
||||
Step := 40;
|
||||
ELSE
|
||||
Counter := Counter +1;
|
||||
Step := 10;
|
||||
END_IF
|
||||
40:
|
||||
Out := SizeFound;
|
||||
SizeDetection := TRUE;
|
||||
Step := 0;
|
||||
|
||||
bOutBusy := FALSE;
|
||||
110:
|
||||
SizeDetection := TRUE;
|
||||
Out := 0;
|
||||
Step := 0;
|
||||
|
||||
bOutBusy := FALSE;
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
</POU>
|
||||
</TcPlcObject>
|
||||
Reference in New Issue
Block a user