'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 SHF/MOF 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
SHF;' 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
'