KUKA機(jī)器人保養(yǎng)ABB安全區(qū)域和中斷一起連用案例解析

MODULE XXXX

!定義臨時(shí)全局區(qū)域數(shù)據(jù)

VAR wztemporary conveyor;

!定義全局區(qū)域形狀數(shù)據(jù)

VAR shapedata volume;

!定義中斷識(shí)別號(hào)

VAR intnum empty;

!定義全局區(qū)域形狀設(shè)定數(shù)據(jù)位置點(diǎn)1和點(diǎn)2

pers pos corner1:=[363.41,41.87,317.67];

pers pos corner2:=[613.41,-258.13,567.67];

!定義計(jì)算位置示教點(diǎn)

VAR

robtarget AnQua:=[[363.41,41.87,317.67],[0.261461,0.63764,0.229939,0.687155],[0,0,1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!定義左右移動(dòng)位置點(diǎn)

TASK

VAR

robtarget Zuo_AnQuanDian:=[[444.89,-401.76,501.87],[0.0966565,0.923747,0.326119,0.176057],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

TASK

VAR

robtarget You_AnQuanDian:=[[444.90,-164.34,501.86],[0.0966831,0.923746,0.32611,0.176061],[-1,0,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

!安全區(qū)域定義

PROC AnQuanQuYu_CheShi()

!計(jì)算安全區(qū)域大小

corner1.x:=AnQua.trans.x;

corner1.y:=AnQua.trans.y;

corner1.z:=AnQua.trans.z;

corner2.x:=AnQua.trans.x+250;

corner2.y:=AnQua.trans.y-300;

corner2.z:=AnQua.trans.z+250;

!定義一個(gè)箱形全局區(qū)域

WZBoxDef \Outside, volume, corner1, corner2;

!啟用全局區(qū)域限制監(jiān)控

!WZLimSup \Temp, conveyor, volume;

!啟用全局區(qū)域，設(shè)置數(shù)字信號(hào)輸出

WZDOSet \Temp, conveyor, \Before, volume, ZhongDuan1,1;

!停用臨時(shí)全局區(qū)域監(jiān)控

!WZDisable conveyor;

!重新啟用臨時(shí)全局區(qū)域監(jiān)控

!WZEnable conveyor;

!擦除臨時(shí)全局區(qū)域監(jiān)控

!WZFree conveyor;

!調(diào)用中斷例行程序

ZhongDuan;

 

!移動(dòng)左示教點(diǎn)（在安全區(qū)外，會(huì)觸發(fā)安全區(qū)）

Movej Zuo_AnQuanDian,v500,fine,tool0\WObj:=wobj0;

!Movej You_AnQuanDian,v500,fine,tool0\WObj:=wobj0;

!Movej Qian_AnQuanDian,v500,fine,tool0\WObj:=wobj0;

!Movej Hou_AnQuanDian,v500,fine,tool0\WObj:=wobj0;

ENDPROC

 

!中斷例行程序

PROC ZhongDuan()

!將中斷與軟中斷程序相連

CONNECT empty WITH QianHou_ZhongDuan;

!當(dāng)中斷識(shí)別號(hào)ZhongDuan1置為1時(shí)，自動(dòng)執(zhí)行軟中斷程序（QianHou_ZhongDuan）。

ISignalDO ZhongDuan1, 1, empty;

ENDPROC

!軟中斷程序

TRAP QianHou_ZhongDuan

!定義中斷發(fā)生時(shí)的保存位置的程序點(diǎn)

VAR robtarget p1;

!停止機(jī)械臂的移動(dòng)

StopMove;

! 發(fā)生中斷時(shí)，存儲(chǔ)路徑

StorePath;

!讀取中斷發(fā)生的位置保存給p1

p1:=CRobT();

!中斷發(fā)生后執(zhí)行網(wǎng)右走程序

Movej Hou_AnQuanDian,v500,fine,tool0\WObj:=wobj0;

!中斷結(jié)束后移動(dòng)到發(fā)生中斷時(shí)的位置

MoveL p1,v500,fine,tool0\WObj:=wobj0;

!中斷之后，恢復(fù)路徑（繼續(xù)從中斷點(diǎn)移動(dòng)到Zuo_AnQuanDian示教點(diǎn)）

RestoPath;

!重啟機(jī)械臂移動(dòng)

StartMove;

!完成程序的執(zhí)行

RETURN;

ENDTRAP

 

來源：廣科智能 轉(zhuǎn)載注明出處