天煌教儀機器人程序.

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數(shù)據(jù)1:工件放置的位置mil :接收PLC數(shù)據(jù) 2:工件參數(shù)（其中個位數(shù)為編號，十位數(shù)為顏色和高度）m12 :接收PLC數(shù)據(jù) 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個工件參數(shù)寄存2 號裝配臺 4個工

6、件參數(shù)寄存3 號裝配臺 4個工件參數(shù)寄存 視覺發(fā)來的 1 號裝配臺 4個工件編號 視覺發(fā)來的 2號裝配臺 4個工件編號 視覺發(fā)來的 3號裝配臺 4個工件編號 視覺發(fā)來的 1 號裝配臺 4個工件顏色 視覺發(fā)來的 2號裝配臺 4個工件顏色 視覺發(fā)來的 3號裝配臺 4個工件顏色 視覺發(fā)來的 1 號裝配臺 4個工件角度 視覺發(fā)來的 2號裝配臺 4個工件角度 視覺發(fā)來的 3號裝配臺 4個工件角度 視覺發(fā)來的 1 號裝配臺 4個工件高度 視覺發(fā)來的 2號裝配臺 4個工件高度 視覺發(fā)來的 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十位數(shù)主循環(huán)原點返回處理 招手動作，展示用輸出完成復位信號為 1已就緒 （X36 ） 一個裝配流程結束是否結束， 1為結束1號裝配臺正常2號裝配臺正常3號裝配臺正常拆解標志拆解倉庫第 9個倉位有無標志位1號裝配臺正常2號裝配臺正常3號裝配臺正常工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零工件參數(shù)清零等待接收運行信號為 1 可取盒（ Y14）輸出完成復位信號為 0 -X36接收PLC裝配流程數(shù)據(jù)夾取工件盒到裝配臺子程序夾取吸盤工裝子程序卩12到P0的過渡裝配檢測循環(huán)運行到吸取等待位置機器人等待吸取信號為

9、1 - 已到位（ X33）打開網(wǎng)絡端口（與 PLC相接的以太網(wǎng)端口）m10 工件放置的位置m11 工件參數(shù) （其中個位數(shù)為編號，為顏色和高度）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, 說明一個裝配流程結束 ,轉視覺檢測流程 繼續(xù)放置下一個工件視覺豎向檢測放置吸盤工裝子程序夾取視覺工裝子程序

13、視覺豎向檢測子程序放置視覺工裝子程序豎向視覺信息比對子程序夾取吸盤工裝子程序 工件角度旋轉調(diào)整子程序放置吸盤工裝子程序視覺橫向檢測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/ 工件數(shù)據(jù)獲得 / *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運輸數(shù)據(jù)設定獲得現(xiàn)在工件位置開始距離范圍在機器人跟隨工件停止距離范圍在機器人跟隨工件輸送模式號碼工件位置確認處理已經(jīng)通過，去下個工件開始追蹤等待工件到來 改變工件等待姿勢 輸送位置

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移動到工件等待姿勢設置工件等待標記監(jiān)視機器人跟隨工件太遠追蹤操作開始設置移動到追蹤半空位置移動到吸取位置吸取開吸取確定移動到追蹤半空位置追蹤操作和設定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移動到放置位置放料移動到目標位置下降 工件位置微調(diào) 1mm 工件位置微調(diào) 2m

20、m 工件位置微調(diào) 3mm吸取關釋放確認ReturnI*放置工件位置微調(diào)子程序（ 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*放置工件位置微調(diào)子程序（ 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放置工件位置微調(diào)子程序（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 *輸運數(shù)據(jù)設定處理子程序 *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，觸發(fā)一次拍照數(shù)據(jù) 1為編號、數(shù)據(jù) 2為顏色、數(shù)據(jù) 3 為角度輸出拍照信號清零

35、1號臺2號工位輸出拍照信號為 1，觸發(fā)一次拍照輸出拍照信號清零1號臺3號工位輸出拍照信號為 1，觸發(fā)一次拍照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