机床上下料程序（一）

MODULE MainModule
PERS robtarget pHome:=[[937.45,0.00,1300.09],[0.542513,-1.25177E-
08,0.840047,-8.08408E-09],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pPick_1:=[[398.00,-1221.02,602.52],
[0.489822,0.509842,0.486942,-0.512857],[-1,0,-1,1],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pPick_2:=[[1238.92,90.36,1038.78],
[0.497512,0.502473,0.497215,0.502773],[0,2,-2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pPlace_1:=[[1238.92,90.36,1038.78],
[0.497512,0.502473,0.497215,0.502773],[0,2,-2,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pPlace_2:=[[-26.94,1148.63,610.31],[0.486507,-
0.513229,0.488309,0.511333],[1,1,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pFltby:=[[1238.92,-147.86,1038.78],
[0.497512,0.502473,0.497215,0.502773],[-1,1,-1,0],
[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
（红色字体为在程序数据中新建的数据）
PROC Main()
rInitall;
WHILE TRUE DO
IF di_start = 1 THEN
rPickPlace_1;
rPickPlac_2;
ELSE
WaitTime 0.3;
ENDIF
ENDWHILE
ENDPROC
PROC rInitall()
Reset do_grip; （复位夹具，松开夹具）
Reset do_open; （复位机床门，关门）
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
ENDPROC
PROC rPickPlace_1()
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
MoveJ Offs(pPick_1,0,0,300), v2000, fine, MyNewTool\WObj:=wobj0;
MoveL pPick_1, v500, fine, MyNewTool\WObj:=wobj0;
Set do_grip; （置位夹具）
WaitDI di_VaOk, 1;
MoveL Offs(pPick_1,0,0,300), v2000, fine, MyNewTool\WObj:=wobj0;
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
 Set do_open;
WaitDI di_opendone, 1;
MoveJ pFltby, v1000, z50, MyNewTool\WObj:=wobj0;
MoveJ Offs(pPlace_1,200,0,0), v500, z50, MyNewTool\WObj:=wobj0;
MoveL pPlace_1, v500, fine, MyNewTool\WObj:=wobj0;
Reset do_grip;
WaitDI di_VaOk, 0;
MoveL Offs(pPlace_1,200,0,0), v500, z50, MyNewTool\WObj:=wobj0;
MoveJ pFltby, v1000, z50, MyNewTool\WObj:=wobj0;
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
Reset do_open;
ENDPROC
PROC rPickPlac_2()
WaitTime 5;
Set do_open;
WaitDI di_opendone, 1;
MoveJ pFltby, v1000, z50, MyNewTool\WObj:=wobj0;
MoveJ Offs(pPick_2,200,0,0), v500, z50, MyNewTool\WObj:=wobj0;
MoveL pPick_2, v500, fine, MyNewTool\WObj:=wobj0;
set do_grip;
WaitDI di_VaOk, 1;
MoveL Offs(pPick_2,200,0,0), v500, z50, MyNewTool\WObj:=wobj0;
MoveJ pFltby, v1000, z50, MyNewTool\WObj:=wobj0;
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
Reset do_open;
MoveJ Offs(pPlace_2,300,0,0), v1000, fine, tool0;
MoveL pPlace_2, v500, fine, MyNewTool\WObj:=wobj0;
Reset do_grip;
WaitDI di_VaOk, 0;
MoveL Offs(pPlace_2,300,0,0), v1000, fine, tool0;
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
ENDPROC
PROC Teacher()
MoveJ pHome, v1000, fine, MyNewTool\WObj:=wobj0;
MoveJ pPick_2, v1000, fine, MyNewTool\WObj:=wobj0;
MoveJ pPick_1, v1000, fine, MyNewTool\WObj:=wobj0;
MoveJ pPlace_1, v1000, fine, MyNewTool\WObj:=wobj0;
MoveJ pPlace_2, v1000, fine, MyNewTool\WObj:=wobj0;
MoveJ pFltby, v1000, fine, MyNewTool\WObj:=wobj0;
ENDPROC

ENDMODULE