Go to the documentation of this file.00001 CLOSE
00002
00003 ;###############################################
00004 ;; \file
00005 ;; Define motion for Energy in Accel DCM
00006 ;;
00007 ;; Original Author: Tom Cobb.
00008 ;; Modified by Emma Shepherd to add support for
00009 ;; backlash/hysterisis correction (see PROG_CS_motion_backlash.pmc)
00010 ;;
00011 ;; Defined axes:
00012 ;; - X (addr 6) = Energy in kEv
00013 ;; - Y (addr 7) = Offset in mm
00014 ;;
00015 ;; Macros (and example values):
00016 ;; - COORD = $(COORD) CS number (only works for CS 1..9), e.g. 2
00017 ;; - PLC = $(PLC) PLC number, should be CS number+15, e.g. 17
00018 ;; - BRAGG = $(BRAGG) Axisnum for Bragg, e.g. 2
00019 ;; - T2 = $(T2) Axisnum for T2, e.g. 3
00020 ;################################################
00021
00022 ; Change to CS$(COORD)
00023 &$(COORD)
00024
00025 ; Set relevant axes to use kinematics
00026 #$(BRAGG)->I
00027 #$(T2)->I
00028
00029 ; These are set by motor_in_cs.template
00030 #define BMOVE P(4700+$(BRAGG))
00031 #define BMRES P(4800+$(BRAGG))
00032 #define BOFF P(4900+$(BRAGG))
00033 #define TMOVE P(4700+$(T2))
00034 #define TMRES P(4800+$(T2))
00035 #define TOFF P(4900+$(T2))
00036
00037 ; These are set by the motion program for backlash correction
00038 #define BBDST Q(400+$(BRAGG))
00039 #define TBDST Q(400+$(T2))
00040
00041 ; Crystal d spacing in angstrom
00042 ; NOTE: default is silicon 111 at room temp,
00043 ; at liquid N2 it is 3.13475 or 3.13492 depending on who you believe...)
00044 #define DSPACING Q20
00045 DSPACING=3.1355
00046 ; Default mode is to move Bragg and T2
00047 #define LOCKT2 Q21
00048 LOCKT2=0
00049 ; EvLambda constant
00050 #define EVLAMBDA Q22
00051 EVLAMBDA=12.3985
00052
00053 ; Calculate Energy and Offset from Bragg and T2
00054 #define BPOS (BMRES*P$(BRAGG)+BOFF)
00055 #define TPOS (TMRES*P$(T2)+TOFF)
00056 #define ENERGY Q7
00057 #define OFFSET Q8
00058 #define SETERROR M(100*$(COORD)+5082)=1
00059 ; Local variables
00060 #define NLAMBDA Q128
00061 OPEN FORWARD
00062 CLEAR
00063 NLAMBDA=2*DSPACING*sin(BPOS)
00064 IF (ABS(NLAMBDA)>0.01)
00065 ENERGY = EVLAMBDA/NLAMBDA
00066 OFFSET = 2*TPOS*cos(BPOS)
00067 ELSE
00068 SETERROR
00069 ENDIF
00070 CLOSE
00071
00072 ; Calculate Bragg and T2 from Energy and Offset
00073 ; Local variables
00074 #define SINTHETA Q228
00075 #define BPOS Q229
00076 #define TPOS Q230
00077 OPEN INVERSE
00078 CLEAR
00079 SINTHETA=EVLAMBDA/(2*DSPACING*ENERGY)
00080 IF (ABS(SINTHETA)<1)
00081 ; calculate bragg in EGUs
00082 BPOS=asin(SINTHETA)
00083 ; then in cts
00084 P$(BRAGG)=(BPOS+BBDST-BOFF)/BMRES
00085 ; Don't calculate T2 if we are in 'locked T2' mode
00086 IF (LOCKT2=0)
00087 ; calculate T2 in EGUs
00088 TPOS=OFFSET/(2*cos(asin(SINTHETA)))
00089 ; then in cts
00090 P$(T2)=(TPOS+TBDST-TOFF)/TMRES
00091 ENDIF
00092 ELSE
00093 SETERROR
00094 ENDIF
00095 CLOSE
00096
00097 ; A PLC(sx+15) needs to be made to do position reporting
00098 ; Readbacks should be in &{axisnum}Q81..89
00099 ; As forward kinematic, but with Px = mx62/(Ix08*32) and no error reporting
00100 #define BPOS (BMRES*m$(BRAGG)62/(I$(BRAGG)08*32)+BOFF)
00101 #define TPOS (TMRES*m$(T2)62/(I$(T2)08*32)+TOFF)
00102 #define ENERGY Q87
00103 #define OFFSET Q88
00104 ; Local variables
00105 #define NLAMBDA Q328
00106 OPEN PLC $(PLC)
00107 CLEAR
00108 ADDRESS&$(COORD)
00109 NLAMBDA=2*DSPACING*sin(BPOS)
00110 IF (ABS(NLAMBDA)>0.01)
00111 ENERGY = EVLAMBDA/NLAMBDA
00112 OFFSET = 2*TPOS*cos(BPOS)
00113 ENDIF
00114 ; If bragg or t2 motor record did the last move, set demands = readbacks
00115 if (BMOVE = 1)
00116 or (TMOVE = 1)
00117 BMOVE = 0
00118 TMOVE = 0
00119 Q77 = Q87
00120 Q78 = Q88
00121 endif
00122 CLOSE
00123 ENABLE PLC $(PLC)
00124
00125