ABB机器人程序实例ROBOT studio 6.01(附带与工业相机网络通讯实

ABB机器人（ROBOT studio 6.01）程序实例

MODULE MainModule PERS tooldata

tGripper:=[TRUE,[[0.533078,1.51617,583.739],[1,0,0,0]],[30,[0,0,50],[1,0,0,0],0,0,0]];

TASK PERS wobjdata

VisionWobj:=[FALSE,TRUE,\,[[0,0,0],[1,0,0,0]],[[-934.534,1807.34,-76.7707],[0.400996,0.0128267,-0.0292473,-0.915523]]]; TASK PERS wobjdata

WobjCompressor1:=[FALSE,TRUE,\,[[518.656,-1088.9,164.25],[0,0,0,1]],[[686.651,296.298,-588.529],[0.917114,1.69419E-06,-7.35001E-05,-0.398626]]]; TASK PERS wobjdata

WobjCompressor2:=[FALSE,TRUE,\,[[518.656,-1088.9,164.25],[0,0,0,1]],[[-944.871,-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 num

CompensationErr{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 jointtarget

jposHome:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget

pHome:=[[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 robtarget

PZhanbanSafe:=[[-162.69,-1703.07,2270.38],[0.0109452,-0.955357,-0.295179,-0.00645602],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

CONST robtarget

VisionA:=[[-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 robtarget

PickA:=[[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 robtarget

PlaceA:=[[-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 robtarget

PZhanbanUpA:=[[549.64,541.39,821.21],[0.000808037,-0.999985,-0.00498684,-0.00209158],[-2,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget

PZhanbanDownA:=[[548.40,560.53,179.18],[0.00417276,0.999929,-0.00436864,0.0102239],[-2,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget

PlaceZhanbanA:=[[2456.73,-1154.76,205.32],[0.0106126,-0.0247295,-0.999603,0.00838785],[1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; PERS num

CompensationA{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 num

CompensationA1{8,3}:=[[860,936,0],[0,0,0],[215,133,99999999],[430,264,99999999],[645,403,99999999],[99999999,533,99999999],[99999999,670,99999999],[99999999,800,99999999]]; PERS num

CompensationA2{8,3}:=[[860,0,0],[0,133,0],[215,264,99999999],[430,403,99999999],[645,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[99999999,936,99999999]];

CONST num nPalletHighUpA:=100; CONST num nPalletHighDownA:=200;

CONST robtarget

VisionB:=[[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 robtarget

PickB:=[[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 robtarget

PlaceB:=[[-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 robtarget

PZhanbanUpB:=[[511.86,550.08,92.05],[0.00172284,-0.00758955,-0.99997,-0.000606164],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget

PZhanbanDownB:=[[511.86,550.08,92.05],[0.00172284,-0.00758955,-0.99997,-0.000606164],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget

PlaceZhanbanB:=[[511.86,550.08,92.05],[0.00172284,-0.00758955,-0.99997,-0.000606164],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; PERS num

CompensationB{8,3}:=[[0,0,300],[210,136,0],[430,264,99999999],[645,403,99999999],[855,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[99999

999,936,99999999]]; PERS num

CompensationB1{8,3}:=[[860,936,300],[0,0,0],[215,133,99999999],[430,264,99999999],[645,403,99999999],[99999999,533,99999999],[99999999,670,99999999],[99999999,800,99999999]]; PERS num

CompensationB2{8,3}:=[[860,0,300],[0,133,0],[215,264,99999999],[430,403,99999999],[645,533,99999999],[99999999,670,99999999],[99999999,800,99999999],[99999999,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 wobjdata

wobj1:=[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 DO rPickCal; rPick; ENDWHILE WaitTime 0.3; ENDPROC

PROC 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; ENDPROC

PROC rPickCal ()

Set do00_JawsOpen ; bSMPreOrAbs:=TRUE; IF nCount =0 THEN rCompressorInPos; ENDIF

WHILE bSMPreOrAbs=TRUE DO Incr nCount ;

IF nCount =1 THEN MoveL Offs

(Vision ,0,0,Compensation{nCountZ,3}+100) ,vBigMaxEmpty ,z20 ,tGripper\\WObj:=WobjCompressor ;

MoveJ Offs

(Vision ,0,0,Compensation{nCountZ,3}),vBigMaxEmpty ,fine ,tGripper\\WObj:=WobjCompressor ;

GOTO C; ENDIF Incr reg2 ; rnXCal ;

pVision:=Offs

(Vision ,Compensation{nCountX,1} ,Compensation{nCountY,2} ,Compensation{nCountZ,3});

MoveL

pVision,vMidEmpty ,fine,tGripper\\WObj:=WobjCompressor; C:

nPresenceOrAbsence:=1; ! rServer;

! socketsend client_socket \\Str:=\

! 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 \ 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 \ ! TPWrite \ ! Stop ; ! ENDIF

WaitTime 1;

IF di02_PhotoelectricSensor =1 THEN bSMPreOrAbs:=FALSE ; ELSE

bSMPreOrAbs:=TRUE ; ENDIF

Incr nCountX;

! TEST di16_giBCD ! CASE 1:

IF nCountX=6 THEN nCountX :=1; Incr nCountY;

IF nCountY=9 THEN nCountY :=1;

SystemStopAction \\Halt; ENDIF ENDIF

IF nCount =40 THEN nCount :=0; Incr nCountZ; nCountX:=1; nCountY:=1;

nInpos:=0;

IF reg2=0 THEN rZhanban ;

IF nCountZ=2 THEN rCompressorOutPos; nCountZ:=1; ENDIF ENDIF reg2:=0; ENDIF ! DEFAULT: ! TPErase ;

! TPWrite \check it \

! ENDTEST ENDWHILE ENDPROC

PROC rPick ()

! TEST di16_giBCD ! CASE 1:

IF nCountX=1 THEN

CompensationTwo:=CompensationA1; ELSE

CompensationTwo:=CompensationA2; ENDIF pPick:=Offs

(Pick,CompensationTwo{nCountX,1}+nX ,CompensationTwo{nCountY,2},CompensationTwo{nCountZ,3});

! CASE 2:

! pPick:=Offs

(Pick,CompensationTwo{nCountX,1}+nX ,CompensationTwo{nCountY,2},CompensationTwo{nCountZ,3});

! DEFAULT: ! TPErase ;

! TPWrite \it \

! ENDTEST WaitTime 0.2; nCamOut:=1; ! rServer;

! socketsend client_socket \\Str:=\

! 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 THEN nCount :=0; nCountX:=1; nCountY:=1; reg2:=0; nInpos:=0; rZhanban ;

IF nCountZ=2 THEN rCompressorOutPos; nCountZ:=1; ENDIF ENDIF ! DEFAULT: ! TPErase ;

! TPWrite \it \

! ENDTEST ENDPROC

PROC 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 ; ENDPROC

PROC rnXCal()

! TEST di16_giBCD ! CASE 1:

TEST nCount

CASE 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 ! ENDTEST ENDPROC

PROC rAngle()

! TEST di16_giBCD ! CASE 1:

TEST nCount

CASE 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 ! ENDTEST ENDPROC

PROC rZhanban ()

nTotalPalletHigh:=nTotalPalletHigh+nPalletHigh; TEST nCountZ CASE 1:

PZhanban:=PZhanbanUp; nPalletHigh:=nPalletHighUp; CASE 2:

PZhanban:=PZhanbanDown; nPalletHigh:=nPalletHighDown; DEFAULT : TPErase ;

TPWrite \; ENDTEST

MoveJ 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 THEN

MoveJ PZhanbanSafe ,vMinEmpty ,z20 , tGripper\\WObj:= WobjCompressor ; ENDIF

pActualpos:=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 THEN set do06_PalletEmpty ; waittime 1;

reset do06_PalletEmpty; nTotalPalletHigh:=0; nPalletHigh:=0; ENDIF ENDPROC

PROC rCompressorInPos() ! bInpos:=TRUE;

! WHILE bInpos=TRUE DO

! IF di03_Compressor1InPos =1 THEN WobjCompressor:=WobjCompressor1; bInpos:= FALSE; nInpos:=2;

! ELSEIF di04_Compressor2InPos =1 THEN ! WobjCompressor:=WobjCompressor2; ! bInpos:=FALSE; ! nInpos:=3; ! ELSE

! bInpos:=TRUE; ! ENDIF

! WaitTime 0.3; ! ENDWHILE ENDPROC

PROC rCompressorOutPos()

IF WobjCompressor=WobjCompressor1 THEN Set do04_Compressor1Empty ; WaitTime 1;

Reset do04_Compressor1Empty;

ELSEIF WobjCompressor=WobjCompressor1 THEN Set do05_Compressor2Empty ; WaitTime 1;

Reset do05_Compressor2Empty; ENDIF ENDPROC

FUNC 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.xComparePos.trans.y-25 AND ActualPos.trans.yComparePos.trans.z-25 AND ActualPos.trans.zComparePos.rot.q1-0.1 AND ActualPos.rot.q1ComparePos.rot.q2-0.1 AND ActualPos.rot.q2ComparePos.rot.q3-0.1 AND ActualPos.rot.q3ComparePos.rot.q4-0.1 AND ActualPos.rot.q4

PROC rCheckHomePos()

IF NOT CurrentPos(pHome,tGripper) THEN

pActualpos:=CRobT(\\Tool:=tGripper\\WObj:=wobj0); pActualpos.trans.z:=pHome.trans.z; MoveL pActualpos,v300,z30,tGripper; MoveJ pHome,v500,fine ,tGripper; ENDIF ENDPROC

PROC rKindChoose()

! IF di16_giBCD=1 THEN

Compensation:=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 \ ! ENDIF ENDPROC

PROC 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;

ENDPROC

PROC rMoveAbsj() MoveAbsJ jposHome, v1000, fine, tGripper\\WObj:=wobj0; ENDPROC

PROC rServer()

SocketClose server_socket; SocketClose client_socket; SocketCreate server_socket;

SocketBind server_socket, \, 3001; SocketListen server_socket; SocketAccept server_socket,

client_socket\\ClientAddress:=client_ip; ENDPROC

PROC rClient()

SocketClose client_socket; SocketCreate client_socket;

SocketConnect client_socket, \, 1000; SocketReceive client_socket \\Str:=stReceived; ENDPROC

ENDMODULE

本文来源：https://www.bwwdw.com/article/s0y6.html