任务:码垛(将1号托盘物料搬运到2号托盘,再从2号托盘搬回1号托盘)
编程思路:
1.初始化
机器人回原点(机器人在任何位置时,先上升与Home点相同的高度);这里使用CRobT函数读取当前机器人位置数据,使用trans将原点的Z值赋与当前位置,最后再回原点。目的使机器人上升到安全高度,避免与其他物体相撞。
PActualpos := CRobT(\Tool:=tool0);
PActualpos.trans.z := pome.trans.z;
MoveL PActualpos, v400, fine, tool0;
MoveJ pome, v400, fine, tool0;
复位吸取物料控制的IO口
Reset out1;
2.例行程序的创建
正向取物料:rPick(); 反向取物料:rPick1();
正向放物料:rPlace(); 正向放物料:rPlace1();
物料位置的存储与读取:
PROC Pposition1()
TEST nCount
CASE 5:
Pplace11111 := Offs(pick10,116,236,0);
CASE 3:
Pplace11111 := Offs(pick10,118,0,0);
CASE 4:
Pplace11111 := Offs(pick10,118,116,0);
CASE 2:
Pplace11111 := Offs(pick10,0,116,0);
CASE 1:
Pplace11111 := pick10;
ENDTEST
ENDPROC
核心程序:
PROC Plan1()
IF nCount < 6 THEN
Pposition1;
Pposition2;
rPick1;
rPlace1;
nCount := nCount + 1;
ENDIF
ENDPROC
PROC Plan2()
IF nCount < 11 AND nCount > 5 THEN
Pposition21;
Pposition12;
rPick;
rPlace;
nCount := nCount + 1;
ENDIF
ENDPROC
3.主程序
进入WHILE循环之前 要 进行初始化程序(执行一遍)
PROC main()
rInitALL;
WHILE TRUE DO
Plan1;
Plan2;
ENDWHILE
ENDPROC
4.全部程序
MODULE MainModule
CONST jointtarget Phome:=[[0,0,0,0,90,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget PActualpos:=[[451,0,807.1],[7.48733E-9,0,-1,0],[-1,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR robtarget pome:=[[451.00,-0.00,807.10],[7.48733E-9,2.23406E-16,-1,5.8648E-21],[-1,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS robtarget PActualpos10:=[[451.00,-100.25,339.68],[6.55086E-8,-1.74789E-8,-1,-2.44434E-8],[-1,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pickHI10:=[[363.36,319.99,555.98],[7.73632E-8,-8.36364E-9,-1,5.01728E-8],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS robtarget pick10:=[[362.49,204.39,363.89],[2.85755E-9,-7.86116E-8,1,1.24461E-8],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget placeH11:=[[501.87,-179.26,406.34],[8.74706E-8,-9.88528E-9,-1,3.10498E-8],[-1,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget place10:=[[501.87,-179.26,259.88],[8.25076E-8,-6.76428E-8,-1,3.28225E-8],[-1,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pickHI20:=[[451.00,0.00,807.10],[2.81375E-8,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget placeH21:=[[451.00,0.00,807.10],[2.81375E-8,0,-1,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget pickHI30:=[[451.00,0.00,807.10],[7.48734E-9,1.84201E-15,-1,3.33866E-20],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget placeH31:=[[451.00,0.00,807.10],[7.48734E-9,1.84201E-15,-1,3.33866E-20],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget placeH41:=[[451.00,0.00,807.10],[7.48734E-9,1.84201E-15,-1,3.33866E-20],[0,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS robtarget Pplace11111:=[[478.49,440.39,363.89],[2.85755E-9,-7.86116E-8,1,1.24461E-8],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS robtarget Pplace2222:=[[501.87,-179.26,259.88],[8.25076E-8,-6.76428E-8,-1,3.28225E-8],[-1,-1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PROC main()
rInitALL;
WHILE TRUE DO
Plan1;
Plan2;
ENDWHILE
ENDPROC
PROC rInitALL()
!CONST robtarget PActualpos10:=[[451.00,-100.25,339.68],[5.73553E-9,1.74789E-8,1,8.60696E-9],[-1,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
Reset out1;
nCount := 1;
PActualpos := CRobT(\Tool:=tool0);
PActualpos.trans.z := pome.trans.z;
MoveL PActualpos, v400, fine, tool0;
MoveJ pome, v400, fine, tool0;
ENDPROC
PROC rSetPos()
MoveAbsJ Phome\NoEOffs, v400, fine, tool0;
MoveL PActualpos, v400, fine, tool0;
MoveL pick10, v400, fine, tool0;
MoveL pickHI10, v400, fine, tool0;
MoveL placeH11, v400, fine, tool0;
MoveL place10, v400, fine, tool0;
ENDPROC
PROC rPick()
MoveJ Offs(Pplace2222,0,0,100), v400, fine, tool0;
MoveL Pplace2222, v400, fine, tool0;
Set out1;
MoveJ Offs(Pplace2222,0,0,100), v400, fine, tool0;
MoveAbsJ Phome\NoEOffs, v400, fine, tool0;
ENDPROC
PROC rPick1()
MoveJ Offs(Pplace11111,0,0,100), v400, fine, tool0;
MoveL Pplace11111, v400, fine, tool0;
Set out1;
MoveJ Offs(Pplace11111,0,0,100), v400, fine, tool0;
MoveAbsJ Phome\NoEOffs, v400, fine, tool0;
ENDPROC
PROC rPlace()
MoveJ Offs(Pplace11111,0,0,100), v400, fine, tool0;
MoveL Pplace11111, v400, fine, tool0;
Reset out1;
MoveJ Offs(Pplace11111,0,0,100), v400, fine, tool0;
MoveAbsJ Phome\NoEOffs, v400, fine, tool0;
ENDPROC
PROC rPlace1()
MoveJ Offs(Pplace2222,0,0,100), v400, fine, tool0;
MoveL Pplace2222, v400, fine, tool0;
Reset out1;
MoveJ Offs(Pplace2222,0,0,100), v400, fine, tool0;
MoveAbsJ Phome\NoEOffs, v400, fine, tool0;
ENDPROC
PROC Pposition1()
TEST nCount
CASE 5:
Pplace11111 := Offs(pick10,116,236,0);
CASE 3:
Pplace11111 := Offs(pick10,118,0,0);
CASE 4:
Pplace11111 := Offs(pick10,118,116,0);
CASE 2:
Pplace11111 := Offs(pick10,0,116,0);
CASE 1:
Pplace11111 := pick10;
ENDTEST
ENDPROC
PROC Pposition12()
TEST nCount
CASE 10:
Pplace11111 := Offs(pick10,116,236,0);
CASE 8:
Pplace11111 := Offs(pick10,118,0,0);
CASE 9:
Pplace11111 := Offs(pick10,118,116,0);
CASE 7:
Pplace11111 := Offs(pick10,0,116,0);
CASE 6:
Pplace11111 := pick10;
ENDTEST
ENDPROC
PROC Pposition2()
TEST nCount
CASE 5:
Pplace2222 := Offs(place10,168,0,20);
CASE 3:
Pplace2222 := Offs(place10,236,0,0);
CASE 4:
Pplace2222 := Offs(place10,59,0,20);
CASE 2:
Pplace2222 := Offs(place10,118,0,0);
CASE 1:
Pplace2222 := place10;
ENDTEST
ENDPROC
PROC Pposition21()
TEST nCount
CASE 6:
Pplace2222 := Offs(place10,168,0,20);
CASE 8:
Pplace2222 := Offs(place10,236,0,0);
CASE 7:
Pplace2222 := Offs(place10,59,0,20);
CASE 9:
Pplace2222 := Offs(place10,118,0,0);
CASE 10:
Pplace2222 := place10;
ENDTEST
ENDPROC
PROC Plan1()
IF nCount < 6 THEN
Pposition1;
Pposition2;
rPick1;
rPlace1;
nCount := nCount + 1;
ENDIF
ENDPROC
PROC Plan2()
IF nCount < 11 AND nCount > 5 THEN
Pposition21;
Pposition12;
rPick;
rPlace;
nCount := nCount + 1;
ENDIF
ENDPROC
ENDMODULE
总结反思
1.对于程序数据的创建不够熟悉
2.MainModule中例行程序太多,应当单独建立模块存储该任务的例行程序
3.算法有待改进,程序可以更加精简