TASK PERS tooldata TCP_Spindle:=[TRUE,[[291.33,167.604,132.752],[1,0,0,0]],[0.1,[1,1,1],[1,0,0,0],0,0,0]];
PERS pos ActualPos2:=[9E+09,9E+09,9E+09];
PERS pos HomePos:=[9E+09,9E+09,9E+09];
PERS pos ActualPosTemp:=[9E+09,9E+09,9E+09];
PERS pos p1010Pos:=[9E+09,9E+09,9E+09];
CONST robtarget pHome:= [[795.85,6.37,18.16],[0.00603037,0.708691,-0.705406,0.0111058],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pActualPos:=[[482.2,2.35,1142.99],[0.011582,0.00662,-0.999867,0.009436],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget phome1:=[[1058.60,81.29,1173.72],[0.7072,0.032109,0.705328,-0.03496],[-1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1010:=[[826.29,1139.54,986.43],[0.510404,-0.498693,0.483949,0.506544],[0,1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1030:=[[826.30,1139.53,981.11],[0.510403,-0.498694,0.483955,0.506537],[0,1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1050:=[[826.25,653.95,981.08],[0.51037,-0.498771,0.483993,0.5059],[0,1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1070:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.9575,0.2714],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1090:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.9575,0.2714],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1110:=[[500.55,-1594.16,1287.39],[0.00607319,0.708728,-0.705367,0.0111581],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p1130:=[[500.58,-1614.22,18.18],[0.00603701,0.708694,-0.705403,0.0111083],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2010:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.9575,0.2714],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2030:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.9575,0.2714],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget
p2110:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.9575,0.2714],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget
p2130:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.9575,0.2714],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
ClearI_O;
!Program 1 类型1 搬运
!Program 2 类型2搬运
!Auto Return Home 自动回原位程序，已经取消。
！类型1原位放件，原位→总装托盘上，原位→测试托盘上。
ENDPROC
！自动循环模式下 工件类型1搬运
！判断条件，总装准备就绪，测试准备就绪，机器人夹爪上无件。
！调用搬运程序
！自动模式下 工件类型2搬运
！类型1搬运程序
！复位PHome 信号
！等待PLC→“ABB机器人夹具上无件”
！圆弧插补以速度V50MM/S,Z0为精度半径到达P1030 Y轴偏移-500mm点。
！等待PLC→ABB “夹具松开到位”信号
！等待PLC→ABB “1.3类型拔销到位”信号
！直线插补的运行方式 走到P1030点位。
！直线插补的运行方式 走到P1010点位。
！给PLC发送“总装线类型1取件位”到位。
！等待夹具夹紧到位。
！等待1.3插销气缸到位
！复位“总装线类型1取件位”到位信号。
！直线插补的运行方式 移动到P1010 Z轴方向+300点位，也就是取件位的正上方位置。
！回原位点，此时已经取件完成，再在去放件位。
！去放件位P1110 Z轴方向+200点位，也就是在放件位的上方
！直线运行到 P1110点，也就是放件位
！置位ABB→PLC 机器人到达“测试线类型1放件位”到达
！等待夹具松开到位。
！等待夹具1.3拔销到位
！复位ABB→PLC 机器人到达“测试线类型1放件位”到达
！直线走到 P1110 Y轴-20点位。此处是为了防止夹具挂到工件，将夹具平移一下。
！直线走到P1130点，也就是抬高到安全位置。
！给PLC发送 类型1工作完成信号。PLC以此信号得到PROCESS CMP。
！回原位。
!给PLC发送 ABB在原位
！自动回原位
MoveJ pHome, v50, fine, tool0;
！自动回原位判断
！自动回原位判断安全范围，也就是空间2点间的距离。在PLC程序备份中可以使用 空间两点距离计算.xls 来计算空间2点距离。
MoveJ pHome, v50, fine, tool0;
ErrWrite "Robot not in home position", "Move robot manually near homeposition";
！在屏幕上显示出错误提示“机器人不在安全位置，请将机器人手动移动到安全位置”
!When ROBT at HT Position,safe range .
MoveJ pHome, v50, fine, tool0;
ErrWrite "Robot not in home position", "Move robot manually near homeposition";
！判断当前位置是否在原位。
!---------------------------------------------------------------------------------
!Abstract : Function to compare current manipulator position with a given position
!---------------------------------------------------------------------------------
！自动回原位，已经取消
！PLC手动模式&ABB自动模式下 在HMI上点击 去类型1总装放件
！PLC手动模式&ABB自动模式下 在HMI上点击 去类型1测试线放件
！清除IO信号
Reset do_Type1_Working;
ENDMODULE下载本文
