1、任務(wù)要求：
搬運(yùn)系統(tǒng)由搬運(yùn)機(jī)器人、真空吸盤及控制裝置、傳送帶、周轉(zhuǎn)箱等主要部件組成。系統(tǒng)要求利用安裝在機(jī)器人上的真空吸盤，抓取由傳送帶輸送而來的工件；然后，再將工件逐一、依次放置到周轉(zhuǎn)箱的1～4號(hào)位置中。一旦周轉(zhuǎn)箱放滿4個(gè)工件，系統(tǒng)輸出“周轉(zhuǎn)箱滿”指示燈信號(hào)，提示操作者更換周轉(zhuǎn)箱；周轉(zhuǎn)箱更換后，繼續(xù)進(jìn)行搬運(yùn)作業(yè)。
[if !supportLists]2、[endif]動(dòng)作順序表
[if !supportLists]3.???? [endif]輸入輸出信號(hào)點(diǎn)
4、路徑規(guī)劃
[if !supportLists]5、[endif]變量定義
[if !supportLists]6、[endif]程序設(shè)計(jì)
[if !supportLists]7、[endif]程序代碼
MODULE mainmodu (SYSMODULE) // 主模塊mainmodu及屬性
! Module name : Mainmodule for Transfer // 注釋
! Robot type : IRB 120
! Software : Robot Ware 6.01
! Created : 2017-06-06
!*************************************** // 定義程序數(shù)據(jù)（根據(jù)實(shí)際情況設(shè)定）
CONST robtarget p Home:=[……] ; // 作業(yè)原點(diǎn)
CONST robtarget p Pick:=[……] ; ? //抓取點(diǎn)
CONST robtarget p Place Base:=[……] ; ?? //放置基準(zhǔn)點(diǎn)
CONST speeddata v Empty High:=[……] ; // 空載高速
CONST speeddata v Empty Low:=[……] ; // 空載低速
CONST speeddata v Load High:=[……] ; // 帶載高速
CONST speeddata v Load Low:=[……] ; ? //帶載低速
CONST num n Xoffset:=…… ; // 周轉(zhuǎn)箱X向間距
CONST num n Yoffset:=…… ; // 周轉(zhuǎn)箱Y向間距
CONST num n Zoffset:=…… ; // Z向低速接近距離
PERS tooldata t Gripper:= [……] ; // 作業(yè)工具
PERS loaddata Load Full:= [……] ; // 作業(yè)負(fù)載
PERS wobjdata wobj CNV:= [……] ; // 傳送帶坐標(biāo)系
PERS wobjdata wobj Buffer:= [……] ; // 周轉(zhuǎn)箱坐標(biāo)系
PERS robtarget p Place:=[……] ; // 當(dāng)前放置點(diǎn)
PERS num n Count ; // 工件計(jì)數(shù)器
VAR bool b Pick OK ; // 工件抓取狀態(tài)
!***********************************************************
PROC mainprg () // 主程序
r Initialize ; // 調(diào)用初始化程序
WHILE TRUE DO // 無限循環(huán)
r Pick Panel ; // 調(diào)用工件抓取程序
r Place In Buffer ; // 調(diào)用工件放置程序
r Check Buffer ; // 調(diào)用周轉(zhuǎn)箱檢查程序
Waittime 0.5 // 暫停0.5s
ENDWHILE // 循環(huán)結(jié)束
ENDPROC // 主程序結(jié)束
!***********************************************************
PROC r Initialize () // 初始化程序
r Check Home Pos ; // 調(diào)用作業(yè)原點(diǎn)檢查程序
n Count:=1 // 工件計(jì)數(shù)器預(yù)置
b Pick OK:=FALSE ; // 撤銷抓取狀態(tài)
Reset do32_Vacuum ON // 關(guān)閉吸盤
ENDPROC // 初始化程序結(jié)束
!***********************************************************
PROC r Pick Panel () // 工件抓取程序
IF b Pick OK:=FALSE THEN
Move JOffs(p Pick,0,0,n Zoffset),v Empty High,z20,t
Gripper\wobj:=wobj CNV;// 移動(dòng)到p Pick上方減速點(diǎn)
Wait DI di01_In Pick Pos, 1 ; // 等待傳送帶到位di01=1
Move L p Pick, v Empty Low, fine, t Gripper\ wobj :=wobj CNV ; // p
Pick點(diǎn)定位
Set do32_Vacuum ON ; // 吸盤ON（do32=1）
Wait DI di02_ Vacuum OK, 1 ; // 等待抓取完成di02=1
b Pick OK:=TRUE ; // 設(shè)定抓取狀態(tài)
Grip Load Load Full ; // 設(shè)定作業(yè)負(fù)載
Move LOffs(p Pick,0,0,n Zoffset),v Load Low,z20,t Gripper\wobj:=wobj
CNV;// 提升到p Pick上方減速點(diǎn)
ELSE
TPErase ; // 示教器清屏
TPWrite ''Cycle Restart Error'' ; // 顯示出錯(cuò)信息
TPWrite ''Cycle can't start with Panel on Gripper'' ;
TPWrite ''Please check the Gripper and then restart next cycle'' ;
Stop ; // 程序停止
ENDIF
ENDPROC //工件抓取程序結(jié)束
!***********************************************************
PROC r Place In Buffer () // 工件放置程序
IF b Pick OK:=TRUE THEN
r Calculate Pos ; // 調(diào)用放置點(diǎn)計(jì)算程序
Wait DI di03_Buffer Ready, 1 ; // 等待周轉(zhuǎn)箱到位di03=1
Move JOffs(p Place,0,0,n Zoffset),v Load High,z20,t
Gripper\wobj:=wobj Buffer;// 移動(dòng)到p Place上方減速點(diǎn)
Move L p Place,
v Load Low, fine, t Gripper\ wobj :=wobj Buffer ; // p Pick點(diǎn)定位
Reset
do32_Vacuum ON ; // 吸盤OFF（do32=0）
Wait DI di02_ Vacuum OK, 0 ; // 等待放開di02=0
Waittime 0.5 // 暫停0.5s
b Pick OK:=FALSE ; // 撤銷抓取狀態(tài)
Grip Load Load0 ; // 撤銷作業(yè)負(fù)載
Move LOffs(p Place,0,0,n Zoffset),v Empty Low,z20,t
Gripper\wobj:=wobj Buffer;// 移動(dòng)到p Place上方減速點(diǎn)
Move J p Home, v Empty High, fine, t Gripper ; // 返回作業(yè)原點(diǎn)
n Count:= n Count +1 // 工件計(jì)數(shù)器加1
ENDIF
ENDPROC //工件放置程序結(jié)束
!***********************************************************
PROC r Check
Buffer () // 周轉(zhuǎn)箱檢查程序
IF n Count＞4 THEN
Set do34_Buffer Full ; // 周轉(zhuǎn)箱滿ON（do34=1）
Wait DI di03_Buffer Ready, 0 ; // 等待取走周轉(zhuǎn)箱di03=0
Reset do34_Buffer Full ; // 周轉(zhuǎn)箱滿OFF（do34=0）
n Count:= 1 //工件計(jì)數(shù)器復(fù)位
ENDIF
ENDPROC //周轉(zhuǎn)箱檢查程序結(jié)束
!***********************************************************
PROC r Calculate
Pos () // 放置點(diǎn)計(jì)算程序
TEST n Count // 計(jì)數(shù)器測試
CASE 1：
p Place := p Place Base ; // 放置點(diǎn)1
CASE 2：
p Place := Offs(p Place Base, n Xoffset, 0, 0) ; // 放置點(diǎn)2
CASE 3：
p Place := Offs(p Place Base, 0, n Yoffset, 0) ; // 放置點(diǎn)3
CASE 4：
p Place := Offs(p Place Base, n Xoffset, n Yoffset, 0) ; // 放置點(diǎn)4
DEFAULT：
TPErase ; // 示教器清屏
TPWrite ''The Count Number is Error'' ; // 顯示出錯(cuò)信息
Stop ;
ENDTEST
ENDPROC //放置點(diǎn)計(jì)算程序結(jié)束
!***********************************************************
PROC Check Home
Pos () // 作業(yè)原點(diǎn)檢查程序
VAR robtarget p ActualPos ; // 程序數(shù)據(jù)定義
IF NOT In Home Pos( p Home, t Gripper) THEN// 利用功能程序判別作業(yè)原點(diǎn)，非作業(yè)原點(diǎn)時(shí)進(jìn)行如下處理
p ActualPos:=CRob T(\Tool:= t Gripper \ wobj :=wobj0) ;// 讀取當(dāng)前位置
p ActualPos.trans.z:= p Home.trans.z ; // 改變Z坐標(biāo)值
Move L p ActualPos, v Empty High, z20, t Gripper ; // Z軸退至p Home
Move L p Home, v Empty High, fine, t Gripper ; // X、Y軸定位到p Home
ENDIF
ENDPROC //作業(yè)原點(diǎn)檢查程序結(jié)束
!***********************************************************
FUNC bool In
Home Pos (robtarget ComparePos, INOUT tooldata TCP)// 作業(yè)原點(diǎn)判別程序
VAR num Comp_Count:=0 ;
VAR robtarget Curr_Pos ;
Curr_Pos:= CRob T(\Tool:= t Gripper \ wobj :=wobj0) ;// 讀取當(dāng)前位置，進(jìn)行以下判別
IF Curr_Pos.trans.x＞ComparePos.trans.x－20 AND
Curr_Pos.trans.x＜ComparePos.trans.x+20 Comp_Count:= Comp_Count+1 ;
IF Curr_Pos.trans.y＞ComparePos.trans.y－20 AND
Curr_Pos.trans.y＜ComparePos.trans.y+20 Comp_Count:= Comp_Count+1 ;
IF Curr_Pos.trans.z＞ComparePos.trans.z－20 AND
Curr_Pos.trans.z＜ComparePos.trans.z+20 Comp_Count:= Comp_Count+1 ;
IF Curr_Pos.rot.q1＞ComparePos.rot.q1－0.05 AND
Curr_Pos.rot.q1＜ComparePos.rot.q1+0.05 Comp_Count:= Comp_Count+1 ;
IF Curr_Pos.rot.q2＞ComparePos.rot.q2－0.05 AND
Curr_Pos.rot.q2＜ComparePos.rot.q2+0.05 Comp_Count:= Comp_Count+1 ;
IF Curr_Pos.rot.q3＞ComparePos.rot.q3－0.05 AND
Curr_Pos.rot.q3＜ComparePos.rot.q3+0.05 Comp_Count:= Comp_Count+1 ;
IF Curr_Pos.rot.q4＞ComparePos.rot.q4－0.05 AND
Curr_Pos.rot.q4＜ComparePos.rot.q4+0.05 Comp_Count:= Comp_Count+1 ;
RETUN Comp_Count=7 ; // 返回Comp_Count=7的邏輯狀態(tài)
ENDFUNC //作業(yè)原點(diǎn)判別程序結(jié)束
!***********************************************************
ENDMODULE // 主模塊結(jié)束
!***********************************************************
