--*****************************************--
-- Created by Anthony Yoo
-- User: Robotics Studio
-- Date: 2018/1/29
-- Time: 17:44
-- Manufacturer: ADTECH(ShenZhen) Technology CO.,LTD
-- Country of origin: China
-- Machine P/N: AR4215-E-02
--*****************************************--
--点位--
local StandbyPos = p39 --待机点
local GetScrewPos = p40 --取螺丝位置
local TakePhotoPos = {p41,p42,p43} --拍照点
local InstallScrewCCDPos = {x=0,y=0,z=0,c=0,h=1} --打螺丝位置像素坐标
local CartPos = {x=0,y=0,z=0,c=0,h=1}
local InstallScrewCartPos = {{x=0,y=0,z=0,c=0,h=1}, --打螺丝位置笛卡尔坐标
{x=0,y=0,z=0,c=0,h=1},
{x=0,y=0,z=0,c=0,h=1},
}
local Install_Z,Install_C = -118,-34 --打螺丝高度和角度
--输入--
local LineOK = 0 --线体到位
local ScrewSignal = 14 --螺丝到位信号
local SuctionOK = 13 --螺丝吸到位信号
--输出--
local SuctionON,SuctionOFF = 6,7 --吸气,吹气
local MotorRun = 9 --转动批头
local Lamp = 18 --光源控制
local ScrewOver = 0 --打螺丝完成
--其他变量--
local CCDNo = "CAM0" --CCD号
local Par = "Spd=10 In=15 ON" --打螺丝扭力到达参数
local ChkDrop = "In=13 ON" --检测运动中是否掉螺丝了
local count,j,k,num,XX,ScrewNum = 1,0,0,4,10,3 --拍照点计数，拍照失败计数，偏移坐标次数及偏移距离，螺丝数
local p,r,q,YY = 0,0,0,10
local oneHave,twoHave,threeHave,numT = 0,0,0,{}
--初始化IO--
local InitIO = {SuctionON,MotorRun,ScrewOver}
for i = 1,#InitIO do
DO(InitIO[i],OFF)
end
DO({SuctionOFF,Lamp},{ON,ON});Delay(500); --吹气打开，光源关闭
SetU(0);SetT(0);
MovJ(J3,-5)
MArchP(StandbyPos,-5,10,10) --待机点
function main()
while true do
::TakePhoto::
--if count == ScrewNum + 1 then count = 1 goto Put end --goto Put放这里会报下面的错误，不过已找到了解决办法
--AR: d:\projects\111\TESTQHZY.AR:101: <goto Put> at line 51 jumps into the scope of local 'n' 编译失败！
SetU(0);SetT(0);DO(Lamp,OFF); --用户0，工具0坐标系，打开CCD光源
MovP(TakePhotoPos[count]+XYZC(j*XX,q*YY,0,0)) --拍照点
Delay(100)
--WDI(LineOK,ON) --等待载具到位
CCDclr(CCDNo);CCDsent(CCDNo,"0,0,0,0;"); --清除网络缓存，触发CCD拍照(触发格式"0,0,0,0;"不指定模板号，"0,0,0,0,1;"指定为1号模板)
local n,data=CCDrecv(CCDNo) --等待接收数据，发送测试坐标：3,279.5,-114.5,0;(格式: "模板号,x坐标,y坐标,c坐标;")
local Model = data[1].NO --模板号(视觉配置的格式为: NO,xx,yy,cc;)
if (data[n][1] ~= 0 and data[n][2] ~= 0) and n == 1 and Model == "3" then --模板匹配正确且数据正常
InstallScrewCCDPos.x = data[n][1]
InstallScrewCCDPos.y = data[n][2]
CartPos = GetDynCCDPos(CCDNo,InstallScrewCCDPos) --CCD像素坐标转换成笛卡尔坐标
InstallScrewCartPos[count].x = CartPos.x --*.x *.y *.z *.c都要写，否则只走一个点位
InstallScrewCartPos[count].y = CartPos.y
InstallScrewCartPos[count].z = Install_Z
InstallScrewCartPos[count].c = Install_C
InstallScrewCartPos[count].h = 1
if count == 1 then oneHave = 1 end --如果第1个有
if count == 2 then twoHave = 1 end --如果第2个有
if count == 3 then threeHave = 1 end --如果第3个有
count = count + 1
j,k,p,q,r = 0,0,0,0,0 --拍到了就置位
if count == ScrewNum + 1 then
count = 1
goto Put
else
goto TakePhoto
end
elseif (data[n][1] ~= 0 and data[n][2] ~= 0) and n == 1 and Model == "2" then --模板匹配错误但数据正常
count = count + 1 --跳到下一个位置拍
goto TakePhoto
else
data = nil
if k == 0 then j = j + 1 end --向前偏移num次
if j == num then j,k = 0,1 end
if k == 1 then j = j - 1 end --向后偏移num次
if j == -num then j,p,k = 0,1,2 end
if p == 1 then q = q + 1 end --向左偏移num次
if q == num then q,p,r = 0,0,1 end
if r == 1 then q = q - 1 end --向右偏移num次
if q == -num then k,p,q,r,count = 0,0,0,0,count+1 end --前后左右都偏移了num次后跳到下一个位置去扫描
---********---
if j==0 and k==0 and p==0 and q==0 and r==0 and count==ScrewNum+1 and (oneHave==1 or twoHave==1 or threeHave==1) then
count = 1
goto Put
else
if j==0 and k==0 and p==0 and q==0 and r==0 and count==ScrewNum+1 and oneHave==0 and twoHave==0 and threeHave==0 then
DO(ScrewOver,ON);Delay(50);DO(ScrewOver,OFF); --前后左右扫描了ScrewNum个位置都没扫到，载具流走
count = 1
end
end
---********---
goto TakePhoto
end
::Put::
DO(Lamp,ON) --关闭光源
if oneHave == 1 and twoHave == 0 and threeHave == 0 then numT = {1} --位置1有，跑第1个位置
elseif oneHave == 0 and twoHave == 1 and threeHave == 0 then numT = {2} --位置2有，跑第2个位置
elseif oneHave == 0 and twoHave == 0 and threeHave == 1 then numT = {3} --位置3有，跑第3个位置
elseif oneHave == 1 and twoHave == 1 and threeHave == 0 then numT = {1,2} --位置1,2有，跑第1,2个位置
elseif oneHave == 1 and twoHave == 0 and threeHave == 1 then numT = {1,3} --位置1,3有，跑第1,3个位置
elseif oneHave == 0 and twoHave == 1 and threeHave == 1 then numT = {2,3} --位置2,3有，跑第2,3个位置
elseif oneHave == 1 and twoHave == 1 and threeHave == 1 then numT = {1,2,3} --位置1,2,3有，跑第1,2,3个位置
end
oneHave,twoHave,threeHave = 0,0,0 --拍照点拍到有物料置为无效
for ii = 1,#numT do
::GetScrew::
SetU(0);SetT(0);
--WDI(ScrewSignal,ON) --等待螺丝到位信号
Delay(10)
DO(MotorRun,ON) --转动电批
MArchP(GetScrewPos,-5,10,10) --取螺丝位置
DO({SuctionOFF,SuctionON},{OFF,ON}) --吸螺丝
Delay(300)
MovJ(J3,GetScrewPos.z+25) --在取螺丝位置上方一点
Delay(500)
if DI(SuctionOK) == ON then --检测没吸到螺丝
DO({SuctionON,SuctionOFF},{OFF,ON}) --吹螺丝
goto GetScrew
end
SetU(0);SetT(2);
MovP(InstallScrewCartPos[numT[ii]]+XYZC(0,0,25,0),ChkDrop) --打螺丝上方并检测螺丝是否掉了
Delay(100)
if DI(SuctionOK) == ON then --检测没吸到螺丝
DO({SuctionON,SuctionOFF},{OFF,ON}) --吹螺丝
goto GetScrew
end
MovL(InstallScrewCartPos[numT[ii]],Par) --打螺丝
DO({SuctionON,SuctionOFF},{OFF,ON}) --吹螺丝
MovJ(J3,-5) --抬起
DO(MotorRun,OFF) --停止电批转动
Delay(10)
DO(MotorRun,ON)
if ii == #numT then
numT = {}
DO(ScrewOver,ON);Delay(50);DO(ScrewOver,OFF); --打螺丝完成
end
end
goto TakePhoto
Delay(2)
end
end
ADTECH AR机器人视觉打螺丝的应用(Lua脚本的实现)
猜你喜欢
转载自blog.csdn.net/iss_mk_gmbh/article/details/79224091
今日推荐
周排行