MODULE MainModule
CONST robtarget
pHome:=[[517.87,-0.01,708.53],[0.506292,-0.4935,0.509881,-0.490049],[-1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; //定义起始点
PROC main() //主程序
rInitial; //调用初始化程序
WHILE TRUE DO
IF nRunMode=2 THEN
rTempCheck; //调用测温程序
ELSEIF nRunMode=1 THEN //单工位运行方式
rPick;
rPlace;
IF DI03_LoadTest=1 THEN //单次运行
STOP;
ENDIF
ELSE
STOP;
ENDIF
WaitTime 0.5;
ENDWHILE
ENDPROC
PROC Initial() //初始化程序
ConfL\On;
ConfJ\On;
rMoveHome;
IF bPickPart=FALSE THEN //未取件
Reset DO15_ClampClose;
Reset DO14_ClampOpen;
ELSE
STOP;
EXIT;
ENDIF
Reset DO01_DoorOpenCmd; //炉门开
Reset DO02_DoorCloseCmd; //炉门关
Reset DO03_ClampOpenCmd; //夹具开
Reset DO04_ClampCloseCmd; //夹具关
Reset DO05_BurnRollFwd; //加热炉滚筒前进
Reset DO06_FlowFormWork; //旋压工作
AccSet 100,100; //加速度设置
VelSet 100, 2000; //速度设置
ENDPROC
PROC rGripperOpen() //夹具开
Reset DO04_ClampCloseCmd; //夹具关复位
Set DO03_ClampOpenCmd; //夹具开职位
WaitDI DI07_ClampOpened, 1; //等待开完成
ENDPROC
PROC rGripperClose() //夹具关
Reset DO03_ClampOpenCmd;
Set DO04_ClampCloseCmd;
WaitTime nClampTime;
ENDPROC
PROC rBurnerOpen() //炉门开
Reset DO02_DoorCloseCmd;
PulseDO \PLength:=2, DO01_DoorOpenCmd; //2秒开门脉冲
ENDPROC
PROC rBurnerClose() //炉门关
Reset DO01_DoorOpenCmd;
PulseDO \PLength:=2, DO02_DoorCloseCmd; //2秒关门脉冲
IF nRunMode=1 THEN
PulseDO \PLength:=2,DO05_BurnRollFwd; //加热炉工件前进
ENDPROC
PROC rPick() //取件
ClkReset clock2; //循环复位
ClkStart clock2; //循环计时
rGripperOpen;
MoveJ pHome, vMaxEmpty, Z50, tool0; //预抓点
WaitDI DI05_BurnerReady,1; //加热炉就绪
rBurnerOpen;
WaitUntil DI06_BurnOpened=1;
MoveL RelTool(pPick,-pOffsPick{ nProductType}.x,-pOffsPick{ nProductType}.y,-pOffsPick{ nProductType}.z), vMinEmpty, fine, tool0; \\取件偏移点
rGripperClose;
MoveL RelTool(pPick,-pOffsPick{ nProductType}.x-110,-pOffsPick{ nProductType}.y,-pOffsPick{ nProductType}.z), vMinload, z5, tool0; \\取件偏移点上移110 MoveJ pPrePick, vMaxEmpty, Z5, tool0;
MoveJ p80, vMaxEmpty, Z5, tool0;
rBurnClose;
IF DI08_ClampClosed=1 THEN
STOP;
EXIT;
ELSE
bPickPart:=TRUE;
ENDIF
rCoolingDown; \\冷却
MoveJ pPathToPlace1, vMaxLoad, z5, tool0; \\去放置路径点1
MoveJ pPathToPlace2, vMaxLoad, z5, tool0; \\去放置路径点2
MoveJ pPathToPlace3, vMaxLoad, z5, tool0; \\去放置路径点3
ENDPROC
PROC rPlace() \\放置
MoveJ pPrePlace, vMaxLoad, z10, tGripper;
WaitDI DI02_LoadAllowed,1;
MoveL RelTool(pPlace,0,0,nPrePlaceHeight{ nProductType}), vMidLoad, z5, tool0;
MoveL RelTool(pPlace,0,0,nPlaceHeight{ nProductType}), vMinLoad, fine, tool0; \\放置偏移点
rGripperOpen;
MoveL reltool(pPlace,0,0,-1500), vMaxEmpty, z5, tool0;
bPickPart:=FALSE;
MoveJ p240, vMaxEmpty, z5, tool0; \\返回路径点1
MoveJ p250, vMaxEmpty, z5, tool0; \\返回路径点2
MoveJ p260, vMaxEmpty, z5, tool0; \\返回路径点3
MoveJ p270, vMaxEmpty, z5, tool0; \\返回路径点4
Reset DO06_FlowFormWork;
count:=count+1;
ClkStop clock2;
lastcycletime:=ClkRead(clock2); \\单次循环运行时间
IF bShowNumAndTime=1 THEN
TPWrite "No of produced parts="\Num:=count; \\加工产品数
TPWrite "Time took last cycle="\Num:=lastcycletime; \\单次循环运行时间
ENDPROC
PROC rTempCheck() \\温度检测
rMoveHome;
rGripperOpen;
MoveJ pPrePick, vMaxEmpty, Z50, tool0;
WaitDI DI05_BurnerReady,1; \\加热炉准备完成
rFurnaceOpen;
WaitUntil DI06_BurnOpened=1; \\等待炉门打卡
MoveL offs(pPick,-1000,0,0), vMinEmpty, z10, tool0;
MoveL offs(pPick,pOffsPick{ nProductType}.x,pOffsPick{ nProductType}.y,pOffsPick{ nProductType}.z), vMinEmpty, fine, tool0;
rGripperClose;
MoveL offs(pPick,pOffsPick{ nProductType}.x,pOffsPick{ nProductType}.y,pOffsPick{ nProductType}.z+80), vMinEmpty, fine, tool0;
pActualPos:=CRobT(\Tool:=tool0);
MoveL reltool(pActualPos,0,0,-1600), vMinEmpty, fine, tool0; \\垂直法兰盘方向偏移
rFurnaceClose; \\炉门关闭
TPErase; \\清屏
TPWrite "Time for checking temperature! ";
TPWrite "And return current programm with start button in TPU! ";
STOP; \\测温中继续请按start
rFurnaceOpen;
WaitUntil DI06_BurnOpened=1;
MoveL offs(pPick,pOffsPick{ nProductType}.x,pOffsPick{ nProductType}.y,pOffsPick{ nProductType}.z+80), vMinEmpty, fine, tool0; \\放置偏移点上方80
MoveL offs(pPick,pOffsPick{ nProductType}.x,pOffsPick{ nProductType}.y,pOffsPick{ nProductType}.z), vMinEmpty, fine, tool0;
rGripperOpen;
MoveL offs(pPick,-1000,0,0), vMinEmpty, Fine, tool0; \\出炉
rFurnaceClose; \\关门
rMoveHome;
STOP;
ENDPROC
PROC rCoolingDown() \\冷却程序
MoveL reltool(pCold,0,0,250),vMidLoad, Z10, tool0;
MoveL reltool(pCold,0,0, nColdHeigh{ nProductType}),vMinLoad, fine, tool0; \\冷却偏移点
Waittime nColdWaitTime; \\冷却时间
MoveJ reltool(pCold,0,0,250),vMidLoad, fine, tool0;
ENDPROC
PROC rMoveHome() \\回原点程序
VAR bool bMoveHome:=TRUE;
VAR num nTimes:=0;
bMoveHome:=TRUE;
nTimes:=0;
VelSet 100,500; \\速度设置
IF CurrentPos(pHome,tool0,10) THEN
ELSEIF CurrentPos(pPrePick,tool0,10) THEN
ELSEIF CurrentPos(pPrePlace,tool0,10) THEN
MoveJ p240, vMaxEmpty, z5, tool0;
MoveJ p250, vMaxEmpty, z5, tool0;
MoveJ p260, vMaxEmpty, z5, tool0;
ELSEIF CurrentPos(p260,tool0,10) THEN
ENDIF
MoveJ pHome, v500, fine, tool0;
WHILE CurrentPos(pHome,tool0,100)=FALSE DO
TPErase;
TPWrite "Can't find way to home, Please jog robot to home position in Manual mode! ";
TPWrite "And restart again! ";
Stop; \\无法自动回原点，请手动回原点
MoveJ pHome, v500, fine, tool0;
EXITCYCLE;
ENDWHILE
ENDPROC
