'TAIPAN - CONTROLLER 6
'
'AUTHOR - DANIEL BARTLETT
'DATE - 23.02.2011
'
'A-SLIT S1 LEFT BLADE
'B-SLIT S1 RIGHT BLADE
'C-SLIT S1 TOP BLADE
'D-SLIT S1 BOTTOM BLADE
'E-SLIT S2 LEFT BLADE
'F-SLIT S2 RIGHT BLADE
'G-SLIT S2 TOP BLADE
'H-SLIT S2 BOTTOM BLADE
'
'
'-----------------------------------------------------------------------------
#AUTO
'THIS IS THE PROGRAM THAT RUNS AUTOMATICALLY ON CONTROLLER STARTUP
'
MT-2,-2,-2,-2,-2,-2,-2,-2;'CONFIGURES ALL AXES AS STEPPER
MO;'ALL MOTORS OFF
CN1;'CONFIGURES LIMIT SWITCHES. NC CONTACTS USE CN1. NO CONTACT USE CN-1
II 8,,,0;'CONFIGURES THE 8TH INPUT TO ACTIVATE THE #ININT ROUTINE
OP 65281;'SETS OUTPUTS 9-16 ON = RELAYS OFF
IA 137,157,203,136;'CONFIGURES IP ADDRESS
AC 60416,60416,60416,60416,60416,60416,60416,60416;' SET ACELERATION
DC 60416,60416,60416,60416,60416,60416,60416,60416;' SET DECELERATION
SP 30000,30000,30000,30000,30000,30000,30000,30000;' 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,13,0<-26>2;'HEIDENHAIN ROQ425
'SIA=1,25,25,0<-26>2;'HEIDENHAIN RCN226
'SIA=1,25,12,-1<-26>1;'VEGA RESOLVER TO SSI CONVERTER
'SIA=1,25,13,0<-26>1;'IVO GXM2H
'SIA=1,25,13,0<-26>2;'KUEBLER 5863
'SIA=1,31,13,0<26>2;'IVO STRING PULL, 31 BIT CONTROLLER REQUIRED
'SIA=1,24,18,6<-26>2;'IVO PLATYPUS COLLIMATION TILT
'SIA=1,15,15,0<-26>2;'GURLEY 15BIT
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
S1HOMED=0;'SLIT S1 NOT HOMED
S2HOMED=0;'SLIT S2 NOT HOMED
HOMERUN=0;'HOME ROUTINES NOT RUN
JS #THREAD0
EN
'
'
'-----------------------------------------------------------------------------
#THREAD0
'THERE MUST BE A THREAD RUNNING FOR #LIMSWI, #TCPERR AND #ININT TO WORK
'
JS #CHKDCEL
JS #CHKSPD
JP #THREAD0
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
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
'
'
'-----------------------------------------------------------------------------
#CHKSPD
'ROUTINE TO CHECK THE SPEED OF THE AXES IS NOT GREATER THAN MAX ALLOWED
'
IF (_SPA>30000)
SPA=30000
ENDIF
'
IF (_SPB>30000)
SPB=30000
ENDIF
'
IF (_SPC>30000)
SPC=30000
ENDIF
'
IF (_SPD>30000)
SPD=30000
ENDIF
'
IF (_SPE>30000)
SPE=30000
ENDIF
'
IF (_SPF>30000)
SPF=30000
ENDIF
'
IF (_SPG>30000)
SPG=30000
ENDIF
'
IF (_SPH>30000)
SPH=30000
ENDIF
EN
'
'
'-----------------------------------------------------------------------------
#TCPERR
'EMPTY TCP ERROR ROUTINE
'
RE
'
'
'-----------------------------------------------------------------------------
#ININT
'INTERUPT ROUTINE TO STOP ALL AXES
'WHEN SAFETY & INTERLOCK SYSTEM REQUESTS IT
CN-1;'RECONFIGURES LIMITS. RESULT:ALL LIMITS SEEN AS HIT. WILL RUN #LIMSWI
WT250
CN1;'SETS LIMITS BACK TO NORMAL
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=100000
LSDHIT=1
ENDIF
ENDIF
'
IF (_LFE=0)|(_LRE=0)
IF (LSEHIT=0)&(_BGE=1)
OLDDCE=_DCE
DCE=100000
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=100000
LSGHIT=1
ENDIF
ENDIF
'
IF (_LFH=0)|(_LRH=0)
IF (LSHHIT=0)&(_BGH=1)
OLDDCH=_DCH
DCH=100000
LSHHIT=1
ENDIF
ENDIF
RE
'
'
'-----------------------------------------------------------------------------
#MOVE
'THIS ROUTINE IS USED TO MOVE AN AXIS WITH A SSI ENCODER
'USING A MOVE-CHECK-CORRECT METHOD
'THE FOLLOWING VARIABLES MUST BE INITIALISED FIRST
'~a=0 TO 7 SETS THE MOTOR, 0=A AXIS 1=B AXIS ETC
'~b=0 TO 7 SETS THE ENCODER, 0=1ST SSI CHANNEL 1=2ND CHANNEL ETC
'TRIES= MAXIMUM NUMBER OF TRIES
'POS=DESIRED ENCODER POSITION
'
'~a=0:~b=0:TRIES=5:POS=12345678:XQ #MOVE,1
'
DBAND=3
SH~a
WT500;'WAIT TO STABILISE
IF (_BG~a=1);'MOVING, FORCE STOP. PRA WHEN MOVING CAUSES THREAD CRASH
ST~a
MC~a
ENDIF
TEMP=_TP~b;'SET TEMPORARY VARIABLE
PR~a=2500
IF (_LF~a=0);'ALREADY ON LIMIT
MG "Forward limit actuated" {EA}
MG "Ratio not calculated" {EA}
JP #MOVEEND
ENDIF
BG~a;'PERFORM A SMALL MOVE TO DETERMIN MOTOR:ENCODER RATIO
MC~a
WT500;'WAIT TO STABILISE
IF (TEMP=_TP~b);'FAILED TO MOVE
MG "No movement detected" {EA}
MG "Ratio not calculated" {EA}
JP #MOVEEND
ENDIF
RATIO=(TEMP-_TP~b)*10;'SETS ENCODER/MOTOR RATIO
'
#LOOP
IF (_BG~a=1);'MOVING, FORCE STOP. PRA WHEN MOVING CAUSES THREAD CRASH
ST~a
MC~a
ENDIF
PR~a=25000*((_TP~b-POS)/RATIO);'CALC STEPS TO DESTINATION
IF ((_PR~a>0)&(_LF~a=0));'IF POSITIVE MOVE AND ALREADY ON LIMIT
MG "Forward limit actuated" {EA}
MG "Positive move aborted" {EA}
JP #MOVEEND
ENDIF
IF ((_PR~a<0)&(_LR~a=0));'IF NEGATIVE MOVE AND ALREADY ON LIMIT
MG "Reverse limit actuated" {EA}
MG "Negative move aborted" {EA}
JP #MOVEEND
ENDIF
IF (_BG~a=1);'ALREADY MOVING
ST~a
MC~a
ENDIF
SH~a
BG~a
MC~a;'WAIT FOR MOTION TO COMPLETE
TRIES=TRIES-1;'INCREMENT ATTEMPT COUNTER
MG "Attempts to go: " {N} {EA}
MG TRIES {F1.0} {EA}
JP #LOOP, ((@ABS[_TP~b-POS]>DBAND)&(TRIES>0))
'HAVE ANOTHER GO IF OUTSIDE DEAD BAND AND NOT TOO MANY ATTEMPTS
#MOVEEND
MG "TARGET   :" {N} {EA}
MG POS {F8.0} {EA}
MG "POSITION :" {N} {EA}
MG _TP~b {F8.0} {EA}
MG "Program ends" {EA}
EN
'
'
'-----------------------------------------------------------------------------
#HOME
'THIS ROUTINE CALLS THE HOME ROUTINES FOR EACH SLIT PACKAGE IN TURN
'IT IS INVOKED BY SICS AS: XQ #HOME,7
IF (HOMERUN=1)
JP #ENDHOME
ENDIF
JS #HOMES1
JS #HOMES2
IF (S1HOMED=1)&(S2HOMED=1)
HOMERUN=1
ENDIF
#ENDHOME
EN
'-----------------------------------------------------------------------------
#HOMES1
'THIS ROUTINE HOMES THE S1 SLIT PACKAGE
'
IF (S1HOMED=1);'ALREADY HOMED
JP #ENDS1
ENDIF
'
IF (_LFA=0)&(_LRA=0)
'BOTH LIMITS ACTUATED. SLIT PACKAGE PROBABLY NOT PLUGGED IN.
JP #ENDS1
ENDIF
'
SHABCD;'TURN ON THE MOTORS
SP 30000,30000,30000,30000,,,,;'SET SPEED
IF (_LRA=0);'ON THE REVERSE LIMIT SWITCH
PRA=250*161/2*10;'MOVE AWAY 10mm
BGA
ENDIF
'
IF (_LRB=0);'ON THE REVERSE LIMIT SWITCH
PRB=250*161/2*10;'MOVE AWAY 10mm
BGB
ENDIF
'
IF (_LRC=0);'ON THE REVERSE LIMIT SWITCH
PRC=250*161/2*10;'MOVE AWAY 10mm
BGC
ENDIF
'
IF (_LRD=0);'ON THE REVERSE LIMIT SWITCH
PRD=250*161/2*10;'MOVE AWAY 10mm
BGD
ENDIF
'
MCABCD;'WAIT FOR MOTION TO BE COMPLETE
JG -30000,-30000,-30000,-30000,,,,;'JOG INTO REVERSE LIMIT
BGABCD
MCABCD
PR 80500,80500,80500,80500,,,,;'MOVE AWAY 4MM
BGABCD
MCABCD
JG -5000,-5000,-5000,-5000,,,,;'JOG SLOWLY INTO LIMITS
BGABCD
MCABCD
DP 0,0,0,0,,,,;'DEFINE HOME POSITION
SP 30000,30000,30000,30000,,,,;'SET SPEED BACK TO NORMAL
S1HOMED=1;'HOMING OF SLIT S1 COMPLETE
#ENDS1
EN
'
'
'-----------------------------------------------------------------------------
#HOMES2
'THIS ROUTINE HOMES THE S2 SLIT PACKAGE
'
IF (S2HOMED=1);'ALREADY HOMED
JP #ENDS2
ENDIF
'
IF (_LFE=0)&(_LRE=0)
'BOTH LIMITS ACTUATED. SLIT PACKAGE PROBABLY NOT PLUGGED IN.
JP #ENDS2
ENDIF
'
SHEFGH;'TURN ON THE MOTORS
SP ,,,,30000,30000,30000,30000;'SET SPEED
IF (_LRE=0);'ON THE REVERSE LIMIT SWITCH
PRE=250*161/2*10;'MOVE AWAY 10mm
BGE
ENDIF
'
IF (_LRF=0);'ON THE REVERSE LIMIT SWITCH
PRF=250*161/2*10;'MOVE AWAY 10mm
BGF
ENDIF
'
IF (_LRG=0);'ON THE REVERSE LIMIT SWITCH
PRG=250*161/2*10;'MOVE AWAY 10mm
BGG
ENDIF
'
IF (_LRH=0);'ON THE REVERSE LIMIT SWITCH
PRH=250*161/2*10;'MOVE AWAY 10mm
BGH
ENDIF
'
MCEFGH;'WAIT FOR MOTION TO BE COMPLETE
JG ,,,,-30000,-30000,-30000,-30000;'JOG INTO REVERSE LIMIT
BGEFGH
MCEFGH
PR ,,,,80500,80500,80500,80500;'MOVE AWAY 4MM
BGEFGH
MCEFGH
JG ,,,,-5000,-5000,-5000,-5000;'JOG SLOWLY INTO LIMITS
BGEFGH
MCEFGH
DP ,,,,0,0,0,0;'DEFINE HOME POSITION
SP ,,,,30000,30000,30000,30000;'SET SPEED BACK TO NORMAL
S2HOMED=1;'HOMING OF SLIT S1 COMPLETE
#ENDS2
EN
'
'
'-----------------------------------------------------------------------------