667 lines
17 KiB
Plaintext
Executable File
667 lines
17 KiB
Plaintext
Executable File
'TAIPAN - CONTROLLER 1
|
|
'
|
|
'$Revision:$
|
|
'$Date:$
|
|
'Author: Daniel Bartlett
|
|
'Last revision by: $dbx:$
|
|
'
|
|
'A-MONOCHROMATOR TRANSLATION - TILT
|
|
'B-MONOCHROMATOR TRANSLATION - LINEAR
|
|
'C-SPARE
|
|
'D-MAGNET ARM ROTATE
|
|
'E-30' COLLIMATOR ROTATE, NOT ENCODED
|
|
'F-FLIGHT TUBE ROTATE
|
|
'G-15' COLLIMATOR ROTATE, NOT ENCODED. ENCODER IS FLIGHT TUBE BACK OF MOTOR
|
|
'H-DETECTOR ROTATE _TPH=18120009 BEAM CL
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#AUTO
|
|
'THIS PROGRAM RUNS AUTOMATICALLY ON CONTROLLER START UP
|
|
'
|
|
ERRORLEV=0;'SETS ERROR LEVEL TO "INITIALISING"
|
|
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
|
|
OP 65280;'SETS OUTPUTS 9-16 ON = RELAYS OFF
|
|
IA 137,157,203,131;'CONFIGURES IP ADDRESS
|
|
AC 25000,25000,,150000,12500,5000,12500,25000;'SET ACELERATION
|
|
DC 25000,25000,,150000,12500,5000,12500,25000;'SET DECELERATION
|
|
SP 25000,25000,,150000,25000,50000,25000,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,24,12,0<-2>1;'RESOLVER VIA VEGA 2790 BOARD
|
|
SIB=1,24,12,0<-2>1;'RESOLVER VIA VEGA 2790 BOARD
|
|
SID=1,24,12,0<-2>1;'RESOLVER VIA VEGA 2790 BOARD
|
|
SIF=1,25,13,0<-2>2;'KUEBLER 5863
|
|
SIG=1,25,13,0<-2>1;'IVO GXM2H
|
|
SIH=1,25,25,0<-2>2;'HEIDENHAIN RCN226
|
|
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
|
|
'INITIALISE THE BIT FOR AXIS F AIRPAD CONTROL
|
|
REQF=0
|
|
RSPF=0
|
|
'INITIALISE THE BIT FOR AXIS H AIRPAD CONTROL
|
|
REQH=0
|
|
RSPH=0
|
|
HMTAPEDN=0;'TAPE HOME ROUTINE NOT RUN
|
|
HMWEDGDN=0;'WEDGE HOME ROUTINE NOT RUN
|
|
ANGLE=((_TPF-190161)*-0.006696)+25.77375473;'SET ANGLE TO CURRENT POSITION
|
|
'ERRORLEV=-1;'COMPLETED INITIALISATION
|
|
II 6,8,,0;'CONFIGURES THE 6TH, 7TH & 8TH INPUT TO ACTIVATE #ININT ROUTINE
|
|
ERRORLEV=0;'THIS SETS TO "MANUAL MODE" FOR SPICE, NO WEDGES MOVED
|
|
JS #THREAD0
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#THREAD0
|
|
'THREAD THAT CALLS HOMING & WEDGE CONTROL THREADS & RUNS GENERAL ROUTINES
|
|
'
|
|
IF ((_XQ1=-1)&(ERRORLEV=-1))
|
|
'THREAD 1 NOT USED AND NO ERRORS
|
|
XQ #WDGCTRL,1;'EXECUTE WEDGE CONTROL ROUTINE IN THREAD 1
|
|
ENDIF
|
|
IF (ERRORLEV>0);'IN ERROR
|
|
HX1;'HALT EXECUTION OF THREAD 1
|
|
STDF;'STOP MAGNET ARM AND FLIGHT TUBE
|
|
ENDIF
|
|
JS #CHKDCEL;'CHECKS AND RESETS DECELERATIONS AFTER A LIMIT HIT
|
|
JS #CHKSPD;'CHECKS SPEEDS TO SEE IF ABOVE MAXIMUMS
|
|
JS #AIRPAD;'CONTROLS THE AIRPADS ON AXIS F & H
|
|
JP #THREAD0
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#WDGCTRL
|
|
'ROUTINE TO CONTROL THE CO-ORDINATED MOTION OF THE FLIGHT TUBE AND MAGNET ARM.
|
|
'THE NUMBER OF WEDGES TO BE MOVED IS CALCULATED FROM THE DESTINATION POSITION.
|
|
'THE WEDGES ARE MOVED ALL AT ONCE, THEN THE FLIGHT TUBE CONTINUES TO THE
|
|
'FINAL POSITION.
|
|
'
|
|
JS #CALCANG;'CALCULATE DIRECTION AND DESTINATION IN ENCODER COUNTS
|
|
IF (DIRF=0);'IN POSITION
|
|
MO DF;'TURN OFF THE MAGNET ARM AND FLIGHT TUBE
|
|
JP #ENDWC;'JUMP TO THE END
|
|
ENDIF
|
|
'
|
|
FINPROG=1;'F AXIS IN PROGRESS BIT SET TO TRUE
|
|
IF (DIRF=1);'HEADING CLOCKWISE
|
|
IF ((DESTF<191710)&(DESTF>186185));'NEEDS 3 LEFT, 0 RIGHT (15-52DEG)
|
|
IF ((_TPF<191710)&(_TPF>186185));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE;'WEDGES NEED TO BE MOVED
|
|
FTPOS=186511;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 3 R UNLATCH POINT
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF ((DESTF<186186)&(DESTF>184094));'NEEDS 2 LEFT, 1 RIGHT (52-66DEG)
|
|
IF ((_TPF<186186)&(_TPF>184094));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE;'WEDGES NEED TO BE MOVED
|
|
FTPOS=184421;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 2 R UNLATCH POINT
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF ((DESTF<184095)&(DESTF>182003));'NEEDS 1 LEFT, 2 RIGHT (66-80DEG)
|
|
IF ((_TPF<184095)&(_TPF>182003));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE;'WEDGES NEED TO BE MOVED
|
|
FTPOS=182255;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 1 R UNLATCH POINT
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF ((DESTF<182004)&(DESTF>181256));'NEEDS 0 LEFT, 3 RIGHT (80-85DEG)
|
|
IF ((_TPF<182004)&(_TPF>181256));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE
|
|
'DESTINATION IN RANGE REQUIRING 0 WEDGES LEFT, 3 WEDGES RIGHT (80-85DEG)
|
|
'AND NOT CURRENTLY WITHIN THE RANGE AND HEADING CLOCKWISE IS NOT POSSIBLE
|
|
'A COUNTER CLOCKWISE ROTATION IS THE ONLY WAY INTO THIS ZONE
|
|
ERRORLEV=51;' SET ERROR
|
|
JP #ENDWC
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF (@IN[14]=0);'WEDGE SHOULD BE IN UNLATCH POS BUT RIGHT SWITCH NOT MADE
|
|
ERRORLEV=41;'SET ERROR, JUMP OUT
|
|
JP #ENDWC
|
|
ENDIF
|
|
ARMPOS=2897;ARMTRIES=0;JS #ARMMOVE;'SEND ARM TO R PICK UP POINT
|
|
CB16;'TURN MAGNET ON
|
|
WT 500;'WAIT 1/2 SEC FOR MAGNET TO LATCH
|
|
ARMPOS=1061;ARMTRIES=0;JS #ARMMOVE;'SEND ARM+ALL WEDGES TO L DROP OFF POINT
|
|
SB16;'TURN MAGNET OFF
|
|
JP #GOFINAL;'WEDGES MOVED, GO TO FINAL POSTION
|
|
ENDIF
|
|
'
|
|
IF (DIRF=-1);'HEADING COUNTER CLOCKWISE
|
|
IF ((DESTF<191710)&(DESTF>186185));'NEEDS 3 LEFT, 0 RIGHT (15-52DEG)
|
|
IF ((_TPF<191710)&(_TPF>186185));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE
|
|
'DESTINATION IN RANGE REQUIRING 3 WEDGES LEFT, 0 WEDGES RIGHT (15-52DEG)
|
|
'AND NOT CURRENTLY IN THE RANGE AND HEADING COUNTER CLOCKWISE IS NOT POSSIBLE
|
|
'A COUNTER CLOCKWISE ROTATION IS THE ONLY WAY INTO THIS ZONE
|
|
ERRORLEV=52;'SET ERROR
|
|
JP #ENDWC
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF ((DESTF<186186)&(DESTF>184094));'NEEDS 2 LEFT, 1 RIGHT (52-66DEG)
|
|
IF ((_TPF<186186)&(_TPF>184094));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE;'WEDGES NEED TO BE MOVED
|
|
FTPOS=185914;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 3 LEFT UNLATCH POINT
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF ((DESTF<184095)&(DESTF>182003));'NEEDS 1 LEFT, 2 RIGHT (66-80DEG)
|
|
IF ((_TPF<184095)&(_TPF>182003));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE;'WEDGES NEED TO BE MOVED
|
|
FTPOS=183823;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 2 LEFT UNLATCH POINT
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF ((DESTF<182004)&(DESTF>181256));'NEEDS 0 LEFT, 3 RIGHT (80-85DEG)
|
|
IF ((_TPF<182004)&(_TPF>181256));'CURRENTLY IN THE RANGE. NO WEDGES MOVED
|
|
JP #GOFINAL
|
|
ELSE;'WEDGES NEED TO BE MOVED
|
|
FTPOS=181900;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 1 LEFT UNLATCH POINT
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF (@IN[13]=0);'WEDGE SHOULD BE IN UNLATCH POS BUT LEFT SWITCH NOT MADE
|
|
ERRORLEV=42;'SET ERROR, JUMP OUT
|
|
JP #ENDWC
|
|
ENDIF
|
|
ARMPOS=1206;ARMTRIES=0;JS #ARMMOVE;'SEND ARM TO L PICK UP POINT
|
|
CB16;'TURN MAGNET ON
|
|
WT 500;'WAIT 1/2 SEC FOR MAGNET TO LATCH
|
|
ARMPOS=3060;ARMTRIES=0;JS #ARMMOVE;'SEND ARM+ALL WEDGES TO R DROP OFF POINT
|
|
SB16;'TURN MAGNET OFF
|
|
JP #GOFINAL;'WEDGES MOVED, GO TO FINAL POSTION
|
|
ENDIF
|
|
'
|
|
#GOFINAL
|
|
FINTRIES=0;JS #FINE;'FINE TUNE FLIGHT TUBE
|
|
#ENDWC;'END OF THREAD WEGCTRL
|
|
FINPROG=0;'F AXIS IN PROGRESS BIT SET TO FALSE
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#CALCANG
|
|
'ROUTINE TO CALCULATE THE SSI AND TAPE POSITIONS FROM THE 'ANGLE' VARIABLE
|
|
'
|
|
DBAND3=1;'SETS TAPE ENCODER DEAD BAND
|
|
DESTF=((ANGLE-25.77375473)/-0.006696)+190161
|
|
'CALCULATE ENCODER VALUE FROM USER INPUTTED ANGLE VARIABLE
|
|
IF ((DESTF>191710)|(DESTF<181508))
|
|
'OUTSIDE HARD LIMITS
|
|
ERRORLEV=4
|
|
WT500
|
|
ENDIF
|
|
IF (@ABS[_TPF-DESTF]<DBAND3);'WITHIN DEAD BAND
|
|
DIRF=0;'IN POSITION
|
|
ELSE
|
|
IF (DESTF>_TPF)
|
|
DIRF=1;'CLOCKWISE ROTATION NEEDED
|
|
ENDIF
|
|
IF (DESTF<_TPF)
|
|
DIRF=-1;'COUNTER CLOCKWISE ROTATION NEEDED
|
|
ENDIF
|
|
ENDIF
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#FTMOVE
|
|
'ROUTINE TO MOVE THE FLIGHT TUBE USING A MOVE-CHECK-CORRECT METHOD
|
|
'
|
|
'@IN[13]=0 IS THE LEFT CAM SWITCH IN THE UNACTUATED POSITION
|
|
'@IN[14]=0 IS THE RIGHT CAM SWITCH IN THE UNACTUATED POSITION
|
|
'
|
|
DBAND1=1
|
|
PRF=25000*((FTPOS-_TPF)/-30.92);'CALC STEPS TO DESTINATION
|
|
IF ((_PRF>0)&(_LFF=0));'IF POSITIVE MOVE AND ALREADY ON LIMIT
|
|
ERRORLEV=2;'SET ERROR, JUMP OUT
|
|
JP #FTEND
|
|
ENDIF
|
|
IF ((_PRF<0)&(_LRF=0));'IF NEGATIVE MOVE AND ALREADY ON LIMIT
|
|
ERRORLEV=3;'SET ERROR, JUMP OUT
|
|
JP #FTEND
|
|
ENDIF
|
|
SHF
|
|
BGF
|
|
MCF;'WAIT FOR MOTION TO COMPLETE
|
|
FTTRIES=FTTRIES+1;'INCREMENT ATTEMPT COUNTER
|
|
IF (FTTRIES>4);'IF TOO MANY ATTEMPTS
|
|
ERRORLEV=1;'SET ERROR, JUMP OUT
|
|
JP #FTEND
|
|
ENDIF
|
|
JP #FTMOVE, (@ABS[_TPF-FTPOS]>DBAND1)
|
|
'JUMP BACK FOR MORE TRIES IF OUTSIDE DEADBAND1
|
|
#FTEND
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#ARMMOVE
|
|
'ROUTINE TO MOVE THE MAGNET ARM WITH A CLOSED LOOP RESOLVER POSITION
|
|
'
|
|
'CW LIMIT 122
|
|
'CCW LIMIT 3373
|
|
'
|
|
'FT COUNTER CLOCKWISE
|
|
'LEFT PICK UP POINT 1206
|
|
'TO THE
|
|
'RIGHT DROP OFF POINT 3060
|
|
'
|
|
'FT CLOCKWISE
|
|
'RIGHT PICK UP POINT 2897
|
|
'TO THE
|
|
'LEFT DROP OFF POINT 1061
|
|
'
|
|
'
|
|
DBAND2=2;'HEAVILY GEARED, DEADBAND OF 2 ACHIEVABLE
|
|
PRD=25000*(((_TPD&4095)-ARMPOS)/13.5);'CALC STEPS TO DESTINATION
|
|
'&4095 MASKS OUT UNUSED 12 MOST SIGNIFICANT BITS
|
|
IF ((_PRD>0)&(_LFD=0));'IF POSITIVE MOVE AND ALREADY ON LIMIT
|
|
ERRORLEV=12;'SET ERROR, JUMP OUT
|
|
JP #ARMEND
|
|
ENDIF
|
|
IF ((_PRD<0)&(_LRD=0));'IF NEGATIVE MOVE AND ALREADY ON LIMIT
|
|
ERRORLEV=13;'SET ERROR, JUMP OUT
|
|
JP #ARMEND
|
|
ENDIF
|
|
ACD=75000
|
|
DCD=75000
|
|
SPD=150000
|
|
SHD
|
|
BGD
|
|
MCD;'WAIT FOR MOTION TO COMPLETE
|
|
ARMTRIES=ARMTRIES+1;'INCREMENT ATTEMPT COUNTER
|
|
IF (ARMTRIES>4);'IF TOO MANY ATTEMPTS
|
|
ERRORLEV=11;'SET ERROR, JUMP OUT
|
|
JP #ARMEND
|
|
ENDIF
|
|
JP #ARMMOVE, (@ABS[ARMPOS-(_TPD&4095)]>DBAND2)
|
|
'JUMP BACK FOR MORE TRIES IF OUTSIDE DEADBAND2
|
|
MOD;'TURN MOTOR OFF
|
|
#ARMEND
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#FINE
|
|
'ROUTINE TO MOVE THE FLIGHT TUBE USING A MOVE-CHECK-CORRECT METHOD
|
|
'AND THEN TURN OFF AIRPADS ONCE IN POSITION
|
|
'
|
|
'70.24 DEGREES TOTAL FOR 64 SEGMENTS
|
|
'2 ADJACENT SEGMENTS = 40000 COUNTS
|
|
'TOTAL COUNTS FOR 70.24 DEGREES = 1,280,000
|
|
'0.000054875 DEGREES PER COUNT
|
|
PRF=25000*((DESTF-_TPF)/-30.92);'CALC STEPS TO DESTINATION
|
|
IF ((_PRF>0)&(_LFF=0));'IF POSITIVE MOVE AND ALREADY ON LIMIT
|
|
ERRORLEV=22;'SET ERROR, JUMP OUT
|
|
JP #FINEEND
|
|
ENDIF
|
|
IF ((_PRF<0)&(_LRF=0));'IF NEGATIVE MOVE AND ALREADY ON LIMIT
|
|
ERRORLEV=23;'SET ERROR, JUMP OUT
|
|
JP #FINEEND
|
|
ENDIF
|
|
SHF
|
|
BGF
|
|
MCF;'WAIT FOR MOTION TO COMPLETE
|
|
WT2000;'WAIT TO SETTLE
|
|
FINTRIES=FINTRIES+1;'INCREMENT ATTEMPT COUNTER
|
|
IF (FINTRIES>4);'IF TOO MANY ATTEMPTS
|
|
ERRORLEV=21;'SET ERROR, JUMP OUT
|
|
JP #FINEEND
|
|
ENDIF
|
|
JP #FINE, (@ABS[_TPF-DESTF]>DBAND3)
|
|
'JUMP BACK FOR MORE TRIES IF OUTSIDE DEADBAND3
|
|
MOF;' TURN OFF FLIGHT TUBE MOTOR (AND HENCE AIRPADS)
|
|
#FINEEND
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#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;'RESET LIMIT HIT INDICATOR BIT
|
|
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
|
|
'ADAPTED FROM ECHIDNA AIRPAD CONTROL BY DOUGLAS CLOWES
|
|
'
|
|
'CONTROLS THE AIRPAD SOLENOID VALVES FOR SAMPLE STAGE
|
|
' FOLLOW SHF/MOF COMMANDS ENTERED MANUALLY
|
|
IF (_MOF=0)&(RSPF=0);'MOTOR IS ON BUT AIR IS OFF
|
|
REQF=1
|
|
ENDIF
|
|
IF (_MOF=1)&(RSPF=1);'MOTOR IS OFF BUT AIR IS ON
|
|
SHF;' FOLLOWING STEPS MUST BE RUN BEFORE MOTOR CAN BE TURNED OFF
|
|
REQF=0
|
|
ENDIF
|
|
'
|
|
IF (REQF=1)&(RSPF=0);'REQUESTING SAMPLE STAGE MOTOR BE MOVABLE
|
|
SHF;' TURN SAMPLE STAGE MOTOR ON
|
|
WT500;' WAIT FOR MOTOR TO STABILISE
|
|
SB 10;'SAMPLE STAGE AIR PAD OFF-SOLENOID OFF
|
|
CB 9;'SAMPLE STAGE AIR PAD ON-SOLENOID ON
|
|
WT1000;
|
|
RSPF=1
|
|
ENDIF
|
|
IF (REQF=0)&(RSPF=1)&(_BGF=0);'MOTION FINISHED
|
|
WT1000;' WAIT FOR MOTOR TO SETTLE
|
|
SB 9;'SAMPLE STAGE AIR PAD ON-SOLENOID OFF
|
|
CB 10;'SAMPLE STAGE AIR PAD OFF-SOLENOID ON
|
|
WT500;'WAIT 1/2 SEC
|
|
SB 10;'SAMPLE STAGE AIR PAD OFF-SOLENOID OFF
|
|
MOF;' TURN SAMPLE STAGE MOTOR OFF
|
|
RSPF=0
|
|
ENDIF
|
|
'
|
|
'CONTROLS THE AIRPAD SOLENOID VALVES FOR DETECTOR STAGE
|
|
' FOLLOW SHH/MOH COMMANDS ENTERED MANUALLY
|
|
IF (_MOH=0)&(RSPH=0);'MOTOR IS ON BUT AIR IS OFF
|
|
REQH=1
|
|
ENDIF
|
|
IF (_MOH=1)&(RSPH=1);'MOTOR IS OFF BUT AIR IS ON
|
|
SHH;' FOLLOWING STEPS MUST BE RUN BEFORE MOTOR CAN BE TURNED OFF
|
|
REQH=0
|
|
ENDIF
|
|
'
|
|
IF (REQH=1)&(RSPH=0);'REQUESTING DETECTOR MOTOR BE MOVABLE
|
|
SHH;' TURN MOTOR ON
|
|
WT500;' WAIT FOR MOTOR TO STABILISE
|
|
SB 12;'DETECTOR AIR PAD OFF-SOLENOID OFF
|
|
CB 11;'DETECTOR AIR PAD ON-SOLENOID ON
|
|
WT1000;
|
|
RSPH=1
|
|
ENDIF
|
|
IF (REQH=0)&(RSPH=1)&(_BGH=0);'MOTION FINISHED
|
|
WT1000;' WAIT FOR MOTOR TO SETTLE
|
|
SB 11;'DETECTOR AIR PAD ON-SOLENOID OFF
|
|
CB 12;'DETECTOR AIR PAD OFF-SOLENOID ON
|
|
WT500;'WAIT 1/2 SEC
|
|
SB 12;'DETECTOR AIR PAD OFF-SOLENOID OFF
|
|
MOH;' TURN MOTOR OFF
|
|
RSPH=0
|
|
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>25000)
|
|
SPE=25000
|
|
ENDIF
|
|
'
|
|
IF (_SPF>50000)
|
|
SPF=50000
|
|
ENDIF
|
|
'
|
|
IF (_SPG>25000)
|
|
SPG=25000
|
|
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
|
|
'TO STOP AXIS F AND H WHEN LIGHT CURTAINS BROKEN BUT TO ALLOW MOVEMENT AWAY
|
|
'
|
|
IF @IN[8]=0
|
|
'SAFETY INTERLOCK SYSTEM ALL AXES SLOWDOWN
|
|
CN-1;'RECONFIGURES LIMITS. RESULT:ALL LIMITS SEEN AS HIT. WILL RUN #LIMSWI
|
|
WT250
|
|
CN1;'SETS LIMITS BACK TO NORMAL
|
|
ERRORLEV=100
|
|
JP #ENININT
|
|
ENDIF
|
|
'
|
|
IF @IN[6]=0
|
|
'LOW ANGLE LIGHT CURTAIN BROKEN
|
|
IF _TVF>0
|
|
'AXIS HEADING TOWARDS LIGHT CURTAIN
|
|
STF
|
|
ENDIF
|
|
IF _TVH>0
|
|
'AXIS HEADING TOWARDS LIGHT CURTAIN
|
|
STH
|
|
'ERRORLEV=101
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
IF @IN[7]=0
|
|
'HIGH ANGLE LIGHT CURTAIN BROKEN
|
|
IF _TVF<0
|
|
'AXIS HEADING TOWARDS LIGHT CURTAIN
|
|
STF
|
|
ENDIF
|
|
IF _TVH<0
|
|
'AXIS HEADING TOWARDS LIGHT CURTAIN
|
|
STH
|
|
'ERRORLEV=102
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
#ENININT
|
|
RI
|
|
'
|
|
'
|
|
'--------------------------------------------------------------------------
|
|
#CMDERR
|
|
'ROUTINE TO HANDLE COMMAND ERRORS
|
|
'
|
|
ZS0
|
|
EN1
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#LIMSWI
|
|
'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
|
|
ENDIF
|
|
'
|
|
IF (_LFB=0)|(_LRB=0)
|
|
IF (LSBHIT=0)&(_BGB=1)
|
|
OLDDCB=_DCB
|
|
DCB=100000
|
|
LSBHIT=1
|
|
ENDIF
|
|
ENDIF
|
|
'
|
|
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=100000
|
|
LSFHIT=1
|
|
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
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
#HMWEDGE
|
|
'ROUTINE TO GET WEDGES TO A KNOW POSITION
|
|
'THE FLIGHT TUBE IS MOVED TO THE RIGHT UNLATCH POSITION OF WEDGE 3
|
|
'THE MAGNET ARM IS THEN MOVED FROM THE RIGHT PICK UP POINT TO THE LEFT DROP
|
|
'OFF POINT WITH THE MAGNET ON. THIS WILL PICK UP ALL THE WEDGES NO MATTER
|
|
'WHERE THEY ARE, AND DRAG THEM AROUND TO THE LEFT DROP OFF POINT.
|
|
'
|
|
FTPOS=186511;FTTRIES=0;JS #FTMOVE;'GO TO WEDGE 3 RIGHT UNLATCH POSITION
|
|
ARMPOS=2897;ARMTRIES=0;JS #ARMMOVE;'RIGHT PICK UP POINT
|
|
CB16;'TURN MAGNET ON
|
|
WT 500;'WAIT 1/2 SEC FOR MAGNET TO LATCH
|
|
ARMPOS=1061;ARMTRIES=0;JS #ARMMOVE;'LEFT DROP OFF, PICK UP WEDGES ENROUTE
|
|
SB16;'TURN MAGNET OFF
|
|
MOF
|
|
ANGLE=((_TPF-190161)*-0.006696)+25.37375473;'SET ANGLE TO CURRENT POSITION
|
|
HMWEDGDN=1;'SET DONE FLAG
|
|
EN
|
|
'
|
|
'
|
|
'-----------------------------------------------------------------------------
|
|
'ERROR LEVELS
|
|
'-1 NO ERROR, NORMAL CONDITION
|
|
'0 CONTROLLER INITIALISING - RUNNING AUTO ROUTINE
|
|
'1 #FTMOVE TOOK TOO MANY ATTEMPTS TO GET INTO POSITION
|
|
'2 #FTMOVE TUBE FORWARD LIMIT HIT AND DRIVE ATTEMPTED
|
|
'3 #FTMOVE TUBE REVERSE LIMIT HIT AND DRIVE ATTEMPTED
|
|
'4 ANGLE INPUT OUTSIDE HARD LIMITS
|
|
'5 #HMTAPE TOOK TOO MANY ATTMEPTS
|
|
'11 #ARMMOVE TOOK TOO MANY ATTEMPTS TO GET INTO POSITION
|
|
'12 #ARMMOVE FORWARD LIMIT HIT AND DRIVE ATTEMPTED
|
|
'13 #ARMMOVE REVERSE LIMIT HIT AND DRIVE ATTEMPTED
|
|
'21 #FINE TOOK TOO MANY ATTEMPTS TO GET INTO POSITION
|
|
'22 #FINE FORWARD LIMIT HIT AND DRIVE ATTEMPTED
|
|
'23 #FINE REVERSE LIMIT HIT AND DRIVE ATTEMPTED
|
|
'31 #TAPEHOME LIMITS FAILURE
|
|
'41 WEDGES SHOULD BE AT RIGHT PICK UP POINT BUT CAM SWITCH NOT MADE
|
|
'42 WEDGES SHOULD BE AT LEFT PICK UP POINT BUT CAM SWITCH NOT MADE
|
|
'51 CLOCKWISE MOVE INTO 82-85DEG RANGE NOT POSSIBLE
|
|
'52 COUNTER CLOCKWISE MOVE INTO 15-55DEG RANGE NOT POSSIBLE
|
|
'100 SAFETY AND INTERLOCK SYSTEM HAS INITIATED A SLOWDOWN BY ESTOP
|
|
'101 SAFETY AND INTERLOCK SYSTEM HAS INITIATED A SLOWDOWN BY LIGHT CURTAIN
|
|
'102 SAFETY AND INTERLOCK SYSTEM HAS INITIATED A SLOWDOWN BY LIGHT CURTAIN
|
|
'
|
|
|
|
|