码垛机器人程序代码.doc

码垛机器人程序代码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;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 singlelaxxxxyer_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;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;VAR 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_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 bool 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;var 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_Distance1000 :=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_program() 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; ELSE 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; ELSE 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; 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:=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 flag8=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 flag6=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 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 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_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 nCouny_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:=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_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 THEN 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; lock1:=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 DI15_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=true 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 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 AND 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=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_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_600 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_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_Materials; 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; ENDIF IF DI7_2UP_Down1=TRUE AND nCouny_R Totallaxxxxyer_R_700 AND two_number=TRUE AND DI15_start=0 and lock1=false THEN One_number:=FALSE; place_2_700; lock1:=TRUE; ENDIF ENDWHILE !DN750!WHILE DN750=9 DO rDN750; AccSet 1010;VelSet 80 3000; 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_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; 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 THEN One_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_start=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 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 THEN One_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_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_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 AND 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 THEN One_number:=FALSE; place_2_900; lock1:=TRUE; ENDIF ENDWHILE !DN1000!WHILE DN1000=12 DO rDN1000;AccSet 1010;VelSet 503000; 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_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 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 THEN One_number:=FALSE; place_2_1000; lock1:=TRUE; ENDIF ENDWHILEENDPROCPROC place_1_250()rPosition1_250;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;rPlaceRD1_250;ENDPROCPROC place_1_300()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;rPlaceRD1_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 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 speed1 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 GripperTool1WObj:=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;ReSet 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 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;rPlaceRD1_700;ENDPROCPROC place_1_750()rPosition1_750;MoveJ Offs(Place100nPlaceh) speed2 z100 GripperTool1WObj:=wobj0;MoveL Place1 speed3 fine GripperTool1WObj:=wobj0;ReSet DO2_safety;WaitTime 2;MoveJ Offs(Place100nPlaceh) speed3 z100 GripperTool1WObj:=wobj0;MoveJ put22 speed1 z100 GripperTool1;MoveJ