鼠标装配机器人程序
2020-03-02 11:08:27 来源：范文大全收藏下载本文
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,tool3\\WObj:=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,tool3\\WObj:=Workobject_2;
MoveJ pickGwait2,v800,z50,tool3\\WObj:=Workobject_2; MoveJoffs(vrPickG,0,0,50),v500,z50,tool3\\WObj:=Workobject_2; MoveL vrPickG,v50,fine,tool3\\WObj:=Workobject_2; SetDO8_xipan ; WaitTime 0.3; MoveLoffs(vrPickG,0,0,50),v200,fine,tool3\\WObj:=Workobject_2; MoveJ pickGwait2,v800,z50,tool3\\WObj:=Workobject_2; MoveJ pickGwait1,v800,z50,tool3\\WObj:=Workobject_2; MoveJ pickGwait4,v800,z50,tooldata2\\WObj:=Workobject_2; ENDPROC PROC rPlaceG() MoveJ placeGwait,v800,z50,tool3\\WObj:=Workobject_2; MoveJoffs(placeGpot,0,0,50),v500,z50,tool3\\WObj:=Workobject_2; MoveL placeGpot,v50,fine,tool3\\WObj:=Workobject_2; ResetDO8_xipan ; WaitTime 0.3; MoveLoffs(placeGpot,0,0,100),v200,fine,tool3\\WObj:=Workobject_2; MoveJ placeGwait,v800,z50,tool3\\WObj:=Workobject_2;
CallMathG; ENDPROC
PROC rPreG() MoveJ PreGwait,v800,z100,tool3\\WObj:=Workobject_2; MoveJoffs(PrePot1,0,0,20),v500,z50,tool3\\WObj:=Workobject_2; MoveL PrePot1,v50,fine,tool3\\WObj:=Workobject_2; MoveLoffs(PrePot1,0,0,20),v80,fine,tool3\\WObj:=Workobject_2; MoveL PrePot2,v50,fine,tool3\\WObj:=Workobject_2; MoveL PrePot3,v50,fine,tool3\\WObj:=Workobject_2; MoveL PrePot2,v50,fine,tool3\\WObj:=Workobject_2; MoveJoffs(PrePot2,0,0,20),v200,fine,tool3\\WObj:=Workobject_2; MoveJ PreGwait,v800,z100,tool3\\WObj:=Workobject_2; ENDPROC
PROC rPickUSB()
rCallPosUSB; MoveJ PickUSBwait,v800,z50,tooldata2\\WObj:=Workobject_2; MoveJoffs(vrPickUSB,0,0,50),v500,z50,tooldata2\\WObj:=Workobject_2; MoveL vrPickUSB,v50,fine,tooldata2\\WObj:=Workobject_2; SetDO5_usb; WaitTime 0.3; MoveLoffs(vrPickUSB,0,0,100),v500,z50,tooldata2\\WObj:=Workobject_2; MoveJ PickUSBwait,v800,z50,tooldata2\\WObj:=Workobject_2; ENDPROC
PROC rPlaceUSB() MoveJoffs(Put_usb,0,-100,0),v800,z100,tooldata2\\WObj:=Workobject_2; MoveJoffs(Put_usb,0,-50,0),v500,z50,tooldata2\\WObj:=Workobject_2; MoveL Put_usb,v50,fine,tooldata2\\WObj:=Workobject_2; ResetDO5_usb; WaitTime 0.3; MoveLoffs(Put_usb,0,-10,0),v10,fine,tooldata2\\WObj:=Workobject_2; SetDO5_usb; WaitTime 0.3; MoveL Put_usb,v10,fine,tooldata2\\WObj:=Workobject_2; WaitTime 1; MoveJoffs(Put_usb,0,-100,0),v800,z100,tooldata2\\WObj:=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,tool3\\WObj:=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:=tool3\\WObj:=Workobject_2); !pActualpos.trans.z:=pHome.trans.z;
!MoveL pActualpos,v100,z10,tool3\\WObj:=Workobject_2; !MoveL pHome,v100,fine,tool3\\WObj:=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:=tool3\\WObj:=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
人人范文网 m.inrrp.com.cn 手机版