工业机器人演示程序
项目名必须为PROJECT, 主程序名必须为MAIN
MAIN status_robot.val := 100 WHILE TRUE DO WAIT start_on.val = 103 CALL inttoreal() WaitIsFinished() IF changetool.val = 0 THEN status_robot.val := 200 CALL pick() CALL put() status_robot.val := 100 ELSIF changetool.val = 1 THEN status_robot.val := 200 CALL tuopan_pick_put() status_robot.val := 100 status_end.val := 101 END_IF END_WHILE
INTTOREAL dx_pick := dx_pick1.val dx_pick := dx_pick / 10 mm
dy_pick := dy_pick1.val dy_pick := dy_pick / 10 dz_pick := dz_pick1.val dz_pick := dz_pick / 10 oz_pick := oz_pick1.val oz_pick := oz_pick / 10 dx_put := dx_put1.val
dx_put := dx_put / 10 dy_put := dy_put1.val dy_put := dy_put / 10 dz_put := dz_put1.val dz_put := dz_put / 10 oz_put := oz_put1.val oz_put := oz_put / 10
主程序 上传机器人当前状态：100=待机 当TRUE 时运行程序（循环） 等待机器人启动信号：103=启动 调用程序inttoreal （整型到实数转换） 等待完成 如果工具号=0，然后 机器人状态：200=运行 调用程序pick （礼物拾取） 调用程序put （礼物装箱） 机器人状态：待机 如果工具号=1 机器人状态：运行 调用程序托盘进栈 机器人状态：待机 程序结束 整数转实数 拾取点X 轴偏移量转为实数 拾取点X 轴偏移量除以10得到实际偏移，单位拾取点Y 轴偏移量转为实数 拾取点Y 轴实际偏移量 拾取点Z 轴偏移量转为实数 拾取点Z 轴实际偏移量 拾取点旋转角度转为实数 拾取点实际旋转角度
放置点X 轴偏移量转为实数
PICK PTP(xipan_tool_pos) Tool(xipan_tool) WaitIsFinished() //Lin(inipoint) Pickpot:= inipoint Pickpot.x := inipoint.x -dx_pick Pickpot.y := inipoint.y -dy_pick -laser_offset Pickpot.z := inipoint.z + dz_pick + 100
WaitIsFinished() Lin(pickpot) WaitIsFinished() pickpot.z := pickpot.z -100 WaitIsFinished() Lin(pickpot, lin300) WaitIsFinished() dout24.Set(TRUE) dout26.Set(TRUE) WaitTime(2000) WaitIsFinished() IF xiqu_status.val = TRUE THEN pickpot.z := pickpot.z + 100 WaitIsFinished() Lin(pickpot) ELSIF xiqu_status.val = FALSE THEN status_robot.val := 300 PAUSE END_IF PUT Tool(xipan_tool) //Lin(put_inipoint) putpot := put_inipoint putpot.x := put_inipoint.x + dx_put putpot.y := put_inipoint.y + dy_put -laser_offset putpot.z := put_inipoint.z + dz_put + 150
WaitIsFinished() Lin(putpot) WaitIsFinished() IF xiqu_status.val = TRUE THEN
礼品拾取 切换到礼物拾取工具位置 切换到礼物拾取工具坐标系 等待完成 拾取坐标偏移基准点（不参与实际动作） 拾取点赋值 拾取点X 轴偏移计算 拾取点Y 轴偏移计算
拾取点Z 轴偏移计算(增加100mm 作为准备点 等待完成 以直线方式运行到拾取准备点 等待完成 减少100mm 到拾取点 等待完成 以300cm/min的速度运行到拾取点 等待完成 真空打开 吸盘1打开 延时2秒 等待完成 如果真空检测为TRUE 上升100mm 作为退出点 等待完成 运动到退出位置 如果真空检测为FALSE 机器人状态：300=吸取错误 暂停
礼物装箱
切换到礼物拾取工具坐标系
放置坐标偏移基准点（不参与实际动作） 放置点赋值
放置点X 轴坐标计算 放置点Y 轴坐标计算
放置点Z 轴坐标计算(增加150mm 作为准备点
等待完成
运动到放置准备点 等待完成
如果真空检测为TRUE
putpot.z := put_ready.z -100 putpot.a := putpot.a + oz_pick WaitIsFinished() Lin(putpot) WaitIsFinished() putpot.z := putpot.z -50 WaitIsFinished() Lin(putpot) WaitIsFinished() dout24.Set(FALSE) dout26.Set(FALSE)
WaitIsFinished()
Putpot.z := putpot.z + 200 WaitIsFinished() Lin(putpot)
WaitIsFinished()
ELSIF xiqu_status.val = FALSE THEN status_robot.val := 400
PAUSE END_IF
tuopan_pick_put PTP(tupan_tool) Tool(tuopan_tool) Lin(pick_ready) Lin(pick_up) WaitIsFinished()
dout24.Set(TRUE) dout25.Set(TRUE)
WaitTime(2000) WaitIsFinished()
IF xiqu_status.val = TRUE THEN Lin(pick_after) Lin(put_up) IF xiqu_status.val = TRUE THEN Lin(put_down) WaitIsFinished()
dout24.Set(FALSE) dout25.Set(FALSE) PTP(tupan_tool) ELSIF xiqu_status.val = FALSE THEN dout24.Set(FALSE) dout25.Set(FALSE)
WaitIsFinished()
高度下降100mm 作为姿态调整点 旋转角度计算 等待完成
运动到姿态调整点 等待完成
高度减少50mm （实际放置点）
运动到放置位置 真空关闭 吸盘1关闭
高度上升200mm 作为退出点 运动到退出点
如果真空检测为FALSE
机器人状态：400=放置错误
托盘进栈
切换到托盘拾取工具位置 切换到托盘拾取工具坐标系 运行到拾取准备位置 运行到拾取位置 真空打开 吸盘2打开
*如果真空检测为TRUE 运动到退出位置 运动到放置准备位置 如果真空检测为TRUE 运动到放置准备位置 真空关闭 吸盘2关闭 回到开始点
如果真空检测为FALSE 真空关闭 吸盘2关闭
status_robot.val := 400 PAUSE END_IF
ELSIF xiqu_status.val = FALSE THEN status_robot.val := 300 PAUSE END_IF
机器人状态：放置错误
*如果真空检测为FALSE 机器人状态：吸取错误