教学设备的示例程序,如有侵权,立删
MODULE SX815Q1CONST 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 DOIF DI10 = 1 THENReset DO7; Set DO11; rPickBottle; ENDIFIF DI11 = 1 THENSet DO11; rTakeLid; ENDIFIF DI12 = 1 AND DI13 = 0 THENSet DO11; rTakeWhiteLabel; ELSEIF DI12 = 1 AND DI13 = 1 THENSet DO11; rTakeBlackLabel; ENDIFENDWHILEENDPROCPROC 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; ENDPROCPROC 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; ENDPROCPROC 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 THENnCountQ1 : = 1 ; MoveJ pHomeQ1, v600, z10, tool0; ELSEIF DI10 = 0 THENMoveJ pHomeQ1, v600, fine, tool0\WObj: = wobj0; ENDIFENDPROCPROC rCalculatePos ( ) TEST nCountQ1CASE 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 ) ; ENDTESTENDPROCPROC 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; ENDPROCPROC 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; ENDPROCPROC 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 THENnCountXQ1 : = 0 ; nCountYQ1 : = 1 ; ELSEIF nCountXQ1>= 6 AND nCountYQ1= 1 THEN nCountXQ1 : = 0 ; nCountYQ1 : = 0 ; ENDIFrPasteLabel; ENDPROcPROC 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 THENnCountXQ2 : = 0 ; nCountYQ2 : = 1 ; ELSEIF nCountXQ2>= 6 AND nCountYQ2= 1 THENnCountXQ2 : = 0 ; nCountYQ2 : = 0 ; ENDIFrPasteLabel; ENDPROCPROC 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 THENnCountQ2 : = 0 ; Reset DO11; MoveJ pHomeQ1, v600, fine, tool0\WObj: = wobj0; ELSEIF DI12 = 1 THENMoveJ pQ5, v600, z50, tool0\WObj: = wobj0; ENDIFENDPROCENDMODULE