876 lines
23 KiB
Plaintext
Executable File
876 lines
23 KiB
Plaintext
Executable File
'MD=MAIN DRIVE, (3,0)=1800000, (0,3)=180000
|
|
'RIGHT LIM SWI AT 16000
|
|
'-----THREAD 0 BEGINS-------------------------------------------------
|
|
#MAIN
|
|
'-----------INITIALISATION STARTS---------------------------
|
|
SH AB
|
|
SIA=1,25,12,1<-1>2
|
|
SIC=1,25,13,0<-2>1
|
|
REFPOS=1400000'REFERENCE POSITION TO DETERMINE CW OR CCW
|
|
RPUP=16320'RIGHT PICK UP POINT(RESOLVER VALUE)
|
|
RDOP=16400'RIGHT DROP OFF POINT(RESOLVER VALUE)
|
|
LPUP=14575'LEFT PICK UP POINT(RESOLVER VALUE)
|
|
LDOP=14510'LEFT DROP OFF POINT(RESOLVER VALUE)
|
|
ULMIN=0'INITIALISE UNLATCH MIN VARIABLE
|
|
ULMAX=0'INITIALISE UNLATCH MAX VARIABLE
|
|
BGCCW1=550000'MD CCW, WDG 1 UNLATCH MIN(SSI VALUE)
|
|
ENCCW1=726100'MD CCW, WDG 1 UNLATCH MAX(SSI VALUE)
|
|
BGCCW2=1094777'CCW WDG 2 UNLACTH MIN(SSI VALUE)
|
|
ENCCW2=1275398'CCW WDG 2 UNLATCH MAX(SSI VALUE)
|
|
BGCCW3=1683200'CCW WDG 3 UNLATCH MIN(SSI VALUE)
|
|
ENCCW3=1840000'CCW WDG 3 UNLATCH MAX(SSI VALUE)
|
|
BGCW3=1690000'CW WDG 3 UNLATCH MIN(NEW)
|
|
ENCW3=1630000'CW WDG 3 UNLATCH MAX(NEW)
|
|
BGCW2=1110000'CW WDG 2 UNLATCH MIN(NEW)
|
|
ENCW2=1040000'CW WDG 2 UNLATCH MAX(NEW)
|
|
BGCW1=560000'CW WDG 1 UNLATCH MIN(NEW)
|
|
ENCW1=500000'CW WDG 1 UNLATCH MAX(NEW)
|
|
LTCH1=0
|
|
UNLTCH1=0
|
|
LTCH2=0
|
|
UNLTCH2=0
|
|
LTCH3=0
|
|
UNLTCH3=0
|
|
ARMREF=0
|
|
ARMCUR=0
|
|
HIGHMIN=2770000'EXTREME LEFT MIN(85 DEG)
|
|
HIGHMAX=2775000'EXTREME LEFT MAX(85DEG)
|
|
LOWMIN=132000'EXTREME RIGHT MIN(15DEG)
|
|
LOWMAX=125000'EXTREME RIGHT MAX(15DEG)
|
|
EXTREME=0'THIS FLAG GOES TO '1' IF MD AT EXTREME LEFT/RIGHT SIDE
|
|
BSCALE=8192/25000'CALIBRATION FOR MAIN DRIVE MOTOR
|
|
ASCALE=13.732/25000'CALIBRATION FOR ARM ACTUATOR MOTOR
|
|
DEADBAND=10
|
|
ERROR=0
|
|
DIR=2'THIS MEANS MD IS STOP AT THE BEGINNING
|
|
STOP=0'THIS FLAG GOES TO '1' IF MD STOPS
|
|
ARMRDY=0'THIS FLAG GOES TO '1' IF ARM IS READY AT PICK UP POINT
|
|
OKFLG=0'ok flag for executing wedge shuffle routine
|
|
ERRORFLG=0'Error flag for limit switches
|
|
FBERROR=0'Feed Back Error flag
|
|
CNTSTOP=0'THIS FLAG MAKES COUNTER TO COUNT ONLY ONCE
|
|
WDGCCW=0
|
|
WDGCW=0
|
|
LPUPFLG=0
|
|
RPUPFLG=0
|
|
RECOVER=0
|
|
COUNTER=0
|
|
LOST=0
|
|
DONE=0'DONE = 1 IF WEDGE SHUFFLE ROUTINE HAS BEEN COMPLETED
|
|
'------------INITIALISE WEDGE PATTERN TO (0,0)---------------------
|
|
'THIS WILL EXECUTE RECOVERY ROUTINE UPON CONTROLLER POWER UP
|
|
CNTLEFT=0'NUMBER OF WEDGES ON THE LEFT SIDE
|
|
CNTRIGHT=0'NUMBER OF WEDGES ON THE RIGHT SIDE
|
|
'wedge pattern will be (0,3) after recovery routine has been executed
|
|
ACA=1500000
|
|
DCA=1500000
|
|
SPA=150000
|
|
ACB=25000
|
|
DCB=25000
|
|
SPB=25000
|
|
SB 1
|
|
WT 500'WAIT FOR 0.5 SEC
|
|
'---------INITIALISATION ENDS-------------------------------
|
|
'EXECUTE THREAD 1---------------------------------
|
|
XQ #THREAD1,1
|
|
'EXECUTE THREAD 2---------------------------------
|
|
XQ #THREAD2,2
|
|
'EXECUTE THREAD 3---------------------------------
|
|
XQ #THREAD3,3
|
|
'EXECUTE THREAD 4---------------------------------
|
|
XQ #THREAD4,4
|
|
'EXECUTE THREAD 5---------------------------------
|
|
XQ #THREAD5,5
|
|
#LOOP
|
|
'SICS asks for a request before moving A axis (tube)
|
|
IF (REQA=1)&(RSPA=0)
|
|
'controller checks if motor can be driven and wedges can be moved
|
|
'if motor can be driven and everything is ok
|
|
RSPA=1
|
|
'(if motor fault)
|
|
'RSPA=-1
|
|
ENDIF
|
|
'if wedge shuffle routine has done,then SICS sends REQA=0
|
|
IF (REQA=0)&(RSPA=1)
|
|
'GALIL resets RSPA flag
|
|
RSPA=0
|
|
ENDIF
|
|
JP #LOOP
|
|
EN
|
|
'-----THREAD 0 ENDS-------------------------------------------------
|
|
'---------------------THREAD 1 BEGINS---------------------------------
|
|
#THREAD1
|
|
#LOOP1
|
|
'if everything is ok
|
|
IF (REQA=1)&(RSPA=1)
|
|
'EXECUTING RECOVERY PROCEDURE IF WEDGE PATTERN IS (0,0)
|
|
IF (CNTLEFT=0)&(CNTRIGHT=0)
|
|
JS #WDG1CW
|
|
JS #RECLPUP,(LOST=0)
|
|
JS #RECRDOP,(LOST=0)
|
|
ELSE
|
|
'NORMAL ROUTINE -> MAIN DRIVE MOVES TO THE DESTINATION POSITION
|
|
JS #MOVEMD,(RUNA=1)&(STOP=0)&(DONE=0)&(ERRORFLG=0)&(FBERROR=0)
|
|
ENDIF
|
|
ENDIF
|
|
JP #LOOP1
|
|
EN
|
|
'---RECOVERY ROUTINE-------------------------------
|
|
'SUBROUTINE "WDG1CW" BEGINS-------------------------
|
|
#WDG1CW
|
|
'DROP OFF A WEDGE
|
|
SB 1
|
|
'===========================================================
|
|
IF (COUNTER < 2)'MOVE MD TO 27 DEG IF WEDGE LOST IS DETECTED ONCE
|
|
RECOVER=1
|
|
MG "RECOVERY PROCEDURE.MOVE MD CW TO 27 DEG UNTIL WDG1 UNLATCHES" {EB}
|
|
'525000 IS SSI ENCODER VALUE WHEN MD AT 27 DEG
|
|
ERROR=_TPC - 525000
|
|
'-----------------------------
|
|
'IF MD IS AT 27 DEG AND WDG1 UNLATCHES,THEN STOP MD
|
|
IF @ABS[ERROR]<DEADBAND
|
|
ST B
|
|
ELSE
|
|
'IF MD ISN'T AT 27 DEG, THEN MOVE MD TO 27 DEG UNTIL WDG1 UNLATCHES
|
|
IPB=(ERROR/BSCALE)
|
|
MC B
|
|
MG "TWO THETA AT 27 DEG,ENCODER = ",_TPC {EB} {F8.0}
|
|
ENDIF
|
|
'-----------------------------
|
|
'IF WEDGE LOST IS DETECTED FOR THE SECOND TIME, THEN STOP ALL MOTION
|
|
ELSE
|
|
ST
|
|
MC
|
|
LOST=1
|
|
MG "2ND TIME WEDGE LOST IS DETECTED,PLZ CHECK CAM LIM SWI & MAGNET ARM" {EB}
|
|
ENDIF
|
|
'===========================================================
|
|
EN
|
|
'SUBROUTINE "WDG1CW" ENDS---------------------------
|
|
'SUBROUTINE "RECRPUP" BEGINS-------------------------
|
|
'WEDGE IS UNLATCHED,ARM GOES TO LPUP FOR PICKING UP WEGDE 1
|
|
#RECLPUP
|
|
MG "ARM IS MOVING TO LPUP" {EB}
|
|
ERROR=LPUP - _TPA
|
|
'IF ARM HAS REACHED LPUP
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP ARM
|
|
ST A
|
|
MC A
|
|
MG "ARM ARRIVED AT LPUP" {EB}
|
|
ELSE
|
|
'IF ARM STILL HASN'T REACHED LPUP,THEN MOVE ARM TO LPUP
|
|
IPA=(ERROR/ASCALE)
|
|
'AFTER MOTION COMPLETE,ARM HAS REACHED RPUP
|
|
MC A
|
|
'WAIT FOR 0.5 SEC
|
|
WT 500
|
|
MG "ARM ARRIVED AT LPUP" {EB}
|
|
MG "LPUP:",_TPA {EB}{F5.0}
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "RECLPUP" ENDS-------------------------
|
|
'ARM GRABS WDG 1 AND PUSH ALL WEDGES TO THE RIGHT SIDE,
|
|
'ARM STOP AT RDOP.THE PATTERN SHOULD BE (0,3)
|
|
'SUBROUTINE "RECRDOP" BEGINS-------------------------
|
|
#RECRDOP
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'THEN ACTIVATE MAGNET
|
|
CB 1
|
|
MG "MAGNET IS ACTIVATED.ARM IS MOVING TO RDOP" {EB}
|
|
MG "ARM IS COLLECTING WEDGES TO RDOP" {EB}
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
ERROR=RDOP - _TPA
|
|
'IF ARM HAS REACHED LDOP
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP ARM
|
|
ST A
|
|
ELSE
|
|
'IF ARM HASN'T REACHED LDOP,MOVE ARM TO LDOP
|
|
IPA=(ERROR/ASCALE)
|
|
'WDG HAS REACHED LDOP
|
|
MC A
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'DEACTIVATE MAGNET
|
|
SB 1
|
|
MG "ARM ARRIVED AT RDOP" {EB}
|
|
MG "DEACTIVATE MAGNET" {EB}
|
|
MG "RDOP:",_TPA {EB}{F5.0}
|
|
MG "PATTERN SHOULD BE (0,3)" {EB}
|
|
MG "MD IS CONTINUING TO MOVE TO THE DESTINATION POSITION" {EB}
|
|
'INITIALISE VARIABLES
|
|
OKFLG=0
|
|
ERRORFLG=0
|
|
FBERROR=0
|
|
STOP=0
|
|
DONE=0
|
|
ARMRDY=0
|
|
CNTSTOP=0
|
|
RPUPFLG=0
|
|
LPUPFLG=0
|
|
WDGCCW=0
|
|
WDGCW=0
|
|
'ALL 3 WEDGES ON RIGHT SIDE,PATTERN (0,3)
|
|
CNTLEFT=0
|
|
CNTRIGHT=3
|
|
RECOVER=0
|
|
WT 1000
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "MOVEMD" BEGINS-------------------------
|
|
#MOVEMD
|
|
ERROR=_TPC - DSTA
|
|
'IF MD HAS REACHED FINAL DESTINATION
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP MD
|
|
ST B
|
|
MC B
|
|
'IT INDICATES MD HAS REACHED FINAL DEST
|
|
DONE=1
|
|
'Wedge Shuffle Routine is complete
|
|
RUNA=0
|
|
POSA=DSTA
|
|
MG "WEDGE SHUFFLE ROUTINE HAS BEEN COMPLETED" {EB}
|
|
ELSE
|
|
MG "MD IS MOVING TO THE DESTINATION" {EB}
|
|
'IF MD HASN'T REACHED FINAL DESTINATION THEN MOVE MD
|
|
IPB=(ERROR/BSCALE)
|
|
MC B
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "MOVEMD" ENDS---------------------------
|
|
'---------------------THREAD 1 ENDS-----------------------------------
|
|
'^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^THREAD 2 BEGINS^^^^^^^^^^^^^^^^^^
|
|
#THREAD2
|
|
#LOOP2
|
|
'DETERMINE MAIN DRIVE DIRECTION
|
|
JS #CKMDDIR
|
|
'DECIDE UNLATCH RANGE,LOCATION OF THE WEDGE LATCH
|
|
JS #PROCESS
|
|
'CHECK LEFT & RIGHT CAM SWITCH BEFORE MOVING WEDGE
|
|
JS #CKLSMD,(STOP=0)&(RECOVER=0)&(ERRORFLG=0)
|
|
'STOP MD WHEN MD IS WITHIN UNLATCH RANGE
|
|
JS #STOPMD,(OKFLG=1)
|
|
JS #ERROR,(ERRORFLG=1)|(FBERROR=1)
|
|
JP #LOOP2
|
|
EN
|
|
'SUBROUTINE "CKMDDIR" BEGINS------------------------
|
|
#CKMDDIR
|
|
AT 0
|
|
AT 500
|
|
REFPOS=_TPC
|
|
AT 1000
|
|
CURPOS=_TPC
|
|
POSA=_TPC
|
|
'IF MD MOVES CLOCKWISE THEN VARIABLE "DIR" IS '1'
|
|
IF CURPOS < REFPOS
|
|
DIR=1
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF MD MOVES COUNTER CLOCKWISE THEN VARIABLE "DIR" IS '0'
|
|
IF CURPOS > REFPOS
|
|
DIR=0
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF MD STOPS THEN VARIABLE "DIR" IS '2'
|
|
IF CURPOS = REFPOS
|
|
DIR=2
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "CKMDDIR" ENDS--------------------------
|
|
'SUBROUTINE "PROCESS BEGINS------------------------
|
|
#PROCESS
|
|
'*************************************************
|
|
'IF WEDGE PATTERN (0,3) & MD MOVES CW
|
|
IF (DIR=1)&(CNTLEFT=0)&(CNTRIGHT=3)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN MD HITS LOW ANGLE LIM SWI
|
|
ULMIN=LOWMIN
|
|
ULMAX=LOWMAX
|
|
ENDHIT=127500
|
|
BGNHIT=181200
|
|
'WHEN MD HAS REACHED EXTREME RIGHT THEN STOP MD
|
|
EXTREME=1
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF WEDGE PATTERN (0,3) & MD MOVES CCW
|
|
IF (DIR=0)&(CNTLEFT=0)&(CNTRIGHT=3)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN WEDGE 1 HITS RIGHT CAM LIM SWI
|
|
ULMIN=BGCCW1
|
|
ULMAX=ENCCW1
|
|
UNLTCH1=588000
|
|
LTCH1=676000
|
|
ENDIF
|
|
'*************************************************
|
|
'IF WEDGE PATTERN (1,2) & MD MOVES CW
|
|
IF (DIR=1)&(CNTLEFT=1)&(CNTRIGHT=2)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN WEDGE 1 HITS LEFT CAM LIM SWI
|
|
ULMIN=BGCW1
|
|
ULMAX=ENCW1
|
|
ENDHIT=BGCCW1
|
|
BGNHIT=ENCCW1
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF WEDGE PATTERN (1,2) & MD MOVES CCW
|
|
IF (DIR=0)&(CNTLEFT=1)&(CNTRIGHT=2)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN WEDGE 2 HITS RIGHT CAM LIM SWI
|
|
ULMIN=BGCCW2
|
|
ULMAX=ENCCW2
|
|
UNLTCH2=1148560
|
|
LTCH2=1240700
|
|
ENDIF
|
|
'*************************************************
|
|
'IF WEDGE PATTERN (2,1) & MD MOVES CW
|
|
IF (DIR=1)&(CNTLEFT=2)&(CNTRIGHT=1)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN WEDGE 2 HITS LEFT CAM LIM SWI
|
|
ULMIN=BGCW2
|
|
ULMAX=ENCW2
|
|
ENDHIT=BGCCW2
|
|
BGNHIT=ENCCW2
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF WEDGE PATTERN (2,1) & MD MOVES CCW
|
|
IF (DIR=0)&(CNTLEFT=2)&(CNTRIGHT=1)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN WEDGE 3 HITS RIGHT CAM LIM SWI
|
|
ULMIN=BGCCW3
|
|
ULMAX=ENCCW3
|
|
UNLTCH3=1725200
|
|
LTCH3=1779100
|
|
ENDIF
|
|
'*************************************************
|
|
'IF WEDGE PATTERN (3,0) & MD MOVES CW
|
|
IF (DIR=1)&(CNTLEFT=3)&(CNTRIGHT=0)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN WEDGE 3 HITS LEFT CAM LIM SWI
|
|
ULMIN=BGCW3
|
|
ULMAX=ENCW3
|
|
ENDHIT=BGCCW3
|
|
BGNHIT=ENCCW3
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF WEDGE PATTERN (3,0) & MD MOVES CCW
|
|
IF (DIR=0)&(CNTLEFT=3)&(CNTRIGHT=0)
|
|
'UNLATCH RANGE IS AT THE LOCATION WHEN MD HITS HIGH ANGLE LIM SWI
|
|
ULMIN=HIGHMIN
|
|
ULMAX=HIGHMAX
|
|
'WHEN MD HAS REACHED EXTREME LEFT THEN STOP MD
|
|
EXTREME=1
|
|
ENDIF
|
|
EN
|
|
'*************************************************
|
|
'SUBROUTINE "PROCESS" ENDS--------------------------
|
|
'SUBROUTINE "CKLSMD" BEGINS-------------------------
|
|
#CKLSMD
|
|
'IF MD MOVES CCW===================================================
|
|
'CHECK LEFT CAM LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
'IF LEFT CAM LIM SWI CLICKS BEFORE WDG SHUFFLE
|
|
IF (DIR=0)&(@IN[1]=1)
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
MG "ERROR.MD CCW" {EB}
|
|
MG "LEFT CAM LIM SWI SHOULDN'T BE ACTIVATED BEFORE MOVING A WDG" {EB}
|
|
ENDIF
|
|
'CHECK RIGHT CAM LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
'IF RIGHT CAM LIM SWI DOESN'T CLICK & WEDGE HAS PASSED THE UNLATCHED RANGE
|
|
IF (DIR=0)&(@IN[2]=0)&(_TPC >= ULMAX)
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
MG "ERROR.MD CCW" {EB}
|
|
MG "MD PASSED UNLATCHED RANGE BUT RIGHT CAM LIM SWI ISN'T ACTIVATED" {EB}
|
|
ENDIF
|
|
'DOES IT CLICK INSIDE OR OUTSIDE THE ALLOWABLE RANGE?
|
|
IF (DIR=0)&(@IN[2]=1)&(CNTLEFT=0)&(CNTRIGHT=3)
|
|
IF ((_TPC <= 181200)&(_TPC >= 127500))|((_TPC <= ENCCW1)&(_TPC >= BGCCW1))
|
|
MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(0,3).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
IF (DIR=0)&(@IN[2]=1)&(CNTLEFT=1)&(CNTRIGHT=2)
|
|
IF ((_TPC <= ENCCW1)&(_TPC >= BGCCW1))|((_TPC <= ENCCW2)&(_TPC >= BGCCW2))
|
|
MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(1,2).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
IF (DIR=0)&(@IN[2]=1)&(CNTLEFT=2)&(CNTRIGHT=1)
|
|
IF ((_TPC <= ENCCW2)&(_TPC >= BGCCW2))|((_TPC <= ENCCW3)&(_TPC >= BGCCW3))
|
|
MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(2,1).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
IF (DIR=0)&(@IN[2]=1)&(CNTLEFT=3)&(CNTRIGHT=0)
|
|
IF ((_TPC <= ENCCW3)&(_TPC >= BGCCW3))
|
|
MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(3,0).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
'IF MD CCW AND RIGHT CAM LIM SWI IS ACTIVATED
|
|
IF (DIR=0)&(@IN[2]=1)
|
|
'DOES IT CLICK WITHIN THE UNLATCH RANGE
|
|
IF (_TPC <= ULMAX)&(_TPC >= ULMIN)
|
|
'UNLATCH WDG 1
|
|
IF (_TPC <= LTCH1)&(_TPC >= UNLTCH1)
|
|
OKFLG=1
|
|
ERRORFLG=0
|
|
MG "OK.MD CCW.WDG1 UNLATCH" {EB}
|
|
ENDIF
|
|
'UNLATCH WDG 2
|
|
IF (_TPC <= LTCH2)&(_TPC >= UNLTCH2)
|
|
OKFLG=1
|
|
ERRORFLG=0
|
|
MG "OK.MD CCW.WDG2 UNLATCH" {EB}
|
|
ENDIF
|
|
'UNLATCH WDG 3
|
|
IF (_TPC <= LTCH3)&(_TPC >= UNLTCH3)
|
|
OKFLG=1
|
|
ERRORFLG=0
|
|
MG "OK.MD CCW.WDG3 UNLATCH" {EB}
|
|
ENDIF
|
|
ENDIF
|
|
ENDIF
|
|
'=========================================================================
|
|
'MD MOVES CW
|
|
'CHECK RIGHT CAM LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
IF (DIR=1)&(@IN[2]=1)
|
|
'RIGHT CAM SWI IS SUPPOSED TO CLICK WITHIN CERTAIN RANGE BEFORE WDG SHUFFLE
|
|
IF (_TPC >= ENDHIT)&(_TPC <= BGNHIT)
|
|
MG "OK.MD CW.LATCH HITS RIGHT CAM LIM SWI" {EB}
|
|
ELSE
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
MG "ERROR.MD CW.RIGHT CAM LIM SWI NOT SUPPOSED TO CLICK" {EB}
|
|
ENDIF
|
|
ENDIF
|
|
'CHECK LEFT CAM LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
'IF MD CW & LEFT CAM LIM SWI ISN'T ACTIVATED & MD HAS PASSED THE UNLATCHED RANGE
|
|
IF (DIR=1)&(@IN[1]=0)&(_TPC <= ULMAX)
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
MG "ERROR.MAIN DRIVE CW" {EB}
|
|
MG "MD PASSED UNLATCH RANGE BUT LEFT CAM LIM SWI ISN'T ACTIVATED" {EB}
|
|
ENDIF
|
|
'IF MD CW AND LEFT CAM LIM SWI IS ACTIVATED^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
|
IF (DIR=1)&(@IN[1]=1)
|
|
'DOES IT CLICK WITHIN UNLATCH RANGE
|
|
IF (_TPC <= ULMIN)&(_TPC >= ULMAX)
|
|
IF (_TPC <= 1650000)&(_TPC >= 1630000)
|
|
OKFLG=1
|
|
ERRORFLG=0
|
|
MG "OK.MD CW.WDG3 UNLATCH" {EB}
|
|
ENDIF
|
|
IF (_TPC <= 1065000)&(_TPC >= 1045000)
|
|
OKFLG=1
|
|
ERRORFLG=0
|
|
MG "OK.MD CW.WDG2 UNLATCH" {EB}
|
|
ENDIF
|
|
IF (_TPC <= 525000)&(_TPC >= 505000)
|
|
OKFLG=1
|
|
ERRORFLG=0
|
|
MG "OK.MD CW.WDG1 UNLATCH" {EB}
|
|
ENDIF
|
|
ELSE
|
|
OKFLG=0
|
|
ERRORFLG=1
|
|
'RUNA=-1
|
|
MG "ERROR.MD CW.LEFT CAM LIM SWI CLICKS OUTSIDE UNLATCH RANGE" {EB}
|
|
ENDIF
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "CKLSMD" ENDS--------------------------
|
|
'SUBROUTINE "STOPMD" BEGINS-------------------------
|
|
#STOPMD
|
|
'IF MD MOVES CLOCKWISE++++++++++++++++++++++++++++
|
|
IF (DIR=1)
|
|
ST B
|
|
MC B
|
|
'THIS INDICATES MAIN DRIVE HAS STOP
|
|
STOP=1
|
|
MG "MD STOP" {EB}
|
|
'IF MD HAS REACHED LOW ANGLE (15 DEG) & HITS LIM SWI
|
|
IF (EXTREME=1)'-------------------------
|
|
'THEN ABORT MOTION ONLY,BUT PROG IS STILL RUNNING
|
|
AB 1
|
|
ELSE
|
|
'WAIT 0.5 SEC
|
|
WT 500
|
|
'SET LPUP FLAG
|
|
LPUPFLG=1
|
|
ENDIF
|
|
'---------------------------------------
|
|
ENDIF
|
|
'============================================
|
|
ENDIF
|
|
'+++++++++++++++++++++++++++++++++++++++++++++++++
|
|
'IF MD MOVES COUNTER CLOCKWISE++++++++++++++++++++
|
|
IF (DIR=0)
|
|
'THEN STOP MD
|
|
ST B
|
|
MC B
|
|
'THIS INDICATES MAIN DRIVE HAS STOP
|
|
STOP=1
|
|
MG "MD STOP" {EB}
|
|
'IF MD HAS REACHED HIGH ANGLE (85 DEG) & HITS LIM SWI
|
|
IF (EXTREME=1)'-------------------------
|
|
'THEN ABORT MOTION ONLY,BUT PROG IS STILL RUNNING
|
|
AB 1
|
|
ELSE
|
|
'WAIT 0.5 SEC
|
|
WT 500
|
|
'SET RPUP FLAG
|
|
RPUPFLG=1
|
|
ENDIF
|
|
'---------------------------------------
|
|
ENDIF
|
|
'============================================
|
|
ENDIF
|
|
'+++++++++++++++++++++++++++++++++++++++++++++++++
|
|
EN
|
|
'SUBROUTINE "STOPMD" ENDS---------------------------
|
|
'SUBROUTINE "ERROR" BEGINS--------------------------
|
|
#ERROR
|
|
'IF LIMIT SWITCH IS FAULTY,THEN STOP ALL AXIS
|
|
ST
|
|
EN
|
|
'^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^THREAD 2 ENDS^^^^^^^^^^^^^^^^^^^^^^
|
|
''''''''''''''''''THREAD 3 BEGINS'''''''''''''''''''''''''''''''''''''
|
|
#THREAD3
|
|
#LOOP3
|
|
'IF ARM ISN'T AT PICK UP POINT THEN MOVE ARM TO PICK UP POINT
|
|
IF (ARMRDY=0)
|
|
'MOVE ARM TO LPUP IF WEDGE HITS LEFT CAM LIM SWI
|
|
JS #LPUP,(LPUPFLG=1)
|
|
'MOVE ARM TO RPUP IF WEDGE HITS RIGHT CAM LIM SWI
|
|
JS #RPUP,(RPUPFLG=1)
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF ARM IS AT PICK UP POINT THEN MOVE ARM TO DROP OFF POINT
|
|
IF (ARMRDY=1)
|
|
'MOVE ARM TO RDOP AFTER ARM PICKS UP WEDGE AT LPUP
|
|
JS #RDOP,(LPUPFLG=1)
|
|
'MOVE ARM TO LDOP AFTER ARM PICKS UP WEDGE AT RPUP
|
|
JS #LDOP,(RPUPFLG=1)
|
|
ENDIF
|
|
JP #LOOP3
|
|
EN
|
|
'SUBROUTINE "LPUP" BEGINS-------------------------
|
|
'LEFT CAM LIMIT SWITCH IS HIT,WEDGE IS UNLATCH,MOVE ARM TO LPUP
|
|
#LPUP
|
|
MG "ARM IS MOVING TO LPUP" {EB}
|
|
ERROR=LPUP - _TPA
|
|
'IF ARM HAS REACHED LPUP
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP ARM
|
|
ST A
|
|
ELSE
|
|
'IF ARM STILL HASN'T REACHED LPUP,THEN MOVE ARM TO LPUP
|
|
IPA=(ERROR/ASCALE)
|
|
'AFTER MOTION COMPLETE,ARM HAS REACHED LPUP
|
|
MC A
|
|
'WAIT FOR 0.5 SEC
|
|
WT 500
|
|
MG "LPUP:",_TPA {EB}{F5.0}
|
|
MG "ARM ARRIVED AT LPUP" {EB}
|
|
'THIS INDICATES ARM IS READY TO PICK UP A WDG
|
|
ARMRDY=1
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "LPUP" ENDS---------------------------
|
|
'SUBROUTINE "RPUP" BEGINS-------------------------
|
|
'RIGHT CAM LIMIT SWITCH IS HIT,WEDGE IS UNLATCH,MOVE ARM TO RPUP
|
|
#RPUP
|
|
MG "ARM IS MOVING TO RPUP" {EB}
|
|
ERROR=RPUP - _TPA
|
|
'IF ARM HAS REACHED RPUP
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP ARM
|
|
ST A
|
|
ELSE
|
|
'IF ARM STILL HASN'T REACHED RPUP,THEN MOVE ARM TO RPUP
|
|
IPA=(ERROR/ASCALE)
|
|
'AFTER MOTION COMPLETE,ARM HAS REACHED RPUP
|
|
MC A
|
|
'WAIT FOR 0.5 SEC
|
|
WT 500
|
|
MG "RPUP:",_TPA {EB}{F5.0}
|
|
MG "ARM ARRIVED AT RPUP" {EB}
|
|
'THIS INDICATES ARM IS READY TO PICK UP A WDG
|
|
ARMRDY=1
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "RPUP" ENDS---------------------------
|
|
'SUBROUTINE "RDOP" BEGINS-------------------------
|
|
'ARM PICKS UP A WEDGE AT LPUP THEN MOVE ARM TO RDOP
|
|
#RDOP
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'ACTIVATE MAGNET
|
|
CB 1
|
|
MG "PICK UP A WEDGE" {EB}
|
|
MG "ARM IS MOVING A WDG TO RDOP" {EB}
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
ERROR=RDOP - _TPA
|
|
'IF ARM HAS REACHED RDOP
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP ARM
|
|
ST A
|
|
EN
|
|
ELSE
|
|
'IF ARM HASN'T REACHED RDOP,MOVE ARM TO RDOP
|
|
IPA=(ERROR/ASCALE)
|
|
'WDG HAS REACHED RDOP
|
|
MC A
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'IF RIGHT CAM LIM SWI DOESN'T CLICK AFTER ARM PASSED THE SWITCH
|
|
IF (WDGCW=0)
|
|
ST A
|
|
MC A
|
|
CNTLEFT=0
|
|
CNTRIGHT=0
|
|
RECOVER=1
|
|
'COUNTING UP WEDGE LOST
|
|
COUNTER=COUNTER+1
|
|
SB 1
|
|
MG "FAILED TO DETECT WEDGE MOVEMENT TO RDOP" {EB}
|
|
MG "CHECK RIGHT CAM LIMIT SWITCH AND RESOLVER" {EB}
|
|
MG "LEFT WEDGES:",CNTLEFT {EB}{F1.0}
|
|
MG "RIGHT WEDGES:",CNTRIGHT {EB}{F1.0}
|
|
MG "RECOVERY ROUTINE: ",COUNTER {EB}{F1.0}
|
|
MG "----------------------------------------"{EB}
|
|
ELSE
|
|
'RESET WEDGE LOST COUNTER
|
|
COUNTER=0
|
|
'DEACTIVATE MAGNET, PROG STOP GOING TO CKPULSE SUBROUTINE
|
|
SB 1
|
|
MG "ARM ARRIVED AT RDOP" {EB}
|
|
MG "DROP OFF A WEDGE" {EB}
|
|
MG "RDOP:",_TPA {EB}{F5.0}
|
|
MG "---------------------------------" {EB}
|
|
ENDIF
|
|
'THIS ALLOWS PROG TO CHECK CAM LIM SWI BEFORE MOVING A WDG
|
|
OKFLG=0
|
|
'THIS ALLOWS MD TO MOVE AGAIN AFTER WEDGE HAS BEEN MOVED
|
|
STOP=0
|
|
'THIS ALLOWS PROG TO GO TO SUB "LPUP"
|
|
LPUPFLG=0
|
|
'THIS ALLOWS ARM TO GO TO PICK UP POINT
|
|
ARMRDY=0
|
|
'THIS ALLOWS WDG MOVEMENT DETECTION TO THE RIGHT SIDE
|
|
WDGCW=0
|
|
'THIS ALLOWS COUNTER TO COUNT IF RLS CLICKS
|
|
CNTSTOP=0
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "RDOP" ENDS---------------------------
|
|
'SUBROUTINE "LDOP" BEGINS-------------------------
|
|
'ARM PICKS UP A WEDGE AT RPUP THEN MOVE ARM TO LDOP
|
|
#LDOP
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'THEN ACTIVATE MAGNET
|
|
CB 1
|
|
MG "PICK UP A WEDGE" {EB}
|
|
MG "ARM IS MOVING A WDG TO LDOP" {EB}
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
ERROR=LDOP - _TPA
|
|
'IF ARM HAS REACHED LDOP
|
|
IF @ABS[ERROR]<DEADBAND
|
|
'STOP ARM
|
|
ST A
|
|
EN
|
|
ELSE
|
|
'IF ARM HASN'T REACHED LDOP,MOVE ARM TO LDOP
|
|
IPA=(ERROR/ASCALE)
|
|
'WDG HAS REACHED LDOP
|
|
MC A
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'IF LEFT CAM LIM SWI DOESN'T CLICK AFTER ARM HAS PASSED THE SWITCH
|
|
IF (WDGCCW=0)
|
|
ST A
|
|
MC A
|
|
CNTLEFT=0
|
|
CNTRIGHT=0
|
|
RECOVER=1
|
|
'COUNTING UP WEDGE LOST
|
|
COUNTER=COUNTER+1
|
|
SB 1
|
|
MG "FAILED TO DETECT WEDGE MOVEMENT TO LDOP"{EB}
|
|
MG "CHECK LEFT CAM LIMIT SWITCH AND RESOLVER"{EB}
|
|
MG "LEFT WEDGES:",CNTLEFT {EB}{F1.0}
|
|
MG "RIGHT WEDGES:",CNTRIGHT {EB}{F1.0}
|
|
MG "RECOVERY ROUTINE: ",COUNTER {EB}{F1.0}
|
|
MG "----------------------------------------" {EB}
|
|
ELSE
|
|
'RESET WEDGE LOST COUNTER
|
|
COUNTER=0
|
|
'DEACTIVATE MAGNET
|
|
SB 1
|
|
MG "ARM ARRIVED AT LDOP" {EB}
|
|
MG "DROP OFF A WEDGE" {EB}
|
|
MG "LDOP:",_TPA {EB}{F5.0}
|
|
MG "--------------------------------" {EB}
|
|
ENDIF
|
|
'THIS ALLOWS PROG TO CHECK CAM LIM SWI BEFORE MOVING A WDG
|
|
OKFLG=0
|
|
'THIS ALLOWS MD TO MOVE AGAIN AFTER WEDGE HAS BEEN MOVED
|
|
STOP=0
|
|
'THIS ALLOWS PROG TO GO TO SUB "RPUP"
|
|
RPUPFLG=0
|
|
'THIS ALLOWS ARM TO GO TO PICK UP POINT
|
|
ARMRDY=0
|
|
'THIS ALLOWS WDG MOVEMENT DETECTION TO THE LEFT SIDE
|
|
WDGCCW=0
|
|
'THIS ALLOWS COUNTER TO COUNT IF LLS CLICKS
|
|
CNTSTOP=0
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "LDOP" ENDS---------------------------
|
|
''''''''''''''''''THREAD 3 ENDS'''''''''''''''''''''''''''''''''''''
|
|
''''''''''''''''''THREAD 4 BEGINS'''''''''''''''''''''''''''''''''''''
|
|
#THREAD4
|
|
#LOOP4
|
|
'IF MAGNET IS ACTIVATED THEN CHECK CAM LIM SWI PULSE WHEN ARM BRINGING A WEDGE
|
|
JS #CKPULSE,(@OUT[1]=0)
|
|
'IF MAGNET IS ACTIVATED & COUNTER HASN'T ACTIVATED
|
|
IF (@OUT[1]=0)&(CNTSTOP=0)
|
|
'THEN COUNTER COUNTS UP OR DOWN,DECIDES WEDGE PATTERN,DEACTIVATES COUNTER
|
|
JS #COUNTER
|
|
ENDIF
|
|
JP #LOOP4
|
|
EN
|
|
'SUBROUTINE "CKPULSE" BEGINS----------------------
|
|
#CKPULSE
|
|
'IF ARM IS BRINGING A WEDGE TO LDOP+++++++++++++++
|
|
IF (RPUPFLG=1)&(@IN[1]=1)
|
|
'IF CHECK PULSE WHEN ARM IS AT LEFT CAM LIM SWI
|
|
IF (_TPA >= 14400)&(_TPA <= 14650)
|
|
'CAM LEFT LIM SWI DETECTS WDG MOVEMENT TO THE LEFT SIDE
|
|
WDGCCW=1
|
|
ELSE
|
|
ST A
|
|
MC A
|
|
MG "ERROR.LEFT CAM LIMIT ACTIVATES OUTSIDE RANGE" {EB}
|
|
WDGCCW=0
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
'======================================================================
|
|
'IF ARM IS BRINGING A WEDGE TO RDOP+++++++++++++++
|
|
IF (LPUPFLG=1)&(@IN[2]=1)
|
|
'CHECK RIGHT CAM SWI PULSE AT RIGHT SWI LOCATION
|
|
IF (_TPA >= 16300)&(_TPA <= 16550)
|
|
'RIGHT CAM LIM SWI DETECTS WDG MOVEMENT TO THE RIGHT SIDE
|
|
WDGCW=1
|
|
ELSE
|
|
ST A
|
|
MC A
|
|
MG "ERROR.RIGHT CAM LIMIT ACTIVATES OUTSIDE RANGE" {EB}
|
|
WDGCW=0
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
'CHECK LEFT CAM LIM SWI AFTER PICK UP A WDG
|
|
IF (LPUPFLG=1)&(@IN[1]=1)
|
|
IF (_TPA >= 14650)
|
|
ST A
|
|
MC A
|
|
MG "ERROR.LEFT CAM LIMIT NOT SUPPOSED TO CLICK AFTER ARM PICKS UP A WDG" {EB}
|
|
WDGCW=0
|
|
'RUNA=-1
|
|
ENDIF
|
|
ENDIF
|
|
'+++++++++++++++++++++++++++++++++++++++++++++++++
|
|
EN
|
|
'SUBROUTINE "CKPULSE" ENDS------------------------
|
|
'SUBROUTINE "COUNTER" BEGINS----------------------
|
|
'THIS SUBROUTINE WORKS ONLY ONE CYCLE
|
|
#COUNTER
|
|
'IF LEFT CAM LIM SWI IS HIT WHILE ARM BRINGING A WEDGE TO LDOP
|
|
IF (WDGCCW=1)
|
|
'ADD 1 TO COUNTER LEFT VARIABLE
|
|
CNTLEFT=CNTLEFT+1
|
|
'MINUS 1 FROM COUNTER RIGHT VARIABLE
|
|
CNTRIGHT=CNTRIGHT-1
|
|
'DEACTIVATE COUNTER
|
|
CNTSTOP=1
|
|
MG "LEFT WEDGES:",CNTLEFT {EB}{F1.0}
|
|
MG "RIGHT WEDGES:",CNTRIGHT {EB}{F1.0}
|
|
ENDIF
|
|
'-------------------------------------------------
|
|
'IF RIGHT CAM LIM SWI IS HIT WHILE ARM BRINGING A WEDGE TO RDOP
|
|
IF (WDGCW=1)
|
|
'MINUS 1 FROM COUNTER LEFT VARIABLE
|
|
CNTLEFT=CNTLEFT-1
|
|
'ADD 1 TO COUNTER RIGHT VARIABLE
|
|
CNTRIGHT=CNTRIGHT+1
|
|
'DEACTIVATE COUNTER
|
|
CNTSTOP=1
|
|
MG "LEFT WEDGES:",CNTLEFT {EB}{F1.0}
|
|
MG "RIGHT WEDGES:",CNTRIGHT {EB}{F1.0}
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "COUNTER" ENDS------------------------
|
|
'''''''''''''''''''THREAD 4 ENDS''''''''''''''''''''''''''''''''''''''
|
|
'################THREAD 5 BEGINS#######################################'
|
|
#THREAD5
|
|
#LOOP5
|
|
JS #ENCODCK,(FBERROR=0)
|
|
JP #LOOP5
|
|
EN
|
|
'SUBROUTINE "ENCODCK" BEGINS----------------------
|
|
#ENCODCK
|
|
'IF SSI ENCODER OUTSIDE THE ACCEPTABLE RANGE
|
|
IF (_TPC <= HIGHMAX)&(_TPC >= LOWMAX)
|
|
FBERROR=0
|
|
ELSE
|
|
FBERROR=1
|
|
'ABORT ALL MOTION,BUT PROGRAM STILL RUNNING
|
|
AB 1
|
|
MC
|
|
MG "ENCODER VALUE IS OUTSIDE RANGE=", _TPC {EB}
|
|
MG "15 DEG = 139175" {EB}
|
|
MG "85 DEG = 2775000" {EB}
|
|
MG "CHECK SSI ENCODER CONNECTION" {EB}
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "ENCODECK" ENDS------------------------
|
|
'------------LIMIT SWITCH SUBROUTINE BEGINS---------------------------
|
|
'THIS SUBROUTINE IS EXECUTED WHENEVER A LIMIT SWITCH IS DEPRESSED
|
|
#LIMSWI
|
|
IF _LFA=0
|
|
MG "RIGHT LIMIT SWITCH ARM IS ACTIVATED" {EB}
|
|
ENDIF
|
|
IF _LRA=0
|
|
MG "LEFT LIMIT SWITCH ARM IS ACTIVATED" {EB}
|
|
ENDIF
|
|
IF _LFB=0
|
|
MG "LOW ANGLE LIMIT SWITCH MAIN DRIVE IS ACTIVATED" {EB}
|
|
ENDIF
|
|
IF _LRB=0
|
|
MG "HIGH ANGLE LIMIT SWITCH MAIN DRIVE IS ACTIVATED" {EB}
|
|
ENDIF
|
|
ST
|
|
RE |