MODULE banyun
!=============================================================
! 数据声明
!=============================================================
! --- 抓瓶子点 ---
CONST robtarget zhuapz:=[[277.18,-376.69,266.26],[0.00498965,0.000132259,0.999969,-0.00610663],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
! --- 四个放瓶子的点 ---
CONST robtarget p10:=[[0.35,314.73,404.64],[1.37698E-05,4.49775E-05,1,3.66856E-06],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p20:=[[-85.89,321.49,404.65],[3.20777E-05,6.02536E-05,1,-9.59981E-05],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p30:=[[-85.89,373.61,404.64],[3.55543E-05,0.00010116,1,-0.000126393],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p40:=[[-3.74,373.62,404.64],[2.77209E-05,9.35033E-05,1,-8.70277E-05],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget pzSSZ{4} := [p10,p20,p30,p40]; ! 顺时针瓶子点位数组
VAR robtarget pzNSZ{4} := [p40,p30,p20,p10]; ! 逆时针瓶子点位数组
! --- 吸/放盖子点 ---
CONST robtarget xigaizi:=[[110.82,320.13,316.29],[0.452693,-0.498191,0.507738,0.537659],[0,1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget fanggaizi:=[[15.85,326.56,335.99],[0.379101,-0.536931,0.401363,0.637884],[0,1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
! --- 标签相关 ---
CONST robtarget p50:=[[289.33,-56.06,178.38],[0.447426,0.51164,0.505364,-0.531641],[0,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget pQuBQ{24}; ! 24个取标签位置
VAR bool quleBQ{24}; ! 记录状态：FALSE=未吸走, TRUE=已吸走
! --- 四个放标签点 ---
CONST robtarget p60:=[[-46.35,296.12,340.29],[0.689421,-0.17058,0.699534,0.0790756],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p70:=[[-46.35,311.62,340.29],[0.689399,-0.170627,0.69954,0.0791089],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p80:=[[-46.35,341.64,340.30],[0.689377,-0.170702,0.699541,0.0791355],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p90:=[[-46.35,363.80,340.30],[0.68937,-0.170739,0.699538,0.0791503],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget pFangBQ{4} := [p60,p70,p80,p90];
! --- Home 点 ---
CONST robtarget homedian:=[[244.57,9.24,566.04],[0.00498775,0.000136977,0.999969,-0.00609305],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget homediangai:=[[247.72,62.50,430.72],[0.452662,-0.49822,0.507755,0.537642],[-1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget homedianbq:=[[276.32,138.43,439.65],[0.620027,0.240368,0.678894,-0.311276],[0,-2,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
! 记录需要搬运的瓶子数量
VAR num nPZS;
!声明中断
VAR intnum intno1 :=0;
!=============================================================
! 主程序
!=============================================================
PROC main()
! 0.中断设置
IDelete intno1;
CONNECT intno1 WITH fuwei;
ISignalDI diFW,1,intno1;
IEnable;
!标签位置计算初始化
BQchushihua;
!死循环
WHILE TRUE DO
!搬瓶子流程
WaitDI diPZ,1;
! [安全保护] 获取PLC传来的数量，但限制最大为4
nPZS := giPZS;
IF nPZS > 4 THEN
nPZS := 4;
ENDIF
!根据模式的不同，选择顺时针或者逆时针搬运
IF diMS = 0 THEN
banPZ(pzSSZ);
ELSEIF diMS =1 THEN
banPZ(pzNSZ);
ENDIF
!告诉PLC瓶子搬完了，脉冲0.5秒
PulseDO \PLength:=0.5, doPZOK;
!搬盖子流程
WaitDI diGZ,1;
banGZ;
!告诉PLC盖子搬完了，脉冲0.5秒
PulseDO \PLength:=0.5, doGZOK;
!搬标签流程
WaitDI diBQ,1;
banBQ;
!告诉PLC标签搬完了，脉冲0.5秒
PulseDO \PLength:=0.5, doBQOK;
ENDWHILE
ENDPROC
!=============================================================
! 子程序：搬瓶子
!=============================================================
PROC banPZ(robtarget pList{*})
FOR i FROM 1 TO nPZS DO
WaitDI diYP,1;
! 取
MoveJ Offs(zhuapz,0,0,80),v500,z50,tool0;
MoveL zhuapz,v500,fine,tool0;
SetDO doJZ,1;
WaitTime 0.5;
MoveL Offs(zhuapz,0,0,80),v500,z50,tool0;
goHome(1);
! 放
MoveJ Offs(pList{i},0,0,200),v500,z50,tool0;
MoveL pList{i},v500,fine,tool0;
SetDO doJZ,0;
WaitTime 0.5;
MoveL Offs(pList{i},0,0,200),v500,z50,tool0;
goHome(1);
ENDFOR
ENDPROC
!=============================================================
! 子程序：搬盖子
!=============================================================
PROC banGZ()
goHome(2);
! 取
MoveJ Offs(xigaizi,0,0,200),v500,z50,tool0;
MoveL xigaizi,v500,fine,tool0;
SetDO doXP1,1;
SetDO doXP2,1;
WaitTime 0.5;
MoveJ Offs(xigaizi,0,0,200),v500,z50,tool0;
! 放
MoveJ Offs(fanggaizi,0,0,200),v500,z50,tool0;
MoveL fanggaizi,v500,fine,tool0;
SetDO doXP1,0;
SetDO doXP2,0;
WaitTime 0.5;
MoveJ Offs(fanggaizi,0,0,200),v500,z50,tool0;
goHome(2);
ENDPROC
!=============================================================
! 子程序：搬标签
!=============================================================
PROC banBQ()
goHome(1);
goHome(3);
! 外循环：处理每一个需要贴标的瓶子
FOR i FROM 1 TO nPZS DO
! 内循环：寻找可用的标签
FOR j FROM 1 TO 24 DO
IF quleBQ{j} = FALSE THEN
! -- 取动作 --
MoveL Offs(pQuBQ{j},0,0,80),v500,z50,tool0;
MoveL pQuBQ{j},v500,fine,tool0;
SetDO doXP1,1;
WaitTime 0.5;
MoveL Offs(pQuBQ{j},0,0,80),v500,z50,tool0;
! -- 放动作 --
MoveL Offs(pFangBQ{i},0,0,280),v500,z50,tool0;
MoveL pFangBQ{i},v500,fine,tool0;
SetDO doXP1,0;
WaitTime 0.5;
MoveL Offs(pFangBQ{i},0,0,280),v500,z50,tool0;
! 标记该位置标签已取走
quleBQ{j} := TRUE;
goHome(3);
! 跳出内层循环，去处理下一个瓶子
GOTO NextBottle;
ENDIF
ENDFOR
NextBottle:
ENDFOR
ENDPROC
!=============================================================
! 通用子程序：回原点
!=============================================================
PROC goHome(num fangshi)
IF fangshi = 1 THEN
MoveJ homedian,v500,z50,tool0;
ELSEIF fangshi = 2 THEN
MoveJ homediangai,v500,z50,tool0;
ELSEIF fangshi = 3 THEN
MoveJ homedianbq,v500,z50,tool0;
ENDIF
ENDPROC
!=============================================================
! 初始化：计算24个标签位置
!=============================================================
PROC BQchushihua()
VAR num i :=1 ;
FOR row FROM 1 TO 4 DO
FOR col FROM 1 TO 6 DO
pQuBQ{i} := Offs(p50,-(row-1)*30,(col-1)*30,0);
quleBQ{i} := FALSE;
i := i+1;
ENDFOR
ENDFOR
ENDPROC
!=============================================================
! 中断程序：复位标签
!=============================================================
TRAP fuwei
BQchushihua;
ENDTRAP
ENDMODULE