内容发布更新时间 : 2024/5/18 16:44:07星期一 下面是文章的全部内容请认真阅读。

MODULE MainMoudle PERS wobjdata WobjPallet_L:=[FALSE,TRUE,\1,0,0,0]]]

！定义左侧码盘工件坐标系WobjPallet_L PERS wobjdata WobjPallet_R:=[FALSE,TRUE,\,0,0,0]]];

！定义右侧码盘工件坐标系WobjPallet_R PERS tooldata tGripper:=[TRUE,[[0,0,527],[1,0,0,0]],[20,[0,0,150],[1,0,0,0],0,0,0]]; ！定义工具坐标系数据tGripper

PERS loaddata LoadFull:=[20,[0,0,300],[1,0,0,0],0,0,0.1]; ！定义有效载荷数据LoadFull

PERS wobjdata CurWobj;

！定义工件坐标系数据CurWobj，此工件坐标系作为当前使用坐标系。即当在左侧码垛时，将左侧码盘坐标系WobjPllaet_L赋值给该数据；当在右侧码垛时，则将WobjPllaet_R赋值给该数据 PERS jointtarget jposHome:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

！定义关节目标点数据，各关节轴数值为0，用于手动将机器人运动至各关节轴机械零位 CONST robtarget pPlaceBase0_L:=[[296.473529255,212.21064316,3.210904169],[0,0.70711295,-0.707100612,0],[-2,0,-3,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; ！左侧不旋转放置基准位置 CONST robtarget pPlaceBase90_L:=[[218.407102669,695.953395421,3.210997808],[0,-0.000001669,1,0],[-2,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; ！左侧旋转90°放置基准位置 CONST robtarget pPlaceBase0_R:=[[296.473529255,212.21064316,3.210904169],[0,0.707221603,-0.70699194,0],[1,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; ！右侧旋转不旋转放置基准位置 CONST robtarget pPlaceBase90_R:=[[218.407102669,695.953395421,3.210997808],[0,-0.00038594,0.999999926,0],[1,0,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; ！右侧旋转90°放置基准位置 CONST robtarget pPick_L:=[[1627.550991372,-426.974661352,-26.736921885],[0,0.707109873,-0.707103689,0],[-1,0,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]]; ！左侧抓取位置 CONST robtarget pPick_R:=[[1611.055992534,442.364097921,-26.736584068],[0,0.707220363,-0.706993181,0],[0,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

！右侧抓取位置 CONST robtarget pHome:=[[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点

PERS robtarget pPlaceBase0; PERS robtarget pPlaceBase90; PERS robtarget pPick; PERS robtarget pPlace;

！定义目标点数据，这些数据是机器人当前使用的目标点。当在左侧、右侧码垛时，将对应的左侧、右侧基准点赋值给这些数据 PERS robtarget pPickSafe;

！机器人将产品抓取后需提升至一定的安全高度，才能向码垛位置移动，随着摆放位置逐层加高，此数据在程序中会被赋予不同的数值，以防止机器人与码放号的产品发生碰撞 PERS num nCycleTime:=3.803;

！定义数字型数据，用于存储单次节拍时间 PERS num nCount_L:=1; PERS num nCount_R:=1;

！定义数字型数据，分别用于左侧、右侧码垛计数，在计算位置子程序中根据该计数计算出相应的放置位置

PERS num nPallet:=2;

！定义数字型数据，利用TEST指令判断此数值，从而决定执行哪侧的码垛任务，1为左侧，2为右侧

PERS num nPalletNo:=1; ！定义数字型数据，利用TEST指令判断此数值，从而决定执行哪垛计数累计，1为左侧，2为右侧

PERS num nPickH:=300; PERS num nPlaceH:=400;

！定义数字型数据，分别对应的是抓取、放置时的一个高度。例如nPickH:=300,则表示机器人快速移动至抓取位置上放300mm处，然后慢速移动至抓取位置，接着慢速将产品提升至抓取位置上放300mm处，最后再快速移动至其他位置 PERS num nBoxL:=605; PERS num nBoxW:=405; PERS num nBoxH:=300;

！定义三个数字型数据，分别对应的是产品长、宽、高。在计算位置程序中，通过在放置基准点上面叠加长、宽、高数值计算处放置位置 VAR clock Timer1; ！定义时钟数据，用于计时 PERS bool bReady:=TRUE; ！定义布尔量数据，作为主程序逻辑判断条件，当左右两侧有任何一侧满足码垛条件时，此布尔量均为TRUE,即机器人会执行码垛任务，否则该布尔量为FLASE，机器人会等待直至条件满足

PERS bool bPalletFull_L:=FALSE; PERS bool bPalletFull_R:=FALSE;

！定义两个布尔数据，当机器人在左侧码垛时，则bPallet_L为TURE，bPallet_R为FALSE,当机器人在右侧时，则相反

PERS bool bGetPosition:=FALSE;

！定义两个布尔量数据，判断是否已计算出当前取放位置 VAR triggdata HookAct; VAR triggdata HookOff;

！定义两个触发数据，分别对应的是夹具上面钩爪收紧及松开动作 VAR intnum iPallet_L; VAR intnum iPallet_R;

！定义两个中断符，对应左侧、右侧码盘更换时所需触发的相应复位操作，如满载信号复位等

PERS speeddata vMinEmpty:=[2000,400,6000,1000]; PERS speeddata vMidEmpty:=[3000,400,6000,1000]; PERS speeddata vMaxEmpty:=[5000,500,6000,1000]; PERS speeddata vMinLoad:=[1000,200,6000,1000]; PERS speeddata vMidLoad:=[2500,500,6000,1000]; PERS speeddata vMaxLoad:=[4000,500,6000,1000];

！定义多种速度数据，分别对应空载时高、中、低速，以及满载时的高、中、低速，便于对机器人的各个动作进行速度控制 PERS num Compensation{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的偏差值

PROC main() ！主程序

rInitAll;

！调用初始化程序，包括复位信号、复位程序数据、初始化中断等 WHILE TRUE DO

！利用WHILE循环，将初始化程序隔离开，即只在第一次运行时需要执行一次初始化程序，之后循环执行拾取放置动作 IF bReady THEN

！利用IF条件判断，当左右两侧至少有一侧满足码垛条件时，判断条件bReady为TRUE,机器人则执行码垛任务 rPick; ！调用抓取程序 rPlace; ！调用放置程序 ENDIF rCycleCheck;

！调用循环检测程序，里面包含写屏显示循环时间、码垛个数、判断当前左右两侧状况等 Wait Time 0.05

！循环等待时间，防止不满足机器人动作条件的情况下程序执行进入无限循环状态，造成机

器人控制CPU过负荷 ENDWHILE ENDPROC

PROC rInitAll() ！初始化程序

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为已完成计算 Reset do00_ClampAct; Reset do01_HookAct;

！初始化夹具，夹板张开和钩爪松开 ClkStop Timer1; ！停止时钟计时

ClkReset Timer1; ！复位时钟

TriggEquip HookAct,100,0.1\\DOp:=do01_HookAct,1;

！定义触发事件：钩爪收紧。朝向指定目标点运动时提前100mm收紧钩爪，即将产品钩住，提前动作时间为0.1秒

TriggEquip HookOff,100\\Start,0.1\\DOp:=do01_HookAct,0;

！定义触发事件：钩爪松开。距离之后加上可选参变量\\Start，则表示在离开起点100mm处松开钩爪，提前动作时间为0.1秒 IDelete iPallet_L;

CONNECT iPallet_L WITH tEjectPallet_L;

ISignalDI di02_PalletInPos_L,0,iPallet_L;

！中断初始化，当左侧满载码盘到位信号为0时，即表示满载码盘被取走，则触发中断程序iPallet_L,复位左侧满载信号、满载布尔量 IDelete iPallet_R;

CONNECT iPallet_R WITH tEjectPallet_R;

ISignalDI di03_PalletInPos_R,0,iPallet_R;

！中断初始化，当右侧满载码盘单位信号变为0时，即表示满载码盘被取走，则触发中断程序iPallet_R，复位右侧满载信号、满载布尔量等 ENDPROC

PROC rPick() ！抓取程序

ClkReset Timer1; ！复位时钟

ClkStart Timer1; ！开始计时

rCalPosition;

！计算位置，包括抓取位置、抓取安全位置、放置位置等

MoveJ Offs(pPick,0,0,nPickH),vMaxEmpty,z50,tGripper\\WObj:=wobj0; ！利用MoveJ移动至抓取位置正上方

MoveL pPick,vMinLoad,fine,tGripper\\WObj:=wobj0; ！利用MoveL移动至抓取位置 Set do00_ClampAct;

！置位夹板信号，将夹板收紧，夹取产品 Waittime 0.3;

！预留夹具动作时间，以保证夹具已将产品收紧，等待时间根据实际情况来调整其大小；若有夹紧反馈信号，则可利用WaitDI指令等待反馈信号变为1，从而替代固定的等待时间 GripLoad LoadFull; ！加载载荷数据

TriggL Offs(pPick,0,0,nPickH),vMinLoad,HookAct,z50,tGripper\\WObj:=wobj0; ！利用TriggL移动至抓取正上方，并调用触发事件HookAct，即在距离到达点100mm处将钩爪收紧，防止产品在快速移动中掉落

MoveL pPickSafe,vMaxLoad,z100,tGripper\\WObj:=wobj0; ！利用MoveL移动至抓取位置 ENDPROC

PROC rPlace() ！放置程序

MoveJ Offs(pPlace,0,0,nPlaceH),vMaxLoad,z50,tGripper\\WObj:=CurWobj; ！利用MoveJ移动至放置位置正上方

TriggL pPlace,vMinLoad,HookOff,fine,tGripper\\WObj:=CurWobj; ！利用TriggL移动至放置位置，并调用触发事件HookOff,即在离开放置位置正上方点为100mm后将钩爪放开

Reset do00_ClampAct;

！复位夹板信号，夹板松开，放下产品 Waittime 0.3;

！预留夹具动作时间，以保证夹具已将产品完全放下，等待时间根据实际情况调整其大小 GripLoad Load0; ！加载载荷数据Load0