天煌教仪机器人程序

'停止 0 '伺服OFF 1 '程序复位 2 '启动 3 '伺服ON 4 '操作权 5

'P0： 等待吸取工件位置 'P1： 机器人初始位置

'P111： 机器人追踪完成到放料中转位置（与P0点左右相对） 'P2： 机器人拿吸盘等待位置（2号工装上方） 'P3： 机器人取吸盘位置（2号工装）

'P4： 机器人取照相机等待位置（3号工装上方） 'P5： 机器人取照相机位置（3号工装） 'P6： 取盒子位置 'P7： 取盖子位置

'P8： 入库等待位置（左，近库架，抓手横向） '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： 在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 拆解盖库料位置 '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号工位放置位置 '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数据1:工件放置的位置

'm11： 接收PLC数据2:工件参数（其中个位数为编号，十位数为颜色和高度） 'm12： 接收PLC数据3:表示是否还有下一个工件（1为没有） 'm100、m105、m110、m115： 1号装配台4个工件参数寄存 'm120、m125、m130、m135： 2号装配台4个工件参数寄存 'm140、m145、m150、m155： 3号装配台4个工件参数寄存

'm200、m205、m210、m215： 视觉发来的1号装配台4个工件编号 'm220、m225、m230、m235： 视觉发来的2号装配台4个工件编号 'm240、m245、m250、m255： 视觉发来的3号装配台4个工件编号 'm201、m206、m211、m216： 视觉发来的1号装配台4个工件颜色 'm221、m226、m231、m236： 视觉发来的2号装配台4个工件颜色 'm241、m246、m251、m256： 视觉发来的3号装配台4个工件颜色 'm202、m207、m212、m217： 视觉发来的1号装配台4个工件角度 'm222、m227、m232、m237： 视觉发来的2号装配台4个工件角度 'm242、m247、m252、m257： 视觉发来的3号装配台4个工件角度 'm203、m208、m213、m218： 视觉发来的1号装配台4个工件高度 'm223、m228、m233、m238： 视觉发来的2号装配台4个工件高度 'm243、m248、m253、m258： 视觉发来的3号装配台4个工件高度 '

*S00MAIN '主程序 GoSub *S10INIT '初始化处理

m5=7 '仓库首个库位设为1号

m50=1

*LOOP '主循环 GoSub *S90HOME '原点返回处理

'GoSub *ZhaoShou ' 招手动作，展示用

M_Out(8)=1 '输出完成复位信号为1——已就绪 （X36） Dly 0.5 '

m0=0 '一个装配流程结束是否结束，1为结束 m1=0 '1号装配台正常 m2=0 m3=0 m7=0 m8=0 m31=0 m32=0 m33=0 m100=0 m105=0 m110=0 m115=0 m120=0 m125=0 m130=0 m135=0 m140=0 m145=0 m150=0 m155=0 '

Wait M_In(9)=1 Dly 0.5

M_Out(8)=0 GoSub *RecDat_PLC GoSub *Jia_He GoSub *Jia_XiPan Mov P111 ' '

*LOOP1 Mvs P0 M_Out(5)=1 Open \ Wait M_Open(2) =1

Input #2, m10,m11,m12,m13 为颜色和高度）

'2号装配台正常 '3号装配台正常 '拆解标志

'拆解仓库第9个仓位有无标志位 '1号装配台正常 '2号装配台正常 '3号装配台正常 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '工件参数清零 '等待接收运行信号为1——可取盒（Y14） '输出完成复位信号为0 --X36 '接收PLC装配流程数据

'夹取工件盒到装配台子程序 '夹取吸盘工装子程序 'P12到P0的过渡 '装配检测循环

'运行到吸取等待位置

'机器人等待吸取信号为1 --已到位（X33） '打开网络端口（与PLC相接的以太网端口） 'm10工件放置的位置

'm11工件参数（其中个位数为编号，十位数 'm12表示是否还有下一个工件（1为没有） 'm13表示是否为有用工件（1为没用） Close #2

'If m13>0 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为工件放置目标位置 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 Select

GoSub *S20TRGET '检测到工件来,进行跟踪工件吸取操作 If m13=1 And m10=0 Then *FeiLiao0 '不要的工件放废料框中 GoSub *S30WKPUT '工件放置处理 *Goon

If m12=1 Then *Vtest 'm12=1,说明一个装配流程结束,转视觉检测流程 GoTo *LOOP1 '继续放置下一个工件 '

*Vtest '视觉竖向检测 GoSub *Fang_XiPan '放置吸盘工装子程序 GoSub *Jia_Camera '夹取视觉工装子程序 GoSub *Camera_V '视觉竖向检测子程序 GoSub *Fang_Camera '放置视觉工装子程序 GoSub *Comparison_V '竖向视觉信息比对子程序 If m1=1 And m2=1 And m3=1 Then *HTEST

GoSub *Jia_XiPan '夹取吸盘工装子程序

GoSub *Rotate '工件角度旋转调整子程序 GoSub *Fang_XiPan '放置吸盘工装子程序 '

*HTEST '视觉横向检测

If m1=1 And m2=1 And m3=1 Then *LOOP '3个装配台已无合格的工件盒存在

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 *ChaiJie GoTo *HTEST *ChaiJie Mov P1

GoSub *CIAN '出库拆解子程序 End ' ' '

'********************** 初始化处理子程序****************************** *S10INIT '初始化处理

M_Out(5)=0 '机器人等待吸取信号为0 ——X33 M_Out(8)=0 '输出完成复位信号为0 ——X36 Def 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 On '打开最佳加速度、减速度 Cnt 0 Clr 1

Ovrd 20 '速度设定

Mvs P3 ' 吸盘工装位置 Dly 0.5 HOpen 1

Wait M_In(900)=1 '等待手爪松开信号为1 Dly 1 '延时1秒 Ovrd 50 '速度设定

Mvs P2 ' 吸盘工装正上方位置 Ovrd 100 '速度设定 Mvs P12 Return ' ' '

'********************** 工件跟踪吸取子程序****************************** *S20TRGET

'///追踪缓冲检查/// *LBFCHK

If M_Trbfct(1)>=1 Then GoTo *LREAD '如果有工件，转到工件数据获得程序 Mov P0 Type 0,0 '移动到拉 位置 MWAIT1=0 GoTo *LBFCHK '///工件数据获得/// *LREAD

TrRd PBPOS,MBENC#,MBWK%,1,MBENCNO% '

GoSub *S40DTSET '运输数据设定 '///工件位置确认 /// *LNEXT

PX50CUR=TrWcur(MBENCNO%,PBPOS,MBENC#) '获得现在工件位置

MX50ST=PRNG.X '开始距离范围在机器人跟随工件 MX50ED=PRNG.Y '停止距离范围在机器人跟随工件 MX50PAT=PTN.X '输送模式号码 GoSub *S50WKPOS '工件位置确认处理 If MY50STS=3 Then GoTo *LBFCHK '已经通过，去下个工件 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 6

PWAIT.Y=PX50CUR.Y '机器人匹配到工件的Y坐标 End Select

PWAIT.Z=PX50CUR.Z+PUP1.X PWAIT.C=PX50CUR.C

Mov PWAIT Type 0,0 '移动到工件等待姿势

MWAIT1=1 '设置工件等待标记 GoTo *LNEXT

'///开始追踪操作/// *LTRST

Accel PAC1.X,PAC1.Y Cnt 1,0,0

Act 1=1 '监视机器人跟随工件太远 Trk On,PBPOS,MBENC#,PTBASE,MBENCNO% '追踪操作开始设置 Ovrd 75

Mov PGT,PUP1.Y Type 0,0 '移动到追踪半空位置 Accel PAC2.X,PAC2.Y

Mvs PGT '移动到吸取位置 HClose 2 '吸取开 Dly PDLY2.Z '吸取确定 Cnt 1

Accel PAC3.X,PAC3.Y Ovrd 100

Mvs PGT,PUP1.Z '移动到追踪半空位置 Trk Off '追踪操作和设定 Act 1=0

Accel 100,100 MWAIT = 0 Return ' ' '

'*************工件放废料框子程序************ *FeiLiao0 Mvs P0

Accel PAC12.X,PAC12.Y '移动到放置位置 Cnt 1,0,0

Mvs PFL1+(+0.00,+0.00,+80.00,+0.00,+0.00,+0.00)

HOpen 2 '吸取关 Dly 0.5 '释放确认

M_Out(5)=0 '机器人等待吸取信号为0 --X33 m13=0 Mvs P0

GoTo *Goon ' ' '

'********************** 工件放置子程序****************************** *S30WKPUT Mvs P111

M_Out(5)=0 '机器人等待吸取信号为0 ——X33

Mov P12

Accel PAC12.X,PAC12.Y '移动到放置位置 Cnt 1,0,0 'Mvs PPT Ovrd 40

Mvs PPT0 '放料移动到目标位置（根据m10的值决定） Mvs ,30 '下降 'If M_In(15)=0 Then *POK

'************************************************* m6=0

GoSub *WeiTiao1 '工件位置微调1mm If m6=1 Then *POK

GoSub *WeiTiao2 '工件位置微调2mm If m6=1 Then *POK

GoSub *WeiTiao3 '工件位置微调3mm *POK

'************************************************* HOpen 2 '吸取关 Dly 0.5 '释放确认 Ovrd 100 Mvs ,-35 Mov P12 Mov P111 '

Return ' '

'********************** 放置工件位置微调子程序（1mm）****************************** *WeiTiao1 m6=0 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(+0.00,+1.00,-15.00,+0.00,+0.00,+0.00) '右移1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(+0.00,-1.00,-15.00,+0.00,+0.00,+0.00) '左移1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(+1.00,+0.00,-15.00,+0.00,+0.00,+0.00) '前移1mm Mvs ,15

Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(-1.00,+0.00,-15.00,+0.00,+0.00,+0.00) '后移1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(+1.00,+1.00,-15.00,+0.00,+0.00,+0.00) '前移、右移各1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(+1.00,-1.00,-15.00,+0.00,+0.00,+0.00) '前移、左移各1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(-1.00,+1.00,-15.00,+0.00,+0.00,+0.00) '后移、右移各1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Mvs ,-15

Mov PPT0+(-1.00,-1.00,-15.00,+0.00,+0.00,+0.00) '后移、左移各1mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK1 Return *PlaceOK1 m6=1 Return ' ' '

'********************** 放置工件位置微调子程序（2mm）****************************** *WeiTiao2 m6=0 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(+0.00,+2.00,-15.00,+0.00,+0.00,+0.00) '右移2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(+0.00,-2.00,-15.00,+0.00,+0.00,+0.00) '左移2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(+2.00,+0.00,-15.00,+0.00,+0.00,+0.00) '前移2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(-2.00,+0.00,-15.00,+0.00,+0.00,+0.00) '后移2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(+2.00,+2.00,-15.00,+0.00,+0.00,+0.00) '前移、右移各2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(+2.00,-2.00,-15.00,+0.00,+0.00,+0.00) '前移、左移各2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(-2.00,+2.00,-15.00,+0.00,+0.00,+0.00) '后移、右移各2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Mvs ,-15

Mov PPT0+(-2.00,-2.00,-15.00,+0.00,+0.00,+0.00) '后移、左移各2mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK2 Return *PlaceOK2 m6=1 Return ' ' '

'********************** 放置工件位置微调子程序（3mm）****************************** *WeiTiao3 m6=0 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(+0.00,+3.00,-15.00,+0.00,+0.00,+0.00) '右移3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(+0.00,-3.00,-15.00,+0.00,+0.00,+0.00) '左移3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(+3.00,+0.00,-15.00,+0.00,+0.00,+0.00) '前移3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(-3.00,+0.00,-15.00,+0.00,+0.00,+0.00) '后移3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(+3.00,+3.00,-15.00,+0.00,+0.00,+0.00) '前移、右移各3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(+3.00,-3.00,-15.00,+0.00,+0.00,+0.00) '前移、左移各3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(-3.00,+3.00,-15.00,+0.00,+0.00,+0.00) '后移、右移各3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Mvs ,-15

Mov PPT0+(-3.00,-3.00,-15.00,+0.00,+0.00,+0.00) '后移、左移各3mm Mvs ,15 Dly 1

If M_In(15)=0 Then *PlaceOK3 Return *PlaceOK3 m6=1 Return '

' '

'********************** 输运数据设定处理子程序****************************** *S40DTSET

PTBASE=P_100(PWK.X) '做参考位置 TrBase PTBASE,MBENCNO% '追踪基本设定 PGT=PTBASE*POFSET '吸取点设置 GoSub *S46ACSET '中断定义 Return ' '

'********************** 中断定义处理子程序****************************** *S46ACSET

Select PTN.X '输送位置模式号码 Case 1 '前右到左 MSTP1=PRNG.Z '下次停止距离

Def Act 1,P_Fbc(1).Y>MSTP1 GoTo *S91STOP,S 'To *S91STOP if followed far long Break Case 2 '前左到右 MSTP1=-PRNG.Z

Def Act 1,P_Fbc(1).Y

Case 3 '左后到前 Case 5 '右后到前 MSTP1=PRNG.Z

Def Act 1,P_Fbc(1).X>MSTP1 GoTo *S91STOP,S Break

Case 4 '左前到后 Case 6 '右前到后 MSTP1=-PRNG.Z

Def Act 1,P_Fbc(1).X

End Select Return ' '

'********************** 追踪中断处理子程序****************************** *S91STOP Act 1=0 Trk Off

HClose 2 'Release suction 释放吸取 P91P=P_Fbc(1) '获得当前位置 P91P.Z=P1.Z

Mvs P91P Type 0,0 '上升

Mov P1 '返回原点一次 GoTo *LBFCHK '

'

'********************** 工件位置确认子程序****************************** *S50WKPOS

'PX50CUR:当前工件位置 'MX50ST:开始追踪范围 'MX50ED:停止追踪范围

'MX50PAT:追踪位置模式号码

'MY50STS:结果（1、等待 2、开始追踪 3、下个工件） MY50STS=0 '清除返回值 Select MX50PAT '输送模式

Case 1 '前到左 M50STT=-MX50ST '开始值是负值 M50END=MX50ED

MY50STS=2 '追踪可能 End Select Return ' ' ' '

'********************** 夹取视觉相机工装子程序****************************** *Jia_Camera

Mvs P4 ' 视觉相机工装正上方位置 HOpen 1

Wait M_In(900)=1 '等待手爪松开信号为1 Dly 0.5 ' 延时 Ovrd 20 '速度设定

Mvs P5 ' 视觉相机工装位置 Dly 0.5

HClose 1 ' 夹取视觉相机工装 Wait M_In(901)=1 '等待手爪夹紧信号为1 Dly 0.5 '延时 Ovrd 50

Mvs P4 ' 视觉相机工装正上方位置 Mvs P12

Ovrd 100 '速度设定 Return ' '

'********************** 放置视觉相机工装子程序****************************** *Fang_Camera

Mvs P4 ' 视觉相机工装正上方位置 Dly 0.5

Ovrd 20 '速度设定 Mvs P5+(+0.00,+0.00,+90.00,+0.00,+0.00,+0.00) Ovrd 10 '速度设定

Mvs P5 ' 视觉相机工装位置 Dly 0.5

HOpen 1 ' 夹取视觉相机工装 Wait M_In(900)=1 '等待手爪松开信号为1 Dly 0.5

Ovrd 50 '速度设定

Mvs P4 ' 视觉相机工装正上方位置 Ovrd 100 '速度设定 Mvs P12 Return ' '

'********************** 视觉竖向检测子程序****************************** *Camera_V '

'检测1号装配台

If m1=1 Then *Camera_V2 '1号装配台不需要装配，转2号装配台 M_Out(13)=0 '输出拍照信号为清零 Open \Wait M_Open(1) =1

Ovrd 30 '速度设定 Mvs PV10 '1号台1号工位 Dly 1

M_Out(13)=1 '输出拍照信号为1，触发一次拍照 Dly 0.5

Input #1, m200,m201,m202 '数据1为编号、数据2为颜色、数据3为角度 Close #1

M_Out(13)=0 '输出拍照信号清零 '

Dly 0.5

Open \Wait M_Open(1) =1

Mvs PV11 '1号台2号工位 Dly 1

M_Out(13)=1 '输出拍照信号为1，触发一次拍照 Dly 0.5

Input #1, m205,m206,m207 Close #1

M_Out(13)=0 '输出拍照信号清零 '

Dly 0.5

Open \Wait M_Open(1) =1

Mvs PV12 '1号台3号工位 Dly 1

M_Out(13)=1 '输出拍照信号为1，触发一次拍照

Dly 0.5

Input #1, m210,m211,m212 Close #1

M_Out(13)=0 '输出拍照信号清零 '

Dly 0.5

Open \Wait M_Open(1) =1

Mvs PV13 '1号台4号工位 Dly 1

M_Out(13)=1 Dly 0.5

Input #1, m215,m216,m217 Close #1

M_Out(13)=0 '

'********************** '检测2号装配台 *Camera_V2

M_Out(13)=0 If m2=1 Then *Camera_V3 Dly 0.5

Open \Wait M_Open(1) =1

Mvs PV20 Dly 1

M_Out(13)=1 Dly 0.5

Input #1, m220,m221,m222 Close #1

M_Out(13)=0 '

Dly 0.5

Open \Wait M_Open(1) =1

Mvs PV21 Dly 1

M_Out(13)=1 Dly 0.5

Input #1, m225,m226,m227 Close #1

M_Out(13)=0 '

Dly 0.5

Open \Wait M_Open(1) =1

'输出拍照信号为1，触发一次拍照 '输出拍照信号清零 '输出拍照信号清零

'2号装配台不需要装配，转3号装配台 '2号台1号工位 '输出拍照信号为1，触发一次拍照 '输出拍照信号清零 '2号台2号工位 '输出拍照信号为1，触发一次拍照 '输出拍照信号清零

HOpen 1

Wait M_In(900)=1 '等待手爪松开信号为1 Dly 1 Ovrd 100 Mvs PFL1 Mvs P0 Mvs P111 Mov P12 m1=1 GoTo *next7 '

*FeiLiao2

Mov P12 Mvs P22 Ovrd 50 Mvs,60 Dly 1 HClose 1

Wait M_In(901)=1 Dly 1 Mvs P22 Mvs P12 Mov P111 Mvs P0 Mvs PFL2 Mvs,50 Dly 1 HOpen 1

Wait M_In(900)=1 Dly 1 Ovrd 100 Mvs PFL2 Mvs P0 m2=1 Mvs P111 Mov P12 GoTo *next15 '

*FeiLiao3

Mov P12 Mvs P23 Ovrd 50 Mvs,60 Dly 1 HClose 1

Wait M_In(901)=1 '抓手换成竖向

'2号装配台上方位置（竖向） '等待手爪夹紧信号为1 '延时1秒 '等待手爪松开信号为1 '抓手换成竖向

'3号装配台上方位置（竖向） '等待手爪夹紧信号为1

Dly 1 '延时1秒 Mvs P23 Mvs P12 Mov P111 Mvs P0 Mvs PFL3 Mvs,50 Dly 1

HOpen 1

Wait M_In(900)=1 '等待手爪松开信号为1 Dly 1 Ovrd 100 Mvs PFL3 Mvs P0

Mvs P111 Mov P12 m3=1 Return ' ' '

'********************** 工件角度旋转调整子程序****************************** *Rotate

'调整1号装配台

'***************调整1号装配台1号工件*****************

If m1=1 Then *next33 '1号装配台不需要调整，转2号装配台 If Abs(m202)<5 Then *next30 '偏差小于5度则不调整 If m202>179 Then m202=179 If m202<-179 Then m202=-179

Mvs PPT11 '到1号装配台1号工件上方 Mvs, 45 '下降 HClose 2 '吸取开 Dly 0.5 '释放确认 Mvs ,-15 '抬升 J1=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)

J1.J6=Rad(m202) '将角度转换为弧度代入 J2=J_Curr-J1 '取当前所在位置 Mov J2 '旋转 PPT0=P_Curr+(+0.00,+0.00,+15.00,+0.00,+0.00,+0.00) Mvs,2 '下降 If M_In(15)=0 Then *POK11

'************************************************* m6=0

GoSub *WeiTiao1 '工件位置微调1mm If m6=1 Then *POK11

GoSub *WeiTiao2 '工件位置微调2mm

If m6=1 Then *POK11

GoSub *WeiTiao3 '工件位置微调3mm *POK11

'************************************************* HOpen 2 '吸取关

Dly 1 '释放确认 Mvs,-35 '抬升 Dly 0.5 '

'***************调整1号装配台2号工件***************** *next30

If Abs(m207)<5 Then *next31 '偏差小于5度则不调整 If m207>179 Then m207=179 If m207<-179 Then m207=-179

Mvs PPT12 '到1号装配台2号工件上方 Mvs, 45 '下降 HClose 2 '吸取开 Dly 0.5 '释放确认 Mvs,-15 '抬升 J1=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)

J1.J6=Rad(m207) '将角度转换为弧度代入 J2=J_Curr-J1 '取当前所在位置 Mov J2 '旋转 PPT0=P_Curr+(+0.00,+0.00,+15.00,+0.00,+0.00,+0.00) Mvs,2 '下降 If M_In(15)=0 Then *POK12

'************************************************* m6=0

GoSub *WeiTiao1 '工件位置微调1mm If m6=1 Then *POK12

GoSub *WeiTiao2 '工件位置微调2mm If m6=1 Then *POK12

GoSub *WeiTiao3 '工件位置微调3mm *POK12

'************************************************* HOpen 2 '吸取关

Dly 1 '释放确认 Mvs,-35 '抬升 Dly 0.5 '

'***************调整1号装配台3号工件***************** *next31

If Abs(m212)<5 Then *next32 '偏差小于5度则不调整 If m212>179 Then m212=179 If m212<-179 Then m212=-179

Mvs PPT13 '到1号装配台3号工件上方

Mvs, 45 '下降 HClose 2 '吸取开 Dly 0.5 '释放确认 Mvs,-15 '抬升 J1=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)

J1.J6=Rad(m212) '将角度转换为弧度代入 J2=J_Curr-J1 '取当前所在位置 Mov J2 '旋转 PPT0=P_Curr+(+0.00,+0.00,+15.00,+0.00,+0.00,+0.00) Mvs,2 '下降 If M_In(15)=0 Then *POK13

'************************************************* m6=0

GoSub *WeiTiao1 '工件位置微调1mm If m6=1 Then *POK13

GoSub *WeiTiao2 '工件位置微调2mm If m6=1 Then *POK13

GoSub *WeiTiao3 '工件位置微调3mm *POK13

'************************************************* HOpen 2 '吸取关

Dly 1 '释放确认 Mvs,-35 '抬升 Dly 0.5 '

'***************调整1号装配台4号工件***************** *next32

If Abs(m217)<5 Then *next33 '偏差小于5度则不调整 If m217>179 Then m217=179 If m217<-179 Then m217=-179

Mvs PPT14 '到1号装配台4号工件上方 Mvs, 45 '下降 HClose 2 '吸取开 Dly 0.5 '释放确认 Mvs,-15 '抬升 J1=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)

J1.J6=Rad(m217) '将角度转换为弧度代入 J2=J_Curr-J1 '取当前所在位置 Mov J2 '旋转 PPT0=P_Curr+(+0.00,+0.00,+15.00,+0.00,+0.00,+0.00) Mvs,2 '下降 If M_In(15)=0 Then *POK14

'************************************************* m6=0

GoSub *WeiTiao1 '工件位置微调1mm If m6=1 Then *POK14

GoSub *WeiTiao2 '工件位置微调2mm If m6=1 Then *POK14

GoSub *WeiTiao3 '工件位置微调3mm *POK14

'************************************************* HOpen 2 '吸取关

Dly 1 '释放确认 Mvs,-35 '抬升 Dly 0.5 '

'调整2号装配台 *next33

'***************调整2号装配台1号工件*****************

If m2=1 Then *next37 '2号装配台不需要调整，转3号装配台 If Abs(m222)<5 Then *next34 '偏差小于5度则不调整 If m222>179 Then m222=179 If m222<-179 Then m222=-179

Mvs PPT21 '到2号装配台1号工件上方 Mvs, 45 '下降 HClose 2 '吸取开 Dly 0.5 '释放确认 Mvs,-15 '抬升 J1=(+0.00,+0.00,+0.00,+0.00,+0.00,+0.00)

J1.J6=Rad(m222) '将角度转换为弧度代入 J2=J_Curr-J1 '取当前所在位置 Mov J2 '旋转 PPT0=P_Curr+(+0.00,+0.00,+15.00,+0.00,+0.00,+0.00) Mvs,2 '下降 If M_In(15)=0 Then *POK21

'************************************************* m6=0

GoSub *WeiTiao1 '工件位置微调1mm If m6=1 Then *POK21

GoSub *WeiTiao2 '工件位置微调2mm If m6=1 Then *POK21

GoSub *WeiTiao3 '工件位置微调3mm *POK21

'************************************************* HOpen 2 '吸取关

Dly 1 '释放确认 Mvs,-35 '抬升 Dly 0.5 '

'***************调整2号装配台2号工件***************** *next34

If Abs(m227)<5 Then *next35 '偏差小于5度则不调整

本文来源：https://www.bwwdw.com/article/f0ag.html