ed1258ba5d
First is a WIP version of Burhan's code from 2007-07-13. r3273 | dcl | 2011-11-24 09:44:02 +1100 (Thu, 24 Nov 2011) | 3 lines
809 lines
22 KiB
Plaintext
Executable File
809 lines
22 KiB
Plaintext
Executable File
'MD=MAIN DRIVE, (3,0)=1800000, (0,3)=160000
|
|
'RIGHT LIM SWI AT 16000
|
|
'-----THREAD 0 BEGINS-------------------------------------------------
|
|
#MAIN
|
|
'-----------INITIALISATION STARTS---------------------------
|
|
SIA=1,25,12,1<-1>2
|
|
SIC=1,25,13,0<-2>1
|
|
REFPOS=1400000'REFERENCE POSITION TO DETERMINE CW OR CCW
|
|
DESTPOS=1000000'MAIN DRIVE FINAL DESTINATION(SSI VALUE)
|
|
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=576000'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=1846000'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
|
|
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
|
|
CNTLEFT=0'NUMBER OF WEDGES ON THE LEFT SIDE
|
|
CNTRIGHT=0'NUMBER OF WEDGES ON THE RIGHT SIDE
|
|
CNTSTOP=0'THIS FLAG MAKES COUNTER TO COUNT ONLY ONCE
|
|
BSCALE=8192/25000
|
|
ASCALE=13.732/25000
|
|
DEADBAND=10
|
|
ERROR=0
|
|
DIR=2'THIS MEANS MD IS STOP AT THE BEGINNING
|
|
STOP=0'THIS FLAG GOES TO '1' IF MD STOPS
|
|
DONE=0'THIS FLAG TELLS SICS THAT MAIN DRIVE HAS COMPLETED ITS MOVE
|
|
ARMRDY=0'THIS FLAG GOES TO '1' IF ARM IS READY AT PICK UP POINT
|
|
OKFLG=0
|
|
WDGCCW=0
|
|
WDGCW=0
|
|
LPUPFLG=0
|
|
RPUPFLG=0
|
|
ACA=1500000
|
|
DCA=1500000
|
|
SPA=150000
|
|
ACB=25000
|
|
DCB=25000
|
|
SPB=25000
|
|
SB 8
|
|
WT 500'WAIT FOR 0.5 SEC
|
|
'---------INITIALISATION ENDS-------------------------------
|
|
IF (CNTLEFT=0)&(CNTRIGHT=0)
|
|
JS #WDG1CW
|
|
JS #RECLPUP
|
|
JS #RECRDOP
|
|
ENDIF
|
|
'EXECUTE THREAD 1---------------------------------
|
|
XQ #THREAD1,1
|
|
'EXECUTE THREAD 2---------------------------------
|
|
XQ #THREAD2,2
|
|
'EXECUTE THREAD 3---------------------------------
|
|
XQ #THREAD3,3
|
|
'EXECUTE THREAD 4---------------------------------
|
|
XQ #THREAD4,4
|
|
EN
|
|
'-----THREAD 0 ENDS-------------------------------------------------
|
|
'---------------------THREAD 1 BEGINS---------------------------------
|
|
#THREAD1
|
|
#LOOP1
|
|
'IF WEDGE NOT BLOCKING BEAM & FINAL DEST HASN'T REACHED THEN MOVE MD
|
|
IF (STOP=0)&(DONE=0)
|
|
'MOVE MAIN DRIVE TO FINAL DEST
|
|
JS #MOVEMD
|
|
ENDIF
|
|
JP #LOOP1
|
|
EN
|
|
'SUBROUTINE "MOVEMD" BEGINS-------------------------
|
|
#MOVEMD
|
|
ERROR=_TPC - DESTPOS
|
|
'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
|
|
MG "WEDGE SHUFFLE ROUTINE HAS BEEN COMPLETED" {EB}
|
|
AB
|
|
ELSE
|
|
'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,RDOP,LDOP
|
|
JS #PROCESS
|
|
'CKECK LEFT & RIGHT SWITCH BEFORE MOVING WEDGE
|
|
JS #CKLSMD,(STOP=0)
|
|
'STOP MD WHEN MD IS WITHIN UNLATCH RANGE
|
|
JS #STOPMD,(OKFLG=1)
|
|
JP #LOOP2
|
|
EN
|
|
'SUBROUTINE "CKMDDIR" BEGINS------------------------
|
|
#CKMDDIR
|
|
AT 0
|
|
AT 500
|
|
REFPOS=_TPC
|
|
AT 1000
|
|
CURPOS=_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 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 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 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 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 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 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 LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
'IF LEFT LIM SWI IS CLICKED BEFORE WDG SHUFFLE
|
|
IF (DIR=0)&(@IN[1]=1)
|
|
ST B
|
|
MC B
|
|
OKFLG=0
|
|
MG "ERROR.MD CCW" {EB}
|
|
MG "LEFT SWI SHOULDN'T BE ACTIVATED BEFORE WDG SHUFFLE" {EB}
|
|
AB
|
|
ENDIF
|
|
'CHECK RIGHT LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
'IF RIGHT LIM SWI ISN'T CLICKED & MD HAS PASSED THE UNLATCHED RANGE
|
|
IF (DIR=0)&(@IN[2]=0)&(_TPC >= ULMAX)
|
|
ST B
|
|
MC B
|
|
OKFLG=0
|
|
MG "ERROR.MD CCW" {EB}
|
|
MG "MD PASSED UNLATCHED RANGE BUT RIGHT SWI ISN'T ACTIVATED" {EB}
|
|
AB
|
|
ENDIF
|
|
'IF RIGHT LIM SWI IS CLICKED
|
|
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 LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(0,3).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
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 LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(1,2).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
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 LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(2,1).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
ENDIF
|
|
ENDIF
|
|
IF (DIR=0)&(@IN[2]=1)&(CNTLEFT=3)&(CNTRIGHT=0)
|
|
IF ((_TPC <= ENCCW3)&(_TPC >= BGCCW3))
|
|
MG "OK.MD CCW.LATCH HITS RIGHT LIM SWI" {EB}
|
|
ELSE
|
|
MG "ERROR.MD CCW.(3,0).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB}
|
|
ENDIF
|
|
ENDIF
|
|
IF (DIR=0)&(@IN[2]=1)
|
|
IF (_TPC <= ULMAX)&(_TPC >= ULMIN)
|
|
'UNLATCH WDG 1
|
|
IF (_TPC <= LTCH1)&(_TPC >= UNLTCH1)
|
|
OKFLG=1
|
|
MG "OK.MD CCW.WDG1 UNLATCH" {EB}
|
|
ENDIF
|
|
'UNLATCH WDG 2
|
|
IF (_TPC <= LTCH2)&(_TPC >= UNLTCH2)
|
|
OKFLG=1
|
|
MG "OK.MD CCW.WDG2 UNLATCH" {EB}
|
|
ENDIF
|
|
'UNLATCH WDG 3
|
|
IF (_TPC <= LTCH3)&(_TPC >= UNLTCH3)
|
|
OKFLG=1
|
|
MG "OK.MD CCW.WDG3 UNLATCH" {EB}
|
|
ENDIF
|
|
ENDIF
|
|
ENDIF
|
|
'=========================================================================
|
|
'MD MOVES CW
|
|
'CHECK RIGHT LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
IF (DIR=1)&(@IN[2]=1)
|
|
'RIGHT SWI IS SUPPOSED TO CLICK AT CERTAIN LOCATION BEFORE WDG SHUFFLE
|
|
IF (_TPC >= ENDHIT)&(_TPC <= BGNHIT)
|
|
MG "OK.MD CW.LATCH HITS RIGHT LIM SWI" {EB}
|
|
ELSE
|
|
ST B
|
|
MC B
|
|
OKFLG=0
|
|
MG "ERROR.MD CW.RIGHT SWI NOT SUPPOSED TO CLICK" {EB}
|
|
AB
|
|
ENDIF
|
|
ENDIF
|
|
'CHECK LEFT LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
'IF MD CW,LEFT LIM SWI ISN'T CLICKED,MD HAS PASSED UNLATCHED RANGE
|
|
IF (DIR=1)&(@IN[1]=0)&(_TPC <= ULMAX)
|
|
ST B
|
|
MC B
|
|
OKFLG=0
|
|
MG "ERROR.MAIN DRIVE CW" {EB}
|
|
MG "MD PASSED UNLATCH RANGE BUT LEFT SWI ISN'T ACTIVATED" {EB}
|
|
AB
|
|
ENDIF
|
|
'IF LEFT LIM SWI IS CLICKED WHEN MD CW^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
|
IF (DIR=1)&(@IN[1]=1)
|
|
'IF LEFT LIM SWI CLICKS AT UNLATCH RANGE
|
|
IF (_TPC <= ULMIN)&(_TPC >= ULMAX)
|
|
IF (_TPC <= 1650000)&(_TPC >= 1630000)
|
|
OKFLG=1
|
|
MG "OK.MD CW.WDG3 UNLATCH" {EB}
|
|
ENDIF
|
|
IF (_TPC <= 1065000)&(_TPC >= 1045000)
|
|
OKFLG=1
|
|
MG "OK.MD CW.WDG2 UNLATCH" {EB}
|
|
ENDIF
|
|
IF (_TPC <= 525000)&(_TPC >= 505000)
|
|
OKFLG=1
|
|
MG "OK.MD CW.WDG1 UNLATCH" {EB}
|
|
ENDIF
|
|
ELSE
|
|
OKFLG=0
|
|
MG "ERROR.MD CW.LEFT 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 "STOP=1" {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 "STOP=1" {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---------------------------
|
|
'^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^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 LIM SWI
|
|
JS #LPUP,(LPUPFLG=1)
|
|
'MOVE ARM TO RPUP IF WEDGE HITS RIGHT 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 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}
|
|
'THIS INDICATES ARM IS READY TO PICK UP A WDG
|
|
ARMRDY=1
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "LPUP" ENDS---------------------------
|
|
'SUBROUTINE "RPUP" BEGINS-------------------------
|
|
'RIGHT 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}
|
|
'THIS INDICATES ARM IS READY TO PICK UP A WDG
|
|
ARMRDY=1
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "RPUP" ENDS---------------------------
|
|
'SUBROUTINE "LDOP" BEGINS-------------------------
|
|
'ARM PICKS UP A WEDGE AT RPUP THEN MOVE ARM TO LDOP
|
|
#LDOP
|
|
MG "ARM ARRIVED AT RPUP" {EB}
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'THEN ACTIVATE MAGNET
|
|
CB 8
|
|
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
|
|
'DEACTIVATE MAGNET
|
|
SB 8
|
|
MG "ARM ARRIVED AT LDOP" {EB}
|
|
MG "DROP OFF A WEDGE" {EB}
|
|
MG "LDOP:",_TPA {EB}{F5.0}
|
|
MG "--------------------------------" {EB}
|
|
'THIS ALLOWS PROG TO CHECK 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---------------------------
|
|
'SUBROUTINE "RDOP" BEGINS-------------------------
|
|
'ARM PICKS UP A WEDGE AT LPUP THEN MOVE ARM TO RDOP
|
|
#RDOP
|
|
MG "ARM ARRIVED AT LPUP" {EB}
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'ACTIVATE MAGNET
|
|
CB 8
|
|
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
|
|
'PROG STOP GOING TO SUB "RDOP"
|
|
LPUPFLG=0
|
|
'DEACTIVATE MAGNET, PROG STOP GOING TO CKPULSE SUBROUTINE
|
|
SB 8
|
|
MG "ARM ARRIVED AT RDOP" {EB}
|
|
MG "DROP OFF A WEDGE" {EB}
|
|
MG "RDOP:",_TPA {EB}{F5.0}
|
|
MG "---------------------------------" {EB}
|
|
'THIS ALLOWS PROG TO CHECK 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---------------------------
|
|
''''''''''''''''''THREAD 3 ENDS'''''''''''''''''''''''''''''''''''''
|
|
''''''''''''''''''THREAD 4 BEGINS'''''''''''''''''''''''''''''''''''''
|
|
#THREAD4
|
|
#LOOP4
|
|
'IF MAGNET IS ACTIVATED THEN CHECK LIM SWI PULSE WHEN ARM BRINGING A WEDGE
|
|
JS #CKPULSE,(@OUT[8]=0)
|
|
'IF MAGNET IS ACTIVATED & COUNTER HASN'T ACTIVATED
|
|
IF (@OUT[8]=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 LIM SWI
|
|
IF (_TPA >= 14500)&(_TPA <= 14600)
|
|
'LEFT LIM SWI DETECTS WDG MOVEMENT TO THE LEFT SIDE
|
|
WDGCCW=1
|
|
ENDIF
|
|
'CHECK LEFT SWI PULSE BETWEEN RPUP & LEFT SWI
|
|
IF (_TPA >= 14650)&(_TPA <= RPUP)
|
|
MG "ERROR.LEFT LIM ACTIVATES BEFORE LATCH HITS CAM" {EB}
|
|
WDGCCW=0
|
|
AB
|
|
ENDIF
|
|
ENDIF
|
|
'IF LEFT LIM SWI DOESN'T CLICK AFTER ARM PASSED THE LEFT SWI
|
|
IF (RPUPFLG=1)&(_TPA <= 14450)
|
|
IF (WDGCCW=0)
|
|
MG "FAILED TO DETECT WEDGE MOVEMENT TO LDOP"{EB}
|
|
MG "CHECK LEFT LIMIT SWITCH AND RESOLVER"{EB}
|
|
MG "LEFT WEDGES:",CNTLEFT {EB}{F1.0}
|
|
MG "RIGHT WEDGES:",CNTRIGHT {EB}{F1.0}
|
|
MG "---------------------------------" {EB}
|
|
AB
|
|
ENDIF
|
|
ENDIF
|
|
'======================================================================
|
|
'IF ARM IS BRINGING A WEDGE TO RDOP+++++++++++++++
|
|
IF (LPUPFLG=1)&(@IN[2]=1)
|
|
'CHECK RIGHT SWI PULSE AT RIGHT SWI LOCATION
|
|
IF (_TPA >= 16360)&(_TPA <= 16390)
|
|
'RIGHT LIM SWI DETECTS WDG MOVEMENT TO THE RIGHT SIDE
|
|
WDGCW=1
|
|
ENDIF
|
|
'CHECK RIGHT SWI PULSE BETWEEN LPUP & RIGHT SWITCH
|
|
IF (_TPA >= LPUP)&(_TPA <= 16260)
|
|
MG "ERROR.RIGHT LIMIT ACTIVATES BEFORE ARM PASSES RIGHT SWI" {EB}
|
|
WDGCW=0
|
|
AB
|
|
ENDIF
|
|
ENDIF
|
|
'IF RIGHT LIM SWI DOESN'T CLICK AFTER ARM PASSED THE RIGHT SWI
|
|
IF (LPUPFLG=1)&(_TPA >= 16400)
|
|
IF (WDGCW=0)
|
|
MG "FAILED TO DETECT WEDGE MOVEMENT TO RDOP" {EB}
|
|
MG "CHECK RIGHT LIMIT SWITCH AND RESOLVER" {EB}
|
|
MG "LEFT WEDGES:",CNTLEFT {EB}{F1.0}
|
|
MG "RIGHT WEDGES:",CNTRIGHT {EB}{F1.0}
|
|
MG "---------------------------------"{EB}
|
|
AB
|
|
ENDIF
|
|
ENDIF
|
|
'+++++++++++++++++++++++++++++++++++++++++++++++++
|
|
EN
|
|
'SUBROUTINE "CKPULSE" ENDS------------------------
|
|
'SUBROUTINE "COUNTER" BEGINS----------------------
|
|
'THIS SUBROUTINE WORKS ONLY ONE CYCLE
|
|
#COUNTER
|
|
'IF LEFT 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 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
|
|
'CHECKING MAIN DRIVE ENCODER
|
|
'JS #ENCHECK
|
|
'JS #RECHECK
|
|
'JP #LOOP5
|
|
'EN
|
|
''SUBROUTINE "ENCHECK" BEGINS----------------------
|
|
'#ENCHECK
|
|
'IF SSI ENCODER BEYOND THE ACCEPTABLE RANGE
|
|
'IF (_TPB >= HIGHMAX) | (_TPB <= LOWMAX)
|
|
'MG "ENCODER VALUE IS OUTSIDE RANGE", _TPB {EA}
|
|
'MG "15 DEG = 139175" {EA}
|
|
'MG "85 DEG = 2775000" {EA}
|
|
'MG "CHECK SSI ENCODER CONNECTION" {EA}
|
|
'AB
|
|
'ENDIF
|
|
'IF SSI VALUE DOESN'T CHANGE BUT MD IS IN MOTION
|
|
'IF (_BGB=1)&(DIR=2)
|
|
'MG "MD IN MOTION,BUT SSI VALUE ISN'T CHANGING" {EA}
|
|
'MG "CHECK SSI ENCODER CONNECTION" {EA}
|
|
'AB
|
|
'ENDIF
|
|
'ENDIF
|
|
'EN
|
|
'SUBROUTINE "ENCHECK" ENDS------------------------
|
|
'SUBROUTINE "RECHECK" BEGINS----------------------
|
|
'#RECHECK
|
|
'IF ARM RESOLVER BEYOND THE ACCEPTABLE RANGE
|
|
'IF (_TPA >=17455) | (_TPA <=13455)
|
|
'MG "RESOLVER VALUE IS OUTSIDE RANGE", _TPA {EA}
|
|
'MG "CW EXTREME = 17455" {EA}
|
|
'MG "CCW EXTREME = 13455" {EA}
|
|
'MG "CHECK RESOLVER CONNECTION" {EA}
|
|
'AB
|
|
'ENDIF
|
|
'IF RESOLVER VALUE DOESN'T CHANGE BUT ARM STILL ROTATING
|
|
'IF (_BGA=1)
|
|
'AT 0
|
|
'AT 1000
|
|
'ARMREF=_TPA
|
|
'AT 2000
|
|
'ARMCUR=_TPA
|
|
'IF (ARMREF=ARMCUR)
|
|
'MG "RESOLVER VALUE ISN'T CHANGING" {EA}
|
|
'MG "CHECK RESOLVER CONNECTION" {EA}
|
|
'AB
|
|
'ENDIF
|
|
'ENDIF
|
|
'EN
|
|
'SUBROUTINE "ENCHECK" ENDS------------------------
|
|
'################THREAD 5 ENDS#######################################'
|
|
'---RECOVERY ROUTINE------------------------
|
|
'SUBROUTINE "WDG3CCW" BEGINS-------------------------
|
|
#WDG1CW
|
|
MG "RECOVERY PROCEDURE.MD CW TO 27 DEG UNTIL WDG1 UNLATCHES" {EB}
|
|
ERROR=_TPC - 525000
|
|
'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
|
|
AB
|
|
ELSE
|
|
'IF MD HASN'T REACHED FINAL DESTINATION THEN MOVE MD
|
|
IPB=(ERROR/BSCALE)
|
|
MC B
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "WDG3CCW" ENDS---------------------------
|
|
'SUBROUTINE "RECRPUP" BEGINS-------------------------
|
|
'WEDGE IS UNLATCH,ARM PICKS UP WDG 1 FROM LPUP
|
|
#RECLPUP
|
|
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 RPUP
|
|
MC A
|
|
'WAIT FOR 0.5 SEC
|
|
WT 500
|
|
MG "LPUP:",_TPA {EB}{F5.0}
|
|
'THIS INDICATES ARM IS READY TO PICK UP A WDG
|
|
ARMRDY=1
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "RECLPUP" ENDS-------------------------
|
|
'-ARM GRABS WDG 1 AND PUSH ALL WEDGES TO THE RIGHT SIDE,
|
|
'ARM STOP AT RDOP.THE PATTERN MUST BE (0,3)
|
|
'SUBROUTINE "RECRDOP" BEGINS-------------------------
|
|
#RECRDOP
|
|
MG "ARM ARRIVED AT LPUP" {EB}
|
|
'WAIT 1 SEC
|
|
WT 1000
|
|
'THEN ACTIVATE MAGNET
|
|
CB 8
|
|
MG "PICK UP WEDGE 1" {EB}
|
|
MG "ARM IS MOVING WDG1 (POSSIBLY PUSHING OTHER 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
|
|
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
|
|
'DEACTIVATE MAGNET
|
|
SB 8
|
|
MG "ARM ARRIVED AT RDOP" {EB}
|
|
MG "DROP OFF WEDGE 1 " {EB}
|
|
MG "RDOP:",_TPA {EB}{F5.0}
|
|
MG "PATTERN (0,3).MD CCW.START SHUFFLE ROUTINE UNTIL PATTERN (3,0)" {EB}
|
|
'THIS ALLOWS MD TO MOVE AGAIN AFTER WEDGE HAS BEEN MOVED
|
|
STOP=0
|
|
'THIS INDICATES ARM ISN'T READY TO PICK UP ANY WDG
|
|
ARMRDY=0
|
|
'THIS ALLOWS COUNTER TO COUNT WHEN NEXT RLS OR LLS CLICKS
|
|
'COUNTER IS ACTIVATED
|
|
CNTSTOP=0
|
|
'ALL 3 WEDGES ON LEFT SIDE,PATTERN (3,0)
|
|
CNTLEFT=0
|
|
CNTRIGHT=3
|
|
'MOVE ALL 3 WEDGES TO RIGHT SIDE (0,3)
|
|
DESTPOS=2000000
|
|
WT 1000
|
|
ENDIF
|
|
EN
|
|
'SUBROUTINE "RECLDOP" 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
|
|
AB
|
|
RE
|
|
'------------LIMIT SWITCH SUBROUTINE ENDS----------------------------- |