ABB左右码垛详细解说.docx
《ABB左右码垛详细解说.docx》由会员分享，可在线阅读，更多相关《ABB左右码垛详细解说.docx（24页珍藏版）》请在冰豆网上搜索。
ABB左右码垛详细解说
MODULEMainMoudle
PERSwobjdataWobjPallet_L:
=[FALSE,TRUE,"",[[-456.216,-2058.49,-233.373],[1,0,0,0]],[[0,0,0],[1,0,0,0]]]
！
定义左侧码盘工件坐标系WobjPallet_L
PERSwobjdataWobjPallet_R:
=[FALSE,TRUE,"",[[-421.764,1102.39,-233.373],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
！
定义右侧码盘工件坐标系WobjPallet_R
PERStooldatatGripper:
=[TRUE,[[0,0,527],[1,0,0,0]],[20,[0,0,150],[1,0,0,0],0,0,0]];
！
定义工具坐标系数据tGripper
PERSloaddataLoadFull:
=[20,[0,0,300],[1,0,0,0],0,0,0.1];
！
定义有效载荷数据LoadFull
PERSwobjdataCurWobj;
！
定义工件坐标系数据CurWobj，此工件坐标系作为当前使用坐标系。
即当在左侧码垛时，将左侧码盘坐标系WobjPllaet_L赋值给该数据；当在右侧码垛时，则将WobjPllaet_R赋值给该数据
PERSjointtargetjposHome:
=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
！
定义关节目标点数据，各关节轴数值为0，用于手动将机器人运动至各关节轴机械零位
CONSTrobtargetpPlaceBase0_L:
=[[296.473529255,212.21064316,3.210904169],[0,0.70711295,-0.707100612,0],[-2,0,-3,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
！
左侧不旋转放置基准位置
CONSTrobtargetpPlaceBase90_L:
=[[218.407102669,695.953395421,3.210997808],[0,-0.000001669,1,0],[-2,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
！
左侧旋转90°放置基准位置
CONSTrobtargetpPlaceBase0_R:
=[[296.473529255,212.21064316,3.210904169],[0,0.707221603,-0.70699194,0],[1,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
！
右侧旋转不旋转放置基准位置
CONSTrobtargetpPlaceBase90_R:
=[[218.407102669,695.953395421,3.210997808],[0,-0.00038594,0.999999926,0],[1,0,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
！
右侧旋转90°放置基准位置
CONSTrobtargetpPick_L:
=[[1627.550991372,-426.974661352,-26.736921885],[0,0.707109873,-0.707103689,0],[-1,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
！
左侧抓取位置
CONSTrobtargetpPick_R:
=[[1611.055992534,442.364097921,-26.736584068],[0,0.707220363,-0.706993181,0],[0,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
！
右侧抓取位置
CONSTrobtargetpHome:
=[[1505.00,-0.00,878.55],[1.28548E-06,0.707107,-0.707107,-1.26441E-06],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
！
程序起始点，即Home点
PERSrobtargetpPlaceBase0;
PERSrobtargetpPlaceBase90;
PERSrobtargetpPick;
PERSrobtargetpPlace;
！
定义目标点数据，这些数据是机器人当前使用的目标点。
当在左侧、右侧码垛时，将对应的左侧、右侧基准点赋值给这些数据
PERSrobtargetpPickSafe;
！
机器人将产品抓取后需提升至一定的安全高度，才能向码垛位置移动，随着摆放位置逐层加高，此数据在程序中会被赋予不同的数值，以防止机器人与码放号的产品发生碰撞
PERSnumnCycleTime:
=3.803;
！
定义数字型数据，用于存储单次节拍时间
PERSnumnCount_L:
=1;
PERSnumnCount_R:
=1;
！
定义数字型数据，分别用于左侧、右侧码垛计数，在计算位置子程序中根据该计数计算出相应的放置位置
PERSnumnPallet:
=2;
！
定义数字型数据，利用TEST指令判断此数值，从而决定执行哪侧的码垛任务，1为左侧，2为右侧
PERSnumnPalletNo:
=1;
！
定义数字型数据，利用TEST指令判断此数值，从而决定执行哪垛计数累计，1为左侧，2为右侧
PERSnumnPickH:
=300;
PERSnumnPlaceH:
=400;
！
定义数字型数据，分别对应的是抓取、放置时的一个高度。
例如nPickH:
=300,则表示机器人快速移动至抓取位置上放300mm处，然后慢速移动至抓取位置，接着慢速将产品提升至抓取位置上放300mm处，最后再快速移动至其他位置
PERSnumnBoxL:
=605;
PERSnumnBoxW:
=405;
PERSnumnBoxH:
=300;
！
定义三个数字型数据，分别对应的是产品长、宽、高。
在计算位置程序中，通过在放置基准点上面叠加长、宽、高数值计算处放置位置
VARclockTimer1;
！
定义时钟数据，用于计时
PERSboolbReady:
=TRUE;
！
定义布尔量数据，作为主程序逻辑判断条件，当左右两侧有任何一侧满足码垛条件时，此布尔量均为TRUE,即机器人会执行码垛任务，否则该布尔量为FLASE，机器人会等待直至条件满足
PERSboolbPalletFull_L:
=FALSE;
PERSboolbPalletFull_R:
=FALSE;
！
定义两个布尔数据，当机器人在左侧码垛时，则bPallet_L为TURE，bPallet_R为FALSE,当机器人在右侧时，则相反
PERSboolbGetPosition:
=FALSE;
！
定义两个布尔量数据，判断是否已计算出当前取放位置
VARtriggdataHookAct;
VARtriggdataHookOff;
！
定义两个触发数据，分别对应的是夹具上面钩爪收紧及松开动作
VARintnumiPallet_L;
VARintnumiPallet_R;
！
定义两个中断符，对应左侧、右侧码盘更换时所需触发的相应复位操作，如满载信号复位等
PERSspeeddatavMinEmpty:
=[2000,400,6000,1000];
PERSspeeddatavMidEmpty:
=[3000,400,6000,1000];
PERSspeeddatavMaxEmpty:
=[5000,500,6000,1000];
PERSspeeddatavMinLoad:
=[1000,200,6000,1000];
PERSspeeddatavMidLoad:
=[2500,500,6000,1000];
PERSspeeddatavMaxLoad:
=[4000,500,6000,1000];
！
定义多种速度数据，分别对应空载时高、中、低速，以及满载时的高、中、低速，便于对机器人的各个动作进行速度控制
PERSnumCompensation{15,3}:
=[[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]];
！
定义二维数组，用于各摆放位置的偏差调整；15组数据，对应15个摆放位置，每组数据3个数值，对应X、Y、Z的偏差值
PROCmain（）
！
主程序
rInitAll;
！
调用初始化程序，包括复位信号、复位程序数据、初始化中断等
WHILETRUEDO
！
利用WHILE循环，将初始化程序隔离开，即只在第一次运行时需要执行一次初始化程序，之后循环执行拾取放置动作
IFbReadyTHEN
！
利用IF条件判断，当左右两侧至少有一侧满足码垛条件时，判断条件bReady为TRUE,机器人则执行码垛任务
rPick;
！
调用抓取程序
rPlace;
！
调用放置程序
ENDIF
rCycleCheck;
！
调用循环检测程序，里面包含写屏显示循环时间、码垛个数、判断当前左右两侧状况等
WaitTime0.05
！
循环等待时间，防止不满足机器人动作条件的情况下程序执行进入无限循环状态，造成机器人控制CPU过负荷
ENDWHILE
ENDPROC
PROCrInitAll（）
！
初始化程序
rCheckHomePos;
！
调用检测Home点程序，若机器人在Home点，则直接执行后面的指令，否则机器人先安全返回Home点，然后再执行后面的指令
ConfL\OFF;
ConfJ\OFF;
！
关闭轴配置监控
nCount_L:
=1;
nCount_R:
=1;
！
初始化左右两侧码垛计数数据
nPallet:
=1;
！
初始化两侧码垛任务标识，1为左侧，2为右侧
nPalletNo:
=1;
！
初始化两侧码垛计数累计标识，1为左侧，2为右侧
bPalletFull_L:
=FALSE;
bPalletFull_R:
=FALSE;
！
初始化左右两侧码垛满载布尔量
bGetPosition:
=FALSE;
！
初始化计算位置标识，FALSE为未完成计算，TRUE为已完成计算
Resetdo00_ClampAct;
Resetdo01_HookAct;
！
初始化夹具，夹板张开和钩爪松开
ClkStopTimer1;
！
停止时钟计时
ClkResetTimer1;
！
复位时钟
TriggEquipHookAct,100,0.1\DOp:
=do01_HookAct,1;
！
定义触发事件：
钩爪收紧。
朝向指定目标点运动时提前100mm收紧钩爪，即将产品钩住，提前动作时间为0.1秒
TriggEquipHookOff,100\Start,0.1\DOp:
=do01_HookAct,0;
！
定义触发事件：
钩爪松开。
距离之后加上可选参变量\Start，则表示在离开起点100mm处松开钩爪，提前动作时间为0.1秒
IDeleteiPallet_L;
CONNECTiPallet_LWITHtEjectPallet_L;
ISignalDIdi02_PalletInPos_L,0,iPallet_L;
！
中断初始化，当左侧满载码盘到位信号为0时，即表示满载码盘被取走，则触发中断程序iPallet_L,复位左侧满载信号、满载布尔量
IDeleteiPallet_R;
CONNECTiPallet_RWITHtEjectPallet_R;
ISignalDIdi03_PalletInPos_R,0,iPallet_R;
！
中断初始化，当右侧满载码盘单位信号变为0时，即表示满载码盘被取走，则触发中断程序iPallet_R，复位右侧满载信号、满载布尔量等
ENDPROC
PROCrPick（）
！
抓取程序
ClkResetTimer1;
！
复位时钟
ClkStartTimer1;
！
开始计时
rCalPosition;
！
计算位置，包括抓取位置、抓取安全位置、放置位置等
MoveJOffs（pPick,0,0,nPickH）,vMaxEmpty,z50,tGripper\WObj:
=wobj0;
！
利用MoveJ移动至抓取位置正上方
MoveLpPick,vMinLoad,fine,tGripper\WObj:
=wobj0;
！
利用MoveL移动至抓取位置
Setdo00_ClampAct;
！
置位夹板信号，将夹板收紧，夹取产品
Waittime0.3;
！
预留夹具动作时间，以保证夹具已将产品收紧，等待时间根据实际情况来调整其大小；若有夹紧反馈信号，则可利用WaitDI指令等待反馈信号变为1，从而替代固定的等待时间
GripLoadLoadFull;
！
加载载荷数据
TriggLOffs（pPick,0,0,nPickH）,vMinLoad,HookAct,z50,tGripper\WObj:
=wobj0;
！
利用TriggL移动至抓取正上方，并调用触发事件HookAct，即在距离到达点100mm处将钩爪收紧，防止产品在快速移动中掉落
MoveLpPickSafe,vMaxLoad,z100,tGripper\WObj:
=wobj0;
！
利用MoveL移动至抓取位置
ENDPROC
PROCrPlace（）
！
放置程序
MoveJOffs（pPlace,0,0,nPlaceH）,vMaxLoad,z50,tGripper\WObj:
=CurWobj;
！
利用MoveJ移动至放置位置正上方
TriggLpPlace,vMinLoad,HookOff,fine,tGripper\WObj:
=CurWobj;
！
利用TriggL移动至放置位置，并调用触发事件HookOff,即在离开放置位置正上方点为100mm后将钩爪放开
Resetdo00_ClampAct;
！
复位夹板信号，夹板松开，放下产品
Waittime0.3;
！
预留夹具动作时间，以保证夹具已将产品完全放下，等待时间根据实际情况调整其大小
GripLoadLoad0;
！
加载载荷数据Load0
MoveLOffs（pPlace,0,0,nPlaceH）,vMinEmpty,z50,tGripper\WObj:
=CurWobj;
!
利用MoveL移动至放置位置正上方
rPlaceRD;
！
调用放置计数程序，其中会执行计数加1操作，并判断当前码盘是否已满载
MoveJpPickSafe,vMaxEmpty,z50,tGripper\WObj:
=wobj0;
！
利用MoveJ移动至抓取安全位置，以等待执行下一次循环
ClkStopTimer1;
停止计时
nCycleTime:
=ClkRead（Timer1）;
读取时钟数值，并赋值给nCycletime
ENDPROC
PROCrCycleCheck（）
！
周期循环检查
TPErase;
TPWrite"TheRobotisrunning!
";
！
示教器清屏，并显示当前机器人运行状态
TPWrite"Lastcycletimeis:
"\Num:
=nCycleTime;
！
显示上次循环运行时间
TPWrite"ThenumberoftheBoxesintheLeftpalletis:
"\Num:
=nCount_L-1;
TPWrite"ThenumberoftheBoxesintheRightpalletis:
"\Num:
=nCount_R-1;
！
显示当前左右码盘上面已摆放产品个数。
由于nCount_L和nCount_R表示的是下轮循环将要摆放的第多少个产品，此处显示的是码盘上已摆放的产品数，所以在当前计数数值上面减去1
IF（bPalletFull_L=FALSEANDdi02_PalletInPos_L=1ANDdi00_BoxInPos_L=1）OR（bPalletFull_R=FALSEANDdi03_PalletInPos_R=1ANDdi01_BoxInPos_R=1）THEN
bReady:
=TRUE;
ELSE
bReady:
=FALSE;
！
判断当前工作站状况，只要左右两侧有任何一侧满足码垛条件，则布尔量bReady为TRUE，机器人继续执行码垛任务；否则布尔量bReady为FALSE，机器人则等待码垛条件的满足
WaitTime0.1;
ENDIF
ENDPROC
PROCrCalPosition（）
！
计算位置程序
bGetPosition:
=FALSE;
！
复位完成计算位置标识
WHILEbGetPosition=FALSEDO
！
若未完成计算位置，则重复执行WHILE循环
TESTnPallet
！
利用TEST判断执行码垛检测标识的数值，1为左侧，2为右侧
CASE1:
！
若为1，则执行左侧检测
IFbPalletFull_L=FALSEANDdi02_PalletInPos_L=1ANDdi00_BoxInPos_L=1THEN
！
判断左侧是否满足码垛条件，若条件满足则将左侧的基准位置数值赋值给当前执行位置数据
pPick:
=pPick_L;
！
将左侧抓取目标点数据赋值给当前抓取目标点
pPlaceBase0:
=pPlaceBase0_L;
pPlaceBase90:
=pPlaceBase90_L;
！
将左侧放置位置基准目标点数据赋值给当前放置位置基准点
CurWobj:
=WobjPallet_L;
！
将左侧码盘工件坐标系数据赋值给当前工件坐标系
pPlace:
=pPattern（nCount_L）;
！
调用计算放置位置功能程序，同时写入左侧计数参数，从而计算出当前需要摆放的位置数据，并赋值给当前放置目标点
bGetPosition:
=TRUE;
！
已完成计算位置，则将完成计算位置标识为TURE
nPalletNo:
=1;
！
将码垛计数标识置为1，则后续会执行左侧码垛计算累计
ELSE
bGetPosition:
=FALSE;
！
若左侧不满足码垛任务，则完成计算位置位置标识置为FLASE，则程序会再次执行WHILE循环
ENDIF
nPallet:
=2;
！
将码垛检测标识置为2，则下次执行WHILE循环时检测右侧是否满足码垛条件
CASE2:
！
若为2，则执行右侧检测
IFbPalletFull_R=FALSEANDdi03_PalletInPos_R=1ANDdi01_BoxInPos_R=1THEN
！
判断右侧是否满足码垛条件，若条件满足，则将右侧的基准位置数值赋值给当前执行位置数据
pPick:
=pPick_R;
！
将右侧抓取目标点数据赋值给当前抓取目标点
pPlaceBase0:
=pPlaceBase0_R;
pPlaceBase90:
=pPlaceBase90_R;
！
将右侧放置位置基准目标点数据赋值给当前放置位置基准点
CurWobj:
=WobjPallet_R;
！
将右侧码盘工件坐标系数据赋值给当前工件坐标系
pPlace:
=pPattern（nCount_R）;
！
调用计算放置位置功能程序，同时写入右侧计数参数，从而计算出当前需要摆放的位置数据，并赋值给当前放置目标点
bGetPosition:
=TRUE;
！
已完成计算位置，则将完成计算位置标识为TRUE
nPalletNo:
=2;
！
将码垛计数标识置为2，则后续会执行右侧码垛计算累计
ELSE
bGetPosition:
=FALSE;
！
若右侧不满足码垛任务，则将完成计算位置标识置为FLASE，则程序会再次执行WHILE循环
ENDIF
nPallet:
=1;
！
将码垛检测标识置为1，则下次执行WHILE循环时检测左侧是否满足码垛条件
DEFAULT:
TPERASE;
TPWRITE"Thedata'nPallet'iserror,pleasecheckit!
";
Stop;
！
数据nPallet数值出错处理，提示操作员检查并停止运行
ENDTEST
ENDWHILE
！
此种程序结构便于程序的扩展，假设在此两进两出的基础上改为四进四出，则可并列写入CASE3和CASE4。
在CASE中切换nPallet的数值，是为了将各线体作为并列处理，则执行完成左侧后，下次优先检测右侧，之后下次再优先检测左侧
ENDPROC
FUNCrobtargetpPattern（numnCount）
！
计算摆放位置功能程序，调用时需写入计数参数，以区别计算左侧或右侧的摆放位置
VARrobtargetpTarget;
！
定义一个目标点数据，用于返回摆放目标点数据
IFnCount>=1ANDnCount<=5THEN
pPickSafe:
=Offs（pPick,0,0,400）;
ELSEIFnCount>=6ANDnCount<=10THEN
pPickSafe:
=Offs（pPick,0,0,600）;
ELSEIFnCount>=11ANDnCount<=15THEN
pPickSafe:
=Offs（pPick,0,0,800）;
ENDIF
！
利用IF判断当前码垛是第几层（本案例中每层堆放5个产品），根据判断结果来设置抓取安全位置，以保证机器人不会与已码垛产品发生碰撞，抓取安全高度设置由现场实际情况来调整。
此案例中的安全位置是以抓取点为基准偏移出来的，在实际中也可单独去示教一个抓取点后的安全目标点，同样也是根据码垛层数的增加而改变该安全目标点的位置
TESTnCount
！
判定计数nCount的数值，根据此数据的不同数值计算出不同摆放位置的目标点数据
CASE1:
pTarget.trans.x:
=pPlaceBase0.trans.x;
pTarget.trans.y:
=pPlaceBase0.trans.y;
pTarget.trans.z:
=pPlaceBase0.trans.z;
pTarget.rot:
=pPlaceBase0.rot;
pTarget.robconf:
=
升级会员 