教学设备的示例程序,如有侵权,立删
MODULE SX815Q1
CONST robtarget pHomeQ1:=[[396.50,0.00,630.00],[0.499991,0.500009,0.499998,0.500002],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num nCountQ1:=1;
CONST robtarget pPickQ1:=[[239.65,-314.82,326.41],[0.705409,0.708602,0.0136118,-0.00978778],[-1,0,-4,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num nYoffsetBox:=52;
PERS num nXoffsetBox:=53;
CONST robtarget pQ1:=[[269.24,-130.68,557.28],[0.607237,0.591464,0.370339,0.379846],[-1,0,-4,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pQ2:=[[351.60,-1.15,543.45],[0.505306,0.508288,0.494467,0.491743],[0,0,-4,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pPlaceQ1:=[[216.81,248.56,321.05],[0.480963,0.503121,0.521137,0.493923],[0,0,-4,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pQ3:=[[66.68,51.13,102.79],[0.262253,0.25153,0.667846,0.649568],[0,-1,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pQ4:=[[202.13,204.54,572.78],[0.640164,-0.660199,0.290019,-0.264985],[0,0,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPickLid:=[[78.61,215.09,367.38],[0.677428,-0.669433,0.225668,-0.204998],[0,0,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pCoverLid:=[[187.54,215.09,351.48],[0.677436,-0.669418,0.225674,-0.205014],[0,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pWhiteLabel:=[[-40.68,39.23,6.52],[0.492036,-0.514546,0.500906,-0.492175],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pBlackLabel:=[[-40.31,-8.30,7.09],[0.489786,-0.516752,0.498535,-0.49451],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPasteLabel:=[[350.68,202.46,238.84],[0.00391175,-0.713931,0.699939,-0.0192984],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num nXoffsetLabelWhite:=0;
PERS num nYoffsetLabelWhite:=0;
PERS num nYoffsetPaste:=0;
PERS num nCountQ2:=0;
PERS num nCountXQ1:=0;
PERS num nCountYQ1:=0;
TASK PERS wobjdata wobjLabel:=[FALSE,TRUE,"",[[203.686,-266.972,283.206],[0.999953,0.00881391,-3.34236E-05,-0.00397373]],[[0,0,0],[1,0,0,0]]];
CONST robtarget pQ5:=[[385.85,23.98,455.99],[0.486625,-0.548112,0.500202,-0.461049],[0,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num nXoffsetBaseL:=24;
PERS num nYoffsetBaseL:=24;
CONST robtarget pQ15:=[[187.39,437.58,123.57],[0.480724,-0.533609,0.490438,-0.493596],[0,0,-2,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceBaseQ1:=[[163.81,196.56,321.05],[0.480963,0.503121,0.521137,0.493923],[0,0,-4,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS num nYoffsetBaseP:=24;
PERS num nXoffsetLabelBlack:=0;
PERS num nYoffsetLabelBlack:=0;
PERS num nCountXQ2:=0;
PERS num nCountYQ2:=0;
PROC Main()
rInitialize;
WHILE TRUE DO
IF DI10 = 1 THEN
Reset DO7;
Set DO11;
rPickBottle;
ENDIF
IF DI11 = 1 THEN
Set DO11;
rTakeLid;
ENDIF
IF DI12 = 1 AND DI13 = 0 THEN
Set DO11;
rTakeWhiteLabel;
ELSEIF DI12 = 1 AND DI13 = 1 THEN
Set DO11;
rTakeBlackLabel;
ENDIF
ENDWHILE
ENDPROC
PROC rInitialize()
nCountQ1 := 1;
nCountQ2 := 0;
nCountXQ1 := 0;
nCountYQ1 := 0;
nCountXQ2 := 0;
nCountYQ2 := 0;
Reset DO11;
Reset DO8;
Reset DO9;
Reset DO10;
Reset DO14;
Reset DO15;
Reset DO16;
rHome;
ENDPROC
PROC rHome()
VAR jointtarget joints;
joints:=CJointT();
joints.robax.rax_3:=0;
joints.robax.rax_4:=0;
joints.robax.rax_5:=0;
joints.robax.rax_2:=0;
joints.robax.rax_1:=0;
joints.robax.rax_6:=-270;
MoveAbsJ joints\NoEOffs, v60, z10, tool0;
MoveJ pHomeQ1, v100, z0, tool0\WObj:=wobj0;
Set DO7;
WaitTime 1;
ENDPROC
PROC rPickBottle() !????
WaitDI DI10, 1;
MoveJ pQ1, v300, z20, tool0;
MoveJ Offs(pPickQ1,0,0,80), v600, z50, tool0;
MoveL pPickQ1, v200, fine, tool0;
WaitTime 0.5;
Set DO14;
WaitTime 0.5;
MoveL Offs(pPickQ1,0,0,80), v200, z30, tool0;
MoveJ pQ2, v300, z20, tool0;
rPlaceBottle;
ENDPROC
PROC rPlaceBottle() !????
rCalculatePos;
MoveJ Offs(pPlaceQ1,0,0,100), v600, z20, tool0;
MoveL pPlaceQ1, v200, fine, tool0;
Reset DO14;
Set DO8;
WaitTime 0.5;
Reset DO11;
MoveL Offs(pPlaceQ1,0,0,100), v200, z50, tool0;
nCountQ1 := nCountQ1 + 1;
Reset DO8;
IF nCountQ1 > 4 THEN
nCountQ1 := 1;
MoveJ pHomeQ1, v600, z10, tool0;
ELSEIF DI10 = 0 THEN
MoveJ pHomeQ1, v600, fine, tool0\WObj:=wobj0;
ENDIF
ENDPROC
PROC rCalculatePos()
TEST nCountQ1
CASE 1:
pPlaceQ1 := Offs(pPlaceBaseQ1,0,0,0);
CASE 2:
pPlaceQ1 := Offs(pPlaceBaseQ1,nXoffsetBox,0,0);
CASE 3:
pPlaceQ1 := Offs(pPlaceBaseQ1,0,nYoffsetBox,0);
CASE 4:
pPlaceQ1 := Offs(pPlaceBaseQ1,nXoffsetBox,nYoffsetBox,0);
ENDTEST
ENDPROC
PROC rModPos()
MoveJ pQ1, v30, fine, tool0\WObj:=wobj0;
MoveJ pPickQ1, v30, fine, tool0\WObj:=wobj0;
MoveJ pQ2, v30, fine, tool0\WObj:=wobj0;
MoveJ pQ3, v30, fine, tool0;
MoveJ pPlaceBaseQ1, v30, fine, tool0;
MoveJ pQ4, v30, fine, tool0\WObj:=wobj0;
MoveJ pPickLid, v30, fine, tool0;
MoveJ pCoverLid, v30, fine, tool0;
MoveJ pQ5, v30, fine, tool0\WObj:=wobj0;
MoveJ pPasteLabel, v30, fine, tool0;
MoveJ pWhiteLabel, v30, fine, tool0\WObj:=wobjLabel;
MoveJ pBlackLabel, v30, fine, tool0\WObj:=wobjLabel;
ENDPROC
PROC rTakeLid() !???
WaitDI DI11, 1;
MoveJ pQ4, v500, z50, tool0;
MoveJ Offs(pPickLid,0,0,70), v300, z20, tool0;
MoveL pPickLid, v100, fine, tool0;
WaitTime 0.5;
Set DO15;
Set DO16;
WaitTime 0.5;
MoveL Offs(pPickLid,0,0,70), v150, fine, tool0;
MoveJ Offs(pCoverLid,0,0,70), v150, fine, tool0;
MoveL pCoverLid, v100, fine, tool0;
reSet DO15;
reSet DO16;
WaitTime 0.5;
MoveL Offs(pCoverLid,0,0,100), v200, z50, tool0;
Set DO9;
WaitTime 0.5;
Reset DO9;
Reset DO11;
MoveJ pHomeQ1, v600, fine, tool0\WObj:=wobj0;
ENDPROC
PROC rTakeWhiteLabel() !??????
nXoffsetLabelWhite := nCountXQ1 * nXoffsetBaseL;
nYoffsetLabelWhite := nCountYQ1 * nYoffsetBaseL;
MoveJ Offs(pWhiteLabel,nXoffsetLabelWhite,nYoffsetLabelWhite,100), v600, z20, tool0\WObj:=wobjLabel;
MoveL Offs(pWhiteLabel,nXoffsetLabelWhite,nYoffsetLabelWhite,0), v200, fine, tool0\WObj:=wobjLabel;
WaitTime 0.5;
Set DO15;
WaitTime 0.5;
MoveL Offs(pWhiteLabel,nXoffsetLabelWhite,nYoffsetLabelWhite,100), v600, z50, tool0\WObj:=wobjLabel;
nCountXQ1 := nCountXQ1 + 1;
IF nCountXQ1 >= 6 AND nCountYQ1=0 THEN
nCountXQ1 := 0;
nCountYQ1 := 1;
ELSEIF nCountXQ1>=6 AND nCountYQ1=1 THEN
nCountXQ1 := 0;
nCountYQ1 := 0;
ENDIF
rPasteLabel;
ENDPROc
PROC rTakeBlackLabel() !??????
nXoffsetLabelBlack := nCountXQ2 * nXoffsetBaseL;
nYoffsetLabelBlack := nCountYQ2 * nYoffsetBaseL;
MoveJ Offs(pBlackLabel,nXoffsetLabelBlack,nYoffsetLabelBlack,100), v600, z20, tool0\WObj:=wobjLabel;
MoveL Offs(pBlackLabel,nXoffsetLabelBlack,nYoffsetLabelBlack,0), v200, fine, tool0\WObj:=wobjLabel;
WaitTime 0.5;
Set DO15;
WaitTime 0.5;
MoveL Offs(pBlackLabel,nXoffsetLabelBlack,nYoffsetLabelBlack,100), v200, z50, tool0\WObj:=wobjLabel;
nCountXQ2 := nCountXQ2 + 1;
IF nCountXQ2 >= 6 AND nCountYQ2=0 THEN
nCountXQ2 := 0;
nCountYQ2 := 1;
ELSEIF nCountXQ2>=6 AND nCountYQ2=1 THEN
nCountXQ2 := 0;
nCountYQ2 := 0;
ENDIF
rPasteLabel;
ENDPROC
PROC rPasteLabel()
nYoffsetPaste := nCountQ2 * nYoffsetBaseP;
MoveJ pQ5, v600, z50, tool0\WObj:=wobj0;
MoveJ Offs(pPasteLabel,50,nYoffsetPaste,0), v200, z20, tool0;
MoveL Offs(pPasteLabel,0,nYoffsetPaste,0), v200, fine, tool0;
WaitTime 0.5;
Reset DO15;
WaitTime 0.5;
MoveJ Offs(pPasteLabel,50,nYoffsetPaste,0), v200, z30, tool0;
nCountQ2 := nCountQ2 + 1;
Set DO10;
WaitTime 1;
Reset DO10;
IF nCountQ2 >= 4 THEN
nCountQ2 := 0;
Reset DO11;
MoveJ pHomeQ1, v600, fine, tool0\WObj:=wobj0;
ELSEIF DI12 = 1 THEN
MoveJ pQ5, v600, z50, tool0\WObj:=wobj0;
ENDIF
ENDPROC
ENDMODULE