1、码垛机器人程序代码MODULE Module_stackingVAR num DN250:=0;VAR num DN300:=0;VAR num DN350:=0;VAR num DN400:=0;VAR num DN450:=0;VAR num DN500:=0;VAR num DN600:=0;VAR num DN700:=0;VAR num DN750:=0;VAR num DN800:=0;VAR num DN900:=0;VAR num DN1000:=0;VAR num singlelaxxxxyer_250:=4;VAR num singlelaxxxxyer_300:=4;VA
2、R num singlelaxxxxyer_350:=3;VAR num singlelaxxxxyer_400:=3;VAR num singlelaxxxxyer_450:=3;VAR num singlelaxxxxyer_500:=4;VAR num singlelaxxxxyer_600:=2;VAR num singlelaxxxxyer_700:=2;VAR num singlelaxxxxyer_750:=1;VAR num singlelaxxxxyer_800:=1;VAR num singlelaxxxxyer_900:=1;VAR num singlelaxxxxyer
3、_1000:=1;!VAR num Totallaxxxxyer_L_250:=5;VAR num Totallaxxxxyer_L_300:=5;VAR num Totallaxxxxyer_L_350:=5;VAR num Totallaxxxxyer_L_400:=5;VAR num Totallaxxxxyer_L_450:=4;VAR num Totallaxxxxyer_L_500:=4;VAR num Totallaxxxxyer_L_600:=4;VAR num Totallaxxxxyer_L_700:=4;VAR num Totallaxxxxyer_L_750:=4;VA
4、R num Totallaxxxxyer_L_800:=4;VAR num Totallaxxxxyer_L_900:=4;VAR num Totallaxxxxyer_L_1000:=4;!VAR num Totallaxxxxyer_R_250:=5;VAR num Totallaxxxxyer_R_300:=5;VAR num Totallaxxxxyer_R_350:=5;VAR num Totallaxxxxyer_R_400:=5;VAR num Totallaxxxxyer_R_450:=4;VAR num Totallaxxxxyer_R_500:=4;VAR num Tota
5、llaxxxxyer_R_600:=4;VAR num Totallaxxxxyer_R_700:=4;VAR num Totallaxxxxyer_R_750:=4;VAR num Totallaxxxxyer_R_800:=4;VAR num Totallaxxxxyer_R_900:=4;VAR num Totallaxxxxyer_R_1000:=4;!VAR bool One_number:=true;VAR bool two_number:=true;VAR num products_num:=0;VAR num One_num:=1;VAR num two_num:=1;PERS
6、 num nCouny_L:=0; PERS num nCouny_R:=0; PERS num nPickH:=500; PERS num nPlaceh:=500; PERS bool bPallet_L:=FALSE; PERS intnum INTN1;VAR intnum intno1:=0;VAR intnum intno2:=0;!VAR bool flag1:=FALSE;VAR bool flag2:=FALSE;VAR bool flag3:=FALSE;VAR bool flag4:=FALSE;VAR bool flag5:=FALSE;VAR bool flag6:=
7、FALSE;VAR bool flag7:=FALSE;VAR bool flag8:=FALSE;VAR bool flag9:=FALSE;VAR bool lock1:=true;VAR bool DI7_2UP_Down1:=FALSE;VAR bool DI4_1UP_Down1:=FALSE;! CHAN PIN ZHONG XIN JU! var num Line_1_array:=0;var num Line_2_array:=0;var num Line_Distance250 :=430;var num Line_Distance300 :=550;var num Line
8、_Distance350 :=430;var num Line_Distance400 :=0;var num Line_Distance450 :=600;var num Line_Distance500 :=430;var num Line_Distance600 :=430;var num Line_Distance700 :=430;var num Line_Distance750 :=430;var num Line_Distance800 :=430;var num Line_Distance900 :=430;var num Line_Distance1000 :=430;!FA
9、NG ZHI DIAN WEI!CONST speeddata speed1:=100050050001000;CONST speeddata speed2:=50050010001000;CONST speeddata speed3:=30050050001000;PROC main()rInitAll;MoveJ HOMEspeed1z100GripperTool1WObj:=wobj0;WHILE TRUE DODI7_DI4_INTERRUPT;option_program;stacking_program;ENDWHILEENDPROCPROC option_program()IF
10、DI10_3num=1 AND DI11_7num=1 AND DI12_5num=1 AND DI13_6num=1 THEN DN600:=21;flag1:=TRUE;flag4:=TRUE; TPWrite “DN600“;ELSE DN600:=0;flag1:=False;flag4:=False; endifIF DI10_3num=1 AND DI11_7num=1 AND DI12_5num=1 and flag1=FALSE THEN DN500:=15;flag2:=TRUE;flag4:=TRUE; TPWrite “DN500“;ELSE flag2:=False;
11、flag4:=False; DN500:=0;endif!IF DI11_7num=1 AND DI12_5num=1 AND flag1=FALSE AND flag2=FALSE THEN DN1000:=12;flag9:=TRUE;TPWrite “DN1000“;ELSE DN1000:=0;flag9:=False;endif!IF DI12_5num=1 AND DI13_6num=1 AND flag1=FALSE THEN DN900:=11;flag4:=TRUE;flag8:=TRUE;TPWrite “DN900“;ELSE DN900:=0;flag4:=False;
12、flag8:=False;endif!IF DI11_7num=1 AND DI13_6num=1 and flag1=False THEN DN800:=13;flag4:=TRUE;flag7:=TRUE;TPWrite “DN800“;ELSE DN800:=0;flag4:=False;flag7:=False;endifIF DI10_3num=1 AND DI13_6num=1 AND flag1=false THEN DN750:=9;flag6:=TRUE;flag4:=TRUE;TPWrite “DN750“;ELSE DN750:=0;flag6:=False;flag4:
13、=False;endifIF DI10_3num=1 AND DI12_5num=1 AND flag1=false AND flag2=false THEN DN700:=8;flag5:=TRUE; flag4:=TRUE;TPWrite “DN700“;ELSE DN700:=0; flag5:=False;flag4:=False;endifIF DI10_3num=1 AND DI11_7num=1 and flag1=FALSE AND flag2=FALSE THEN DN450:=10;flag4:=TRUE; TPWrite “DN450“; ELSE DN450:=0;fl
14、ag4:=False;endifIF DI13_6num=1 AND flag4=FALSE AND flag2=FALSE and flag1=FALSE AND flag6=False AND flag7=False AND flag8=False THEN DN400:=6;TPWrite “DN400“;ELSE DN400:=0;endifIF DI12_5num=1 AND flag4=FALSE AND flag2=FALSE and flag1=FALSE AND flag5=FALSE AND flag8=False AND flag9=False THEN DN350:=5
15、;TPWrite “DN350“;ELSE DN350:=0;endiFIF DI11_7num=1 AND flag4=FALSE AND flag2=FALSE and flag1=FALSE AND flag7=False AND flag9=False THEN DN300:=7;TPWrite “DN300“;ELSE DN300:=0;endifIF DI10_3num=1 AND flag4=FALSE AND flag2=FALSE and flag1=FALSE AND flag5=FALSE AND flag6=False THEN DN250:=3;TPWrite “DN
16、250“;ELSE DN250:=0;endifENDPROCPROC stacking_program()!DN250!WHILE DN250=3 DOrDN250; AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_250 AND One_number=TRUE
17、AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_250;lock1:=true;ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_250 AND two_number=TRUE AND DI15_start=0 and lock1
18、=false THENOne_number:=FALSE;place_2_250; lock1:=TRUE;ENDIF ENDWHILE!DN300!WHILE DN300=7 DO rDN300;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_300 AND On
19、e_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_300;lock1:=TRUE; ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_300 AND two_number=TRUE AND DI15_st
20、art=0 and lock1=false THENOne_number:=FALSE; place_2_300;lock1:=TRUE;ENDIF ENDWHILE!DN350!WHILE DN350=5 DOrDN350;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxye
21、r_L_350 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_350; lock1:=TRUE; ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_350 AND two_number=T
22、RUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_350;lock1:=TRUE;ENDIF ENDWHILE!DN400!WHILE DN400=6 DOrDN400;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI4_1UP_Down1=TRUE AND nCouny_L
23、 Totallaxxxxyer_L_400 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_400;lock1:=TRUE;ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI7_2UP_Down1=TRUE AND nCouny_RTotallaxxxxyer_R_400 AND t
24、wo_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_400;lock1:=TRUE;ENDIF ENDWHILE!DN450!WHILE DN450=10 DOrDN450;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI4_1UP_Down1=TRUE
25、AND nCouny_L Totallaxxxxyer_L_450 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_450; lock1:=TRUE; ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIFIF DI7_2UP_Down1=TRUE AND nCouny_RTotallaxxxxy
26、er_R_450 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_450;lock1:=TRUE; ENDIF ENDWHILE!DN500!WHILE DN500=15 DO rDN500;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI
27、4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_500 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_500;lock1:=TRUE; ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI7_2UP_Down1=TRUE AND nCo
28、uny_R Totallaxxxxyer_R_500 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_500;lock1:=TRUE; ENDIF ENDWHILE!DN600!WHILE DN600=21 DO rDN600;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:
29、=FALSE;ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_600 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_600;lock1:=TRUE;ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI7_2UP_D
30、own1=TRUE AND nCouny_R Totallaxxxxyer_R_600 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_600;lock1:=TRUE;ENDIF ENDWHILE!DN700!WHILE DN700=8 DO rDN700;AccSet 1010;VelSet 80 3000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Ma
31、terials; lock1:=FALSE;ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_700 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_700;lock1:=TRUE;ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;END
32、IF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_700 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_700;lock1:=TRUE;ENDIF ENDWHILE!DN750!WHILE DN750=9 DOrDN750;AccSet 1010;VelSet 80 3000; IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=tr
33、ue THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_750 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_750;lock1:=TRUE; ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials;
34、lock1:=FALSE;ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_750 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_750;lock1:=TRUE; ENDIF ENDWHILE!DN800!WHILE DN800=13 DO rDN800;AccSet 1010;VelSet 503000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_sta
35、rt=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_800 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_800;lock1:=TRUE; ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN
36、 Pick_Materials; lock1:=FALSE;ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_800 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_800;lock1:=TRUE; ENDIF ENDWHILE!DN900!WHILE DN900=11 DO rDN900;AccSet 1010;VelSet 503000;IF DI1_Prepare=1 AND DI3_conveyor_ST
37、OP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_900 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_900; lock1:=TRUE;ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AN
38、D lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_900 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_900;lock1:=TRUE;ENDIF ENDWHILE!DN1000!WHILE DN1000=12 DOrDN1000;AccSet 1010;VelSet 503000;IF DI1_Prepare=1 A
39、ND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_1000 AND One_number=TRUE AND DI15_start=0 and lock1=false THEN two_number:=FALSE;place_1_1000; lock1:=TRUE;ENDIF IF DI1_Prepare=1 AND DI3_conveyor_STOP=1
40、 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE;ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_1000 AND two_number=TRUE AND DI15_start=0 and lock1=false THENOne_number:=FALSE;place_2_1000;lock1:=TRUE;ENDIF ENDWHILEENDPROCPROC place_1_250()rPosition1_250;MoveL Offs(Place
41、100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;rPlaceRD1_250;ENDPROCPROC place_1_300(
42、)rPosition1_300;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;rPlace
43、RD1_300;ENDPROCPROC place_1_350()rPosition1_350;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME
44、 speed1 z100 GripperTool1;rPlaceRD1_350;ENDPROCPROC place_1_400()rPosition1_400;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 spe
45、ed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;rPlaceRD1_400;ENDPROCPROC place_1_450()rPosition1_450;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 Gripper
46、Tool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;rPlaceRD1_450;ENDPROCPROC place_1_500()rPosition1_500;MoveJ Place_a_1 speed2 z100 GripperTool1;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;Re
47、Set DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;rPlaceRD1_500;ENDPROCPROC place_1_600()rPosition1_600;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3
48、 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;rPlaceRD1_600;ENDPROCPROC place_1_700()rPosition1_700;MoveL Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveL Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ HOME speed1 z100 GripperTool1;
