程式

双站排列机程序(附解释)

'********************************** ‘程序名称,版本,控制器型号
' 樊乘忒粮欣每沉<4-0-SRB3> *
' <4-0-SRB5> *
' Ver2.30
' DATE 2006/08/03 (YK350X+RCX40)
'**********************************
ON ERROR GOTO *ERR_HALT ‘如果发生错误至错误处理单元(最后面)
'
' 辑仑?鼐?
'
RESET DO2() ‘关闭所有输出
RESET DO3()
RESET DO4()
'
SHIFT S0 ‘坐标设定
'
' 水臣抻嫁联?
'
SEND /DSPLNG/ TO L$ ‘读出系统中设定的显示语言
LNG%=1
FOR I=1 TO LEN(L$)
IF MID$(L$,I,1)="'" THEN
IF MID$(L$,I+1,1)="? THEN LNG%=0
ENDIF
NEXT I
IF LNG%=0 THEN PRINT "莆莺? ELSE PRINT "English" ‘操作界面或信息显示日文(LNG%=0)或英文(LNG%=1)
'
' 碧蘅棉??鼐?
'
SET DO(35),100 ‘输出
DELAY 50 ‘等待50ms
'
' 叶弈俑罐菝莼傲
'
IF DI(22)=0 THEN
IF LNG%=0 THEN
HALT "ALARM:叶弈俑 布蕻?
ELSE
HALT "ALARM:MEGATORQUE ALARM" ‘伺服错误
ENDIF
ENDIF
DRIVE ( 3 , 0.0 ),SPEED=20 ‘手臂回起始位置
WAIT DI(53)=1 AND DI(54)=0,2000
IF NOT( DI(53)=1 AND DI(54)=0 ) THEN
IF LNG%=0 THEN
HALT "ALARM:彻嫁皋驾赋 逗尘睫"
ELSE
HALT "ALARM:JIG VACUUM NOT DOWN" ‘错误:转盘吸气
ENDIF
ENDIF
WAIT DI(52)=0,3000
IF DI(52)=1 THEN
IF LNG%=0 THEN
HALT "ALARM:诉?莞蘧莼?ON"
ELSE
HALT "ALARM:PICKING SENSOR ON" ‘挡到感应器
ENDIF
ENDIF
WAIT ARM
'START *M_TURNING,T2
SET DO(22)
DO(27,26,25,24)=0
'
DELAY 70
SET DO(36),70
DELAY 50
'
SET DO(23),300
WAIT DI(23)=1
SET DO(37)
WAIT DI(23)=0
RESET DO(37)
'CUT T2 ‘以上转盘回原点
'
TABLE%=1 ‘初始为B站
'
IF DI(27)=0 THEN AUTO ELSE MANUAL ‘手动/自动
'
HALT
'************
' AUTO
'************
*AUTO:
'
SET DO(20)
'
START *PB_TASK,T2
'
*AUTO_LOOP:
'
WAIT (DI(51)=1 OR KIDOU%=1)AND DI(52)=0
CUT T2
'
' 叶弈俑恫幂
'
'START *M_TURNING,T2 ‘转盘转动
IF TABLE%=1 THEN TABLE%=2 ELSE TABLE%=1
DO(27,26,25,24)=TABLE%
'
DELAY 40
SET DO(36),40
DELAY 20
'
SET DO(23),250
WAIT DI(23)=1,5000
IF DI(23)=0 THEN
IF LNG%=0 THEN
HALT "ALARM:叶弈俑 恫幂窘?
ELSE
HALT "ALARM:MEGATORQUE NOT TURN"
ENDIF
ENDIF
SET DO(37) ‘以上判定转盘是否正常
'CUT T2
'
IF KIDOU%=0 THEN
WAIT DI(23)=0
RESET DO(37)
START *PB_TASK,T2
GOTO *AUTO_LOOP
ELSE
START *PB_TASK,T2
ENDIF
'
' 踢鄹拮袾O警貌
'
IF DI(37,36,35,34)>9 OR DI(33,32,31,30)>9 THEN
IF LNG%=0 THEN
HALT "ALARM:棉嫁讲? 布蕻?
ELSE
HALT "ALARM:DIGITAL-SW ABNORMAL"
ENDIF
ENDIF
WARK_NO%=DI(37,36,35,34)*10+DI(33,32,31,30) ‘程序号
IF WARK_NO%<50 THEN
REV%=0
ELSE
WARK_NO%=WARK_NO%-50
REV%=1
ENDIF ‘以上判定正排倒排(程序号是否大于50,REV%=0正排,1倒排))
IF WARK_NO%>19 THEN
IF LNG%=0 THEN
HALT "ERROR:踢鄹拮?NO. 蛋兽?
ELSE
HALT "ERROR:PROGRAM NO. OVER" ‘程序号不能大于19
ENDIF
ENDIF
IF WARK_NO%>=1 THEN
PREV_QTY%=LOCX( P[(WARK_NO%-1)*200+4] )
IF PREV_QTY%>=1000 THEN
IF LNG%=0 THEN
HALT "ERROR:踢鄹拮袾O. 押?
ELSE
HALT "ERROR:PROGRAM NO. INVALID"
ENDIF
ENDIF
ENDIF ‘以上判定当KEY个数大于40个时将占用400点,那么当前程序号的下一个程序号将不能设定
S_POINT%=WARK_NO%*200 ‘S_POINT% 当前程序号的起始点
'
' 航沉??
'
QUANTITY%=LOCX( P[S_POINT%+4] ) ‘KEY个数
IF QUANTITY%>=1000 THEN
QUANTITY%=QUANTITY%-1000
MAX_QTY%=90
ELSE
MAX_QTY%=40
ENDIF ‘以上设定当KEY个数大于1000时,说明实际个数40个以上,那么最大KEY个数为90,否则为40
IF QUANTITY%<1 OR QUANTITY%>MAX_QTY% THEN
IF LNG%=0 THEN
HALT "ERROR:航尘??布蕻?
ELSE
HALT "ERROR:QUANTITY ABNORMAL SETTING"
ENDIF
ENDIF ‘当KEY个数小于1或大于90时报错
A_SPEED%=LOCY( P[S_POINT%+4] )
IF A_SPEED%<5 THEN A_SPEED=5
SPEED A_SPEED% ‘当自动速度小于5时强制为5
'
SET DO(21)
SET DO(40)
WAIT DI(53)=0 AND DI(54)=1 ,3000
IF NOT( DI(53)=0 AND DI(54)=1 ) THEN
IF LNG%=0 THEN
HALT "ALARM:彻嫁皋驾赋 嫁?籍尘睫"
ELSE
HALT "ALARM:JIG VACUUM NOT UP"
ENDIF
ENDIF ‘以上判定真空气是否充足
IF REV%=0 THEN SET DO(41) ELSE SET DO(44)
WAIT DI(23)=0
RESET DO(37)
'
IF TABLE%=2 THEN
MOTO_OF%=2
OFFSET%=20
ELSE
MOTO_OF%=5
IF MAX_QTY%=40 THEN OFFSET%=100 ELSE OFFSET%=200
ENDIF ‘A站(TABLE%=2),大托盘起始位为02(MOTO_OF%),第一个KEY位置为20(OFFSET%)
‘B站(TABLE%=1),大托盘起始位为05(MOTO_OF%),第一个KEY位置为100(40个KEY)或200(90个KEY)
'IF TABLE%=2 THEN QUANTITY%=QUANTITY%*2
‘接下来开始循环排KEY
FOR I=0 TO (QUANTITY%-1)
IF REV%=0 THEN RESET DO(45) ELSE SET DO(45)
IF REV%=0 THEN ' 静霓郴
AAA_SPEED%=LOCA( P[S_POINT%+OFFSET%+I] )
ABA_SPEED%=LOCB( P[S_POINT%++OFFSET%I] )
AA_SPEED%=LOCA( P[S_POINT%+OFFSET%+MAX_QTY%+I] )
AB_SPEED%=LOCB( P[S_POINT%+OFFSET%+MAX_QTY%+I] )
P[S_POINT%+8]=P[S_POINT%+MOTO_OF%]+P[S_POINT%+OFFSET%+I]
P[S_POINT%+9]=P[S_POINT%+8]
‘正排时读出当时的KEY 大小托盘a b的值及大托盘的位置(起始位置+KEY位置)
ELSE ' 忿?霓郴
AA_SPEED%=LOCA( P[S_POINT%+MAX_QTY%+OFFSET%+(QUANTITY%-1-I)] )
AB_SPEED%=LOCB( P[S_POINT%+MAX_QTY%+OFFSET%+(QUANTITY%-1-I)] )
AAA_SPEED%=LOCA( P[S_POINT%+(QUANTITY%-1-I)] )
ABA_SPEED%=LOCB( P[S_POINT%+(QUANTITY%-1-I)] )
A%=S_POINT%+MOTO_OF%+1
B%=S_POINT%+OFFSET%+MAX_QTY%+(QUANTITY%-1-I)
P[S_POINT%+8]=P[ A% ]+P[ B% ]
P[S_POINT%+9]=P[S_POINT%+8]
ENDIF
‘倒排时读出当时的KEY 大小托盘a b的值及大托盘的位置(起始位置+KEY位置)
LOCZ(P[S_POINT%+8])=LOCZ(P[S_POINT%+1])
MOVE P,P[S_POINT%+8] ‘移到大托盘位置(高度为待机高度)
SET DO(42) ‘吸气
PSNG%=0
WAIT ARM
MOVE P,P[S_POINT%+9] ‘下降到吸KEY处
WAIT ARM
WAIT DI(55)=1,300
IF DI(55)=0 THEN
RESET DO(42)
IF LNG%=0 THEN
PRINT "WARNING:翻沉? 棉方?
ELSE
PRINT "WARNING:NOT VACUUM"
ENDIF
PSNG%=1
ENDIF ‘判定吸气
'
'IF AA_SPEED%>0 AND AA_SPEED%<50 THEN DELAY 20
'IF AA_SPEED%>0 AND AA_SPEED%<40 THEN DELAY 60
'IF AA_SPEED%>0 AND AA_SPEED%<30 THEN DELAY 120
'IF AA_SPEED%>0 AND AA_SPEED%<20 THEN DELAY 100
'
IF AAA_SPEED%<>0 THEN SPEED AAA_SPEED% ‘吸KEY时是否缓速
MOVE P,P[S_POINT%+8] ‘上升
' SPEED A_SPEED%
IF REV%=0 THEN SET DO(45) ELSE RESET DO(45)
IF PSNG%=1 THEN EXIT FOR
'
' 遁静擦材蕹
'
IF REV%=0 THEN ' 静霓郴
P[S_POINT%+10]=P[S_POINT%+MOTO_OF%+1]+P[S_POINT%+OFFSET%+MAX_QTY%+I]
P[S_POINT%+11]=P[S_POINT%+10]
ELSE ' 忿?霓郴
P[S_POINT%+10]=P[S_POINT%+MOTO_OF%]
P[S_POINT%+10]=P[S_POINT%+10]+P[S_POINT%+OFFSET%+(QUANTITY%-1-I)]
P[S_POINT%+11]=P[S_POINT%+10]
ENDIF ‘读取小托盘位置
LOCZ(P[S_POINT%+10])=LOCZ(P[S_POINT%+1])
SPEED A_SPEED%
IF ABA_SPEED%<>0 THEN SPEED ABA_SPEED% ‘移动时是否缓速
MOVE P,P[S_POINT%+10] ‘移到小托盘位置处(高度为待机高度)
WAIT ARM
SPEED A_SPEED%
IF AA_SPEED%<>0 THEN SPEED AA_SPEED% ‘下降时是否缓速
MOVE P,P[S_POINT%+11] ‘放KEY
WAIT ARM
RESET DO(42)
SET DO(43) ‘关掉吸气,吹气
WAIT DI(55)=0
SPEED A_SPEED%
IF AB_SPEED%<>0 THEN SPEED AB_SPEED% ‘上升时是否缓速
MOVE P,P[S_POINT%+10] ‘上升
RESET DO(43) ‘关闭吹气
SPEED A_SPEED%
NEXT I ‘继续下一个KEY直至全部排完
'
RESET DO(21)
RESET DO(45)
IF REV%=0 AND PSNG%=0 THEN SET DO(32),300
'
RESET DO(41)
RESET DO(44)
RESET DO(40)
WAIT DI(53)=1 AND DI(54)=0 ,3000
IF NOT( DI(53)=1 AND DI(54)=0 ) THEN
IF LNG%=0 THEN
HALT "ALARM:彻嫁皋驾赋 逗尘睫"
ELSE
HALT "ALARM:JIG VACUUM NOT DOWN"
ENDIF
ENDIF
'
GOTO *AUTO_LOOP
'
HALT
'
' 纺蕹PB遁祭礁
'
*PB_TASK:
KIDOU%=0
RESET DO(30)
*PB_LOOP:
WAIT DI(27)=1 OR DI(50)=1 OR DI(52)=1
IF DI(27)=1 THEN HALT "PROGRAM HALT"
IF DI(50)=1 AND DI(52)=0 THEN
KIDOU%=1
SET DO(30)
ENDIF
IF DI(52)=1 THEN
KIDOU%=0
RESET DO(30)
ENDIF
GOTO *PB_LOOP
EXIT TASK
'
' 叶弈俑貌糎ALT
'
*M_TURNING:
WAIT DI(52)=1
IF LNG%=0 THEN
HALT "ALARM:诉?莞蘧莼?ON"
ELSE
HALT "ALARM:PICKING SENSOR ON"
ENDIF
EXIT TASK
'
'**************
' MANUAL
'**************
*MANUAL:
'
IF DI(37,36,35,34)>9 OR DI(33,32,31,30)>9 THEN
IF LNG%=0 THEN
HALT "ALARM:棉嫁讲? 布蕻?
ELSE
HALT "ALARM:DIGITAL-SW ABNORMAL"
ENDIF
ENDIF
WARK_NO%=DI(37,36,35,34)*10+DI(33,32,31,30)
IF WARK_NO%=40 OR WARK_NO%=41 OR WARK_NO%=42 THEN *INSTALL_JIG
IF WARK_NO%<50 THEN
REV%=0
ELSE
WARK_NO%=WARK_NO%-50
REV%=1
ENDIF
IF WARK_NO%>19 THEN
IF LNG%=0 THEN
HALT "ERROR:踢鄹拮?NO. 蛋兽?
ELSE
HALT "ERROR:PROGRAM NO. OVER"
ENDIF
ENDIF
IF WARK_NO%>=1 THEN
PREV_QTY%=LOCX( P[(WARK_NO%-1)*200+4] )
IF PREV_QTY%>=1000 THEN
IF LNG%=0 THEN
HALT "ERROR:踢鄹拮袾O. 押?
ELSE
HALT "ERROR:PROGRAM NO. INVALID"
ENDIF
ENDIF
ENDIF
S_POINT%=WARK_NO%*200
'
' 航沉??
'
QUANTITY%=LOCX( P[S_POINT%+4] )
IF QUANTITY%>=1000 THEN
QUANTITY%=QUANTITY%-1000
MAX_QTY%=90
ELSE
MAX_QTY%=40
ENDIF
IF QUANTITY%<1 OR QUANTITY%>MAX_QTY% THEN
IF LNG%=0 THEN
HALT "ERROR:航尘??布蕻?
ELSE
HALT "ERROR:QUANTITY ABNORMAL SETTING"
ENDIF
ENDIF
M_SPEED%=LOCZ( P[S_POINT%+4] )
IF M_SPEED%<5 THEN M_SPEED=5
'
SPEED M_SPEED%
SET DO(20)
'
START *PRG_MODE,T3
START *KYUCYAKU,T4
START *UKEZIGU,T5
'
POS_NO%=0
'
*MANUAL_LOOP:
WAIT DI(40)=1 OR DI(41)=1 OR DI(42)=1 OR DI(43)=1 OR DI(51)=1
IF DI(40)=1 THEN
POS_NO%=POS_NO%-1
IF POS_NO%<1 THEN POS_NO%=1
'
GOSUB *P_CAL
DRIVE( 3 , LOCZ(P[S_POINT%+1]) )
IF B%=0 THEN RESET DO(45) ELSE SET DO(45)
MOVE P,P[S_POINT%+8]
ELSEIF DI(41)=1 THEN
POS_NO%=POS_NO%+1
IF POS_NO%>QUANTITY%2 THEN POS_NO%=QUANTITY%2
'
GOSUB *P_CAL
DRIVE( 3 , LOCZ(P[S_POINT%+1]) )
IF B%=0 THEN RESET DO(45) ELSE SET DO(45)
MOVE P,P[S_POINT%+8]
ELSEIF DI(42)=1 THEN
IF POS_NO%<>0 THEN
DRIVE( 3 , LOCZ(P[S_POINT%+1]) )
ENDIF
ELSEIF DI(43)=1 THEN
IF POS_NO%<>0 THEN
GOSUB *P_CAL
MOVE P,P[S_POINT%+9]
ENDIF
ELSEIF DI(51)=1 THEN
IF DI(46)=0 AND DI(47)=0 AND DI(52)=0 AND DI(53)=1 AND DI(54)=0 THEN
CUT T5
DRIVE( 3 , 0.0 )
'START *M_TURNING,T2
IF TABLE%=1 THEN TABLE%=2 ELSE TABLE%=1
DO(27,26,25,24)=TABLE%
'
DELAY 70
SET DO(36),70
DELAY 50
'
SET DO(23),300
WAIT DI(23)=1
SET DO(37)
POS_NO%=0
'CUT T2
START *UKEZIGU,T5
WAIT DI(23)=0
RESET DO(37)
ENDIF
ENDIF
'
WAIT DI(40)=0 AND DI(41)=0 AND DI(42)=0 AND DI(43)=0 AND DI(51)=0
GOTO *MANUAL_LOOP
'
HALT
'
*P_CAL:
IF TABLE%=2 THEN
MOTO_OF%=2
OFFSET%=20
ELSE
MOTO_OF%=5
IF MAX_QTY%=40 THEN OFFSET%=100 ELSE OFFSET%=200
ENDIF
A%=INT((POS_NO%-1) / 2)
B%=(POS_NO%-1) MOD 2
IF B%=0 THEN
OFFSET%=OFFSET%+A%
P[S_POINT%+8]=P[S_POINT%+MOTO_OF%]+P[S_POINT%+OFFSET%]
ELSE
OFFSET%=OFFSET%+MAX_QTY%+A%
P[S_POINT%+8]=P[S_POINT%+MOTO_OF%+1]+P[S_POINT%+OFFSET%]
ENDIF
PRINT "NOTIFY:OFFSET%=";OFFSET% '/// DEBUG ///
P[S_POINT%+9]=P[S_POINT%+8]
LOCZ(P[S_POINT%+8])=LOCZ(P[S_POINT%+1])
RETURN
'
*PRG_MODE:
WAIT DI(27)=0
HALT "PROGRAM HALT"
GOTO *PRG_MODE
'
*KYUCYAKU:
WAIT DI(44)=1 OR DI(45)=1
IF DI(44)=1 THEN
RESET DO(42)
SET DO(43)
WAIT DI(44)=0
RESET DO(43)
ELSEIF DI(45)=1 THEN
SET DO(42)
ENDIF
GOTO *KYUCYAKU
'
*UKEZIGU:
WAIT DI(46)=1 OR DI(47)=1
SET DO(40)
DELAY 500
IF DI(46)=1 AND DI(47)=0 THEN
SET DO(41)
WAIT DI(46)=0
RESET DO(41)
ELSEIF DI(46)=0 AND DI(47)=1 THEN
SET DO(44)
WAIT DI(47)=0
RESET DO(44)
ENDIF
DELAY 500
RESET DO(40)
GOTO *UKEZIGU
'
'*******************
' INSTALL JIG
'*******************
*INSTALL_JIG:
'
START *PRG_MODE,T6
START *BLINK,T7
SET DO(20)
WAIT DI(50)=1
CUT T7
SET DO(30)
'
P8=120.00 60.00 0.00 0.00 0.00 0.00
SET_R!=0.00
IF WARK_NO%=41 THEN SET_R!= 90.00
IF WARK_NO%=42 THEN SET_R!=-90.00
LOCR(P8)=SET_R!
MOVE P,P8,SPEED=25
LOCZ(P8)=60.00
WAIT ARM
MOVE L,P8,VEL=60
WAIT ARM
'
*JIG_LOOP:
'
PRINT JTOXY(WHERE)
WAIT DI(43,42,41,40)<>0
P8=JTOXY(WHERE)
LOCR(P8)=SET_R!
IF DI(40)=1 AND DI(41)=0 THEN
LOCX(P8)=LOCX(P8)-0.10
MOVE L,P8,VEL=8
WAIT DI(40)=0,2000
IF DI(40)=1 THEN
LOCX(P8)=LOCX(P8)-50.00
MOVE L,P8,VEL=8,STOPON DI(40)=0
ENDIF
ELSEIF DI(40)=0 AND DI(41)=1 THEN
LOCX(P8)=LOCX(P8)+0.10
MOVE L,P8,VEL=8
WAIT DI(41)=0,2000
IF DI(41)=1 THEN
LOCX(P8)=LOCX(P8)+50.00
MOVE L,P8,VEL=8,STOPON DI(41)=0
ENDIF
ELSEIF DI(42)=1 AND DI(43)=0 THEN
LOCZ(P8)=LOCZ(P8)-5.00
MOVE L,P8,VEL=20
ELSEIF DI(42)=0 AND DI(43)=1 THEN
LOCZ(P8)=LOCZ(P8)+5.00
MOVE L,P8,VEL=20
ENDIF
WAIT ARM
WAIT DI(43,42,41,40)=0
GOTO *JIG_LOOP
'
HALT
'
*BLINK:
SET DO(30)
DELAY 500
RESET DO(30)
DELAY 500
GOTO *BLINK
'
'*************
' ERROR
'*************
*ERR_HALT:
IF ERR=&H604 AND (ERL=149 OR ERL=353) THEN
PREV_QTY%=0
RESUME NEXT
ENDIF
IF ERR=&H604 THEN
PRINT "ERROR:";ERR/256;".";ERR MOD 256;" 芜草呐?/ Point doesn't exist"
PRINT "LINE :";ERL
ELSE
PRINT "ERROR:";ERR/256;".";ERR MOD 256,"LINE:";ERL
ENDIF
HALT
EXIT TASK