天煌教儀機器人程序課件

1、'停止 0'伺服OFF 1'程序復位 2'啟動 3'伺服ON 4'操作權 5 'P0： 等待吸取工件位置 'P1： 機器人初始位置 'P111： 機器人追蹤完成到放料中轉位置（與P0點左右相對） 'P2： 機器人拿吸盤等待位置（2號工裝上方） 'P3： 機器人取吸盤位置（2號工裝） 'P4： 機器人取照相機等待位置（3號工裝上方） 'P5： 機器人取照相機位置（3號工裝） 'P6： 取盒子位置 'P7： 取蓋子位置 'P8： 入庫等待位置（左，近庫架，抓手橫向） &#

2、39;P81： 入庫中轉位置（與原點P1位置相近） 'P90： 倉庫左下位置 'P91： 倉庫右下位置 'P92： 倉庫左上位置 'P93： 倉庫右上位置 'P12： 裝配單元中轉位置（抓手豎向） 'P13： 裝配單元中轉位置（抓手橫向） 'PH1： 橫向視覺檢測臺上方位置 'PV10： 在1號臺1號工位上拍照位置 'PV11： 在1號臺2號工位上拍照位置 'PV12： 在1號臺3號工位上拍照位置 'PV13： 在1號臺4號工位上拍照位置 'PV20： 在2號臺1號工位上拍照位置 'PV21

3、： 在2號臺2號工位上拍照位置 'PV22： 在2號臺3號工位上拍照位置 'PV23： 在2號臺4號工位上拍照位置 'PV30： 在3號臺1號工位上拍照位置 'PV31： 在3號臺2號工位上拍照位置 'PV32： 在3號臺3號工位上拍照位置 'PV33： 在3號臺4號工位上拍照位置 'PV40： 橫向拍照位置 'P20： 1號裝配臺上方位置（橫向） 'P21： 1號裝配臺上方位置（豎向） 'P22： 2號裝配臺上方位置（豎向） 'P23： 3號裝配臺上方位置（豎向） 'p25 拆解蓋庫料位置 

4、9;p26 拆解盒庫料位置 'PPO1 取1號料 'PPO2 取2號料 'PPO3 取3號料 'PPO4 取4號料 'PGT： 傳輸帶上吸取工件位置 'PPT： 在盒子上放工件準備位置 'PPT11：1號臺1號工位放置位置 'PPT12：1號臺2號工位放置位置 'PPT13：1號臺3號工位放置位置 'PPT14：1號臺4號工位放置位置 'PPT21：2號臺1號工位放置位置 'PPT22：2號臺2號工位放置位置 'PPT23：2號臺3號工位放置位置 'PPT24：2號臺4號工位放置位置

5、 'PPT31：3號臺1號工位放置位置 'PPT32：3號臺2號工位放置位置 'PPT33：3號臺3號工位放置位置 'PPT34：3號臺4號工位放置位置 'PFL1： 放廢料位置1 豎向 'PFL2： 放廢料位置2 豎向 'PFL3： 放廢料位置3 豎向 'PFL4： 放廢料位置4 橫向 'm0： 裝配流程結束標志 'm1： 1號裝配臺情況 'm2： 2號裝配臺情況 'm3： 3號裝配臺情況 'm5： 入庫當前庫位寄存器 'm10： 接收PLC數(shù)據(jù)1:工件放置的位置 'm11

6、： 接收PLC數(shù)據(jù)2:工件參數(shù)（其中個位數(shù)為編號，十位數(shù)為顏色和高度） 'm12： 接收PLC數(shù)據(jù)3:表示是否還有下一個工件（1為沒有） 'm100、m105、m110、m115： 1號裝配臺4個工件參數(shù)寄存 'm120、m125、m130、m135： 2號裝配臺4個工件參數(shù)寄存 'm140、m145、m150、m155： 3號裝配臺4個工件參數(shù)寄存 'm200、m205、m210、m215： 視覺發(fā)來的1號裝配臺4個工件編號 'm220、m225、m230、m235： 視覺發(fā)來的2號裝配臺4個工件編號 'm240、m245、m250、m

7、255： 視覺發(fā)來的3號裝配臺4個工件編號 'm201、m206、m211、m216： 視覺發(fā)來的1號裝配臺4個工件顏色 'm221、m226、m231、m236： 視覺發(fā)來的2號裝配臺4個工件顏色 'm241、m246、m251、m256： 視覺發(fā)來的3號裝配臺4個工件顏色 'm202、m207、m212、m217： 視覺發(fā)來的1號裝配臺4個工件角度 'm222、m227、m232、m237： 視覺發(fā)來的2號裝配臺4個工件角度 'm242、m247、m252、m257： 視覺發(fā)來的3號裝配臺4個工件角度 'm203、m208、m213、

8、m218： 視覺發(fā)來的1號裝配臺4個工件高度 'm223、m228、m233、m238： 視覺發(fā)來的2號裝配臺4個工件高度 'm243、m248、m253、m258： 視覺發(fā)來的3號裝配臺4個工件高度 '*S00MAIN '主程序GoSub *S10INIT '初始化處理m5=7 '倉庫首個庫位設為1號m50=1*LOOP '主循環(huán)GoSub *S90HOME '原點返回處理'GoSub *ZhaoShou ' 招手動作，展示用M_Out(8)=1 '輸出完成復位信號為1已就緒 （X36）Dly 0.5&#

9、39;m0=0 '一個裝配流程結束是否結束，1為結束m1=0 '1號裝配臺正常m2=0 '2號裝配臺正常m3=0 '3號裝配臺正常m7=0 '拆解標志m8=0 '拆解倉庫第9個倉位有無標志位m31=0 '1號裝配臺正常m32=0 '2號裝配臺正常m33=0 '3號裝配臺正常m100=0 '工件參數(shù)清零m105=0 '工件參數(shù)清零m110=0 '工件參數(shù)清零m115=0 '工件參數(shù)清零m120=0 '工件參數(shù)清零m125=0 '工件參數(shù)清零m130=0 '工件參數(shù)清零m

10、135=0 '工件參數(shù)清零m140=0 '工件參數(shù)清零m145=0 '工件參數(shù)清零m150=0 '工件參數(shù)清零m155=0 '工件參數(shù)清零'Wait M_In(9)=1 '等待接收運行信號為1可取盒（Y14）Dly 0.5M_Out(8)=0 '輸出完成復位信號為0 -X36GoSub *RecDat_PLC '接收PLC裝配流程數(shù)據(jù)GoSub *Jia_He '夾取工件盒到裝配臺子程序GoSub *Jia_XiPan '夾取吸盤工裝子程序Mov P111 'P12到P0的過渡''*

11、LOOP1 '裝配檢測循環(huán) Mvs P0 '運行到吸取等待位置 M_Out(5)=1 '機器人等待吸取信號為1 -已到位（X33） Open "COM3:" As #2 '打開網(wǎng)絡端口（與PLC相接的以太網(wǎng)端口） Wait M_Open(2) =1 Input #2, m10,m11,m12,m13 'm10工件放置的位置 'm11工件參數(shù)（其中個位數(shù)為編號，十位數(shù)為顏色和高度） 'm12表示是否還有下一個工件（1為沒有） 'm13表示是否為有用工件（1為沒用） Close #2'If m13>0

12、 Then GoSub *FJ Select m10 Case 1 PPT0= PPT11 '設定PPT11為工件放置目標位置 Break Case 2 PPT0= PPT12 '設定PPT12為工件放置目標位置 Break Case 3 PPT0= PPT13 '設定PPT13為工件放置目標位置 Break Case 4 PPT0= PPT14 '設定PPT14為工件放置目標位置 Break Case 5 PPT0= PPT21 '設定PPT21為工件放置目標位置 Break Case 6 PPT0= PPT22 '設定PPT22為工件放置目標

13、位置 Break Case 7 PPT0= PPT23 '設定PPT23為工件放置目標位置 Break Case 8 PPT0= PPT24 '設定PPT24為工件放置目標位置 Break Case 9 PPT0= PPT31 '設定PPT31為工件放置目標位置 Break Case 10 PPT0= PPT32 '設定PPT32為工件放置目標位置 Break Case 11 PPT0= PPT33 '設定PPT33為工件放置目標位置 Break Case 12 PPT0= PPT34 '設定PPT34為工件放置目標位置 Break End Se

14、lect GoSub *S20TRGET '檢測到工件來,進行跟蹤工件吸取操作 If m13=1 And m10=0 Then *FeiLiao0 '不要的工件放廢料框中 GoSub *S30WKPUT '工件放置處理*Goon If m12=1 Then *Vtest 'm12=1,說明一個裝配流程結束,轉視覺檢測流程 GoTo *LOOP1 '繼續(xù)放置下一個工件'*Vtest '視覺豎向檢測GoSub *Fang_XiPan '放置吸盤工裝子程序GoSub *Jia_Camera '夾取視覺工裝子程序GoSub *Ca

15、mera_V '視覺豎向檢測子程序GoSub *Fang_Camera '放置視覺工裝子程序GoSub *Comparison_V '豎向視覺信息比對子程序If m1=1 And m2=1 And m3=1 Then *HTESTGoSub *Jia_XiPan '夾取吸盤工裝子程序GoSub *Rotate '工件角度旋轉調整子程序GoSub *Fang_XiPan '放置吸盤工裝子程序'*HTEST '視覺橫向檢測If m1=1 And m2=1 And m3=1 Then *LOOP '3個裝配臺已無合格的工件盒存在

16、GoSub *JiaHe_To_H '夾盒到橫向檢測臺子程序GoSub *Jia_Camera '夾取視覺工裝子程序GoSub *Camera_H '視覺橫向檢測子程序GoSub *Fang_Camera '放置視覺工裝子程序GoSub *Comparison_H '橫向視覺信息比對子程序If m20=1 Then *HTEST '已作廢料被扔掉GoSub *Jia_gai '取蓋放蓋子程序GoSub *Ruku '入庫子程序If m7=1 Then *ChaiJieGoTo *HTEST*ChaiJieMov P1GoSub *

17、CIAN '出庫拆解子程序End''''* 初始化處理子程序*S10INIT '初始化處理M_Out(5)=0 '機器人等待吸取信號為0 X33M_Out(8)=0 '輸出完成復位信號為0 X36Def Plt 1,P90,P91,P92,P93,3,3,2 '設置倉庫位置，以P90為起點，以P91為終點A，以P92為終點B，以P93為對角點，行為3，列為3，同方向排列Accel 100,100 '加速度、減速度設定Ovrd 100 '速度設定Loadset 1,1 '最佳加速度、減速度說明OAdl

18、 On '打開最佳加速度、減速度Cnt 0Clr 1TrClr 1 '清除追蹤緩沖MWAIT1=0 '清除工件等待標記M_09#=PWK.X '模式號碼說明If M_Run(2)=0 ThenXRun 2,"CM1",1 '在插槽2選擇程序"CM1"'Wait M_Run(2)=1EndIfPriority PRI.X,1Priority PRI.Y,2Return''''* 原點返回處理*S90HOMEHOpen 1HOpen 2'M_Out(5)=0 '機

19、器人空閑等待狀態(tài)為0 X33P90CURR=P_Fbc(1) '獲得當位置If P90CURR.Z<P1.Z Then '如果當前高度在下面則歸位Ovrd 10 '速度設定為10P90ESC=P90CURR '做回避點P90ESC.Z=P1.ZOvrd 100 '速度設定為100EndIfMov P1 '移動到原點Return''''* 招手演示子程序 *ZhaoShouMov P50Mov P51Mov P50Mov P51Mov P50Mvs P52Mov P53Mov P52Mov P53Mov P52

20、Mvs P50Mvs P54Mov P55Mov P54Mov P55Mov P54Mvs P50Mov P1Return''''* 接收PLC裝配流程數(shù)據(jù)子程序*RecDat_PLCOpen "COM3:" As #2 '打開網(wǎng)絡端口（與PLC相接的以太網(wǎng)端口）Wait M_Open(2) =1M_Out(9)=1 '向PLC請求發(fā)12個工件的參數(shù)Dly 0.2Input #2, m100,m105,m110 ,m115,m120,m125,m130,m135,m140,m145,m150 ,m155 '接收12個工

21、件的參數(shù)Close #2 '關閉網(wǎng)絡端口M_Out(9)=0If m100=0 And m105=0 And m110=0 And m115=0 Then m1=1 '1號裝配臺不需要裝配If m120=0 And m125=0 And m130=0 And m135=0 Then m2=1 '2號裝配臺不需要裝配If m140=0 And m145=0 And m150=0 And m155=0 Then m3=1 '3號裝配臺不需要裝配Return'''''* 夾取工件盒到裝配臺子程序*Jia_HeWait M_In(

22、9)=1 '取盒子信號為1 Y14''取工件盒到3號裝配臺If m3=1 Then *JiaHe2 '1號裝配臺不需要裝配，轉2號裝配臺Mov P13 '抓手換成橫向HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 0.2Mvs P6 '到出盒臺上方Ovrd 20 Mvs P6+(+0.00,+0.00,-50.00,+0.00,+0.00,+0.00)Dly 0.2HClose 1Wait M_In(901)=1 '等待手爪夾緊信號為1Ovrd 100Mvs P6Dly 0.2 '延時Mvs

23、P20 '到1號裝配臺上方（橫向）Ovrd 20Mvs P20+(+0.00,+0.00,-90.00,+0.00,+0.00,+0.00)Dly 0.2HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 0.2 ' 延時Ovrd 100Mvs P20Mvs P13 '過渡點Dly 0.2 ' 延時Mov P12 '2號工裝上方位置（豎向）Mvs P21 '1號裝配臺上方位置（豎向）Ovrd 20Mvs,60Dly 1HClose 1Wait M_In(901)=1 '等待手爪夾緊信號為1Dly 0.2

24、'延時Ovrd 100Mvs P21Mvs P23 '3號裝配臺上方位置（豎向）Ovrd 20Mvs,60Dly 1HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 1Ovrd 100Mvs P23Mvs P12''取工件盒到2號裝配臺*JiaHe2If m2=1 Then *JiaHe3 '2號裝配臺不需要裝配，轉3號裝配臺Mov P13 '抓手換成橫向HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 0.2Wait M_In(9)=1 '取盒子信號為1 Y14Mv

25、s P6 '到出盒臺上方Ovrd 20 Mvs P6+(+0.00,+0.00,-50.00,+0.00,+0.00,+0.00)Dly 0.2HClose 1Wait M_In(901)=1 '等待手爪夾緊信號為1Ovrd 100Mvs P6Dly 0.2Mvs P20 '到1號裝配臺上方（橫向）Ovrd 20Mvs P20+(+0.00,+0.00,-90.00,+0.00,+0.00,+0.00)Dly 0.2HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 0.2Ovrd 100Mvs P20Mvs P13 '過渡點M

26、ov P12 '抓手換成豎向Mvs P21 '1號裝配臺上方位置（豎向）Ovrd 20Mvs,60Dly 0.2HClose 1Wait M_In(901)=1 '等待手爪夾緊信號為1Dly 0.2Ovrd 100Mvs P21Mvs P22 '2號裝配臺上方位置（豎向）Ovrd 20Mvs,60Dly 0.2HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 0.2Ovrd 100Mvs P22Mvs P12''取工件盒到1號裝配臺*JiaHe3If m1=1 Then Return '3號裝配臺不需要

27、裝配，轉3號裝配臺Mov P13 '抓手換成橫向HOpen 1Wait M_In(9)=1 '取盒子信號為1 Y14Mvs P6 '到出盒臺上方Ovrd 20 Mvs P6+(+0.00,+0.00,-50.00,+0.00,+0.00,+0.00)Dly 0.2HClose 1Wait M_In(901)=1 '等待手爪夾緊信號為1Ovrd 100Mvs P6Dly 0.2Mvs P20 '到1號裝配臺上方（橫向）Ovrd 20Mvs P20+(+0.00,+0.00,-90.00,+0.00,+0.00,+0.00)Dly 0.2HOpen 1Wai

28、t M_In(900)=1 '等待手爪松開信號為1Dly 0.2Ovrd 100Mvs P20Mvs P13 '過渡點Mov P12 '抓手換成豎向,下一個動作是夾取吸盤工裝Return'''* 夾取吸盤工裝子程序*Jia_XiPanMov P2 ' 吸盤工裝正上方位置HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 0.3 ' 延時1秒Ovrd 50 '速度設定Mvs P3+(+0.00,+0.00,+90.00,+0.00,+0.00,+0.00)Ovrd 20 '速度設定M

29、vs P3 ' 吸盤工裝位置Dly 1HClose 1 ' 夾取視覺相機工裝Wait M_In(901)=1 '等待手爪夾緊信號為1Dly 1 '延時1秒Ovrd 50 '速度設定Mvs P2 ' 吸盤工裝正上方位置Ovrd 100 '速度設定Mvs P12Return'''* 放置吸盤工裝子程序*Fang_XiPanMov P12Mvs P2 ' 吸盤工裝正上方位置Ovrd 50 '速度設定Mvs P3+(+0.00,+0.00,+90.00,+0.00,+0.00,+0.00)Ovrd 20 &

30、#39;速度設定Mvs P3 ' 吸盤工裝位置Dly 0.5HOpen 1Wait M_In(900)=1 '等待手爪松開信號為1Dly 1 '延時1秒Ovrd 50 '速度設定Mvs P2 ' 吸盤工裝正上方位置Ovrd 100 '速度設定Mvs P12Return''''* 工件跟蹤吸取子程序*S20TRGET'/追蹤緩沖檢查/*LBFCHKIf M_Trbfct(1)>=1 Then GoTo *LREAD '如果有工件，轉到工件數(shù)據(jù)獲得程序Mov P0 Type 0,0 '移動

31、到拉 位置MWAIT1=0GoTo *LBFCHK'/工件數(shù)據(jù)獲得/*LREADTrRd PBPOS,MBENC#,MBWK%,1,MBENCNO% 'GoSub *S40DTSET '運輸數(shù)據(jù)設定'/工件位置確認 /*LNEXTPX50CUR=TrWcur(MBENCNO%,PBPOS,MBENC#) '獲得現(xiàn)在工件位置MX50ST=PRNG.X '開始距離范圍在機器人跟隨工件MX50ED=PRNG.Y '停止距離范圍在機器人跟隨工件MX50PAT=PTN.X '輸送模式號碼GoSub *S50WKPOS '工件位置確認

32、處理If MY50STS=3 Then GoTo *LBFCHK '已經(jīng)通過，去下個工件If MY50STS=2 Then GoTo *LTRST '開始追蹤If MWAIT=1 Then GoTo *LNEXT '等待工件到來'/ 去位置 /PWAIT=P0 ' 改變工件等待姿勢Select PTN.X '輸送位置模式號碼Case 1 To 2 '當輸送在機器人之前PWAIT.X=PX50CUR.X '機器人匹配到工件的X坐標Case 3 To 6PWAIT.Y=PX50CUR.Y '機器人匹配到工件的Y坐標End Se

33、lectPWAIT.Z=PX50CUR.Z+PUP1.XPWAIT.C=PX50CUR.CMov PWAIT Type 0,0 '移動到工件等待姿勢MWAIT1=1 '設置工件等待標記GoTo *LNEXT'/開始追蹤操作/*LTRSTAccel PAC1.X,PAC1.YCnt 1,0,0Act 1=1 '監(jiān)視機器人跟隨工件太遠Trk On,PBPOS,MBENC#,PTBASE,MBENCNO% '追蹤操作開始設置Ovrd 75Mov PGT,PUP1.Y Type 0,0 '移動到追蹤半空位置Accel PAC2.X,PAC2.YMvs P

34、GT '移動到吸取位置HClose 2 '吸取開Dly PDLY2.Z '吸取確定Cnt 1Accel PAC3.X,PAC3.YOvrd 100Mvs PGT,PUP1.Z '移動到追蹤半空位置Trk Off '追蹤操作和設定Act 1=0Accel 100,100MWAIT = 0Return''''*工件放廢料框子程序*FeiLiao0Mvs P0Accel PAC12.X,PAC12.Y '移動到放置位置Cnt 1,0,0Mvs PFL1+(+0.00,+0.00,+80.00,+0.00,+0.00,+0

35、.00)HOpen 2 '吸取關Dly 0.5 '釋放確認M_Out(5)=0 '機器人等待吸取信號為0 -X33m13=0Mvs P0GoTo *Goon''''* 工件放置子程序*S30WKPUTMvs P111M_Out(5)=0 '機器人等待吸取信號為0 X33Mov P12Accel PAC12.X,PAC12.Y '移動到放置位置Cnt 1,0,0'Mvs PPTOvrd 40Mvs PPT0 '放料移動到目標位置（根據(jù)m10的值決定）Mvs ,30 '下降'If M_In(15

36、)=0 Then *POK'*m6=0GoSub *WeiTiao1 '工件位置微調1mmIf m6=1 Then *POKGoSub *WeiTiao2 '工件位置微調2mmIf m6=1 Then *POKGoSub *WeiTiao3 '工件位置微調3mm*POK'*HOpen 2 '吸取關Dly 0.5 '釋放確認Ovrd 100Mvs ,-35Mov P12Mov P111'Return'''* 放置工件位置微調子程序（1mm）*WeiTiao1m6=0Dly 1If M_In(15)=0 The

37、n *PlaceOK1Mvs ,-15Mov PPT0+(+0.00,+1.00,-15.00,+0.00,+0.00,+0.00) '右移1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+0.00,-1.00,-15.00,+0.00,+0.00,+0.00) '左移1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+1.00,+0.00,-15.00,+0.00,+0.00,+0.00) '前移1mmMvs ,15Dly

38、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 ,15Dly 1If M_In(15)=0 Then *PlaceOK1Mvs ,-15Mov PPT0+(+1.00,-1.00,-15.00,+0.00,+0.00,+0.00)

39、 '前移、左移各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) '后移、左移各1mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK1Return*PlaceOK1m6=1Return

40、''''* 放置工件位置微調子程序（2mm）*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) '左移2mmMvs ,15Dly 1If M_In(15)=0 Then *PlaceOK2Mvs ,-15Mov PPT0+(+2.00,+0.00,-15.00,+0.00,+0.00,+0.00) '前移2m