ABB 机械手模板一
ABB 机械手常用的编程方式大概有3种：
- 步进控制逻辑和运动指令都写在机械手侧。通常这种场景下，机械手相对独立，很少需要和 PLC 做互动。
- 步进控制逻辑写在PLC侧，运动类指令写在机械手侧。PLC 做主站，把机械手作为 PLC 的一个运动单元使用。
- 步进控制逻辑和运动指令都写在PLC侧。这种控制方式需要 PLC 机器人库支持，一套库可以适用市面上大部分机械人品牌。
下面我们就用一个项目来讨论第一种情况，具体的细节可以观看我在B站的视频，视频下方有项目用到的所有文件。下面是此项目的现场布局。
这个项目的状态机如下，如果不清楚状态机工作原理，可以看一下以前的博文。一定要理解状态机，否则很难理解下面的程序。
根据这个状态机，可以大致写出机械手程序的步进逻辑，也就是程序模板：
MODULE Module1
CONST robtarget pHome:=[[661.17000394,-0.000000007,932.979976626],[0.000000032,0.707106807,0.707106756,0.000000001],[0,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pMoveLeft:=[[215.169852439,647.52813584,933.300032121],[0.000000078,-0.000000318,1,0.000000073],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pMoveRight:=[[505.170661339,-712.4716769,1293.299874561],[-0.000000073,1,0.000000318,0.000000078],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceLeftBoard:=[[1800.850801536,497.513997982,130.529915358],[-0.000000013,0.707106863,-0.7071067,0.000000001],[-1,0,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceRightBoard:=[[600.020546005,497.514289299,130.530055522],[0.000000001,-0.707106756,0.707106806,-0.000000045],[-2,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPickBoardOnLeft:=[[728,826.433,134.998],[0,0.707106781,0.707106781,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceBottomOnLeft:=[[728.000053648,1301.433708287,119.998908843],[0.000000001,-0.000000081,1,-0.000000008],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceTopOnLeft:=[[1278,826.433,134.998],[0,0.707106781,0.707106781,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPickBottom:=[[600.111614175,24.934806253,478.963028778],[0.000000069,0.000000965,1,-0.000000044],[1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPickTop:=[[1450.011903438,500.006872851,300.523785297],[0.000000028,0.70710679,0.707106772,-0.000000023],[1,0,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPickBoardOnRight:=[[755,700.001,134.999],[0,0.707106781,0.707106781,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceBottomOnRight:=[[755,225.001,120],[0,1,0,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceTopOnRight:=[[1305,700.001,134.999],[0,0.707106781,0.707106781,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR num choice:=0;
VAR bool isLeftEmpty:=TRUE;
VAR bool isRightEmpty:=TRUE;
VAR bool canMoveLeftBoard:=FALSE;
VAR bool canMoveRightBoard:=FALSE;
VAR intnum intno1:=0;
VAR intnum intno2:=0;
PROC main()
Init;
WHILE TRUE DO
TEST choice
CASE 0:
GoHome;
choice:=1;
CASE 1:
Wait;
IF isLeftEmpty=TRUE THEN
isLeftEmpty:=FALSE;
choice:=2;
ELSEIF isRightEmpty=TRUE THEN
isRightEmpty:=FALSE;
choice:=3;
ELSEIF canMoveLeftBoard=TRUE THEN
canMoveLeftBoard:=FALSE;
choice:=4;
ELSEIF canMoveRightBoard=TRUE THEN ! 转移条件
canMoveRightBoard:=FALSE; ! 条件使用完，就置为false
choice:=5; ! 设置为需要转移的布号
ENDIF
CASE 2:
LeftAssembly;
PulseDO\PLength:=1.0,Do03_LeftAssemblyDone;
choice:=0;
CASE 3:
RightAssembly;
PulseDO\PLength:=1.0,Do04_RightAssemblyDone;
choice:=0;
CASE 4:
LeftMove;
isLeftEmpty:=TRUE;
canMoveLeftBoard:=FALSE;
choice:=0;
CASE 5:
RightMove;
isRightEmpty:=TRUE;
canMoveRightBoard:=FALSE;
choice:=0;
DEFAULT:
TPWrite "choice error";
Stop;
ENDTEST
WaitTime 0.5;
ENDWHILE
ENDPROC
PROC Init()
IDelete intno1;
CONNECT intno1 WITH LeftBindingDone;
ISignalDI Di03_IsLeftBindingDone,1,intno1;
IDelete intno2;
CONNECT intno2 WITH RightBindingDone;
ISignalDI Di04_IsRightBindingDone,1,intno2;
choice:=0;
isLeftEmpty:=TRUE;
isRightEmpty:=TRUE;
canMoveLeftBoard:=FALSE;
canMoveRightBoard:=FALSE;
Reset Do03_LeftAssemblyDone;
Reset Do04_RightAssemblyDone;
Reset Do01_Vacuum1;
ENDPROC
PROC GoHome()
VAR robtarget pactualPos;
IF NOT CheckAtHomePos(pHome,Tool_Sucker) THEN
pactualPos:=CRobT(\Tool:=Tool_Sucker\WObj:=wobj0);
MoveL Offs(pactualPos,0,0,100),v200,z10,Tool_Sucker;
MoveJ pHome,v1000,fine,Tool_Sucker;
ENDIF
ENDPROC
FUNC bool CheckAtHomePos(robtarget comparePos,INOUT tooldata TCP)
VAR robtarget actualPos;
VAR bool isAtHomePos:=TRUE;
actualPos:=CRobT(\Tool:=TCP\Wobj:=wobj0);
IF actualPos.trans.x<comparePos.trans.x-200 OR actualPos.trans.x>comparePos.trans.x+200 THEN
isAtHomePos:=FALSE;
ELSEIF actualPos.trans.y<comparePos.trans.y-200 OR actualPos.trans.y>comparePos.trans.y+200 THEN
isAtHomePos:=FALSE;
ELSEIF actualPos.trans.z<comparePos.trans.z-200 OR actualPos.trans.z>comparePos.trans.z+200 THEN
isAtHomePos:=FALSE;
ELSEIF actualPos.rot.q1<comparePos.rot.q1-200 OR actualPos.rot.q1>comparePos.rot.q1+200 THEN
isAtHomePos:=FALSE;
ELSEIF actualPos.rot.q2<comparePos.rot.q2-200 OR actualPos.rot.q2>comparePos.rot.q2+200 THEN
isAtHomePos:=FALSE;
ELSEIF actualPos.rot.q3<comparePos.rot.q3-200 OR actualPos.rot.q3>comparePos.rot.q3+200 THEN
isAtHomePos:=FALSE;
ELSEIF actualPos.rot.q4<comparePos.rot.q4-200 OR actualPos.rot.q4>comparePos.rot.q4+200 THEN
isAtHomePos:=FALSE;
ENDIF
RETURN isAtHomePos;
ENDFUNC
PROC Wait()
WaitTime 0.5;
ENDPROC
PROC LeftAssembly()
ENDPROC
PROC RightAssembly()
ENDPROC
PROC LeftMove()
ENDPROC
PROC RightMove()
ENDPROC
PROC Point()
MoveL pHome,v2000,fine,Tool_Sucker\WObj:=wobj0;
MoveL pMoveLeft,v2000,fine,Tool_Sucker\WObj:=wobj0;
MoveL pMoveRight,v2000,fine,Tool_Sucker\WObj:=wobj0;
MoveL pPlaceLeftBoard,v2000,fine,Tool_Sucker\WObj:=Wobj_Board;
MoveL pPlaceRightBoard,v2000,fine,Tool_Sucker\WObj:=Wobj_Board;
MoveL pPickBoardOnLeft,v2000,fine,Tool_Sucker\WObj:=Wobj_Left;
MoveL pPlaceBottomOnLeft,v2000,fine,Tool_Sucker\WObj:=Wobj_Left;
MoveL pPlaceTopOnLeft,v2000,fine,Tool_Sucker\WObj:=Wobj_Left;
MoveL pPickBottom,v2000,fine,Tool_Sucker\WObj:=Wobj_materials;
MoveL pPickTop,v2000,fine,Tool_Sucker\WObj:=Wobj_materials;
MoveL pPickBoardOnRight,v2000,fine,Tool_Sucker\WObj:=Wobj_Right;
MoveL pPlaceBottomOnRight,v2000,fine,Tool_Sucker\WObj:=Wobj_Right;
MoveL pPlaceTopOnRight,v2000,fine,Tool_Sucker\WObj:=Wobj_Right;
ENDPROC
TRAP LeftBindingDone
canMoveLeftBoard:=TRUE;
ENDTRAP
TRAP RightBindingDone
canMoveRightBoard:=TRUE;
ENDTRAP
ENDMODULE
下面我简单介绍一下这个模板。
- 用一个死循环来表达这个状态机，在进入循环前，调用 init 函数，对一些变量进行初始化
- 在死循环里，通过 Test 语句完成各状态转换。
- case 语句的第一行，一般为调用执行动作子程序, 接着写转移条件，最后在转移条件下写待转移的步号。
- GoHome 这个子程序为回原点用，都是套路写法，就不做赘述。
- Point 子程序为方便示校用。
