码垛机器人程序代码.docx

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_

2、300:=4;VAR 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 single

3、laxxxxyer_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

4、_750:=4;VAR 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;V

5、AR num Totallaxxxxyer_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_

6、num:=1;PERS 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 b

7、ool flag6:=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;v

8、ar num Line_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_Distance100

9、0 :=430;!FANG 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_p

10、rogram() IF 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; endif IF DI10_3num=1 AND DI11_7num=1 AND DI12_5num=1 and flag1=FALSE THEN DN500:=15; flag2:=TRUE; flag4:=TRUE; TPWrite DN500; EL

11、SE flag2:=False; 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; EL

12、SE DN900:=0; flag4:=False; 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

13、; ELSE DN750:=0; flag6:=False; flag4:=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:

14、=TRUE; TPWrite DN450; ELSE DN450:=0; flag4:=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 fl

15、ag8=False AND flag9=False THEN DN350:=5;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 f

16、lag6=False THEN DN250:=3; TPWrite DN250; ELSE DN250:=0;endifENDPROCPROC stacking_program() !DN250!WHILE DN250=3 DO rDN250; 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 DI4_1UP_Down1=TRUE AND nCouny_L

17、Totallaxxxxyer_L_250 AND One_number=TRUE 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; ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_250

18、AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_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; ENDIF IF DI4_1UP

19、_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_300 AND One_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; ENDIF IF DI7_2UP_Down1=TRUE AND nCoun

20、y_R Totallaxxxxyer_R_300 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_number:=FALSE; place_2_300; lock1:=TRUE; ENDIF ENDWHILE!DN350! WHILE DN350=5 DO rDN350;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1

21、:=FALSE; ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_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; ENDIF IF DI7

22、_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_350 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_number:=FALSE; place_2_350; lock1:=TRUE; ENDIF ENDWHILE !DN400!WHILE DN400=6 DO rDN400;AccSet 1010;VelSet 1003000;IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI15_start=1 AND lock1=true T

23、HEN Pick_Materials; lock1:=FALSE; ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L 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; l

24、ock1:=FALSE; ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_RTotallaxxxxyer_R_400 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_number:=FALSE; place_2_400; lock1:=TRUE; ENDIF ENDWHILE !DN450!WHILE DN450=10 DO rDN450;AccSet 1010;VelSet 1003000; IF DI1_Prepare=1 AND DI3_conveyor_STOP=1 AND DI1

25、5_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE; ENDIF IF DI4_1UP_Down1=TRUE 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=t

26、rue THEN Pick_Materials; lock1:=FALSE; ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_RTotallaxxxxyer_R_450 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_number:=FALSE; place_2_450; lock1:=TRUE; ENDIF ENDWHILE !DN500!WHILE DN500=15 DO rDN500; AccSet 1010;VelSet 1003000; IF DI1_Prepare=1 AND

27、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_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 A

28、ND DI15_start=1 AND lock1=true THEN Pick_Materials; lock1:=FALSE; ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_500 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_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:=FALSE; ENDIF IF DI4_1UP_Down1=TRUE AND nCouny_L Totallaxxxxyer_L_600 AND One_number