From 66064554a8ba1360613074402f7d3eb8ccb881c9 Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Thu, 24 Nov 2011 10:01:02 +1100 Subject: [PATCH] Burhan's WIP version 6.0 from 2007-10-04 r3274 | dcl | 2011-11-24 10:01:02 +1100 (Thu, 24 Nov 2011) | 1 line --- .../instrument/tas/DMC2280/controller1.txt | 681 ++++++++++-------- 1 file changed, 370 insertions(+), 311 deletions(-) diff --git a/site_ansto/instrument/tas/DMC2280/controller1.txt b/site_ansto/instrument/tas/DMC2280/controller1.txt index 4bd054b0..121ba6f8 100755 --- a/site_ansto/instrument/tas/DMC2280/controller1.txt +++ b/site_ansto/instrument/tas/DMC2280/controller1.txt @@ -1,24 +1,24 @@ -'MD=MAIN DRIVE, (3,0)=1800000, (0,3)=160000 +'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 -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) +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=1846000'CCW WDG 3 UNLATCH MAX(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) @@ -31,41 +31,46 @@ 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 -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 +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 -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 +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 8 +SB 1 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--------------------------------- @@ -74,21 +79,148 @@ XQ #THREAD2,2 XQ #THREAD3,3 'EXECUTE THREAD 4--------------------------------- XQ #THREAD4,4 +'EXECUTE THREAD 5--------------------------------- +XQ #THREAD5,5 +#LOOP +'SICS asks for a request before moving axes A and B +'IF (REQA=1)&(REQB=1) +'controller checks if motor can be driven and wedges can be moved +'if motor can be driven and everything is ok +'RSPA=1 +'RSPB=1 +'else(if motor fault) +'RSPA=-1 +'RSPB=-1 +'endif +'endif +JP #LOOP 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 +'if everything is ok +IF (RSPA=1)&(RSPB=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)&(RUNB=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]= ULMAX) -ST B -MC B OKFLG=0 +ERRORFLG=1 MG "ERROR.MD CCW" {EB} -MG "MD PASSED UNLATCHED RANGE BUT RIGHT SWI ISN'T ACTIVATED" {EB} -AB +MG "MD PASSED UNLATCHED RANGE BUT RIGHT CAM LIM SWI ISN'T ACTIVATED" {EB} ENDIF -'IF RIGHT LIM SWI IS CLICKED +'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 LIM SWI" {EB} +MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB} ELSE -MG "ERROR.MD CCW.(0,3).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB} +MG "ERROR.MD CCW.(0,3).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} +OKFLG=0 +ERRORFLG=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 LIM SWI" {EB} +MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB} ELSE -MG "ERROR.MD CCW.(1,2).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB} +MG "ERROR.MD CCW.(1,2).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} +OKFLG=0 +ERRORFLG=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 LIM SWI" {EB} +MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB} ELSE -MG "ERROR.MD CCW.(2,1).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB} +MG "ERROR.MD CCW.(2,1).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} +OKFLG=0 +ERRORFLG=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 LIM SWI" {EB} +MG "OK.MD CCW.LATCH HITS RIGHT CAM LIM SWI" {EB} ELSE -MG "ERROR.MD CCW.(3,0).RIGHT SWI ISN'T SUPPOSED TO CLICK" {EB} +MG "ERROR.MD CCW.(3,0).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} +OKFLG=0 +ERRORFLG=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 LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ +'CHECK RIGHT CAM LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ IF (DIR=1)&(@IN[2]=1) -'RIGHT SWI IS SUPPOSED TO CLICK AT CERTAIN LOCATION BEFORE WDG SHUFFLE +'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 LIM SWI" {EB} +MG "OK.MD CW.LATCH HITS RIGHT CAM LIM SWI" {EB} ELSE -ST B -MC B OKFLG=0 -MG "ERROR.MD CW.RIGHT SWI NOT SUPPOSED TO CLICK" {EB} -AB +ERRORFLG=1 +MG "ERROR.MD CW.RIGHT CAM LIM SWI NOT SUPPOSED TO CLICK" {EB} ENDIF ENDIF -'CHECK LEFT LIM SWI-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ -'IF MD CW,LEFT LIM SWI ISN'T CLICKED,MD HAS PASSED UNLATCHED RANGE +'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) -ST B -MC B OKFLG=0 +ERRORFLG=1 MG "ERROR.MAIN DRIVE CW" {EB} -MG "MD PASSED UNLATCH RANGE BUT LEFT SWI ISN'T ACTIVATED" {EB} -AB +MG "MD PASSED UNLATCH RANGE BUT LEFT CAM LIM SWI ISN'T ACTIVATED" {EB} ENDIF -'IF LEFT LIM SWI IS CLICKED WHEN MD CW^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +'IF MD CW AND LEFT CAM LIM SWI IS ACTIVATED^^^^^^^^^^^^^^^^^^^^^^^^^^^^ IF (DIR=1)&(@IN[1]=1) -'IF LEFT LIM SWI CLICKS AT UNLATCH RANGE +'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 -MG "ERROR.MD CW.LEFT SWI CLICKS OUTSIDE UNLATCH RANGE" {EB} +ERRORFLG=1 +MG "ERROR.MD CW.LEFT CAM LIM SWI CLICKS OUTSIDE UNLATCH RANGE" {EB} ENDIF ENDIF EN @@ -348,7 +500,7 @@ ST B MC B 'THIS INDICATES MAIN DRIVE HAS STOP STOP=1 -MG "STOP=1" {EB} +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 @@ -371,7 +523,7 @@ ST B MC B 'THIS INDICATES MAIN DRIVE HAS STOP STOP=1 -MG "STOP=1" {EB} +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 @@ -389,15 +541,20 @@ 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 LIM SWI +'MOVE ARM TO LPUP IF WEDGE HITS LEFT CAM LIM SWI JS #LPUP,(LPUPFLG=1) -'MOVE ARM TO RPUP IF WEDGE HITS RIGHT LIM SWI +'MOVE ARM TO RPUP IF WEDGE HITS RIGHT CAM LIM SWI JS #RPUP,(RPUPFLG=1) ENDIF '------------------------------------------------- @@ -411,7 +568,7 @@ ENDIF JP #LOOP3 EN 'SUBROUTINE "LPUP" BEGINS------------------------- -'LEFT LIMIT SWITCH IS HIT,WEDGE IS UNLATCH,MOVE ARM TO LPUP +'LEFT CAM LIMIT SWITCH IS HIT,WEDGE IS UNLATCH,MOVE ARM TO LPUP #LPUP MG "ARM IS MOVING TO LPUP" {EB} ERROR=LPUP - _TPA @@ -427,13 +584,14 @@ 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 LIMIT SWITCH IS HIT,WEDGE IS UNLATCH,MOVE ARM TO RPUP +'RIGHT CAM LIMIT SWITCH IS HIT,WEDGE IS UNLATCH,MOVE ARM TO RPUP #RPUP MG "ARM IS MOVING TO RPUP" {EB} ERROR=RPUP - _TPA @@ -449,65 +607,19 @@ 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 "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]= 14500)&(_TPA <= 14600) -'LEFT LIM SWI DETECTS WDG MOVEMENT TO THE LEFT SIDE +'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 -ENDIF -'CHECK LEFT SWI PULSE BETWEEN RPUP & LEFT SWI -IF (_TPA >= 14650)&(_TPA <= RPUP) -MG "ERROR.LEFT LIM ACTIVATES BEFORE LATCH HITS CAM" {EB} +ELSE +ST A +MC A +MG "ERROR.LEFT CAM LIMIT ACTIVATES OUTSIDE RANGE" {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 +'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 -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} +ELSE +ST A +MC A +MG "ERROR.RIGHT CAM LIMIT ACTIVATES OUTSIDE RANGE" {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 +'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 ENDIF ENDIF '+++++++++++++++++++++++++++++++++++++++++++++++++ @@ -620,7 +800,7 @@ EN 'SUBROUTINE "COUNTER" BEGINS---------------------- 'THIS SUBROUTINE WORKS ONLY ONE CYCLE #COUNTER -'IF LEFT LIM SWI IS HIT WHILE ARM BRINGING A WEDGE TO LDOP +'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 @@ -632,7 +812,7 @@ 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 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 @@ -646,149 +826,29 @@ 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]= LOWMAX) +FBERROR=0 ELSE -'IF MD HASN'T REACHED FINAL DESTINATION THEN MOVE MD -IPB=(ERROR/BSCALE) -MC B +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 "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]