台达机械手矩阵（粗版）

这周刚好接触了下台达机械手，可以开源化做函数，故写了个矩阵的逻辑。看着稍微有点复杂。

function spacing(S_homepoint,S_endpoint,S_outlinepoint,S_hcount,S_zcount)

sx=ReadPoint(S_homepoint,”X”)
sy=ReadPoint(S_homepoint,”Y”)
ex=ReadPoint(S_endpoint,”X”)
ey=ReadPoint(S_endpoint,”Y”)
ox=ReadPoint(S_outlinepoint,”X”)
oy=ReadPoint(S_outlinepoint,”Y”)
hx=ex-sx
hy=ey-sy
zx=ox-sx
zy=oy-sy
HXaverage=hx/(S_hcount-1)
HYaverage=hy/(S_hcount-1)
ZXaverage=zx/(S_zcount-1)
ZYaverage=zy/(S_zcount-1)
return HXaverage,HYaverage,ZXaverage,ZYaverage

end

—————————-点位读写————————
function Gotopoint(T_hcount,T_zcount,G_homepoint,G_hx,G_hy,G_zx,G_zy,T_pickcount)
Pallet=T_pickcount/T_hcount
ret1,ret2=MODF(Pallet)
if ret2>0 then
Pallet_i=ret1+1
else
Pallet_i=ret1
end

if ret2==0 then
   Pallet_j=T_hcount
else
   Pallet_j=ret2*T_hcount
end

G_sx=ReadPoint(G_homepoint,”X”)
G_sy=ReadPoint(G_homepoint,”Y”)
WritePoint(“PickPoint”, “X”,G_sx+(Pallet_i-1)*G_zx+(Pallet_j-1)*G_hx)
WritePoint(“PickPoint”, “Y”,G_sy+(Pallet_i-1)*G_zy+(Pallet_j-1)*G_hy)

end
RobotServoOn()
fast=90
medium=30
slow=10
SpdJ(medium)
AccJ(medium)
DecJ(medium)
SpdL(30)
AccL(30)
DecL(30)
h=10 —–矩阵X轴方向数目
z=2 —–矩阵Y轴方向数目

HX,HY,ZX,ZY = spacing(“homepoint”,”endpoint”,”outlinepoint”,h,z) —–计算矩阵固定偏差值
PickCount1=1 —–矩阵起点计数
::mmp::
Gotopoint(h,z,”homepoint”,HX,HY,ZX,ZY,PickCount1) ——点位坐标计算
–MovP(“PickPoint”,”PASS”)
–MovP(“PickPoint”-Z(20))
MovP(“PickPoint”)
–MArchP 拱门状态
PickCount1=PickCount1+1
if PickCount1>h*z then ——矩阵run over
PAUSE()
PickCount1=1
end
goto mmp
其中，homepoint:矩阵起点
endpoint矩阵第二个点
outlinepoint矩阵第三个点
上述的形状为homepoint为起始点，分别连接至endpoint和outlinepoint。形成的形状（平行四边形或者矩形）
pickpoint为随意设置的一个点位，用来保存矩阵的走点；
h为矩阵的X轴方向的数目
z为矩阵的Y轴方向的数目
以上，欢迎补充和建议；