From cece6638f7da523389fd52e4fe8b23468ac2cc5e Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Thu, 24 Nov 2011 10:18:34 +1100 Subject: [PATCH] Burhan's WIP version from 2008-01-07 I believe that this was working, at least to a point, but may not be his final version. r3276 | dcl | 2011-11-24 10:18:34 +1100 (Thu, 24 Nov 2011) | 2 lines --- .../instrument/tas/DMC2280/controller1.txt | 1369 +++++++++-------- 1 file changed, 711 insertions(+), 658 deletions(-) diff --git a/site_ansto/instrument/tas/DMC2280/controller1.txt b/site_ansto/instrument/tas/DMC2280/controller1.txt index 4b5ec18d..983730b1 100755 --- a/site_ansto/instrument/tas/DMC2280/controller1.txt +++ b/site_ansto/instrument/tas/DMC2280/controller1.txt @@ -1,876 +1,929 @@ -'MD=MAIN DRIVE, (3,0)=1800000, (0,3)=180000 -'RIGHT LIM SWI AT 16000 -'-----THREAD 0 BEGINS------------------------------------------------- -#MAIN +'TAIPAN - CONTROLLER 1 +' +'AUTHORS:DANIEL BARTLETT,BURHAN WIDJAJA +' +'FT=FLIGHT TUBE,WSR=WEDGE SHUFFLE ROUTINE +' +'A-MONOCHROMATOR TRANSLATION - TILT +'B-MONOCHROMATOR TRANSLATION - LINEAR +'C-MONOCHROMATOR TRANSLATION - ROTATE +'D-MAGNET ARM +'E-SPARE +'F-FLIGHT TUBE ROTATE (ASSOCIATED WITH TAPE ENDODER) +'G-DEDICATED FOR IVO GXM2H SSI ENCODER- FLIGHT TUBE ROTATE +'H-DETECTOR ROTATE +' +'------------------------------------------------------ +#AUTO +'THIS PROGRAM RUNS AUTOMATICALLY ON CONTROLLER STARUP '-----------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 +MT-2,-2,-2,-2,-2,-2,-2,-2;'CONFIGURES ALL AXES AS STEPPER +MO;'ALL MOTORS OFF +CN 1;'CONFIGURES LIMIT SWITCHES TO ACCEPT NC CONTACTS +II 8,,,0;' CONFIGURES THE 8TH INPUT TO ACTIVATE THE #ININT ROUTINE +OP 65280;' SETS OUTPUTS 9-16 ON = RELAYS OFF +IA 137,157,203,131;' CONFIGURES IP ADDRESS +AC 25000,25000,25000,150000,,25000,,25000;' SET ACELERATION +DC 25000,25000,25000,150000,,25000,,25000;' SET DECELERATION +SP 25000,25000,25000,150000,,50000,,100000;' SET SPEED +SIA=0;'TURN OFF CLOCK PULSES TO ALLOW ENCODER TO INITIALISE +SIB=0 +SIC=0 +SID=0 +SIE=0 +SIF=0 +SIG=0 +SIH=0 +WT1000;'WAIT FOR ENCODER TO INITIALISE +SIA=1,25,12,1<-1>2;'HMI CARD - SLOT 1 - MONO TILT +SIB=1,25,12,1<-1>2;'HMI CARD - SLOT 2 - MONO TRANS +SIC=1,25,12,1<-1>2;'HMI CARD - SLOT 3 - MONO ROTATE +SID=1,25,12,1<-1>2;'HMI CARD - SLOT 4 - MAGNET ARM (RESOLVER S RH-EMR 05038 RFA-1) +SIG=1,25,13,0<-2>1;'IVO GXM2H (SSI) - FLIGHT TUBE ROTATE +SIH=1,25,25,0<-2>2;'HEIDENHAIN RCN226 - DETECTOR ROTATE +LSAHIT=0;' INITIALISE THE BIT SET AFTER A LIMIT IS HIT +LSBHIT=0 +LSCHIT=0 +LSDHIT=0 +'LSEHIT=0 +LSFHIT=0 +'LSGHIT=0 +LSHHIT=0 +APFDONE=0;' INITIALISE THE BIT FOR AXIS F AIRPAD CONTROL +APHDONE=0;' INITIALISE THE BIT FOR AXIS H AIRPAD CONTROL +'RESOLVER VALUES------------------------------------------------------- +'CCWLS = REFERENCE VALUE FOR RESOLVER--DIFFERENT VALUE FOR EACH CARD +CCWLS=54385;'COUNTER CLOCKWISE LIMIT SWITCH +'------------------------------------------ +LCAM1=CCWLS+1015;'LEFT CAM LIMIT SWITCH MIN RANGE +LDOP=CCWLS+1085;'LEFT DROP OFF POINT +LPUP=CCWLS+1145;'LEFT PICK UP POINT +LCAM2=CCWLS+1215;'LEFT CAM LIMIT SWITCH MAX RANGE +RCAM1=CCWLS+2815;'RIGHT CAM LIMIT SWITCH MIN RANGE +RPUP=CCWLS+2905;'RIGHT PICK UP POINT +RDOP=CCWLS+2945;'RIGHT DROP OFF POINT +RCAM2=CCWLS+3015;'RIGHT CAM LIMIT SWITCH MAX RANGE +CWLS=CCWLS+4004;'CLOCKWISE LIMIT SWITCH +'SSI VALUES------------------------------------------------------------ +'LOWLS = REFERENCE VALUE FOR SSI ENCODER------ +LOWLS=108700;'LOW ANGLE(15DEG)LIMIT SWITCH MAX RANGE +'--------------------------------------------------- +'LOOKING TO THE INPUT SOURCE,FROM LEFT TO RIGHT +'WEDGE 1 IS THE MOST LEFT,WEDGE 2 IS IN THE MIDDLE,WEDGE 3 IS THE MOST RIGHT +WL1=LOWLS+12800;'WEDGE LATCH 1 CLICK RANGE +ENCW1=LOWLS+375300;'FT CW,WDG 1 LATCH ENDS TO UNLATCH +BGCW1=LOWLS+451300;'FT CW,WDG 1 LATCH BEGINS TO UNLATCH +BGCCW1=LOWLS+485800;'FT CCW,WDG 1 BEGINS TO UNLATCH +ENCCW1=LOWLS+565000;'FT CCW,WDG 1 ENDS TO UNLATCH +ENCW2=LOWLS+931300;'FT CW, WDG 2 ENDS TO UNLATCH +BGCW2=LOWLS+1001300;'FT CW, WDG 2 BEGINS TO UNLATCH +BGCCW2=LOWLS+1040300;'FT CCW,WDG 2 BEGINS TO UNLATCH +ENCCW2=LOWLS+1120100;'FT CCW,WDG 2 ENDS TO UNLATCH +ENCW3=LOWLS+1506800;'FT CW,WDG 3 ENDS TO UNLATCH +BGCW3=LOWLS+1581300;'FT CW,WDG 3 BEGINS TO UNLATCH +BGCCW3=LOWLS+1605800;'FT CCW,WDG 3 BEGINS TO UNLATCH +ENCCW3=LOWLS+1672400;'FT CCW,WDG 3 ENDS TO UNLATCH +HIGHMIN=LOWLS+2661210;'HIGH ANGLE(85 DEG) LIMIT SWITCH MIN RANGE +HIGHMAX=LOWLS+2661300;'HIGH ANGLE(85 DEG) LIMIT SWITCH MAX RANGE +'GLOBAL VARIABLES---------------------------------------------------- +EXTREME=0;'THIS FLAG GOES TO '1' IF FT IS AT EXTREME LEFT/RIGHT SIDE +BSCALE=8192/25000;'CALIBRATION FOR MAIN DRIVE MOTOR +ASCALE=13.732/25000;'CALIBRATION FOR ARM ACTUATOR MOTOR DEADBAND=10 +TIMER=10000;'10 SECONDS ALLOWED FOR WDG TO TRAVEL BETWEEN PICK UP AND DROP OFF POINT +ULMIN=0;'INITIALISE UNLATCH MIN VARIABLE +ULMAX=0;'INITIALISE UNLATCH MAX VARIABLE 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 +REFPOS=0;'REFERENCE FLIGHT TUBE POSITION +CURPOS=0;'CURRENT FLIGHT TUBE POSITION +DIR=2;'THIS MEANS FT IS STOP AT THE BEGINNING +STOP=0;'THIS FLAG GOES TO '1' IF FT STOPS +ARMRDY=0;'THIS FLAG GOES TO '1' IF ARM IS READY AT PICK UP POINT +OKFLG=0;'OF FLAG FOR EXECUTING WEDGE SHUFFLE ROUTINE +CNTSTOP=0;'COUNTER FLAG +WDGCCW=0;'GOES TO 1 IF WEDGE MOVEMENT TO LEFT SIDE IS DETECTED +WDGCW=0;'GOES TO 1 IF WDG MOVEMENT TO RIGHT SIDE IS DETECTED +LPUPFLG=0;'GOES TO 1 IF ARM GOES TO LPUP THEN RDOP +RPUPFLG=0;'GOES TO 1 IF ARM GOES TO RPUP THEN LDOP +HOMERUN=0;'GOES TO 1 IF #HOME HAS BEEN EXECUTED '------------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 +'THIS WILL EXECUTE HOME ROUTINE ON CONTROLLER POWER UP +'LOOKING TO THE INPUT SOURCE +CNTLEFT=0;'NUMBER OF WEDGES ON THE LEFT SIDE OF THE BEAM LINE +CNTRIGHT=0;'NUMBER OF WEDGES ON THE RIGHT SIDE OF THE BEAM LINE +'WEDGE PATTERN WILL BE (0,3) AFTER HOME ROUTINE HAS BEEN EXECUTED +'ALL 3 WEDGES ON THE RIGHT SIDE +'------------------------------------------------------------------- +SB 1;'DEACTIVATE ARM MAGNET ON CONTROLLER POWER UP +REQF=0;'SICS CONTROL VARIABLES-------------------- +RSPF=0;'SICS +DSTF=0;'SICS +POSF=0;'SICS +RUNF=0;'SICS +JS #THREAD0 +EN '---------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 +'-----THREAD 0 BEGINS--------------------------------------- +#THREAD0 +'SICS ASKS A REQUEST ON CONTROLLER POWER UP OR AFTER ERROR OCCURS +IF (REQF=1)&(RSPF=0)&(HOMERUN=0) +RSPF=-1;'NOT READY FOR NORMAL OPERATION BECAUSE IT HASN'T BEEN HOMED ENDIF -'if wedge shuffle routine has done,then SICS sends REQA=0 -IF (REQA=0)&(RSPA=1) -'GALIL resets RSPA flag -RSPA=0 +'--------------------------------------------------------------- +'SICS ASKS A REQUEST AFTER HOME HAS BEEN EXECUTED +IF (REQF=1)&(RSPF=0)&(HOMERUN=1) +RSPF=1;'READY FOR NORMAL OPERATON BECAUSE IT HAS BEEN HOMED +SH DF;'TURN ON MAGNET ARM(D-AXIS) AND FLIGHT TUBE(F-AXIS) MOTOR ENDIF -JP #LOOP +'--------------------------------------------------------------- +'IF READY TO RUN, SICS SETS RUNF=1 +IF (REQF=1)&(RSPF=1)&(RUNF=1)&(_XQ1<0) +'EXECUTE WEDGE SHUFFLE ROUTINE,UNTIL WSR HAS BEEN COMPLETED (RUNF=0 IN THREAD 1) +XQ #THREAD1,1;'EXECUTE THREAD 1 +XQ #THREAD2,2;'EXECUTE THREAD 2 +XQ #THREAD3,3;'EXECUTE THREAD 3 +XQ #THREAD4,4;'EXECUTE THREAD 4 +XQ #THREAD5,5;'EXECUTE THREAD 5 +'---------------------------------------------------------------- +ENDIF +'IF WEDGE SHUFFLE ROUTINE HAS BEEN COMPLETED,THEN SICS SETS REQF=0 +IF (REQF=0)&(RSPF=1) +RSPF=0;'GALIL RESETS RSPF FLAG AND STOPS EXECUTING THREAD 1,2,3,4,5 +MO DF;'TURN OFF MAGNET ARM AND FLIGHT TUBE MOTOR +ENDIF +'----------------------------------------------------------------- +'THERE MUST BE A THREAD RUNNING FOR #LIMSWI, #TCPERR AND #ININT TO WORK +JS #CHKDCEL +JS #AIRPAD +JS #CHKSPD +POSF=_TDF +ENCF=_TPG +JP #THREAD0 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 +'---------------------HOME BEGINS------------------------------- +'HOME ROUTINE STARTS----------------------------- +#HOME +'EXECUTE HOME ROUTINE -> FLIGHT TUBE MOVES TO 27 DEG +JS #HMWDG1 +JS #HMPUP +JS #HMRDOP 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] FLIGHT TUBE MOVES TO THE DESTINATION POSITION +JS #MOVEFT,(STOP=0);'STOP=1 WHEN WEDGE IS UNLATCHED AND READY TO BE MOVED +'STOP IS RESET TO '0' AFTER WEDGE HAS BEEN DROP OFF +'KEEP LOOPING WHILE ACTIVE +JP #THREAD1,(REQF<>0)|(RSPF<>0) +EN +'SUBROUTINE "MOVEFT" BEGINS------------------------- +#MOVEFT +IF (DSTF=_TDF);'FLIGHT TUBE MOVEMENT IS COMPLETE +ST F;'STOP FLIGHT TUBE MOTION +MC F +RUNF=0;'RESETS RUNF TO 0 ELSE -MG "MD IS MOVING TO THE DESTINATION" {EB} -'IF MD HASN'T REACHED FINAL DESTINATION THEN MOVE MD -IPB=(ERROR/BSCALE) -MC B +PAF=DSTF;'SET THE FLIGHT TUBE DESTINATION POSITION +BG F;'MOVE FLIGHT TUBE +MC F;'WAIT UNTIL MOTION HAS COMPLETED ENDIF EN -'SUBROUTINE "MOVEMD" ENDS--------------------------- +'SUBROUTINE "MOVEFT" 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 +JS #CKFTDIR;'DETERMINE FLIGHT TUBE DIRECTION +JS #PROCESS;'DECIDE UNLATCH RANGE,LOCATION OF THE WEDGE LATCH '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 +JS #CKCAMLS,(STOP=0);'STOP=1 WHEN WEDGE IS UNLATCHED AND READY TO BE MOVED +'STOP IS SET TO '0' AFTER WEDGE HAS BEEN DROP OFF +JS #STOPMD,(OKFLG=1);'STOP FLIGHT TUBE WHEN WEDGE IS WITHIN UNLATCH RANGE +'KEEP LOOPING WHILE ACTIVE +JP #THREAD2,(REQF<>0)|(RSPF<>0) 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 +'SUBROUTINE "CKFTDIR" BEGINS------------------------ +#CKFTDIR +'FT MOVES CLOCKWISE,MOTOR = POSITIVE DIR +IF DSTF > POSF DIR=1 ENDIF '------------------------------------------------- -'IF MD MOVES COUNTER CLOCKWISE THEN VARIABLE "DIR" IS '0' -IF CURPOS > REFPOS +'FT MOVES COUNTER CLOCKWISE,MOTOR = NEGATIVE DIR +IF DSTF < POSF DIR=0 ENDIF '------------------------------------------------- -'IF MD STOPS THEN VARIABLE "DIR" IS '2' -IF CURPOS = REFPOS +'FT STOPS +IF _BGF=0 DIR=2 ENDIF EN -'SUBROUTINE "CKMDDIR" ENDS-------------------------- +'SUBROUTINE "CKFTDIR" ENDS-------------------------- 'SUBROUTINE "PROCESS BEGINS------------------------ #PROCESS '************************************************* -'IF WEDGE PATTERN (0,3) & MD MOVES CW +'IF WEDGE PATTERN (0,3) & FT 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 +'UNLATCH RANGE IS AT THE LOCATION WHEN FT HITS LOW ANGLE LIM SWI +ULMIN=WL1 +ULMAX=LOWLS +BGNHIT=WL1 +ENDHIT=LOWLS +'SOFT STOP BEFORE LIMIT SWITCH IS ACTIVATED +EXTREME=1;'FLIGHT TUBE HAS REACHED EXTREME LOW ANGLE(15 DEG) THEN STOP FLIGHT TUBE ENDIF '------------------------------------------------- -'IF WEDGE PATTERN (0,3) & MD MOVES CCW +'IF WEDGE PATTERN (0,3) & FT 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 +'MAX RANGE FOR RIGHT CAM LS TO CLICK BEFORE SETTING ERROR FLG +MAX=ENCCW1 ENDIF '************************************************* -'IF WEDGE PATTERN (1,2) & MD MOVES CW +'IF WEDGE PATTERN (1,2) & FT 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 +ENDHIT=BGCCW1-11000 ENDIF '------------------------------------------------- -'IF WEDGE PATTERN (1,2) & MD MOVES CCW +'IF WEDGE PATTERN (1,2) & FT 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 +'MAX RANGE FOR RIGHT CAM LS TO CLICK BEFORE SETTING ERROR FLG +MAX=ENCCW2 ENDIF '************************************************* -'IF WEDGE PATTERN (2,1) & MD MOVES CW +'IF WEDGE PATTERN (2,1) & FT 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 +ENDHIT=BGCCW2-11000 ENDIF '------------------------------------------------- -'IF WEDGE PATTERN (2,1) & MD MOVES CCW +'IF WEDGE PATTERN (2,1) & FT 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 +'MAX RANGE FOR RIGHT CAM LS TO CLICK BEFORE SETTING ERROR FLG +MAX=ENCCW3 ENDIF '************************************************* -'IF WEDGE PATTERN (3,0) & MD MOVES CW +'IF WEDGE PATTERN (3,0) & FT 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 +ENDHIT=BGCCW3-11000 ENDIF '------------------------------------------------- -'IF WEDGE PATTERN (3,0) & MD MOVES CCW +'IF WEDGE PATTERN (3,0) & FT 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 +'MAX RANGE FOR RIGHT CAM LS TO CLICK BEFORE SETTING ERROR FLG +MAX=HIGHMAX +'SOFT STOP BEFORE LIMIT SWITCH IS ACTIVATED +EXTREME=1;'FLIGHT TUBE HAS REACHED EXTREME HIGH ANGLE(85 DEG) THEN STOP FLIGHT TUBE ENDIF EN '************************************************* 'SUBROUTINE "PROCESS" ENDS-------------------------- -'SUBROUTINE "CKLSMD" BEGINS------------------------- -#CKLSMD -'IF MD MOVES CCW=================================================== +'SUBROUTINE "CKCAMLS" BEGINS------------------------- +#CKCAMLS +'IF FLIGHT TUBE 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} +HOMERUN=0 +RSPF=-1 +RUNF=-1;'ERROR.FT CCW,LEFT CAM LIM SWI SHOULDN'T BE ACTIVATED BEFORE SHUFFLE WDG +ST F 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) +'IF RIGHT CAM LIM SWI ISN'T ACTVATED & WEDGE HAS PASSED THE UNLATCH RANGE +IF (DIR=0)&(@IN[2]=0)&(_TPG >= MAX) 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} +HOMERUN=0 +RSPF=-1 +RUNF=-2;'ERROR.FT CCW,FT PASSED UNLATCHED RANGE BUT RIGHT CAM LIM SWI ISN'T ACTIVATED +ST F ENDIF -'DOES IT CLICK INSIDE OR OUTSIDE THE ALLOWABLE RANGE? +'RIGHT CAM LS IS ALLOWED TO CLICK IN 2 RANGES FOR EACH PATTERN +'THE 1ST PULSE IS PHANTOM/FALSE, 2ND PULSE IS REAL UNLATCH +'PATTERN (0,3)------------------------------- 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} +IF ((_TPG <= WL1)&(_TPG >= LOWLS))|((_TPG <= ENCCW1)&(_TPG >= BGCCW1)) +IF (_TPG <= ENCCW1)&(_TPG >= BGCCW1);'UNLATCH RANGE FOR WDG 1 +OKFLG=1;'OK.FT CCW.WDG1 UNLATCH.STOP FT THEN SHUFFLE WDG 1 TO LEFT +ENDIF ELSE -MG "ERROR.MD CCW.(0,3).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} OKFLG=0 -ERRORFLG=1 -'RUNA=-1 +HOMERUN=0 +RSPF=-1 +RUNF=-3;'ERROR.FT CCW.(0,3).RIGHT CAM LIM SWI CLICKS OUTSIDE THE SPECIFIED RANGE +ST F ENDIF ENDIF +'PATTERN (1,2)------------------------------- 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} +IF ((_TPG <= ENCCW1)&(_TPG >= BGCCW1))|((_TPG <= ENCCW2)&(_TPG >= BGCCW2)) +IF (_TPG <= ENCCW2)&(_TPG >= BGCCW2);'UNLATCH RANGE FOR WDG 2 +OKFLG=1;'OK.FT CCW.WDG2 UNLATCH.STOP FT THEN SHUFFLE WDG 2 TO LEFT +ENDIF ELSE -MG "ERROR.MD CCW.(1,2).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} OKFLG=0 -ERRORFLG=1 -'RUNA=-1 +HOMERUN=0 +RSPF=-1 +RUNF=-3;'ERROR.FT CCW.(1,2).RIGHT CAM LIM SWI CLICKS OUTSIDE THE SPECIFIED RANGE +ST F ENDIF ENDIF +'PATTERN (2,1)------------------------------- 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} +IF ((_TPG <= ENCCW2)&(_TPG >= BGCCW2))|((_TPG <= ENCCW3)&(_TPG >= BGCCW3)) +IF (_TPG <= ENCCW3)&(_TPG >= BGCCW3);'UNLATCH RANGE FOR WDG 3 +OKFLG=1;'OK.FT CCW.WDG3 UNLATCH.STOP FT THEN SHUFFLE WDG 3 TO LEFT +ENDIF ELSE -MG "ERROR.MD CCW.(2,1).RIGHT CAM LIM SWI ISN'T SUPPOSED TO CLICK" {EB} OKFLG=0 -ERRORFLG=1 -'RUNA=-1 +HOMERUN=0 +RSPF=-1 +RUNF=-3;'ERROR.FT CCW.(2,1).RIGHT CAM LIM SWI CLICKS OUTSIDE THE SPECIFIED RANGE +ST F ENDIF ENDIF +'PATTERN (3,0)------------------------------- 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} +IF ((_TPG < BGCCW3)|(_TPG > ENCCW3)) OKFLG=0 -ERRORFLG=1 -'RUNA=-1 +HOMERUN=0 +RSPF=-1 +RUNF=-3;'ERROR.FT CCW.(3,0).RIGHT CAM LIM SWI CLICKS OUTSIDE THE SPECIFIED RANGE +ST F 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 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 +'RIGHT CAM SWI ISN'T SUPPOSED TO CLICK OUTSIDE THE SPECIFIED RANGE +IF (_TPG > BGNHIT)|(_TPG < ENDHIT) OKFLG=0 -ERRORFLG=1 -'RUNA=-1 -MG "ERROR.MD CW.RIGHT CAM LIM SWI NOT SUPPOSED TO CLICK" {EB} +HOMERUN=0 +RSPF=-1 +RUNF=-4;'"ERROR.FT CW.RIGHT CAM LIM SWI IS NOT SUPPOSED TO CLICK +ST F 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) +'IF FT CW & LEFT CAM LIM SWI ISN'T ACTIVATED & FT HAS PASSED THE UNLATCHED RANGE +IF (DIR=1)&(@IN[1]=0)&(_TPG <= 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} +HOMERUN=0 +RSPF=-1 +RUNF=-5;'ERROR.FT CW.MD PASSED UNLATCH RANGE BUT LEFT CAM LIM SWI ISN'T ACTIVATED +ST F ENDIF -'IF MD CW AND LEFT CAM LIM SWI IS ACTIVATED^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +'IF FT 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 +IF (_TPG <= ULMIN)&(_TPG >= ULMAX) +OKFLG=1;'OK.FT CW.WDG3 UNLATCH ELSE OKFLG=0 -ERRORFLG=1 -'RUNA=-1 -MG "ERROR.MD CW.LEFT CAM LIM SWI CLICKS OUTSIDE UNLATCH RANGE" {EB} +HOMERUN=0 +RSPF=-1 +RUNF=-6;'ERROR.FT CW.LEFT CAM LIM SWI CLICKS OUTSIDE UNLATCH RANGE +ST F ENDIF ENDIF EN -'SUBROUTINE "CKLSMD" ENDS-------------------------- +'SUBROUTINE "CKCAMLS" 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 +'============================================= +IF (DIR=1);'IF FT MOVES CLOCKWISE++++++++++++++++++++++++++++ +ST F;'STOP FT FOR WEDGE SHUFFLE ROUTINE +MC F;'WAIT UNTIL FT COMPLETELY STOP +STOP=1;'THIS INDICATES FT HAS STOP +'-------------------------- +IF (EXTREME=1);'SOFT LIMIT',IF FT HAS REACHED LOW ANGLE (15 DEG) +DCF=200000;'HIGH DECELERATION RATE +ST F +ELSE;'IF WEDGE IS UNLATCHED AND READY TO BE MOVED +WT 500;'WAIT 0.5 SEC +LPUPFLG=1;'SET LPUP FLAG.THIS INITIATES ARM FOR LEFT PICK UP & RIGHT DROP OFF ENDIF -'--------------------------------------- +'-------------------------- ENDIF '============================================ +IF (DIR=0)'IF FT MOVES COUNTER CLOCKWISE++++++++++++++++++++ +ST F;'STOP FT FOR WEDGE SHUFFLE ROUTINE +MC F;'WAIT UNTIL FT COMPLETELY STOP +STOP=1;'THIS INDICATES MAIN DRIVE HAS STOP +'-------------------------- +IF (EXTREME=1)'SOFT LIMIT',IF MD HAS REACHED HIGH ANGLE (85 DEG) +DCF=200000;'HIGH DECELERATION RATE +ST F +ELSE;'IF WEDGE IS UNLATCHED AND READY TO BE MOVED +WT 500;'WAIT 0.5 SEC +RPUPFLG=1;'SET RPUP FLAG.THIS INITIATES ARM FOR RIGHT PICK UP & LEFT DROP OFF 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) +IF (ARMRDY=0); +JS #LPUP,(LPUPFLG=1);'MOVE ARM TO LPUP IF WEDGE HITS LEFT CAM LIM SWI +JS #RPUP,(RPUPFLG=1);'MOVE ARM TO RPUP IF WEDGE HITS RIGHT CAM LIM SWI 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) +JS #RDOP,(LPUPFLG=1);'MOVE ARM TO RDOP AFTER ARM PICKS UP WEDGE AT LPUP +JS #LDOP,(RPUPFLG=1);'MOVE ARM TO LDOP AFTER ARM PICKS UP WEDGE AT RPUP ENDIF -JP #LOOP3 +'KEEP LOOPING WHILE ACTIVE +JP #THREAD3,(REQF<>0)|(RSPF<>0) 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] @OUT[1]=0 +WT 1000;'WAIT 1 SEC +ERROR=RDOP - _TPD;'ARM IS MOVING A WDG TO RDOP +'----------------------------------------------- +IF @ABS[ERROR] @OUT[1]=1 +ELSE;'WDGCW=1,LIM SWI SUCCESSFULLY DETECTS WDG MOVEMENT AND WDG HAS BEEN MOVED TO RDOP +SB 1;'DEACTIVATE MAGNET -> @OUT[1]=1, PROG STOP GOING TO CKPULSE SUBROUTINE +'ARM ARRIVED AT RDOP,DROP OFF A WEDGE 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 +'=================================== +OKFLG=0;'THIS ALLOWS PROG TO CHECK CAM LIM SWI BEFORE MOVING A WDG +STOP=0;'THIS ALLOWS MD TO MOVE AGAIN AFTER WEDGE HAS BEEN MOVED +LPUPFLG=0;'THIS ALLOWS PROG TO GO TO SUB "LPUP" +ARMRDY=0;'THIS ALLOWS ARM TO GO TO PICK UP POINT +WDGCW=0;'THIS ALLOWS WDG MOVEMENT DETECTION TO THE RIGHT SIDE +CNTSTOP=0;'THIS ALLOWS COUNTER TO COUNT IF RLS CLICKS 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] @OUT[1]=0 +WT 1000;'WAIT 1 SEC +ERROR=LDOP - _TPD;'ARM IS MOVING A WDG TO LDOP +'----------------------------------------------- +IF @ABS[ERROR] @OUT[1]=1 +ELSE;'WDGCCW=1,LIM SWI SUCCESSFULLY DETECS WDG MOVEMENT AND WDG HAS BEEN MOVED TO LDOP +SB 1;'DEACTIVATE MAGNET -> @OUT[1]=1,PROG STOP GOING TO CKPULSE SUBROUTINE +'ARM ARRIVED AT LDOP,DROP OFF A WEDGE 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 +'========================================= +OKFLG=0;'THIS ALLOWS PROG TO CHECK CAM LIM SWI BEFORE MOVING A WDG +STOP=0;'THIS ALLOWS MD TO MOVE AGAIN AFTER WEDGE HAS BEEN MOVED +RPUPFLG=0;'THIS ALLOWS PROG TO GO TO SUB "RPUP" +ARMRDY=0;'THIS ALLOWS ARM TO GO TO PICK UP POINT +WDGCCW=0;'THIS ALLOWS WDG MOVEMENT DETECTION TO THE LEFT SIDE +CNTSTOP=0;'THIS ALLOWS COUNTER TO COUNT IF LLS CLICKS 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 +'IF MAGNET IS ACTIVATED(OUT[1]),CHECK CAM LIM SWI PULSE WHILE ARM GOES TO DROP OFF POINT 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 +JS #TIMER,(@OUT[1]=0) +IF (@OUT[1]=0)&(CNTSTOP=0);'IF MAGNET IS ACTIVATED & COUNTER HASN'T ACTIVATED +JS #COUNTER;'COUNTER COUNTS UP/DOWN,DECIDES WEDGE PATTERN,DEACTIVATES COUNTER ENDIF -JP #LOOP4 +'KEEP LOOPING WHILE ACTIVE +JP #THREAD4,(REQF<>0)|(RSPF<>0) 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 +IF (RPUPFLG=1)&(@IN[1]=1);'IF LEFT LIM SWI (@IN[1]) IS LOGIC HIGH +IF (_TPD >= LCAM1)&(_TPD <= LCAM2);'SWITCH SHOULD ACTIVE BETWEEN THIS RANGE +WDGCCW=1;'CAM LEFT LIM SWI DETECTS WDG MOVEMENT TO THE LEFT SIDE +ELSE;'CAM LEFT LIM SWI DOESN'T DETECT WDG MOVEMENT OR SWITCH IS FAULTY +ST D;'STOP ARM +MC D +'ERROR.DOESN'T DETECT WDG MOVEMENT OR SWITCH IS FAULTY +WDGCCW=0;'CAM LEFT LIM SWI DOESN'T DETECT WDG MOVEMENT TO THE LEFT SIDE 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 +IF (LPUPFLG=1)&(@IN[2]=1);'IF RIGHT LIM SWI (@IN[2]) IS ACTIVATED +IF (_TPD >= RCAM1)&(_TPD <= RCAM2);'SWITCH SHOULD ACTIVE BETWEEN THIS RANGE +WDGCW=1;'RIGHT CAM LIM SWI DETECTS WDG MOVEMENT TO THE RIGHT SIDE +ELSE;'CAM RIGHT LIM SWI DOESN'T DETECT WDG MOVEMENT OR SWITCH IS FAULTY +ST D;'STOP ARM +MC D +'ERROR.DOESN'T DETECT WDG MOVEMENT OR SWITCH IS FAULTY +WDGCW=0;'RIGHT CAM LIM SWI DOESN'T DETECT WDG MOVEMENT TO THE RIGHT SIDE 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} +'----------------------------------------------------- +IF (LPUPFLG=1)&(@IN[1]=1);'IF LEFT CAM LS IS STILL ACTIVATED +IF (_TPD >= LCAM2);'AND ARM HAS MOVED AWAY FROM LEFT PICK UP POINT +ST D;'STOP ARM +MC D +'ERROR.LEFT CAM LS REMAINS ACTIVATED WDGCW=0 -'RUNA=-1 +HOMERUN=0 +RSPF=-1 +RUNF=-9;'IT SHOULD BE DEACTIVATED AFTER PICKS UP A WDG ENDIF ENDIF '+++++++++++++++++++++++++++++++++++++++++++++++++ EN 'SUBROUTINE "CKPULSE" ENDS------------------------ +'SUBROUTINE "TIMER" BEGINS------------------------ +#TIMER +AT 0 +AT TIMER +IF (LPUPFLG=1)&(WDGCW=0) +ST D;'STOP MAGNET ARM +MC D;'WAIT UNTIL MOTION COMPLETE +HOMERUN=0 +RSPF=-1 +RUNF=-10;'WDG DOESN'T ARRIVE AT RDOP POINT WITHIN TIME LIMIT +ENDIF +IF (RPUPFLG=1)&(WDGCCW=0) +ST D;'STOP MAGNET ARM +MC D;'WAIT UNTIL MOTION COMPLETE +HOMERUN=0 +RSPF=-1 +RUNF=-11;'WDG DOESN'T ARRIVE AT LDOP POINT WITHIN TIME LIMIT +ENDIF +EN +'SUBROUTINE "TIMER" 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} +IF (WDGCCW=1);'IF LEFT CAM LIM SWI IS HIT WHILE ARM BRINGING A WEDGE TO LDOP +CNTLEFT=CNTLEFT+1;'ADD 1 TO COUNTER LEFT VARIABLE +CNTRIGHT=CNTRIGHT-1;'MINUS 1 FROM COUNTER RIGHT VARIABLE +CNTSTOP=1;'DEACTIVATE COUNTER 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} +IF (WDGCW=1);'IF RIGHT CAM LIM SWI IS HIT WHILE ARM BRINGING A WEDGE TO RDOP +CNTLEFT=CNTLEFT-1;'MINUS 1 FROM COUNTER LEFT VARIABLE +CNTRIGHT=CNTRIGHT+1;'ADD 1 TO COUNTER RIGHT VARIABLE +CNTSTOP=1;'DEACTIVATE COUNTER ENDIF EN 'SUBROUTINE "COUNTER" ENDS------------------------ '''''''''''''''''''THREAD 4 ENDS'''''''''''''''''''''''''''''''''''''' '################THREAD 5 BEGINS#######################################' #THREAD5 -#LOOP5 -JS #ENCODCK,(FBERROR=0) -JP #LOOP5 +JS #FDBACK +'KEEP LOOPING WHILE ACTIVE +JP #THREAD5,(REQF<>0)|(RSPF<>0) EN -'SUBROUTINE "ENCODCK" BEGINS---------------------- -#ENCODCK +'SUBROUTINE "FDBACK" BEGINS---------------------- +#FDBACK '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} +IF (_TPG > HIGHMAX)|(_TPG < LOWLS);'LOWLS(15 DEG) = 139175, HIGHMAX(85 DEG) = 2775000 +DCF=200000;'HIGH DECELERATION RATE +ST F;'STOP FLIGHT TUBE,CHECK SSI ENCODER CONNECTION +RUNF=-12;'ERROR,ENCODER VALUE IS OUTSIDE RANGE +ENDIF +IF (_TPD > CWLS)|(_TPD < CCWLS);'OUTSIDE EXTREME LEFT OR RIGHT LS +DCD=600000;'HIGH DECELERATION RATE +ST D;'STOP MAGNET ARM,CHECK RESOLVER +RUNF=-13;'ERROR,RESOLVER VALUE IS OUTSIDE RANGE ENDIF EN -'SUBROUTINE "ENCODECK" ENDS------------------------ +'SUBROUTINE "FDBACK" ENDS------------------------ '------------LIMIT SWITCH SUBROUTINE BEGINS--------------------------- -'THIS SUBROUTINE IS EXECUTED WHENEVER A LIMIT SWITCH IS DEPRESSED +'-------------------------------------------------------------------------- +#CHKDCEL +'ROUTINE TO RESET DECELERATIONS AFTER A LIMIT HIT +' +IF (LSAHIT=1)&(_BGA=0);'WHEN MOTION STOPS AFTER A LIMIT HIT +DCA=OLDDCA;' RESET DECEL TO OLD VALUE +LSAHIT=0 +ENDIF +IF (LSBHIT=1)&(_BGB=0) +DCB=OLDDCB +LSBHIT=0 +ENDIF +IF (LSCHIT=1)&(_BGC=0) +DCC=OLDDCC +LSCHIT=0 +ENDIF +IF (LSDHIT=1)&(_BGD=0) +DCD=OLDDCD +LSDHIT=0 +ENDIF +'IF (LSEHIT=1)&(_BGE=0) +'DCE=OLDDCE +'LSEHIT=0 +'ENDIF +IF (LSFHIT=1)&(_BGF=0) +DCF=OLDDCF +LSFHIT=0 +ENDIF +'IF (LSGHIT=1)&(_BGG=0) +'DCG=OLDDCG +'LSGHIT=0 +'ENDIF +IF (LSHHIT=1)&(_BGH=0) +DCH=OLDDCH +LSHHIT=0 +ENDIF +EN +' +' +'-------------------------------------------------------------------------- +#AIRPAD +'ROUTINE TO CONTROL AIRPADS +' +'CONTROLS THE AIRPAD SOLENOID VALVES FOR SAMPLE STAGE +IF (_MOF=0);'FLIGHT TUBE MOTOR IS ON +SB 10;'SAMPLE STAGE AIR PAD OFF-SOLENOID OFF +SB 12;'DETECTOR AIR PAD OFF-SOLENOID OFF +CB 9;'SAMPLE STAGE AIR PAD ON-SOLENOID ON +CB 11;'DETECTOR AIR PAD ON-SOLENOID ON +APFDONE=0 +ENDIF +IF (_MOF=1)&(APFDONE=0);'FLIGHT TUBE MOTOR IS OFF +SB 9;'SAMPLE STAGE AIR PAD ON-SOLENOID OFF +SB 11;'DETECTOR AIR PAD OFF-SOLENOID OFF +CB 10;'SAMPLE STAGE AIR PAD OFF-SOLENOID ON +CB 12;'DETECTOR AIR PAD OFF-SOLENOID ON +WT500;'WAIT 1/2 SEC +SB 10;'SAMPLE STAGE AIR PAD OFF-SOLENOID OFF +SB 12;'DETECTOR AIR PAD-SOLENOID OFF +APFDONE=1 +ENDIF +' +'CONTROLS THE AIRPAD SOLENOID VALVES FOR DETECTOR STAGE +IF (_MOH=0);'DETECTOR MOTOR IS ON +SB 12;'DETECTOR AIR PAD OFF-SOLENOID OFF +CB 11;'DETECTOR AIR PAD ON-SOLENOID ON +APHDONE=0 +ENDIF +IF (_MOH=1)&(APHDONE=0);'DETECTOR MOTOR IS OFF +SB 11;'DETECTOR AIR PAD OFF-SOLENOID OFF +CB 12;'DETECTOR AIR PAD OFF-SOLENOID ON +WT500;'WAIT 1/2 SEC +SB 12;'DETECTOR AIR PAD-SOLENOID OFF +APHDONE=1 +ENDIF +EN +' +' +'-------------------------------------------------------------------------- +#CHKSPD +'ROUTINE TO CHECK THE SPEED OF THE AXES IS NOT GREATER THAN MAX ALLOWED +' +IF (_SPA>50000) +SPA=50000 +ENDIF +IF (_SPB>50000) +SPB=50000 +ENDIF +IF (_SPC>50000) +SPC=50000 +ENDIF +IF (_SPD>150000) +SPD=150000 +ENDIF +'IF (_SPE>50000) +'SPE=50000 +'ENDIF +IF (_SPF>50000) +SPF=50000 +ENDIF +'IF (_SPG>50000) +'SPG=50000 +'ENDIF +IF (_SPH>100000) +SPH=100000 +ENDIF +EN +' +' +'-------------------------------------------------------------------------- +#TCPERR +'EMPTY TCP ERROR ROUTINE +' +RE +' +' +'-------------------------------------------------------------------------- +#ININT +'INTERUPT ROUTINE TO STOP ALL AXES +'WHEN SAFETY & INTERLOCK SYSTEM REQUESTS IT +' +STA,B,C,D,E,F,G,H +AM +MO +RI +' +' +'-------------------------------------------------------------------------- +#CMDERR +'ROUTINE TO HANDLE COMMAND ERRORS +' +ZS0 +EN1 +' +' +'-------------------------------------------------------------------------- #LIMSWI -IF _LFA=0 -MG "RIGHT LIMIT SWITCH ARM IS ACTIVATED" {EB} +'ROUTINE TO HASTEN THE DECELERATION OF AN AXIS WHEN A LIMIT SWITCH IS HIT +' +IF (_LFA=0)|(_LRA=0);'LIMIT HIT +IF (LSAHIT=0)&(_BGA=1);'STILL MOVING, FIRST TIME +OLDDCA=_DCA;'REMEMBER OLD DECEL VALUE +DCA=100000;'INCREASE DECEL VALUE FOR RAPID STOP +LSAHIT=1 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} +IF (_LFB=0)|(_LRB=0) +IF (LSBHIT=0)&(_BGB=1) +OLDDCB=_DCB +DCB=100000 +LSBHIT=1 ENDIF -IF _LRB=0 -MG "HIGH ANGLE LIMIT SWITCH MAIN DRIVE IS ACTIVATED" {EB} ENDIF -ST -RE \ No newline at end of file +IF (_LFC=0)|(_LRC=0) +IF (LSCHIT=0)&(_BGC=1) +OLDDCC=_DCC +DCC=100000 +LSCHIT=1 +ENDIF +ENDIF +IF (_LFD=0)|(_LRD=0) +IF (LSDHIT=0)&(_BGD=1) +OLDDCD=_DCD +DCD=600000;'SPD=150000 +LSDHIT=1 +ENDIF +ENDIF +'IF (_LFE=0)|(_LRE=0) +'IF (LSEHIT=0)&(_BGE=1) +'OLDDCE=_DCE +'DCE=200000 +'LSEHIT=1 +'ENDIF +'ENDIF +IF (_LFF=0)|(_LRF=0) +IF (LSFHIT=0)&(_BGF=1) +OLDDCF=_DCF +DCF=200000;'SPF=50000 +LSFHIT=1 +RUNF=-14;'ERROR FLAG FOR SICS +ENDIF +ENDIF +'IF (_LFG=0)|(_LRG=0) +'IF (LSGHIT=0)&(_BGG=1) +'OLDDCG=_DCG +'DCG=200000 +'LSGHIT=1 +'ENDIF +'ENDIF +IF (_LFH=0)|(_LRH=0) +IF (LSHHIT=0)&(_BGH=1) +OLDDCH=_DCH +DCH=200000 +LSHHIT=1 +ENDIF +ENDIF +RE +'