ABB机器人（ROBOT studio 6.01）程序实例MODULE MainModulePERS tooldatatGripper:=[TRUE,[[0.533078,1.51617,583.739],[1,0,0,0]],[30,[0,0,50],[1,0,0,0],0,0,0] ];TASK PERS wobjdataVisionWobj:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[-934.534,1807.34,-76.7707],[0.4 00996,0.0128267,-0.0292473,-0.915523]]];TASK PERS wobjdataWobjCompressor1:=[FALSE,TRUE,"",[[518.656,-1088.9,164.25],[0,0,0,1]],[[686.65 1,296.298,-588.529],[0.917114,1.69419E-06,-7.35001E-05,-0.398626]]];TASK PERS wobjdataWobjCompressor2:=[FALSE,TRUE,"",[[518.656,-1088.9,164.25],[0,0,0,1]],[[-944.87 1,-657.402,-323.406],[0.918098,-1.98999E-05,-6.49686E-06,0.396353]]];PERS wobjdata WobjCompressor;VAR robtarget pActualPos;VAR socketdev server_socket;VAR socketdev client_socket;VAR string client_ip;VAR string stReceived;VAR num NumCharacters:=9;VAR bool bOK;PERS num nXOffs;PERS num nYOffs;PERS num nAngleOffs;VAR string XData:="";VAR string YData:="";VAR string AngleData:="";VAR num nPresenceOrAbsence;PERS num nPickH:=-400;PERS num nCountX;PERS num nCountY;PERS num nCountZ;PERS num nCount;VAR num nPlaceNo;PERS bool bSMPreOrAbs;PERS bool bInpos;VAR robtarget PVision;VAR robtarget Vision;VAR robtarget ppPick;VAR robtarget pPick;PERS robtarget Pick;PERS robtarget ErCiDingWeiPlace;PERS robtarget ErCiDingWeiPick;PERS robtarget pPlace;PERS robtarget PlaceVision;PERS robtarget PZhanban;PERS robtarget PZhanbanUp;PERS robtarget PZhanbanDown;PERS robtarget PlaceZhanban;PERS robtarget Place;PERS bool bKindChoose;VAR num nAngle;VAR num nX;VAR num nCamOut;VAR num nInpos;VAR num nTotalPalletHigh;VAR num nPalletHigh;PERS num nPalletHighUp;PERS num nPalletHighDown;VAR num Compensation{8,3};VAR num CompensationTwo{8,3};PERS numCompensationErr{8,3}:=[[999999,999999,999999],[999999,999999,999999],[999999 ,999999,999999],[999999,999999,999999],[999999,999999,999999],[999999,999999 ,999999],[999999,999999,999999],[999999,999999,999999]];PERS jointtargetjposHome:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetpHome:=[[621.23,-975.96,1166.44],[0.00703884,-0.385671,-0.922573,-0.00826231], [-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPZhanbanSafe:=[[-162.69,-1703.07,2270.38],[0.0109452,-0.955357,-0.295179,-0.006 45602],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetVisionA:=[[-228.96,94.47,643.11],[0.00434955,-0.699954,0.714102,-0.0101798],[-2, -1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPickA:=[[90.09,79.65,61.29],[0.0102135,0.00299714,0.999935,-0.00417093],[-2,-1,1, 0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPlaceA:=[[-644.46,-1019.60,396.56],[0.00841448,-0.692574,0.721298,0.000680825], [-1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPZhanbanUpA:=[[549.64,541.39,821.21],[0.000808037,-0.999985,-0.00498684,-0.00 209158],[-2,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPZhanbanDownA:=[[548.40,560.53,179.18],[0.00417276,0.999929,-0.00436864,0.01 02239],[-2,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPlaceZhanbanA:=[[2456.73,-1154.76,205.32],[0.0106126,-0.0247295,-0.999603,0.00 838785],[1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];PERS numCompensationA{8,3}:=[[0,0,0],[210,136,0],[430,264,99999999],[645,403,99999999], [855,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[99999999 ,936,99999999]];PERS numCompensationA1{8,3}:=[[860,936,0],[0,0,0],[215,133,99999999],[430,264,99999999 ],[645,403,99999999],[99999999,533,99999999],[99999999,670,99999999],[999999 99,800,99999999]];PERS numCompensationA2{8,3}:=[[860,0,0],[0,133,0],[215,264,99999999],[430,403,99999999 ],[645,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[999999 99,936,99999999]];CONST num nPalletHighUpA:=100;CONST num nPalletHighDownA:=200;CONST robtargetVisionB:=[[936.63,959.75,378.90],[0.00373262,0.105479,-0.994406,-0.00407233],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPickB:=[[1180.22,1009.95,213.74],[0.00373443,0.1055,-0.994404,-0.00406331],[-1,0 ,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPlaceB:=[[-1194.30,-1552.83,582.17],[0.0062614,0.795938,-0.605346,-0.000999941] ,[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPZhanbanUpB:=[[511.86,550.08,92.05],[0.00172284,-0.00758955,-0.99997,-0.00060 6164],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPZhanbanDownB:=[[511.86,550.08,92.05],[0.00172284,-0.00758955,-0.99997,-0.000 606164],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];CONST robtargetPlaceZhanbanB:=[[511.86,550.08,92.05],[0.00172284,-0.00758955,-0.99997,-0.0006 06164],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];PERS numCompensationB{8,3}:=[[0,0,300],[210,136,0],[430,264,99999999],[645,403,9999999 9],[855,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[99999999,936,99999999]];PERS numCompensationB1{8,3}:=[[860,936,300],[0,0,0],[215,133,99999999],[430,264,999999 99],[645,403,99999999],[99999999,533,99999999],[99999999,670,99999999],[9999 9999,800,99999999]];PERS numCompensationB2{8,3}:=[[860,0,300],[0,133,0],[215,264,99999999],[430,403,999999 99],[645,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[9999 9999,936,99999999]];CONST num nPalletHighUpB:=100;CONST num nPalletHighDownB:=200;PERS speeddata vMinEmpty:=[1300,100,6000,1000];PERS speeddata vMidEmpty:=[1400,100,6000,1000];PERS speeddata vMaxEmpty:=[1500,100,6000,1000];PERS speeddata vBigMaxEmpty:=[1500,100,6000,1000];PERS speeddata vMinLoad:=[1200,100,6000,1000];PERS speeddata vMaxLoad:=[1300,100,6000,1000];PERS speeddata vBigMaxLoad:=[1400,100,6000,1000];TASK PERS wobjdatawobj1:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[1984.06,-1180.48,453.803],[0.661114, -0.659907,-0.252753,0.252124]]];PROC Main()rInitAll;WHILE TRUE DOrPickCal;rPick;ENDWHILEWaitTime 0.3;ENDPROCPROC rInitAll()AccSet 60,100;Compensation:=CompensationErr;CompensationTwo:=CompensationErr;reg1 :=0;reg2 :=0;nCountX:=1;nCountY:=1;nCount:=0;nCountZ:=1;nTotalPalletHigh:=0;nPalletHigh:=0;nInpos:=0;rKindChoose;rCheckHomePos;Reset do00_JawsOpen ;Reset do01_JawsClose ;Set do00_JawsOpen ;Reset do02_BigJaws1 ;WaitTime 0.3;Reset do03_BigJaws2;Reset do00_JawsOpen ;Reset do13_Placing;Reset do12_PlaceOK;ENDPROCPROC rPickCal ()Set do00_JawsOpen ;bSMPreOrAbs:=TRUE;IF nCount =0 THENrCompressorInPos;ENDIFWHILE bSMPreOrAbs=TRUE DOIncr nCount ;IF nCount =1 THENMoveL Offs(Vision ,0,0,Compensation{nCountZ,3}+100) ,vBigMaxEmpty ,z20 ,tGripper\WOb j:=WobjCompressor ;MoveJ Offs(Vision ,0,0,Compensation{nCountZ,3}),vBigMaxEmpty ,fine ,tGripper\WObj:=W objCompressor ;GOTO C;ENDIFIncr reg2 ;rnXCal ;pVision:=Offs(Vision ,Compensation{nCountX,1} ,Compensation{nCountY,2} ,Compensation{ nCountZ,3});MoveLpVision,vMidEmpty ,fine,tGripper\WObj:=WobjCompressor;C:nPresenceOrAbsence:=1;! rServer;! socketsend client_socket \Str:="T1";! socketReceive client_socket \Str:=stReceived;! bOK:=StrToVal(stReceived,nPresenceOrAbsence);! IF nPresenceOrAbsence =2 OR nPresenceOrAbsence =3 THEN ! bSMPreOrAbs:=FALSE ;! ELSEIF nPresenceOrAbsence =5 THEN! bSMPreOrAbs:=TRUE ;! IF di02_PhotoelectricSensor =1 THEN! TPErase ;! TPWrite "The camera get picture there is a problem, Please check it!(2) ";! SystemStopAction \Halt ;! ELSEIF di02_PhotoelectricSensor =0 THEN! reg2 :=0;! ENDIF! ELSEIF nPresenceOrAbsence =1 OR nPresenceOrAbsence =4 THEN! TPErase ;! TPWrite "The Camera is wrong, and camera will restart ,"; ! TPWrite "after a few seconds the robot will continue ";! Stop ;! ENDIFWaitTime 1;IF di02_PhotoelectricSensor =1 THENbSMPreOrAbs:=FALSE ;ELSEbSMPreOrAbs:=TRUE ;ENDIFIncr nCountX;! TEST di16_giBCD! CASE 1:IF nCountX=6 THENnCountX :=1;Incr nCountY;IF nCountY=9 THENnCountY :=1;SystemStopAction \Halt;ENDIFENDIFIF nCount =40 THENnCount :=0;Incr nCountZ;nCountX:=1;nCountY:=1;nInpos:=0;IF reg2=0 THENrZhanban ;IF nCountZ=2 THENrCompressorOutPos;nCountZ:=1;ENDIFENDIFreg2:=0;ENDIF! DEFAULT:! TPErase ;! TPWrite " The PLC communication there is a problem ,Please check it ";! ENDTESTENDWHILEENDPROCPROC rPick ()! TEST di16_giBCD! CASE 1:IF nCountX=1 THENCompensationTwo:=CompensationA1;ELSECompensationTwo:=CompensationA2;ENDIFpPick:=Offs(Pick,CompensationTwo{nCountX,1}+nX ,CompensationTwo{nCountY,2},Compe nsationTwo{nCountZ,3});! CASE 2:! pPick:=Offs(Pick,CompensationTwo{nCountX,1}+nX ,CompensationTwo{nCountY,2},Compe nsationTwo{nCountZ,3});! DEFAULT:! TPErase ;! TPWrite " The PLC communication there is a problem ,Please check it ";! ENDTESTWaitTime 0.2;nCamOut:=1;! rServer;! socketsend client_socket \Str:="T2";! socketReceive client_socket \Str:=stReceived;! XData:= StrPart(stReceived, 3, NumCharacters);! YData:= StrPart(stReceived, NumCharacters, NumCharacters);! AngleData:= StrPart(stReceived, 2*NumCharacters, NumCharacters);! bOK:=StrToVal(XData,nXOffs);! bOK:=StrToVal(YData,nYOffs);! bOK:=StrToVal(AngleData,nAngleOffs);rAngle;! ppPick :=RelTool (pPick,0,0,0\Rz:= -nAngleOffs+nAngle);ppPick :=RelTool (pPick,0,0,0\Rz:= nAngle);ConfL\Off;! MoveL Offs (ppPick,nXOffs ,nYOffs,120), vMinEmpty, fine , tGripper\WObj:=WobjCompressor;! MoveL Offs (ppPick,nXOffs ,nYOffs,0), v100, fine ,tGripper\WObj:=WobjCompressor;MoveL Offs(ppPick,0 ,0,200), vMinEmpty, fine ,tGripper\WObj:=WobjCompressor;MoveL Offs(ppPick,0 ,0,0), v100, fine ,tGripper\WObj:=WobjCompressor;Reset do00_JawsOpen ;Set do01_JawsClose ;WaitDI di00_JawsInClose,1;! MoveL Offs(ppPick,nXOffs,nYOffs,80),vMinLoad ,z10 ,tGripper\WObj:=WobjCompressor ; ! MoveL Offs(ppPick,nXOffs,nYOffs,400),vMaxLoad ,fine ,tGripper\WObj:=WobjCompressor ;MoveL Offs(ppPick,0,0,80),vMinLoad ,z10 ,tGripper\WObj:=WobjCompressor ;MoveL Offs(ppPick,0,0,400),vMaxLoad ,fine ,tGripper\WObj:=WobjCompressor ;ConfL\On;rPlace;! TEST di16_giBCD! CASE 1:IF nCount =40 THENnCount :=0;nCountX:=1;nCountY:=1;reg2:=0;nInpos:=0;rZhanban ;IF nCountZ=2 THENrCompressorOutPos;nCountZ:=1;ENDIFENDIF! DEFAULT:! TPErase ;! TPWrite " The PLC communication there is a problem ,Please check it ";! ENDTESTENDPROCPROC rPlace ()MoveJ Offs (Place,0,0,200) ,vMaxLoad ,z40 , tGripper\WObj:= WobjCompressor ;Set do13_Placing;Waitdi di06_AssemblyLineOK, 0;MoveL Offs (Place,0,0,50) ,vMinLoad ,z5 , tGripper\WObj:= WobjCompressor;MoveL Place ,v100 ,fine , tGripper\WObj:= WobjCompressor;Reset do01_JawsClose ;Set do00_JawsOpen ;WaitDI di01_JawsInOpen ,1;MoveL Offs (Place,0,0,300) ,vMinEmpty ,fine, tGripper\WObj:= WobjCompressor;Reset do13_Placing ;WaitTime 0.4;Reset do00_JawsOpen ;ENDPROCPROC rnXCal()! TEST di16_giBCD! CASE 1:TEST nCountCASE 1,2,3,4,5,11,12,13,14,15,21,22,23,24,25,31,32,33,34,35 :nX:=0;CASE 6,7,8,9,10,16,17,18,19,20, 26,27,28,29,30,36,37,38,39,40 :nX:=65;ENDTEST! ENDTESTENDPROCPROC rAngle()! TEST di16_giBCD! CASE 1:TEST nCountCASE 1,2,3,4,5,11,12,13,14,15,21,22,23,24,25,31,32,33,34,35 :nAngle:=0;CASE 6,7,8,9,10,16,17,18,19,20, 26,27,28,29,30,36,37,38,39,40 :nAngle:=180;ENDTEST! ENDTESTENDPROCPROC rZhanban ()nTotalPalletHigh:=nTotalPalletHigh+nPalletHigh;TEST nCountZCASE 1:PZhanban:=PZhanbanUp;nPalletHigh:=nPalletHighUp;CASE 2:PZhanban:=PZhanbanDown;nPalletHigh:=nPalletHighDown;DEFAULT :TPErase ;TPWrite" The 'nCountZ' have a trouble ,Please check it ";ENDTESTMoveJ Offs ( PZhanban,0,0,300) ,vBigMaxEmpty ,fine , tGripper\WObj:= WobjCompressor ;Set do02_BigJaws1 ;WaitTime 0.6;Set do03_BigJaws2;WaitDI di08_BigJawsInOpen, 1;MoveL PZhanban ,vMinEmpty ,fine, tGripper\WObj:= WobjCompressor ;WaitTime 0.5;Reset do02_BigJaws1 ;Reset do03_BigJaws2 ;WaitTime 0.3;WaitDI di08_BigJawsInOpen,1;pActualpos:=CRobT(\Tool:=tGripper\WObj:=WobjCompressor1);pActualpos.trans.z:=2000;MoveL pActualpos,vMinEmpty,z20,tGripper\WObj:=WobjCompressor1 ; ! MoveL Offs ( PZhanban,0,0,400) ,vMinEmpty ,z20 ,tGripper\WObj:= WobjCompressor ;WaitDI di05_PalletInPos ,1;IF nInpos =2 THENMoveJ PZhanbanSafe ,vMinEmpty ,z20 , tGripper\WObj:= WobjCompressor ;ENDIFpActualpos:=PlaceZhanban;pActualpos.trans.z:=2000;MoveJ pActualpos,vMinLoad,z20,tGripper\WObj:=WobjCompressor1 ;! MoveJ Offs ( PlaceZhanban ,0,0,nTotalPalletHigh+700 ) ,v500 ,z5 , tGripper\WObj:= WobjCompressor ;MoveL Offs ( PlaceZhanban ,0,0,nTotalPalletHigh) ,vMinLoad ,fine, tGripper\WObj:= WobjCompressor ;WaitTime 0.5;Set do02_BigJaws1 ;Set do03_BigJaws2;WaitDI di08_BigJawsInOpen,1;MoveL Offs( PlaceZhanban ,0,0,nTotalPalletHigh+700 ) ,vMaxEmpty ,fine,tGripper\WObj:= WobjCompressor ;Reset do03_BigJaws2;WaitTime 0.6;Reset do02_BigJaws1;WaitDI di07_BigJawsInClose, 1;rCompressorOutPos;IF nTotalPalletHigh>1200 THENset do06_PalletEmpty ;waittime 1;reset do06_PalletEmpty;nTotalPalletHigh:=0;nPalletHigh:=0;ENDIFENDPROCPROC rCompressorInPos()! bInpos:=TRUE;! WHILE bInpos=TRUE DO! IF di03_Compressor1InPos =1 THENWobjCompressor:=WobjCompressor1;bInpos:= FALSE;nInpos:=2;! ELSEIF di04_Compressor2InPos =1 THEN! WobjCompressor:=WobjCompressor2;! bInpos:=FALSE;! nInpos:=3;! ELSE! bInpos:=TRUE;! ENDIF! WaitTime 0.3;! ENDWHILEENDPROCPROC rCompressorOutPos()IF WobjCompressor=WobjCompressor1 THENSet do04_Compressor1Empty ;WaitTime 1;Reset do04_Compressor1Empty;ELSEIF WobjCompressor=WobjCompressor1 THENSet do05_Compressor2Empty ;WaitTime 1;Reset do05_Compressor2Empty;ENDIFENDPROCFUNC bool CurrentPos(robtarget ComparePos,INOUT tooldata TCP) VAR num Counter:=0;VAR robtarget ActualPos;ActualPos:=CRobT(\Tool:=TCP\WObj:=wobj0);IF ActualPos.trans.x>ComparePos.trans.x-25 AND ActualPos.trans.x<ComparePos.trans.x+25 Counter:=Counter+1;IF ActualPos.trans.y>ComparePos.trans.y-25 AND ActualPos.trans.y<ComparePos.trans.y+25 Counter:=Counter+1;IF ActualPos.trans.z>ComparePos.trans.z-25 AND ActualPos.trans.z<ComparePos.trans.z+25 Counter:=Counter+1;IF ActualPos.rot.q1>ComparePos.rot.q1-0.1 AND ActualPos.rot.q1<ComparePos.rot.q1+0.1 Counter:=Counter+1;IF ActualPos.rot.q2>ComparePos.rot.q2-0.1 AND ActualPos.rot.q2<ComparePos.rot.q2+0.1 Counter:=Counter+1;IF ActualPos.rot.q3>ComparePos.rot.q3-0.1 AND ActualPos.rot.q3<ComparePos.rot.q3+0.1 Counter:=Counter+1;IF ActualPos.rot.q4>ComparePos.rot.q4-0.1 AND ActualPos.rot.q4<ComparePos.rot.q4+0.1 Counter:=Counter+1;RETURN Counter=7;ENDFUNCPROC rCheckHomePos()IF NOT CurrentPos(pHome,tGripper) THENpActualpos:=CRobT(\Tool:=tGripper\WObj:=wobj0);pActualpos.trans.z:=pHome.trans.z;MoveL pActualpos,v300,z30,tGripper;MoveJ pHome,v500,fine,tGripper;ENDIFENDPROCPROC rKindChoose()! IF di16_giBCD=1 THENCompensation:=CompensationA;nPalletHighUp:=nPalletHighUpA;nPalletHighDown:=nPalletHighDownA;Pick:= PickA;Vision :=VisionA;Place:=PlaceA;PZhanbanUp:=PZhanbanUpA;PZhanbanDown:=PZhanbanDownA;PlaceZhanban:=PlaceZhanbanA;! ELSEIF di16_giBCD=2 THEN! Compensation:=CompensationB;! nPalletHighUp:=nPalletHighUpB;! nPalletHighDown:=nPalletHighDownB;! Pick:= PickB;! Vision :=VisionB;! Place:=PlaceB;! PZhanbanUp:=PZhanbanUpB;! PZhanbanDown:=PZhanbanDownB;! PlaceZhanban:=PlaceZhanbanB;! ELSE! TPErase;! TPWrite "Please select product type in the PLC";! ENDIFENDPROCPROC rModPos ()MoveJ pHome, v1000, fine, tGripper\WObj:=Wobj0;MoveJ PZhanbanSafe, v1000, fine, tGripper\WObj:=WobjCompressor1;MoveJ PickA, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ VisionA , v1000, fine, tGripper\WObj:=WobjCompressor1 ;MoveJ PlaceA, v1000, fine, tGripper\WObj:=WobjCompressor1;MoveJ PZhanbanUpA, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ PZhanbanDownA, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ PlaceZhanbanA, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ PickB, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ VisionB , v1000, fine, tGripper\WObj:=WobjCompressor1 ;MoveJ PlaceB, v1000, fine, tGripper\WObj:=WobjCompressor1;MoveJ PZhanbanUpB, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ PZhanbanDownB, v1000, fine, tGripper\WObj:= WobjCompressor1;MoveJ PlaceZhanbanB, v1000, fine, tGripper\WObj:= WobjCompressor1;ENDPROCPROC rMoveAbsj()MoveAbsJ jposHome, v1000, fine, tGripper\WObj:=wobj0;ENDPROCPROC rServer()SocketClose server_socket;SocketClose client_socket;SocketCreate server_socket;SocketBind server_socket, "192.168.125.1", 3001;SocketListen server_socket;SocketAccept server_socket,client_socket\ClientAddress:=client_ip;ENDPROCPROC rClient()SocketClose client_socket;SocketCreate client_socket;SocketConnect client_socket, "192.168.125.120", 1000;SocketReceive client_socket \Str:=stReceived;ENDPROCENDMODULE。