一种适用于主流工业机器人的简单的码垛算法

1. 适用场景
   ● 产品尺寸规整，长宽高都有数值，最好是箱型体
   ● 简单的“长X宽X高”的码法，非五花垛等复杂垛型，垛型方向一致，无需旋转产品。
   ● 抓取和放置方式为从上方进行抓取和放置
2. 要求
   ● 工业机器人需要有位置信息变量且X、Y、Z、W、P、R能被计算
   ● 工业机器人夹爪需要有夹取放置的功能
3. 程序简介
   该程序能够实现简单的码垛，可以根据外部输入如PLC等设置码垛的个数，码垛垛型（XXX），码垛各方向的偏移量，垛满后输出信号提示。
4. 程序设计
   注：该代码块使用KUKA工业机器人编程语言KRL编写，本程序注重提供思路。如有不对的地方还请大佬指正

DEF Pallet1 ( )
//======================================================================================
// 定义变量
DECL INT num_count  //码垛个数计数器
DECL INT num_x,num_y,num_z  //码垛时XYZ方向上的计数器，如x方向上码一个则num_x就等于1
DECL INT num_limit_x,num_limit_y,num_limit_z  //外部输入的码垛限制，如需要码3x3x3的垛。
DECLINT fxied_offset_x,fxied_offset_y,fxied_offset_z  //外部输入的码垛偏移量
//======================================================================================

//======================================================================================
//定义信号
GLOBAL SIGNAL GI_Gap_X $IN[1] TO $IN[16]  //xyz方向的偏移量
GLOBAL SIGNAL GI_Gap_Y $IN[17] TO $IN[32] 
GLOBAL SIGNAL GI_Gap_Z $IN[33] TO $IN[49] 

GLOBAL SIGNAL GI_num $IN[50] TO $IN[65]  //需要的码垛数量

GLOBAL SIGNAL GI_limit_x $IN[61] TO $IN[68]  //需要的码垛垛型，如3x3x3，这里使用8位组成
GLOBAL SIGNAL GI_limit_y $IN[71] TO $IN[78]  //的信号已经够使用了，最大可以输入255
GLOBAL SIGNAL GI_limit_z $IN[81] TO $IN[88] 

GLOBAL SIGNAL GO_Act_Count $OUT[1] to $OUT[16]  //组输出，实际码垛数量

GLOBAL SIGNAL OUT_Pallet_OK $OUT[100]  //bool量，码垛垛满信号
GLOBAL SIGNAL IN_Pallet_OK $IN[100]  //bool量，确认收到垛满信号
//======================================================================================

INI
//======================================================================================
//初始化所有变量
num_count = 0
num_x = 0
num_y = 0
num_z = 0
num_limit_x = 0
num_limit_y = 0
num_limit_z = 0
fxied_offset_x = 0
fxied_offset_y = 0
fxied_offset_z = 0
OUT_Pallet_OK = FALSE
//======================================================================================

//======================================================================================
//接收外部输入的各种参数
LOOP
fxied_offset_x = GI_Gap_X  //接收偏移量
fxied_offset_y = GI_Gap_Y
fxied_offset_z = GI_Gap_Z

num_limit_x = GI_limit_x  //接收垛型要求
num_limit_y = GI_limit_y
num_limit_z = GI_limit_z

IF (fxied_offset_x <> 0) AND (fxied_offset_y <> 0) AND ( fxied_offset_z <> 0) THEN

IF (num_limit_x > 0) AND (num_limit_y > 0) AND (num_limit_z > 0) THEN
EXIT  //判断是否全部参数都已接收完成，如果完成，执行EXIT跳出死循环
ENDIF

ENDIF

ENDLOOP
//======================================================================================

//======================================================================================
LOOP  //开始循环

TAKE()  //取料程序

XP_put = XP_TEACH  //将固定点位赋值给临时点位
XP_put_up = XP_put  //临时点位上方点
XP_put_up.Z = XP_put_up.Z + 100

XP_put.X = XP_put.X + (num_x * fxied_offset_x)  //偏移量计算
XP_put.Y = XP_put.Y + (num_y * fxied_offset_y)
XP_put.Z = XP_put.Z + (num_z * fxied_offset_z)

;------------------------------
;主动作程序
PTP Xhome 
PTP XP_put_up 
LIN XP_put 
Gripper_Opn () //夹爪打开程序
LIN XP_put_up 
PTP Xhome 
;-------------------------------

num_count = num_count + 1  //码垛计数器自增
GO_Act_Count = num_count  //反馈给外部当前码垛个数
num_x = num_x + 1  //x方向计数

IF num_x >= num_limit_x THEN //判断条件，x方向是否码完
num_x = 0
num_y = num_y + 1
ENDIF

IF num_y >= num_limit_y  THEN  //判断条件，y方向是否码完
num_x = 0
num_y = 0
num_z = num_z + 1
ENDIF

IF num_z >= num_limit_z  THEN  //判断条件，z方向是否码完
num_x = 0
num_y = 0
num_z = 0
ENDIF

IF num_count == GI_num  THEN  //如果码垛个数已经达到外部所输入的个数，则发出垛满完成信号
OUT_Pallet_OK = TRUE
num_count = 0
WAIT IN_Pallet_OK == TRUE  //等待外部确认
OUT_Pallet_OK = FALSE  //垛满信号复位
GO_Act_Count = 0
EXIT  //跳出码垛死循环
ENDIF

ENDLOOP
//======================================================================================
END
---------------------------------------------------------------------------
DEF Pos_store()  //点位存放程序
PTP XP_put_up 
PTP XP_put 
PTP XP_TEACH 
END
---------------------------------------------------------------------------
DEF Gripper_Opn ()  //夹爪打开程序

END
---------------------------------------------------------------------------
DEF TAKE ()  // 取料程序

END