ABB机器人程序

ABB机器人程序 MODULE Vanta_pallet VAR intnum intno1:=0; CONST robtarget pHome40:=[[1410.50,527.60,1383.49],[6.53276E-07,-0.781668,-0.623695,1.68587E-06],[0,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pmid:=[[-1770.77,-1253.97,1386.90],[1.81232E-06,-0.607633,0.794219,-1.47514E-07],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pmid10:=[[-1770.79,-1253.96,1759.42],[1.81232E-06,-0.607633,0.794219,-1.68587E-07],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pmid20:=[[745.82,-2037.62,1759.41],[1.52782E-06,-0.9651,0.261881,9.69378E-07],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pmid30:=[[1919.54,-1011.69,1759.41],[1.08528E-06,-0.994758,-0.102263,1.47514E-06],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait12:=[[488.94,-1314.87,-109.65],[0.118948,-0.000182856,-0.992901,0.000469886],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait22:=[[-254.56,530.61,-210.33],[0.00687179,-0.255751,0.966718,-0.000773702],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait13:=[[583.03,-1819.56,-135.45],[0.0849866,-0.692015,-0.712016,0.0832237],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pttt100:=[[2077.61,551.13,-347.02],[1.64373E-06,-0.11121,-0.993797,7.58644E-07],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pPrePlace11:=[[472.56,-30.75,0.15],[0.999081,-0.000118137,-5.01386E-05,-0.0428662],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p11:=[[473.54,-21.94,-1531.41],[0.99908,-0.000118134,-5.01422E-05,-0.0428963],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p21:=[[473.37,-21.62,10.64],[0.99908,-0.000118133,-5.00999E-05,-0.0428952],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p31:=[[446.00,-85.17,-1212.11],[0.999826,-0.000119311,-4.72646E-05,-0.0186837],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait10:=[[-130.34,-1220.86,172.44],[0.00404674,-0.2574,-0.966297,-0.00012764],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait14:=[[-92.45,-1658.15,-8.11],[0.0847627,0.699253,-0.70491,-0.0834517],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait20:=[[-130.34,-1220.86,172.44],[0.00404674,-0.2574,-0.966297,-0.00012764],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait21:=[[-130.34,-1220.86,172.44],[0.00404674,-0.2574,-0.966297,-0.00012764],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait30:=[[-130.34,-1220.86,172.44],[0.00404674,-0.2574,-0.966297,-0.00012764],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait31:=[[-153.77,290.22,-2.69],[0.00107588,0.718883,-0.695099,-0.00663341],[-2,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget pWait32:=[[-41.26,8.82,8.23],[0.00619536,-0.656898,-0.753954,0.000453594],[-2,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait10:=[[2035.28,-45.29,1843.95],[4.05664E-07,0.989492,-0.144587,-1.72802E-06],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait11:=[[2044.37,-909.47,1684.51],[1.77017E-06,-0.284016,-0.95882,-4.42542E-07],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait20:=[[0.08,17.21,-55.95],[5.85764E-05,-0.72154,-0.692374,-1.48442E-05],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait21:=[[1205.91,-1574.10,1898.81],[1.55943E-06,-0.978384,-0.206796,9.27231E-07],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait30:=[[1656.12,-1745.24,1540.86],[1.81232E-06,-0.777438,-0.62896,8.42937E-08],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait31:=[[730.42,2165.79,1816.74],[1.68587E-06,0.640527,-0.767936,7.58644E-07],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget wait23:=[[2370.50,-60.18,1833.54],[1.55943E-06,-0.543105,-0.839666,9.27231E-07],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; VAR num speed1:=30; CONST robtarget pWait15:=[[523.37,-978.71,-34.74],[0.0844961,0.701501,-0.702673,-0.0837217],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; PROC Main() SpeedRefresh 40; MotionSup\On\TuneValue:=250; VelSet 30, 1500; rInitAll; AccSet 40, 40; TPReadNum count1, "place1 num is "; TPReadNum count2, "place2 num is "; TPReadNum count3, "place3 num is "; rEjectPal; WaitUntil di04_RqPick1 = 0 OR di06_p2 = 1 OR di05_p1 = 1; IF di04_RqPick1 = 0 THEN MoveJ wait10, vMaxEmpty, z15, tGripper; ELSEIF di05_p1 = 1 THEN MoveJ wait20, vMaxEmpty, z15, tGripper; ELSEIF di06_p2 = 1 THEN MoveJ wait30, vMaxEmpty, z15, tGripper; ENDIF WHILE TRUE DO ClkReset clock1; ClkStart clock1; !IF bLoadOnGriper = FALSE THEN rPickPart; !ENDIF !IF bLoadOnGriper = TRUE THEN !rPlacePart; !ENDIF ClkStop clock1; TPWrite "CT "\Num:=ClkRead(clock1); !Stop; ENDWHILE ENDPROC PROC rInitAll() rMoveHome; ClampOpen; ClkStop Timer; ClkReset Timer; ConfJ\off; ConfL\off; nInput:=0; !ncode nPassword:=0; nCycleTime:=0; VelSet 90,5000; AccSet 90,90; ENDPROC PROC rEjectPal() IF count1 >= 9 THEN count1:= 0; PulseDO\PLength:=0.2, do06_FullPallet1; ENDIF IF count2 >= 9 THEN count2:=0; PulseDO\PLength:=0.2, do05_RdyPick2; ENDIF IF count3 >= 9 THEN count3:=0; PulseDO\PLength:=0.2, do04_RdyPick1; ENDIF ENDPROC PROC rPickPart() WaitUntil di05_p1 = 1 OR di06_p2 = 1 OR di04_RqPick1 = 0; IF di04_RqPick1 = 0 THEN rPickPart1; ELSEIF di05_p1 = 1 THEN rPickPart2; ELSEIF di06_p2 = 1 THEN rPickPart3; ENDIF ENDPROC PROC rPickPart1() MoveJ pWait1, vMidEmpty, z15, tGripper\WObj:=WPick_1; MoveJ Offs(pPick1_1,0,0,0), vMidEmpty, z15, tGripper\WObj:=WPick_1; WaitTime 2; ClampClose; WaitTime 2; ConfL\Off; MoveL Offs(pPick1_1,0,100 - 200,nBoxH1 * 1.5), vMidLoad, fine, tGripper\WObj:=WPick_1; GripLoad LoadBox; !WaitTime 0.5; GripLoad LoadBox; MoveL Offs(pPick1_1,200 - 1200,-368,500), vMaxLoad, z50, tGripper\WObj:=WPick_1; bLoadOnGriper := TRUE; COU1; IF jiou = 0 THEN rPlace1; ELSEIF jiou = 1 THEN rPlace3; ENDIF ENDPROC PROC rPickPart2() MoveJ pWait2, vMidEmpty, z20, tGripper\WObj:=WPick_2; Wait; MoveL Offs(pPick2_1,0,0,0), vMidEmpty, z10, tGripper\WObj:=WPick_2; WaitTime 2; ClampClose; WaitTime 2; ConfL\Off; MoveL Offs(pPick2_1,0,0,nBoxH1 * 2), vMidLoad, z10, tGripper\WObj:=WPick_2; GripLoad LoadBox; MoveL Offs(pPick2_1,-500,-1500,600), vMaxLoad, z10, tGripper\WObj:=WPick_2; bLoadOnGriper := TRUE; COU2; rPlace2; ENDPROC PROC rPickPart3() MoveJ pWait3, vMidEmpty, z50, tGripper\WObj:=WPick_3; Wait; MoveL Offs(pPick3_1,0,0,0), vMidEmpty, z50, tGripper\WObj:=WPick_3; WaitTime 2; ClampClose; WaitTime 2; ConfL\Off; ConfJ\Off; MoveL Offs(pPick3_1,0,0,-nBoxH1 * 3), vMidEmpty, z15, tGripper\WObj:=WPick_3; MoveJ Offs(pPick3_1,-900,-1800,-600), vMaxLoad, z100, tGripper\WObj:=WPick_3; bLoadOnGriper := TRUE; COU3; rPlace3; ENDPROC PROC Wait() WaitDI di11_Clamp_close, 1; WaitDI di16_up, 0; ENDPROC PROC GripperOpen() Set do15; Set do16; ENDPROC PROC GripperClose() Reset do12; Reset do15; Reset do14_GripperDown; Reset do09; Set do11_GripperClose; Set do08; Set do13; Set do07; PulseDO\PLength:=0.5, do07; ENDPROC PROC GripperDown() Reset do07; ENDPROC PROC GripperUp() Reset do12; !DIWait di07_GripperUp, 1, 3, "Cylinder GripperUp", "Upside sensor-> 1"; ENDPROC PROC ClampOpen() Reset do12; Reset do07; Reset do13; Reset do09; Set do11_GripperClose; Set do08; ENDPROC PROC ClampOpen1() Reset do12; Reset do13; Set do11_GripperClose; ENDPROC PROC ClampOpen2() Reset do07; Reset do09; Set do08; ENDPROC PROC ClampClose() Set do12; Set do13; WaitTime 1; Reset do15; Reset do16; ENDPROC PROC rMoveHome() VAR string HomeOffset; CONST num MinX:=-50; CONST num MaxX:=50; CONST num MinY:=-50; CONST num MaxY:=50; CONST num MinZ:=-50; CONST num MaxZ:=50; VelSet 100,500; IF bCurrentPos(pHome,tGripper)=FALSE THEN ! If no known position is found, check if the robot is in a specified ! window and move him to the first position in the program ActualPos:=CRobT(\Tool:=tGripper\WObj:=wobj0); IF ActualPos.trans.xMaxX OR ActualPos.trans.yMaxY OR ActualPos.trans.zMaxZ THEN HomeOffset:=""; IF ActualPos.trans.xMaxX THEN HomeOffset:=HomeOffset+"X :"+NumToStr(MaxX-ActualPos.trans.x,0)+" "; ELSE HomeOffset:=HomeOffset+"X : OK "; ENDIF IF ActualPos.trans.yMaxY THEN HomeOffset:=HomeOffset+"Y :"+NumToStr(MaxY-ActualPos.trans.y,0)+" "; ELSE HomeOffset:=HomeOffset+"Y : OK "; ENDIF IF ActualPos.trans.zMaxZ THEN HomeOffset:=HomeOffset+"Z :"+NumToStr(MaxZ-ActualPos.trans.z,0)+" "; ELSE HomeOffset:=HomeOffset+"Z : OK "; ENDIF !ErrWrite HomeOffset,"Move robot manually near homeposition"; pPre_Home:=CRobT(\Tool:=tGripper\WObj:=wobj0); pTo_home.trans.x:=pPre_Home.trans.x; pTo_home.trans.y:=pPre_Home.trans.y; pTo_home.trans.z:=1750; pTo_home.rot:=pPre_Home.rot; pTo_home.robconf:=pPre_Home.robconf; MoveJ PTo_home,v500,z10,tGripper; !MoveJDO pHome, v500, z10, tGripper, do02_RobHomePos, 1; MoveJ pHome, v500, z10, tGripper; ActualPos:=CRobT(\Tool:=tGripper\WObj:=wobj0); ENDIF ENDIF MoveJ pHome,v500,fine,tGripper; Reset do12; Reset do13; Set do15; Set do16; !MoveJDO pHome, v500, fine, tGripper, do02_RobHomePos, 1; !Set do02_RobHomePos; !Reset do02_RobHomePos; ENDPROC PROC rPlacePart() WaitUntil di08_b1 = 1 OR di09_b2 = 1 or di10_b3 = 1; IF GIPlaceNum = 1 THEN rPlacePal1; rPlace1; ELSEIF GIPlaceNum = 2 THEN rPlacePal2; rPlace2; ELSEIF GIPlaceNum = 3 THEN rPlacePal3; rPlace3; ELSE MoveJ pHome,v200,fine,tGripper; ENDIF ENDPROC PROC COU1() count3:=count3+1; dec3:=count3-1; Flool1:=dec3 div 4; jiou:=Flool1 mod 2; IF jiou = 0 THEN baishu3:=dec3 mod 4; ENDIF IF jiou = 1 THEN xef1:=dec3 mod 4; baishu3 := jiou * 3 - xef1; ENDIF ENDPROC PROC COU2() count2:=count2+1; dec2:=count2-1; Floom1:=dec2 div 3; baishu2:=dec2 mod 3; jiou2:=Floom1 mod 2; ENDPROC PROC COU3() count1:=count1+1; dec1:=count1-1; Floob1:=dec1 div 3; baishu1:=dec1 mod 3; jiou3:=Floob1 mod 2; ENDPROC PROC rPlace1() ConfJ\Off; ConfL\Off; !MoveJ pWait22, v1000, z20, tGripper\WObj:=wPal1; WaitUntil di07_p3 = 0; IF baishu3 = 3 THEN MoveJ Offs(pWait117,0,jiou * (0 - 800),nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait117,0,jiou * (0 - 800),nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait117,0,jiou * (0 - 800),nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; GripperOpen; WaitTime 1; MoveJ Offs(pWait117,0,jiou * (0 - 800),nBoxH1 * Flool1), vMaxLoad, fine, tGripper\WObj:=wPal1; WaitTime 0.5; ClampOpen; WaitTime 2; WaitTime 0.5; MoveJ Offs(pWait117,0,jiou * (0 - 800),nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait117,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ELSEIF baishu3 = 2 THEN MoveJ Offs(pWait116,0,jiou * (0 - 500),nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait116,0,jiou * (0 - 500),nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait116,0,jiou * (0 - 500),nBoxH1 * Flool1 + 60), vMaxLoad, z20, tGripper\WObj:=wPal1; GripperOpen; WaitTime 2; MoveJ Offs(pWait116,0,jiou * (0 - 500),nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; ClampOpen; WaitTime 0.5; MoveJ Offs(pWait116,0,jiou * (0 - 500),nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait116,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ELSEIF baishu3 = 0 THEN MoveJ Offs(pWait13,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait13,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; Set do15; WaitTime 1; MoveJ Offs(pWait13,0,jiou * (0 + 1000),nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; Reset do13; WaitTime 0.5; MoveJ Offs(pWait13,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 300), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait14,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 300), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait14,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; Set do16; WaitTime 1; MoveJ Offs(pWait14,jiou * (0 - 300),jiou * (0 + 1000),nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; Reset do12; WaitTime 0.5; MoveJ Offs(pWait14,jiou * (0 - 300),jiou * (0 + 1000),nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait14,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ELSEIF baishu3 = 1 THEN MoveJ Offs(pWait15,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait15,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; Set do16; Set do15; WaitTime 1; MoveJ Offs(pWait15,0,jiou * (0 + 1000),nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; Reset do12; Reset do13; WaitTime 0.5; MoveJ Offs(pWait15,0,jiou * (0 + 1000),nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait15,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ENDIF GripLoad LoadEmpty; bLoadOnGriper := FALSE; IF count3 <= 16 GOTO C1; count3:= 0; PulseDO\PLength:=0.2, do04_RdyPick1; C1: WaitUntil di04_RqPick1 = 1 OR di05_p1 = 1 OR di06_p2 = 0; IF di04_RqPick1 = 0 THEN MoveJ wait10,vMaxEmpty,z20,tGripper; ELSEIF di05_p1 = 1 THEN !MoveJ wait21,v3000,z40,tGripper; MoveJ wait20,vMaxEmpty,z10,tGripper; ELSEIF di06_p2 = 1 THEN MoveJ wait30,vMaxEmpty,z20,tGripper; MoveJ wait30,vMidEmpty,z10,tGripper; ENDIF ENDPROC PROC rPlace2() ConfL\Off; ConfJ\Off; WaitUntil di08_b1 = 1; IF baishu2=0 THEN MoveJ Offs(pWait21,0,0,-nBoxH1 * Floom1 - 200), vMaxLoad, z20, tGripper\WObj:=wPal2; MoveJ Offs(pWait21,0,0,-nBoxH1 * Floom1), vMaxLoad, z20, tGripper\WObj:=wPal2; WaitTime 2; ClampOpen; WaitTime 0.5; MoveJ Offs(pWait21,0,0,-nBoxH1 * Floom1 - 1000), vMaxLoad, z20, tGripper\WObj:=wPal2; ELSEIF baishu2=1 THEN MoveJ Offs(pWait22,0,0,nBoxH1 * Floom1 ), vMaxLoad, z20, tGripper\WObj:=wpal21; WaitTime 2; ClampOpen; WaitTime 0.5; MoveJ Offs(pWait22,0,0,nBoxH1 * Floom1 + 1000), vMaxLoad, z20, tGripper\WObj:=wpal21; ELSEIF baishu2=2 THEN MoveJ Offs(pWait23,0,0,-nBoxH1 * Floom1 - 200), vMaxLoad, z20, tGripper\WObj:=wpal22; MoveJ Offs(pWait23,0,0,-nBoxH1 * Floom1 ), vMaxLoad, z20, tGripper\WObj:=wpal22; WaitTime 2; ClampOpen1; WaitTime 0.5; MoveJ Offs(pWait23,0,0,-nBoxH1 * Floom1 - 200), vMaxLoad, z20, tGripper\WObj:=wpal22; MoveJ Offs(pWait24,0,0,nBoxH1 * Floom1 + 200), vMaxLoad, z20, tGripper\WObj:=wpal23; MoveJ Offs(pWait24,0,0,nBoxH1 * Floom1 ), vMaxLoad, z20, tGripper\WObj:=wpal23; WaitTime 2; ClampOpen2; WaitTime 0.5; MoveJ Offs(pWait24,0,0,nBoxH1 * Floom1 + 1000), vMaxLoad, z20, tGripper\WObj:=wpal23; ENDIF GripLoad LoadEmpty; bLoadOnGriper := FALSE; IF count2<24 GOTO C2; count2:=0; PulseDO\PLength:=0.2, do05_RdyPick2; C2: WaitUntil di04_RqPick1 = 1 OR di06_p2 = 0 OR di05_p1 = 1; IF di04_RqPick1 = 1 THEN !MoveJ wait10 ,v3000,z30,tGripper; MoveJ wait10 ,vMaxEmpty,z20,tGripper; ELSEIF di05_p1 = 1 THEN MoveJ wait20, vMaxEmpty, z30, tGripper; ELSEIF di06_p2 = 0 THEN MoveJ wait30,vMaxEmpty,z30,tGripper; !MoveJ wait30,v3000,z30,tGripper; ENDIF ENDPROC PROC rPlace3() ConfL\Off; WaitUntil di07_p3 = 0; IF baishu3 = 1 THEN MoveJ Offs(pWait11,0,0,nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait11,0,0,nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; GripperOpen; WaitTime 1; MoveJ Offs(pWait11,0,0,nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 0.5; ClampOpen; WaitTime 2; WaitTime 0.5; MoveJ Offs(pWait11,0,0,nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait11,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ELSEIF baishu3 = 0 THEN MoveJ Offs(pWait12,0,0,nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait12,0,0,nBoxH1 * Flool1 + 60), vMaxLoad, z20, tGripper\WObj:=wPal1; GripperOpen; WaitTime 2; MoveJ Offs(pWait12,0,0,nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; ClampOpen; WaitTime 0.5; MoveJ Offs(pWait12,0,0,nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait12,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ELSEIF baishu3 = 3 THEN MoveJ Offs(pWait118,0,0,nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait118,0,0,nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; Set do15; WaitTime 1; MoveJ Offs(pWait118,0,0,nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; Reset do13; WaitTime 0.5; MoveJ Offs(pWait118,0,0,nBoxH1 * Flool1 + 300), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait119,0,0,nBoxH1 * Flool1 + 300), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait119,0,0,nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; Set do16; WaitTime 1; MoveJ Offs(pWait119,0,0,nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; Reset do12; WaitTime 0.5; MoveJ Offs(pWait119,0,0,nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait119,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ELSEIF baishu3 = 2 THEN MoveJ Offs(pWait200,0,0,nBoxH1 * Flool1 + 200), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait200,0,0,nBoxH1 * Flool1 + 80), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 1; Set do16; Set do15; WaitTime 1; MoveJ Offs(pWait200,0,0,nBoxH1 * Flool1), vMaxLoad, z20, tGripper\WObj:=wPal1; WaitTime 2; Reset do12; Reset do13; WaitTime 0.5; MoveJ Offs(pWait200,0,0,nBoxH1 * Flool1 + 500), vMaxLoad, z20, tGripper\WObj:=wPal1; MoveJ Offs(pWait200,0,0,1500), vMaxLoad, z20, tGripper\WObj:=wPal1; ENDIF GripLoad LoadEmpty; bLoadOnGriper := FALSE; IF count3 <= 15 GOTO C1; count3:= 0; PulseDO\PLength:=0.2, do04_RdyPick1; C1: WaitUntil di04_RqPick1 = 1 OR di05_p1 = 1 OR di06_p2 = 0; IF di04_RqPick1 = 0 THEN MoveJ wait10,vMaxEmpty,z20,tGripper; ELSEIF di05_p1 = 1 THEN !MoveJ wait21,v3000,z40,tGripper; MoveJ wait20,vMaxEmpty,z10,tGripper; ELSEIF di06_p2 = 1 THEN MoveJ wait30,vMaxEmpty,z20,tGripper; MoveJ wait30,vMidEmpty,z10,tGripper; ENDIF ENDPROC TRAP trCLAMP TPErase; ErrWrite "Clamp close abnormal.", "Check the clamp status!"; EXIT; ENDTRAP PROC rCheckClamp() IDelete intno1; CONNECT intno1 WITH trCLAMP; ISignalDI\Single, di11_Clamp_close, low, intno1; ENDPROC PROC rHome() IF bCurrentPos(pHome,tGripper)= True THEN MoveJ pHome,v200,fine,tGripper; ELSE !ErrWrite HomeOffset,"Move robot manually near homeposition"; TPWrite "Move Robot go home manually"; Stop; EXIT; ENDIF ENDPROC PROC Routine1() !MoveL pttt100, v100, z50, tGripper\WObj:=wobj0; rPlacePal2; rPlace2; ENDPROC ENDMODULE