天煌教仪机器人程序.

1、停止 伺服 OFF 程序复位 启动 伺服 ON操作权P0：012345等待吸取工件位置P1 ：机器人初始位置卩111:机器人追踪完成到放料中转位置（与P0点左右相对）P2：机器人拿吸盘等待位置（ 2号工装上方）P3：机器人取吸盘位置（ 2号工装）P4：机器人取照相机等待位置（ 3号工装上方）P5：机器人取照相机位置（ 3号工装）P6：取盒子位置P7：取盖子位置P8：入库等待位置（左，近库架，抓手横向）卩81 :入库中转位置（与原点 P1位置相近）P90：仓库左下位置P91 ：仓库右下位置P92 ：仓库左上位置P93 ：仓库右上位置P12：装配单元中转位置（抓手竖向）P13：装配单元中转位置（抓

2、手横向）PH1 ：横向视觉检测台上方位置PV10PV11PV12PV13PV20PV21PV22PV23PV30PV31PV32PV33PV40在1号台 1号工位上拍照位置 在1号台 2号工位上拍照位置 在1号台 3号工位上拍照位置 在1号台 4号工位上拍照位置 在2号台 1号工位上拍照位置 在 2号台 2号工位上拍照位置 在 2号台 3号工位上拍照位置 在 2号台 4号工位上拍照位置 在3号台 1号工位上拍照位置 在 3号台 2号工位上拍照位置 在 3号台 3号工位上拍照位置 在 3号台 4号工位上拍照位置 横向拍照位置P20：1号装配台上方位置（横向）P21 ：1 号装配台上方位置（竖向）

3、P22 ：2号装配台上方位置（竖向）P23 ：3号装配台上方位置（竖向）p25拆解盖库料位置p26拆解盒库料位置PPO1 取 1 号料PPO2取 2号料PPO3取 3号料PPO4取 4号料PGT：传输带上吸取工件位置PPT ：在盒子上放工件准备位置PPT11PPT12PPT13PPT14PPT21PPT22PPT23PPT24PPT31PPT32PPT33PPT341号台 1号工位放置位置 1号台 2号工位放置位置 1号台 3号工位放置位置 1号台 4号工位放置位置 2号台 1号工位放置位置 2号台 2号工位放置位置 2号台 3号工位放置位置 2号台 4号工位放置位置 3号台 1号工位放置位置

4、 3号台 2号工位放置位置 3号台 3号工位放置位置 3号台 4号工位放置位置PFL1： 放废料位置 1 竖向PFL2：放废料位置2竖向PFL3：放废料位置3竖向PFL4：放废料位置4横向m0 ： 装配流程结束标志m1 ：1号装配台情况m2 ：2号装配台情况m3 ：3号装配台情况m5 ：入库当前库位寄存器m10 :接收PLC数据1:工件放置的位置mil :接收PLC数据 2:工件参数（其中个位数为编号，十位数为颜色和高度）m12 :接收PLC数据 3:表示是否还有下一个工件（1为没有）m100 、m105 、m110 、m115m120 、m125 、m130 、m135m140 、m145

5、、m150 、m155m200 、m205 、m210 、m215m220 、m225 、m230 、m235m240 、m245 、m250 、m255m201 、m206 、m211 、m216m221 、m226 、m231 、m236m241 、m246 、m251 、m256m202 、m207 、m212 、m217m222 、m227 、m232 、m237m242 、m247 、m252 、m257m203 、m208 、m213 、m218m223 、m228 、m233 、m238m243 、m248 、m253 、m2581 号装配台 4个工件参数寄存2 号装配台 4个工

6、件参数寄存3 号装配台 4个工件参数寄存 视觉发来的 1 号装配台 4个工件编号 视觉发来的 2号装配台 4个工件编号 视觉发来的 3号装配台 4个工件编号 视觉发来的 1 号装配台 4个工件颜色 视觉发来的 2号装配台 4个工件颜色 视觉发来的 3号装配台 4个工件颜色 视觉发来的 1 号装配台 4个工件角度 视觉发来的 2号装配台 4个工件角度 视觉发来的 3号装配台 4个工件角度 视觉发来的 1 号装配台 4个工件高度 视觉发来的 2号装配台 4个工件高度 视觉发来的 3号装配台 4个工件高度 主程序 初始化处理m5=7m50=1*LOOPGoSub *S90HOME GoSub *Zh

7、aoShou M_Out(8)=1Dly 0.5Im0=0m1=0m2=0m3=0m7=0m8=0m31=0m32=0m33=0m100=0m105=0m110=0m115=0m120=0m125=0m130=0m135=0m140=0m145=0m150=0m155=0IWait M_In(9)=1Dly 0.5M_Out(8)=0GoSub *RecDat_PLCGoSub *Jia_HeGoSub *Jia_XiPanMov P111*LOOP1Mvs P0M_Out(5)=1Open COM3: As #2Wait M_Open(2) =1Input #2, m10,m11,m12,m

8、13十位数主循环原点返回处理 招手动作，展示用输出完成复位信号为 1已就绪 （X36 ） 一个装配流程结束是否结束， 1为结束1号装配台正常2号装配台正常3号装配台正常拆解标志拆解仓库第 9个仓位有无标志位1号装配台正常2号装配台正常3号装配台正常工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零工件参数清零等待接收运行信号为 1 可取盒（ Y14）输出完成复位信号为 0 -X36接收PLC装配流程数据夹取工件盒到装配台子程序夹取吸盘工装子程序卩12到P0的过渡装配检测循环运行到吸取等待位置机器人等待吸取信号为

9、1 - 已到位（ X33）打开网络端口（与 PLC相接的以太网端口）m10 工件放置的位置m11 工件参数 （其中个位数为编号，为颜色和高度）m12 表示是否还有下一个工件（ 1为没有）m13 表示是否为有用工件（ 1 为没用）Close #2If m130 Then GoSub *FJSelect m10Case 1PPT0= PPT11 BreakCase 2PPT0= PPT12 BreakCase 3PPT0= PPT13BreakCase 4PPT0= PPT14 BreakCase 5PPT0= PPT21 BreakCase 6PPT0= PPT22 BreakCase 7PPT

10、0= PPT23 BreakCase 8PPT0= PPT24 BreakCase 9PPT0= PPT31 BreakCase 10PPT0= PPT32 BreakCase 11PPT0= PPT33 BreakCase 12PPT0= PPT34BreakEnd SelectGoSub *S20TRGETIf m13=1 And m10=0 Then *FeiLiao0GoSub *S30WKPUT*Goon设定PPT11为工件放置目标位置设定PPT12为工件放置目标位置设定PPT13为工件放置目标位置设定PPT14为工件放置目标位置设定 PPT21 为工件放置目标位置设定PPT22为工

11、件放置目标位置设定PPT23为工件放置目标位置设定PPT24为工件放置目标位置设定PPT31为工件放置目标位置设定PPT32为工件放置目标位置设定PPT33为工件放置目标位置设定PPT34为工件放置目标位置检测到工件来 ,进行跟踪工件吸取操作 不要的工件放废料框中 工件放置处理If m12=1 Then S10INIT初始化处理M_Out(5)=0机器人等待吸取信号为0 X33M_Out(8)=0输出完成复位信号为 0 X36Def Pit 1,P90,P91,P92,P93,3,3,2设置仓库位置，以P90为起点，以P91为终点A,以P92为终点B,以P93为对角点，行为3,列为3,同方向排

12、列加速度、减速度设定速度设定 最佳加速度、减速度说明 打开最佳加速度、减速度Cnt 0Cir 1VtestGoTo *LOOP1I*VtestGoSub *Fang_XiPanGoSub *Jia_CameraGoSub *Camera_VGoSub *Fang_CameraGoSub *Comparison_VIf m1=1 And m2=1 And m3=1 Then *HTESTGoSub *Jia_XiPanGoSub *RotateGoSub *Fang_XiPanm12=1, 说明一个装配流程结束 ,转视觉检测流程 继续放置下一个工件视觉竖向检测放置吸盘工装子程序夹取视觉工装子程序

13、视觉竖向检测子程序放置视觉工装子程序竖向视觉信息比对子程序夹取吸盘工装子程序 工件角度旋转调整子程序放置吸盘工装子程序视觉横向检测3个装配台已无合格的工件盒夹盒到横向检测台子程序夹取视觉工装子程序视觉横向检测子程序放置视觉工装子程序 横向视觉信息比对子程序 已作废料被扔掉取盖放盖子程序入库子程序出库拆解子程序*HTESTIf m1=1 And m2=1 And m3=1 Then *LOOP 存在GoSub *JiaHe_To_HGoSub *Jia_CameraGoSub *Camera_HGoSub *Fang_CameraGoSub *Comparison_HIf m20=1 Then

14、*HTESTGoSub *Jia_gaiGoSub *RukuIf m7=1 Then *ChaiJieGoTo *HTEST*ChaiJieMov P1GoSub *CIANEndI*初始化处理子程序*Accei 100,100Ovrd 100Loadset 1,1OAdi OnTrClr 1 清除追踪缓冲MWAIT1=0清除工件等待标记M_09#=PWK.XIf M_Run(2)=0 ThenXRun 2,CM1,1 Wait M_Run(2)=1模式号码说明 在插槽 2选择程序 CM1EndIfPriority PRI.X,1Priority PRI.Y,2Return1 *原点返回处理

15、 *S90HOMEHOpen 1HOpen 2M_Out(5)=0P90CURR=P_Fbc(1)If P90CURR.Z=1 Then GoTo *LREADMov P0 Type 0,0MWAIT1=0GoTo *LBFCHK/ 工件数据获得 / *LREADTrRd PBPOS,MBENC#,MBWK%,1,MBENCNO% GoSub *S40DTSET/ 工件位置确认 /*LNEXTPX50CUR=TrWcur(MBENCNO%,PBPOS,MBENC#) MX50ST=PRNG.XMX50ED=PRNG.YMX50PAT=PTN.XGoSub *S50WKPOSIf MY50STS

16、=3 Then GoTo *LBFCHKIf MY50STS=2 Then GoTo *LTRSTIf MWAIT=1 Then GoTo *LNEXT/ 去位置 /PWAIT=P0Select PTN.XCase 1 To 2PWAIT.X=PX50CUR.XCase 3 To 6PWAIT.Y=PX50CUR.YEnd SelectPWAIT.Z=PX50CUR.Z+PUP1.XPWAIT.C=PX50CUR.C运输数据设定获得现在工件位置开始距离范围在机器人跟随工件停止距离范围在机器人跟随工件输送模式号码工件位置确认处理已经通过，去下个工件开始追踪等待工件到来 改变工件等待姿势 输送位置

17、模式号码 当输送在机器人之前机器人匹配到工件的 X坐标机器人匹配到工件的 丫坐标Mov PWAIT Type 0,0MWAIT1=1GoTo S30WKPUTMvs P111M_Out(5)=0LNEXT/ 开始追踪操作 /*LTRSTAccel PAC1.X,PAC1.YCnt 1,0,0Act 1=1Trk On,PBPOS,MBENC#,PTBASE,MBENCNO% Ovrd 75Mov PGT,PUP1.Y Type 0,0Accel PAC2.X,PAC2.YMvs PGTHClose 2Dly PDLY2.ZCnt 1Accel PAC3.X,PAC3.YOvrd 100Mvs

18、PGT,PUP1.ZTrk OffAct 1=0Accel 100,100MWAIT = 0Return移动到工件等待姿势设置工件等待标记监视机器人跟随工件太远追踪操作开始设置移动到追踪半空位置移动到吸取位置吸取开吸取确定移动到追踪半空位置追踪操作和设定I*FeiLiao0工件放废料框子程序 *Mvs P0Accel PAC12.X,PAC12.Y 移动到放置位置Cnt 1,0,0Mvs PFL1+(+0.00,+0.00,+80.00,+0.00,+0.00,+0.00)HOpen 2 吸取关Dly 0.5 释放确认M_Out(5)=0机器人等待吸取信号为 0 -X33m13=0Mvs P0

19、GoTo *GoonI*工件放置子程序*机器人等待吸取信号为 0 X33Mov P12Accel PAC12.X,PAC12.YCnt 1,0,0Mvs PPTOvrd 40Mvs PPT0Mvs ,30If M_In(15)=0 Then *POKI*m6=0GoSub *WeiTiao1If m6=1 Then *POKGoSub *WeiTiao2If m6=1 Then *POKGoSub *WeiTiao3*POKI*HOpen 2Dly 0.5Ovrd 100Mvs ,-35Mov P12Mov P111移动到放置位置放料移动到目标位置下降 工件位置微调 1mm 工件位置微调 2m

20、m 工件位置微调 3mm吸取关释放确认ReturnI*放置工件位置微调子程序（ 1mm ）*WeiTiao1m6=0Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+0.00,+1.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+0.00,-1.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+

21、1.00,+0.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15右移 1mm 左移 1mm前移 1mmm10 的值决定）I*放置工件位置微调子程序（ 2mm ）Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(-1.00,+0.00,-15.00,+0.00,+0.00,+0.00) 后移 1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+1.00,+1.00,-15.00,+0.00,+0.00,+0.00)前移、右移各 1mmMvs ,15Dl

22、y 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+1.00,-1.00,-15.00,+0.00,+0.00,+0.00) 前移、左移各 1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(-1.00,+1.00,-15.00,+0.00,+0.00,+0.00) 后移、右移各 1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(-1.00,-1.00,-15.00,+0.00,+0.00,+0.00) 后

23、移、左移各 1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Return*PlaceOK1m6=1Return*WeiTiao2m6=0Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(+0.00,+2.00,-15.00,+0.00,+0.00,+0.00)右移 2mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(+0.00,-2.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=

24、0 Then *PlaceOK2Mvs ,-15Mov PPT0+(+2.00,+0.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(-2.00,+0.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(+2.00,+2.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK

25、2Mvs ,-15Mov PPT0+(+2.00,-2.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(-2.00,+2.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(-2.00,-2.00,-15.00,+0.00,+0.00,+0.00)Mvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Return*PlaceOK

26、2m6=1Return 左移 2mm前移 2mm 后移 2mm前移、右移各 2mm 前移、左移各 2mm 后移、右移各 2mm 后移、左移各 2mmI*WeiTiao3m6=0Dly 1放置工件位置微调子程序（3mm )If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(+0.00,+3.00,-15.00,+0.00,+0.00,+0.00)右移 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(+0.00,-3.00,-15.00,+0.00,+0.00,+0.00) 左移 3m

27、mMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(+3.00,+0.00,-15.00,+0.00,+0.00,+0.00)前移 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(-3.00,+0.00,-15.00,+0.00,+0.00,+0.00) 后移 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(+3.00,+3.00,-15.00,+0.00,+0.00,+0.00)前

28、移、右移各 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(+3.00,-3.00,-15.00,+0.00,+0.00,+0.00) 前移、左移各 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(-3.00,+3.00,-15.00,+0.00,+0.00,+0.00) 后移、右移各 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Mvs ,-15Mov PPT0+(-3.00,-3.00,-15.00,+0

29、.00,+0.00,+0.00) 后移、左移各 3mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK3Return*PlaceOK3m6=1Return1 *输运数据设定处理子程序 *S40DTSETPTBASE=P_100(PWK.X)TrBase PTBASE,MBENCNO%PGT=PTBASE*POFSETGoSub *S46ACSETReturn做参考位置追踪基本设定吸取点设置中断定义I*S46ACSETSelect PTN.XCase 1MSTP1=PRNG.ZDef Act 1,P_Fbc(1).YMSTP1 GoTo *S91STOP,S Cas

30、e 2MSTP1=-PRNG.ZDef Act 1,P_Fbc(1).YMSTP1 GoTo *S91STOP,S BreakCase 4Case 6MSTP1=-PRNG.ZDef Act 1,P_Fbc(1).XMSTP1 GoTo *S91STOP,S BreakEnd Select子程序 *输送位置模式号码前右到左下次停止距离To *S91STOP if followed far long Break 前左到右左后到前右后到前左前到后右前到后ReturnI*S91STOPAct 1=0Trk OffHClose 2P91P=P_Fbc(1)P91P.Z=P1.ZMvs P91P Typ

31、e 0,0Mov P1GoTo *LBFCHK追踪中断处理子程序*Release suction 释放吸取 获得当前位置上升返回原点一次1 *夹取视觉相机工装子程序 *Jia_CameraMvs P4 视觉相机工装正上方位置HOpen 1Wait M_In(900)=1 等待手爪松开信号为 1Dly 0.5 延时Ovrd 20速度设定Mvs P5 视觉相机工装位置Dly 0.5HClose 1 夹取视觉相机工装Wait M_In(901)=1等待手爪夹紧信号为 1Dly 0.5延时Ovrd 50Mvs P4 视觉相机工装正上方位置Mvs P12Ovrd 100速度设定ReturnI*S50WK

32、POS工件位置确认子程序*PX50CUR: 当前工件位置 MX50ST: 开始追踪范围 MX50ED: 停止追踪范围 MX50PAT: 追踪位置模式号码 MY50STS: 结果( 1、等待 MY50STS=0Select MX50PATCase 1M50STT=-MX50STM50END=MX50EDMY50STS=2End SelectReturn2、开始追踪 3 、下个工件）清除返回值输送模式前到左开始值是负值追踪可能I*放置视觉相机工装子程序*Fang_CameraMvs P4Dly 0.5Ovrd 20视觉相机工装正上方位置速度设定Mvs P5+(+0.00,+0.00,+90.00,

33、+0.00,+0.00,+0.00)Ovrd 10 速度设定Mvs P5Dly 0.5HOpen 1Wait M_In(900)=1Dly 0.5Ovrd 50Mvs P4Ovrd 100Mvs P12Return 视觉相机工装位置 夹取视觉相机工装等待手爪松开信号为 1速度设定 视觉相机工装正上方位置速度设定视觉竖向检测子程序*I*Camera_V检测 1号装配台If m1=1 Then *Camera_V2M_Out(13)=0Open COM2: As #1Wait M_Open(1) =1Ovrd 30Mvs PV10Dly 1M_Out(13)=1Dly 0.5Input #1, m

34、200,m201,m202Close #1M_Out(13)=0IDly 0.5Open COM2: As #1Wait M_Open(1) =1Mvs PV11Dly 1M_Out(13)=1Dly 0.5Input #1, m205,m206,m207Close #1M_Out(13)=0IDly 0.5Open COM2: As #1Wait M_Open(1) =1Mvs PV12Dly 1M_Out(13)=11 号装配台不需要装配，转 2号装配台输出拍照信号为清零速度设定1号台1号工位 输出拍照信号为 1，触发一次拍照数据 1为编号、数据 2为颜色、数据 3 为角度输出拍照信号清零

35、1号台2号工位输出拍照信号为 1，触发一次拍照输出拍照信号清零1号台3号工位输出拍照信号为 1，触发一次拍照Dly 0.5Input #1, m210,m211,m212Close #1M_Out(13)=0IDly 0.5Open COM2: As #1Wait M_Open(1) =1Mvs PV13Dly 1M_Out(13)=1Dly 0.5Input #1, m215,m216,m217Close #1M_Out(13)=0II*检测 2号装配台*Camera_V2M_Out(13)=0If m2=1 Then *Camera_V3Dly 0.5Open COM2: As #1Wait M_Open(1) =1Mvs PV20Dly 1M_Out(13)=1Dly 0.5Input #1, m220,m221,m222Close #1M_Out(13)=0IDly 0.5Open COM2: As #1Wait