鼠标装配机器人程序由刀豆文库小编整理，希望给你工作、学习、生活带来方便，猜你可能喜欢“机器人装配系统方案”。
MODULE Module1 CONSTrobtarget
pHome:=[[475.395989043,658.308108557,504.791394735],[0.099045666,0.892399288,-0.369643273,0.239117789],[-1,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
pickGpot:=[[1284.963592872,738.815790313,25.869169392],[0.068204518,-0.002398066,-0.997668473,0.000107188],[-2,0,-1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
pickGwait1:=[[908.411232745,672.850762454,378.307511016],[0.010244907,0.482635243,-0.875743333,0.005646121],[-1,-1,0,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
pickGwait2:=[[1176.437528319,673.642532691,257.234457149],[0.068204435,-0.002398463,-0.997668477,0.000107599],[-2,0,-1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];PERSnum nCountG:=1;PERSnum nCountUSB:=1;PERSnum nCount:=1;VARdnum value;PERSrobtarget
vrPickG:=[[1164.96,438.816,25.8692],[0.0682045,-0.00239807,-0.997668,0.000107188],[-2,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];PERSrobtarget
vrPickUSB:=[[320.366,420.259,32.1482],[0.00603875,-0.703985,0.710163,-0.0060945],[0,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONSTrobtarget
placeGpot:=[[491.933622165,846.937141509,160.846974852],[0.005008794,0.935474011,0.014272505,0.353071638],[-1,-2,0,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
placeGwait:=[[507.240197782,605.725272386,378.307511016],[0.004224647,0.932443514,-0.361126372,0.010908214],[0,0,-1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
PreGwait:=[[500.114949513,623.456625976,378.507861069],[0.705929951,0.70233864,-0.072147603,0.056374304],[-1,-1,1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
PrePot1:=[[504.379458439,774.722785044,233.501667922],[0.705929374,0.702339261,-0.072147646,0.056373739],[-1,-1,1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
PrePot2:=[[459.683447723,774.724224404,225.569285468],[0.685817737,0.68475246,-0.182196214,0.166050111],[-1,-2,1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
PrePot3:=[[468.687935979,774.724120389,217.120366301],[0.685818109,0.684752091,-0.182196202,0.166050111],[-1,-2,1,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTnum nYg:=-150;CONSTnum nXg:=-120;CONSTnum nYu:=60;CONSTnum nXu:=50;CONSTrobtarget
PickUSBpot:=[[220.36551859,360.258970705,32.14823204],[0.006038746,-0.703985263,0.710162757,-0.006094499],[0,0,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
PickUSBwait:=[[504.866605476,422.213803361,328.76973602],[0.006039381,-0.703984162,0.710163837,-0.006095185],[0,-1,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
Put_usb:=[[479.824350594,835.130027981,542.371213165],[0.494243592,-0.499964028,0.495931919,0.509716367],[0,-2,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];PERSrobtarget
vrPick_usb:=[[1284.96,738.816,25.8692],[0.0682045,-0.00239807,-0.997668,0.000107188],[-2,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONSTrobtarget
pickGwait3:=[[647.436094755,690.649278184,728.876166045],[0.493819119,-0.293854288,0.233426425,0.784413437],[-1,0,0,1],[9E9,9E9,9E9,9E9,9E9,9E9]];CONSTrobtarget
pickGwait4:=[[544.760859012,636.772731418,439.244061946],[0.113819259,-0.841453732,0.440897874,0.290877738],[-1,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];PROC main()
rInitAll;WHILETRUEDO ResetDO7_COM;
rpickG;WaitDIDI6_COM,1;
rPlaceG;
rPreG;SetDO6_COM;
rPickUSB;WaitTime 2;
rPlaceUSB;SetDO7_COM;ResetDO6_COM;
CallMath;ENDWHILE ENDPROC PROC rInitAll()
rCheckHomePos;MoveJ pHome,v800,z50,tool3WObj:=Workobject_2;
ResetDO5_usb;ResetDO6_COM;ResetDO7_COM;
nCountG:=1;
nCount:=1;
nCountUSB:=1;AccSet 50,100;WaitTime 0.5;
ENDPROC PROC rpickG()
rCallPosG;MoveJ pickGwait1,v800,z50,tool3WObj:=Workobject_2;
MoveJ pickGwait2,v800,z50,tool3WObj:=Workobject_2;MoveJoffs(vrPickG,0,0,50),v500,z50,tool3WObj:=Workobject_2;MoveL vrPickG,v50,fine,tool3WObj:=Workobject_2;SetDO8_xipan;WaitTime 0.3;MoveLoffs(vrPickG,0,0,50),v200,fine,tool3WObj:=Workobject_2;MoveJ pickGwait2,v800,z50,tool3WObj:=Workobject_2;MoveJ pickGwait1,v800,z50,tool3WObj:=Workobject_2;MoveJ pickGwait4,v800,z50,tooldata2WObj:=Workobject_2;ENDPROC PROC rPlaceG()MoveJ placeGwait,v800,z50,tool3WObj:=Workobject_2;MoveJoffs(placeGpot,0,0,50),v500,z50,tool3WObj:=Workobject_2;MoveL placeGpot,v50,fine,tool3WObj:=Workobject_2;ResetDO8_xipan;WaitTime 0.3;MoveLoffs(placeGpot,0,0,100),v200,fine,tool3WObj:=Workobject_2;MoveJ placeGwait,v800,z50,tool3WObj:=Workobject_2;
CallMathG;ENDPROC
PROC rPreG()MoveJ PreGwait,v800,z100,tool3WObj:=Workobject_2;MoveJoffs(PrePot1,0,0,20),v500,z50,tool3WObj:=Workobject_2;MoveL PrePot1,v50,fine,tool3WObj:=Workobject_2;MoveLoffs(PrePot1,0,0,20),v80,fine,tool3WObj:=Workobject_2;MoveL PrePot2,v50,fine,tool3WObj:=Workobject_2;MoveL PrePot3,v50,fine,tool3WObj:=Workobject_2;MoveL PrePot2,v50,fine,tool3WObj:=Workobject_2;MoveJoffs(PrePot2,0,0,20),v200,fine,tool3WObj:=Workobject_2;MoveJ PreGwait,v800,z100,tool3WObj:=Workobject_2;ENDPROC
PROC rPickUSB()
rCallPosUSB;MoveJ PickUSBwait,v800,z50,tooldata2WObj:=Workobject_2;MoveJoffs(vrPickUSB,0,0,50),v500,z50,tooldata2WObj:=Workobject_2;MoveL vrPickUSB,v50,fine,tooldata2WObj:=Workobject_2;SetDO5_usb;WaitTime 0.3;MoveLoffs(vrPickUSB,0,0,100),v500,z50,tooldata2WObj:=Workobject_2;MoveJ PickUSBwait,v800,z50,tooldata2WObj:=Workobject_2;ENDPROC
PROC rPlaceUSB()MoveJoffs(Put_usb,0,-100,0),v800,z100,tooldata2WObj:=Workobject_2;MoveJoffs(Put_usb,0,-50,0),v500,z50,tooldata2WObj:=Workobject_2;MoveL Put_usb,v50,fine,tooldata2WObj:=Workobject_2;ResetDO5_usb;WaitTime 0.3;MoveLoffs(Put_usb,0,-10,0),v10,fine,tooldata2WObj:=Workobject_2;SetDO5_usb;WaitTime 0.3;MoveL Put_usb,v10,fine,tooldata2WObj:=Workobject_2;WaitTime 1;MoveJoffs(Put_usb,0,-100,0),v800,z100,tooldata2WObj:=Workobject_2;
CallMathU;ResetDO5_usb;ENDPROC
PROC rCallPosG()TEST nCountG CASE 1:
vrPickG:=Offs(pickGpot,0,0,0);CASE 2:
vrPickG:=Offs(pickGpot,0,nYg,0);CASE 3:
vrPickG:=Offs(pickGpot,0,2*nYg,0);CASE 4:
vrPickG:=Offs(pickGpot,nXg,0,0);CASE 5:
vrPickG:=Offs(pickGpot,nXg,nYg,0);CASE 6:
vrPickG:=Offs(pickGpot,nXg,2*nYg,0);DEFAULT: ENDTEST ENDPROC
PROC rCallPosUSB()TEST nCountUSB CASE 1:
vrPickUSB:=Offs(PickUSBpot,0,0,0);CASE 2:
vrPickUSB:=Offs(PickUSBpot,0,nYu,0);CASE 3:
vrPickUSB:=Offs(PickUSBpot,nXu,0,0);CASE 4:
vrPickUSB:=Offs(PickUSBpot,nXu,nYu,0);CASE 5:
vrPickUSB:=Offs(PickUSBpot,2*nXu,0,0);CASE 6:
vrPickUSB:=Offs(PickUSBpot,2*nXu,nYu,0);DEFAULT: ENDTEST ENDPROC
PROC CallMathG()
nCountG:=nCountG+1;ENDPROC
PROC CallMathU()
nCountUSB:=nCountUSB+1;ENDPROC
PROC CallMath()
nCount:=nCount+1;IF nCount>6 THEN
MoveJ pHome,v800,z50,tool3WObj:=Workobject_2;TPErase;TPWrite“WARNING!”;TPWrite“Miion Accomplished”;TPWrite“.”;TPWrite“To restart the task”;TPWrite“please ensure the robot in manual mode!”;TPWrite“When put the part back to its original position”;TPWrite“.”;TPReadFK reg2,“Do you know the current situation?”,stEmpty,stEmpty,stEmpty,stEmpty,“YES”;IF reg2=5 THEN
rInitAll;Stop;ENDIF ENDIF ENDPROC
PROC rCheckHomePos()VARrobtarget pActualPos;IFNOT CurrentPos(pHome,tool3)THEN TPErase;TPWrite“WARNING!”;TPWrite“.”;TPWrite“Please in Manual Mode ”;TPWrite“Operating the robot back to security posture(pHome)”;Stop;!pActualpos:=CRobT(Tool:=tool3WObj:=Workobject_2);!pActualpos.trans.z:=pHome.trans.z;
!MoveL pActualpos,v100,z10,tool3WObj:=Workobject_2;!MoveL pHome,v100,fine,tool3WObj:=Workobject_2;
ENDIF ENDPROC
FUNCbool CurrentPos(robtarget ComparePos,INOUTtooldata TCP)VARnum Counter:=0;VARrobtarget ActualPos;!
!-------------------
!Abstract : Function to compare current ROBOT position with THE given position!-------------------!
ActualPos:=CRobT(Tool:=tool3WObj:=Workobject_2);IF ActualPos.trans.x>ComparePos.trans.x-25 AND ActualPos.trans.xComparePos.trans.y-25 AND ActualPos.trans.yComparePos.trans.z-25 AND ActualPos.trans.zComparePos.rot.q1-0.1 AND ActualPos.rot.q1ComparePos.rot.q2-0.1 AND ActualPos.rot.q2ComparePos.rot.q3-0.1 AND ActualPos.rot.q3ComparePos.rot.q4-0.1 AND ActualPos.rot.q4
