搬运机器人培训说明书

MODULE MainModule 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 ActualPos:=[9E+09,9E+09,9E+09]; 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,1648.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.707289,0.032109,0.705328,-0.0349896],[-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.506459],[0,1,-2,0],[9E+09,9E+0 9,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p1070:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.649575,0.276414],[0,0,-1,1],[9E+09,9E+0 9,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p1090:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.649575,0.276414],[0,0,-1,1],[9E+09,9E+0 9,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,1648.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.649575,0.276414],[0,0,-1,1],[9E+09,9E+0 9,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p2030:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.649575,0.276414],[0,0,-1,1],[9E+09,9E+0 9,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p2110:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.649575,0.276414],[0,0,-1,1],[9E+09,9E+0 9,9E+09,9E+09,9E+09,9E+09]]; CONST robtarget p2130:=[[9E+09,9E+09,9E+09],[0.651074,-0.278836,0.649575,0.276414],[0,0,-1,1],[9E+09,9E+0 9,9E+09,9E+09,9E+09,9E+09]]; PROC main() ClearI_O; IF CurrentPos(pHome,tool0) = TRUE THEN Set do_pHome; ENDIF IF di_auto=1 THEN AutoMoveHome; ENDIF !Program 1 类型 1 搬运 IF di_ProgBit=1 and di_auto=1 and di_Start=1 THEN Autoroutine1; ENDIF !Program 2 类型 2搬运 IF di_ProgBit=2 and di_auto=1 and di_Start=1 THEN Autoroutine2; ENDIF !Auto Return Home 自动回原位程序，已经取消。 !IF di_Robt_At_Phome=0 and di_manual=1 and di_Returnhome=1 and di_Closed=1 and di_Type1_Plug=1 then !Auto_Return_Home; !Endif ！类型 1原位放件，原位→总装托盘上，原位→测试托盘上。 IF di_manual=1 and di_Tool_Check=1 and di_Robt_At_Phome=1 and di_Type1_TAPut=1 then Auto_Put_Type1_TA; Endif IF di_manual=1 and di_Tool_Check=1 and di_Robt_At_Phome=1 and di_Type1_HTPut=1 then Auto_Put_Type1_HT; Endif ENDPROC PROCAutoroutine1() ！自动循环模式下 工件类型 1搬运 If di_TA_Ready=1 and di_HT_Ready=1 and di_Tool_Check=0 then ！判断条件，总装准 备就绪，测试准备就绪，机器人夹爪上无件。 Set do_Type1_Working; Type1_Carry; ！调用搬运程序 Reset do_Type1_Working; Waittime 0.5; endif ENDPROC PROCAutoroutine2() ！自动模式下 工件类型 2搬运 Set do_Type2_Working; Type2_Carry; MoveJ pHome, v10, fine, tool0; Reset do_Type2_Working; Waittime 0.5; ENDPROC PROC Type1_Carry() ！类型 1搬运程序 Reset do_pHome; ！复位 PHome 信号 WaitDI di_Tool_Check, 0; ！等待 PLC→“ABB机器人夹具上无件” MoveJ Offs(p1030,0,-500,0), v50, z0, tool0; ！圆弧插补以速度 V50MM/S,Z0为精度半径 到达 P1030 Y轴偏移-500点。 WaitDI di_Open, 1; ！等待 PLC→ABB “夹具松开到位”信号 WaitDI di_Type1_Pull_Pin, 1;！等待 PLC→ABB “1.3类型拔销到位”信号 MoveL p1030, v50, fine, tool0; ！直线插补的运行方式 走到 P1030点位。 MoveL p1010, v50, fine, tool0;！直线插补的运行方式 走到 P1010点位。 Set do_TA_Type1_Pos_Reache; ！给 PLC发送“总装线类型 1取件位”到位。 WaitDI di_Closed, 1; ！等待夹具夹紧到位。 WaitDI di_Type1_Plug, 1; ！等待 1.3插销气缸到位 Reset do_TA_Type1_Pos_Reache; ！复位“总装线类型 1取件位”到位信号。 Waittime 0.5; MoveL Offs(p1010,0,0,300), v50, z0, tool0; ！直线插补的运行方式 移动到 P1010 Z轴方 向+300点位，也就是取件位的正上方位置。 !take the speed changing box to phome MoveJ phome, v50, z20, tool0; ！回原位点，此时已经取件完成，再在去放件位。 MoveL OFFS(p1110,0,0,200), v50, z0, tool0; ！去放件位 P1110 Z轴方向+200点位，也 就是在放件位的上方 MoveL p1110, v50, fine, tool0; ！直线运行到 P1110点，也就是放件位 Set do_HT_Type1_Pos_Reache; ！置位 ABB→PLC 机器人到达“测试线类型 1放件位” 到达 WaitDI di_Open, 1; ！等待夹具松开到位。 WaitDI di_Type1_Pull_Pin, 1; ！等待夹具 1.3拔销到位 Reset do_HT_Type1_Pos_Reache; ！复位 ABB→PLC 机器人到达“测试线类型 1放件 位”到达 Waittime 0.5; MoveL OFFS(p1110,0,-20,0), v50, z0, tool0; ！直线走到 P1110 Y轴-20点位。此处是为 了防止夹具挂到工件，将夹具平移一下。 MoveL p1130, v50, fine, tool0; ！直线走到 P1130点，也就是抬高到安全位置。 Set do_Type1_WorkCMP; ！给 PLC发送 类型 1工作完成信号。PLC以此信号得到 PROCESS CMP。 MoveJ pHome, v50, fine, tool0; ！回原位。 set do_pHome; !给 PLC发送 ABB在原位 ENDPROC PROC Type2_Carry() Reset do_pHome; WaitDI di_Tool_Check, 0; MoveJ Offs(p2030,0,-500,0), v50, z0, tool0; WaitDI di_Open, 1; WaitDI di_type2_Pull_Pin, 1; MoveL p2030, v50, fine, tool0; MoveL p2010, v50, fine, tool0; Set do_TA_type2_Pos_Reache; WaitDI di_Closed, 1; WaitDI di_Type2_Plug, 1; Reset do_TA_Type2_Pos_Reache; Waittime 0.5; MoveL Offs(p2010,0,0,300), v50, z0, tool0; !take the speed changing box to phome MoveJ phome, v50, z20, tool0; MoveL OFFS(p2110,0,0,200), v50, z0, tool0; MoveL p2110, v50, fine, tool0; Set do_HT_Type2_Pos_Reache; WaitDI di_Open, 1; WaitDI di_type2_Pull_Pin, 1; Reset do_HT_type2_Pos_Reache; Waittime 0.5; MoveL OFFS(p2110,0,-20,0), v50, z0, tool0; MoveL p2130, v50, fine, tool0; Set do_Type2_WorkCMP; MoveJ pHome, v50, fine, tool0; set do_pHome; ENDPROC PROCAutoMoveHome() ！自动回原位 IF CurrentPos(pHome,tool0) THEN MoveJ pHome, v50, fine, tool0; ENDIF IF CurrentPos(pHome,tool0) = TRUE THEN Set do_pHome; ENDIF WHILE CurrentPos(pHome,tool0) = FALSE DO ！自动回原位判断 ActualPos:=CPos(\Tool:=tool0\WObj:=wobj0); ActualPosTemp:=CPos(\Tool:=tool0\WObj:=wobj0); HomePos:=[pHome.trans.x,pHome.trans.y,pHome.trans.z]; ActualPos:=ActualPos-HomePos; !When ROBT at TA Position,safe range . IF ActualPosTemp.y>=pHome.trans.y then IF Sqrt(Pow(ActualPos.x,2)+Pow(ActualPos.y,2)+Pow(ActualPos.z,2))<770 THEN ！自 动回原位判断安全范围，也就是空间 2点间的距离。在 PLC程序备份中可以使用 空间两点 距离计算.xls 来计算空间 2点距离。 MoveJ pHome, v50, fine, tool0; ELSE ErrWrite "Robot not in home position", "Move robot manually near homeposition"; Stop; ！在屏幕上显示出错误提示“机器人不在安全位置，请将机器人手动移动 到安全位置” Waittime 1; ENDIF !When ROBT at HT Position,safe range . Else IF Sqrt(Pow(ActualPos.x,2)+Pow(ActualPos.y,2)+Pow(ActualPos.z,2))<1195 THEN MoveJ pHome, v50, fine, tool0; ELSE ErrWrite "Robot not in home position", "Move robot manually near homeposition"; Stop; Waittime 1; ENDIF Endif ENDWHILE ENDPROC FUNC bool CurrentPos( robtarget pComparePos, INOUT tooldata tool0) ！判断当前位置是否在原位。 VAR num Counter:=0; VAR robtarget pActualPos; !--------------------------------------------------------------------------------- !Abstract : Function to compare current manipulator position with a given position !--------------------------------------------------------------------------------- pActualPos:=CRobT(\Tool:=tool0\WObj:=wobj0); IF pActualPos.trans.x>pComparePos.trans.x-25 AND pActualPos.trans.xpComparePos.trans.y-25 AND pActualPos.trans.ypComparePos.trans.z-25 AND pActualPos.trans.zpComparePos.rot.q1-0.1 AND pActualPos.rot.q1pComparePos.rot.q2-0.1 AND pActualPos.rot.q2pComparePos.rot.q3-0.1 AND pActualPos.rot.q3pComparePos.rot.q4-0.1 AND pActualPos.rot.q4pHome.trans.y+25 then MoveL Offs(CRobT(\Tool:=tool0\WObj:=wobj0),0,0,-reg1), v50, z0, tool0; Movej pHome, v50, fine, tool0; Set do_pHome; Endif IF ActualPos2.ypHome.trans.y +25 then MoveL Offs(CRobT(\Tool:=tool0\WObj:=wobj0),0,-reg3,0), v50, z0, tool0; Movej pHome, v50, fine, tool0; Set do_pHome; Endif IF ActualPos2.y