天煌教仪机器人程序.docx
《天煌教仪机器人程序.docx》由会员分享，可在线阅读，更多相关《天煌教仪机器人程序.docx（88页珍藏版）》请在冰点文库上搜索。
天煌教仪机器人程序
'停止0
'伺服OFF1
'程序复位2
'启动3
'伺服ON4
'操作权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）
Dly0.5
'
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'工件参数清零
m105=0'工件参数清零
m110=0'工件参数清零
m115=0'工件参数清零
m120=0'工件参数清零
m125=0'工件参数清零
m130=0'工件参数清零
m135=0'工件参数清零
m140=0'工件参数清零
m145=0'工件参数清零
m150=0'工件参数清零
m155=0'工件参数清零
'
WaitM_In（9）=1'等待接收运行信号为1——可取盒（Y14）
Dly0.5
M_Out（8）=0'输出完成复位信号为0--X36
GoSub*RecDat_PLC'接收PLC装配流程数据
GoSub*Jia_He'夹取工件盒到装配台子程序
GoSub*Jia_XiPan'夹取吸盘工装子程序
MovP111'P12到P0的过渡
'
'
*LOOP1'装配检测循环
MvsP0'运行到吸取等待位置
M_Out（5）=1'机器人等待吸取信号为1--已到位（X33）
Open"COM3:
"As#2'打开网络端口（与PLC相接的以太网端口）
WaitM_Open
（2）=1
Input#2,m10,m11,m12,m13'm10工件放置的位置
'm11工件参数（其中个位数为编号，十位数为颜色和高度）
'm12表示是否还有下一个工件（1为没有）
'm13表示是否为有用工件（1为没用）
Close#2
'Ifm13>0ThenGoSub*FJ
Selectm10
Case1
PPT0=PPT11'设定PPT11为工件放置目标位置
Break
Case2
PPT0=PPT12'设定PPT12为工件放置目标位置
Break
Case3
PPT0=PPT13'设定PPT13为工件放置目标位置
Break
Case4
PPT0=PPT14'设定PPT14为工件放置目标位置
Break
Case5
PPT0=PPT21'设定PPT21为工件放置目标位置
Break
Case6
PPT0=PPT22'设定PPT22为工件放置目标位置
Break
Case7
PPT0=PPT23'设定PPT23为工件放置目标位置
Break
Case8
PPT0=PPT24'设定PPT24为工件放置目标位置
Break
Case9
PPT0=PPT31'设定PPT31为工件放置目标位置
Break
Case10
PPT0=PPT32'设定PPT32为工件放置目标位置
Break
Case11
PPT0=PPT33'设定PPT33为工件放置目标位置
Break
Case12
PPT0=PPT34'设定PPT34为工件放置目标位置
Break
EndSelect
GoSub*S20TRGET'检测到工件来,进行跟踪工件吸取操作
Ifm13=1Andm10=0Then*FeiLiao0'不要的工件放废料框中
GoSub*S30WKPUT'工件放置处理
*Goon
Ifm12=1Then*Vtest'm12=1,说明一个装配流程结束,转视觉检测流程
GoTo*LOOP1'继续放置下一个工件
'
*Vtest'视觉竖向检测
GoSub*Fang_XiPan'放置吸盘工装子程序
GoSub*Jia_Camera'夹取视觉工装子程序
GoSub*Camera_V'视觉竖向检测子程序
GoSub*Fang_Camera'放置视觉工装子程序
GoSub*Comparison_V'竖向视觉信息比对子程序
Ifm1=1Andm2=1Andm3=1Then*HTEST
GoSub*Jia_XiPan'夹取吸盘工装子程序
GoSub*Rotate'工件角度旋转调整子程序
GoSub*Fang_XiPan'放置吸盘工装子程序
'
*HTEST'视觉横向检测
Ifm1=1Andm2=1Andm3=1Then*LOOP'3个装配台已无合格的工件盒存在
GoSub*JiaHe_To_H'夹盒到横向检测台子程序
GoSub*Jia_Camera'夹取视觉工装子程序
GoSub*Camera_H'视觉横向检测子程序
GoSub*Fang_Camera'放置视觉工装子程序
GoSub*Comparison_H'横向视觉信息比对子程序
Ifm20=1Then*HTEST'已作废料被扔掉
GoSub*Jia_gai'取盖放盖子程序
GoSub*Ruku'入库子程序
Ifm7=1Then*ChaiJie
GoTo*HTEST
*ChaiJie
MovP1
GoSub*CIAN'出库拆解子程序
End
'
'
'
'**********************初始化处理子程序******************************
*S10INIT'初始化处理
M_Out（5）=0'机器人等待吸取信号为0——X33
M_Out（8）=0'输出完成复位信号为0——X36
DefPlt1,P90,P91,P92,P93,3,3,2'设置仓库位置，以P90为起点，以P91为终点A，以P92为终点B，以P93为对角点，行为3，列为3，同方向排列
Accel100,100'加速度、减速度设定
Ovrd100'速度设定
Loadset1,1'最佳加速度、减速度说明
OAdlOn'打开最佳加速度、减速度
Cnt0
Clr1
TrClr1'清除追踪缓冲
MWAIT1=0'清除工件等待标记
M_09#=PWK.X'模式号码说明
IfM_Run
（2）=0Then
XRun2,"CM1",1'在插槽2选择程序"CM1"
'WaitM_Run
（2）=1
EndIf
PriorityPRI.X,1
PriorityPRI.Y,2
Return
'
'
'
'**********************原点返回处理******************************
*S90HOME
HOpen1
HOpen2
'M_Out（5）=0'机器人空闲等待状态为0X33
P90CURR=P_Fbc
（1）'获得当位置
IfP90CURR.ZOvrd10'速度设定为10
P90ESC=P90CURR'做回避点
P90ESC.Z=P1.Z
Ovrd100'速度设定为100
EndIf
MovP1'移动到原点
Return
'
'
'
'**********************招手演示子程序******************************
*ZhaoShou
MovP50
MovP51
MovP50
MovP51
MovP50
MvsP52
MovP53
MovP52
MovP53
MovP52
MvsP50
MvsP54
MovP55
MovP54
MovP55
MovP54
MvsP50
MovP1
Return
'
'
'
'**********************接收PLC装配流程数据子程序******************************
*RecDat_PLC
Open"COM3:
"As#2'打开网络端口（与PLC相接的以太网端口）
WaitM_Open
（2）=1
M_Out（9）=1'向PLC请求发12个工件的参数
Dly0.2
Input#2,m100,m105,m110,m115,m120,m125,m130,m135,m140,m145,m150,m155'接收12个工件的参数
Close#2'关闭网络端口
M_Out（9）=0
Ifm100=0Andm105=0Andm110=0Andm115=0Thenm1=1'1号装配台不需要装配
Ifm120=0Andm125=0Andm130=0Andm135=0Thenm2=1'2号装配台不需要装配
Ifm140=0Andm145=0Andm150=0Andm155=0Thenm3=1'3号装配台不需要装配
Return
'
'
'
'
'**********************夹取工件盒到装配台子程序******************************
*Jia_He
WaitM_In（9）=1'取盒子信号为1Y14
'
'取工件盒到3号装配台
Ifm3=1Then*JiaHe2'1号装配台不需要装配，转2号装配台
MovP13'抓手换成横向
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.2
MvsP6'到出盒台上方
Ovrd20
MvsP6+（+0.00,+0.00,-50.00,+0.00,+0.00,+0.00）
Dly0.2
HClose1
WaitM_In（901）=1'等待手爪夹紧信号为1
Ovrd100
MvsP6
Dly0.2'延时
MvsP20'到1号装配台上方（横向）
Ovrd20
MvsP20+（+0.00,+0.00,-90.00,+0.00,+0.00,+0.00）
Dly0.2
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.2'延时
Ovrd100
MvsP20
MvsP13'过渡点
Dly0.2'延时
MovP12'2号工装上方位置（竖向）
MvsP21'1号装配台上方位置（竖向）
Ovrd20
Mvs,60
Dly1
HClose1
WaitM_In（901）=1'等待手爪夹紧信号为1
Dly0.2'延时
Ovrd100
MvsP21
MvsP23'3号装配台上方位置（竖向）
Ovrd20
Mvs,60
Dly1
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly1
Ovrd100
MvsP23
MvsP12
'
'取工件盒到2号装配台
*JiaHe2
Ifm2=1Then*JiaHe3'2号装配台不需要装配，转3号装配台
MovP13'抓手换成横向
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.2
WaitM_In（9）=1'取盒子信号为1Y14
MvsP6'到出盒台上方
Ovrd20
MvsP6+（+0.00,+0.00,-50.00,+0.00,+0.00,+0.00）
Dly0.2
HClose1
WaitM_In（901）=1'等待手爪夹紧信号为1
Ovrd100
MvsP6
Dly0.2
MvsP20'到1号装配台上方（横向）
Ovrd20
MvsP20+（+0.00,+0.00,-90.00,+0.00,+0.00,+0.00）
Dly0.2
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.2
Ovrd100
MvsP20
MvsP13'过渡点
MovP12'抓手换成竖向
MvsP21'1号装配台上方位置（竖向）
Ovrd20
Mvs,60
Dly0.2
HClose1
WaitM_In（901）=1'等待手爪夹紧信号为1
Dly0.2
Ovrd100
MvsP21
MvsP22'2号装配台上方位置（竖向）
Ovrd20
Mvs,60
Dly0.2
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.2
Ovrd100
MvsP22
MvsP12
'
'取工件盒到1号装配台
*JiaHe3
Ifm1=1ThenReturn'3号装配台不需要装配，转3号装配台
MovP13'抓手换成横向
HOpen1
WaitM_In（9）=1'取盒子信号为1Y14
MvsP6'到出盒台上方
Ovrd20
MvsP6+（+0.00,+0.00,-50.00,+0.00,+0.00,+0.00）
Dly0.2
HClose1
WaitM_In（901）=1'等待手爪夹紧信号为1
Ovrd100
MvsP6
Dly0.2
MvsP20'到1号装配台上方（横向）
Ovrd20
MvsP20+（+0.00,+0.00,-90.00,+0.00,+0.00,+0.00）
Dly0.2
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.2
Ovrd100
MvsP20
MvsP13'过渡点
MovP12'抓手换成竖向,下一个动作是夹取吸盘工装
Return
'
'
'**********************夹取吸盘工装子程序******************************
*Jia_XiPan
MovP2'吸盘工装正上方位置
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly0.3'延时1秒
Ovrd50'速度设定
MvsP3+（+0.00,+0.00,+90.00,+0.00,+0.00,+0.00）
Ovrd20'速度设定
MvsP3'吸盘工装位置
Dly1
HClose1'夹取视觉相机工装
WaitM_In（901）=1'等待手爪夹紧信号为1
Dly1'延时1秒
Ovrd50'速度设定
MvsP2'吸盘工装正上方位置
Ovrd100'速度设定
MvsP12
Return
'
'
'**********************放置吸盘工装子程序******************************
*Fang_XiPan
MovP12
MvsP2'吸盘工装正上方位置
Ovrd50'速度设定
MvsP3+（+0.00,+0.00,+90.00,+0.00,+0.00,+0.00）
Ovrd20'速度设定
MvsP3'吸盘工装位置
Dly0.5
HOpen1
WaitM_In（900）=1'等待手爪松开信号为1
Dly1'延时1秒
Ovrd50'速度设定
MvsP2'吸盘工装正上方位置
Ovrd100'速度设定
MvsP12
Return
'
'
'
'**********************工件跟踪吸取子程序******************************
*S20TRGET
'///追踪缓冲检查///
*LBFCHK
IfM_Trbfct
（1）>=1ThenGoTo*LREAD'如果有工件，转到工件数据获得程序
MovP0Type0,0'移动到拉位置
MWAIT1=0
GoTo*LBFCHK
'///工件数据获得///
*LREAD
TrRdPBPOS,MBENC#,MBWK%,1,MBENCNO%'
GoSub*S40DTSET'运输数据设定
'///工件位置确认///
*LNEXT
PX50CUR=TrWcur（MBENCNO%,PBPOS,MBENC#）'获得现在工件位置
MX50ST=PRNG.X'开始距离范围在机器人跟随工件
MX50ED=PRNG.Y'停止距离范围在机器人跟随工件
MX50PAT=PTN.X'输送模式号码
GoSub*S50WKPOS'工件位置确认处理
IfMY50STS=3ThenGoTo*LBFCHK'已经通过，去下个工件
IfMY50STS=2ThenGoTo*LTRST'开始追踪
IfMWAIT=1ThenGoTo*LNEXT'等待工件到来
'///去位置///
PWAIT=P0'改变工件等待姿势
SelectPTN.X'输送位置模式号码
Case1To2'当输送在机器人之前
PWAIT.X=PX50CUR.X'机器人匹配到工件的X坐标
Case3To6
PWAIT.Y=PX50CUR.Y'机器人匹配到工件的Y坐标
EndSelect
PWAIT.Z=PX50CUR.Z+PUP1.X
PWAIT.C=PX50CUR.C
MovPWAITType0,0'移动到工件等待姿势
MWAIT1=1'设置工件等待标记
GoTo*LNEXT
'///开始追踪操作///
*LTRST
AccelPAC1.X,PAC1.Y
Cnt1,0,0
Act1=1'监视机器人跟随工件太远
TrkOn,PBPOS,MBENC#,PTBASE,MBENCNO%'追踪操作开始设置
Ovrd75
MovPGT,PUP1.YType0,0'移动到追踪半空位置
AccelPAC2.X,PAC2.Y
Mvs
升级会员 