楼主最近还看过
我也有个疑问,哪位大哥能指点一二:
就是机器人在运行过程中,想采用外部信号触发中断,调用回原点子程序,但是每次都是程序直接跳入跟随子程序没法触发跳出。
我把我的程序发出来:
MODULE MainModule
PROC main()
rInitial;
rGoHome;
WHILE TRUE DO
IF Local_DI5_DropHeight=1 THEN
nHeight:=0;
rPick;
IF Local_DI6_StickyPowder=1 THEN
rPutDreg;
rStickyPowder;
ELSE
rPutDreg;
ENDIF
ELSE
nHeight:=150;
rPick;
IF Local_DI6_StickyPowder=1 THEN
rPutDreg;
rStickyPowder;
ELSE
rPutDreg;
ENDIF
ENDIF
IF Local_DI7_ChangeShovel=1 THEN
rChangeShovel;
ENDIF
ENDWHILE
ENDPROC
PROC rInitial()
Reset Local_DO4_ScratchTime;
DeactUnit CNV1;
ConfL\Off;
DropWObj wobjcnv1;
ENDPROC
PROC rGoHome()
pCurrentPos:=CRobT(\Tool:=SlagHand\WObj:=wobj0);
IF (500<pCurrentPos.trans.x) AND pCurrentPos.trans.x<2400 THEN
IF (-800<pCurrentPos.trans.y) AND pCurrentPos.trans.y<500 THEN
!pCurrentPos.trans.z:=pGoHome.trans.z;
MoveJ Offs(pCurrentPos,0,0,nCurrentPos_Z),v100,z50,SlagHand;
MoveJ pGoHome_Mid02,v300,z50,SlagHand;
MoveJ pGoHome_Mid01,v300,z50,SlagHand;
MoveJ pGoHome,v300,z50,SlagHand;
MoveJ pChangeShovel_End,v200,fine,SlagHand;
rAutoMotorOff;
ENDIF
ELSEIF (-600<pCurrentPos.trans.x) AND pCurrentPos.trans.x<600 THEN
IF (-2000<pCurrentPos.trans.y) AND pCurrentPos.trans.y<-500 THEN
MoveJ Offs(pCurrentPos,0,-50,0),v150,z50,SlagHand;
pCurrentPos.trans.z:=pGoHome_Mid01.trans.z;
MoveJ pCurrentPos,v300,z50,SlagHand;
MoveJ pGoHome_Mid01,v300,z50,SlagHand;
MoveJ pGoHome,v300,z50,SlagHand;
MoveJ pChangeShovel_End,v200,z50,SlagHand;
rAutoMotorOff;
ENDIF
ENDIF
IF (-2000<pCurrentPos.trans.x) AND pCurrentPos.trans.x<-500 THEN
IF (-1000<pCurrentPos.trans.y) AND pCurrentPos.trans.y<0 THEN
MoveJ pGoHome,v400,z50,SlagHand;
MoveJ pGoHome_Mid01,v400,z50,SlagHand;
MoveJ pGoHome_Mid02,v500,z50,SlagHand;
MoveJ pWaitPoint,v500,z50,SlagHand;
ENDIF
ENDIF
ENDPROC
PROC rPick()
MoveJ pWaitPoint,vSpeed_pWait,z50,SlagHand;
MoveJ pWait,vSpeed_pWait,z200,SlagHand;
nHeight:=nHeight-Local_Gi_DropDepth;
e:
ActUnit CNV1;
WaitWObj wobjcnv1\RelDist:=0;
IF c1Position>1 THEN
DropWObj wobjcnv1;
WaitUntil c1Connected=0;
DeactUnit CNV1;
GOTO e;
ENDIF
GOTO a;
DropWObj wobjCNV1;
ActUnit CNV1;
WaitWObj wobjCNV1;
a:
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry1\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y1,nOffset_Z1+nHeight),vPickSpeed_01,z10,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry2\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y2,nOffset_Z2+nHeight),vPickSpeed_02,z50,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry3\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y3,nOffset_Z3+nHeight),vPickSpeed_03,z50,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry4\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y4,nOffset_Z4+nHeight),vPickSpeed_04,z10,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry5\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y5,nOffset_Z5+nHeight),vPickSpeed_05,z40,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry6\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y6,nOffset_Z6+nHeight),vPickSpeed_06,z50,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry7\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y7,nOffset_Z7+nHeight),vPickSpeed_07,z50,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry8\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y8,nOffset_Z8+nHeight),vPickSpeed_08,z30,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry9\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y9,nOffset_Z9+nHeight),vPickSpeed_09,z30,SlagHand\WObj:=wobjCNV1;
MoveL Offs(RelTool(pPick,0,0,0\Rx:=nRotationAngle_Rx\Ry:=nRotationAngle_Ry10\Rz:=nRotationAngle_Rz),nOffset_X,nOffset_Y10,nOffset_Z10+nHeight),vPickSpeed_End,z100,SlagHand\WObj:=wobjCNV1;
ENDPROC
PROC rPutDreg()
AccSet 100,10;
MoveJ pPutDreg_Start,vPutDreg_Start,z50,SlagHand;
MoveJ pPutDreg_Mid01,vPutDreg_Mid01,z100,SlagHand;
MoveJ pPutDreg_Mid02,vPutDreg_Mid02,z100,SlagHand;
MoveJ pPutDreg_Mid03,vPutDreg_Mid03,z50,SlagHand;
MoveL pPutDreg_End,vPutDreg_End,fine,SlagHand;
Set Local_DO4_ScratchTime;
WaitTime 0.5;
AccSet 100,100;
DropWObj wobjCNV1;
DeactUnit CNV1;
MoveL RelTool(pPutDreg_End,-400,0,0\Ry:=0),vPutDreg_finish,fine,SlagHand;
Reset Local_DO4_ScratchTime;
ENDPROC
PROC rStickyPowder()
MoveJ pStickyPowder_Start,vStickyPowder_Start,z50,SlagHand;
MoveJ offs(pStickyPowder_in,nStickyPowder_in_X,nStickyPowder_in_Y,nStickyPowder_in_Z),vStickyPowder_in,z20,SlagHand;
WaitTime nTime;
MoveJ pStickyPowder_out,vStickyPowder_out,z50,SlagHand;
PulseDO\PLength:=1.5,Local_DO3_SlagFinish;
ENDPROC
PROC rChangeShovel()
AccSet 100,25;
MoveJ pChangeShovel_Start,v400,z50,SlagHand;
MoveJ pChangeShovel_Mid,v400,Z50,SlagHand;
MoveJ pChangeShovel_End,v200,fine,SlagHand;
rAutoMotorOff;
ENDPROC
PROC rAutoMotorOff()
WaitTime 1;
PulseDO\PLength:=0.5,Local_DO10_AutoMotorOff;
ENDPROC
ENDMODULE