CS_B22_optics_box.pmc

Go to the documentation of this file.
00001 CLOSE
00002 
00003 ;###############################################
00004 ; Define motion of B22's optics box
00005 ; Original Author: Tom Cobb
00006 ; Defined axes:
00007 ;   W (addr 5) = Xexit in same EGUs as actuators
00008 ;   X (addr 6) = Yexit in same EGUs as actuators
00009 ;   Y (addr 7) = ThetaExit in i15 EGUs
00010 ;   Z (addr 8) = PhiExit in i15 EGUs
00011 ; Macros (and example values):
00012 ;   COORD = $(COORD) ;CS number (only works for CS 1..9), e.g. 2
00013 ;   PLC = $(PLC) ;PLC number, should be CS number+15, e.g. 17
00014 ;   M9PITCH = $(M9PITCH) ;Axisnum for M9 pitch actuator, e.g. 1
00015 ;   M9ROLL = $(M9ROLL) ;Axisnum for M9 roll actuator, e.g. 2
00016 ;   XM9 = $(XM9) ;Lateral distance between actuator and pivot ball on M9 in mm
00017 ;   YM9 = $(XM9) ;Distance between M9 pitch actuator and pivot ball on M9 in mm
00018 ;   M10PITCH = $(M10PITCH) ;Axisnum for M10 pitch actuator, e.g. 3
00019 ;   M10ROLL = $(M10ROLL) ;Axisnum for M10 roll actuator, e.g. 4
00020 ;   XM10 = $(XM10) ;Lateral distance between actuator and pivot ball on M10 in mm
00021 ;   YM10 = $(XM10) ;Distance between M10 pitch actuator and pivot ball on M10 in mm
00022 ;   LM9M10 = $(LM9M10) ;Distance between M9 and M10 centres in mm
00023 ;################################################
00024 
00025 ; Change to CS$(COORD)
00026 &$(COORD)
00027 
00028 ; Set relevant axes to use kinematics
00029 #$(M9PITCH)->I
00030 #$(M9ROLL)->I
00031 #$(M10PITCH)->I
00032 #$(M10ROLL)->I
00033 
00034 ; These are set by motor_in_cs.template
00035 #define M9PITCHMOVE P(4700+$(M9PITCH))
00036 #define M9PITCHMRES P(4800+$(M9PITCH))
00037 #define M9PITCHOFF  P(4900+$(M9PITCH))
00038 #define M9ROLLMOVE P(4700+$(M9ROLL))
00039 #define M9ROLLMRES P(4800+$(M9ROLL))
00040 #define M9ROLLOFF  P(4900+$(M9ROLL))
00041 #define M10PITCHMOVE P(4700+$(M10PITCH))
00042 #define M10PITCHMRES P(4800+$(M10PITCH))
00043 #define M10PITCHOFF  P(4900+$(M10PITCH))
00044 #define M10ROLLMOVE P(4700+$(M10ROLL))
00045 #define M10ROLLMRES P(4800+$(M10ROLL))
00046 #define M10ROLLOFF  P(4900+$(M10ROLL))
00047 
00048 ; Setup the inputs, all are in mm
00049 #define XM9      Q20
00050 #define YM9      Q21
00051 #define XM10     Q22
00052 #define YM10     Q23
00053 #define LM9M10   Q24
00054 XM9 = $(XM9)
00055 YM9 = $(YM9)
00056 XM10 = $(XM10)
00057 YM10 = $(XM10)
00058 LM9M10 = $(LM9M10)
00059 
00060 ; Calculate exit X, Y, Theta, and Phi from actuator positions
00061 #define M9PITCHPOS (M9PITCHMRES*P$(M9PITCH)+M9PITCHOFF)
00062 #define M9ROLLPOS (M9ROLLMRES*P$(M9ROLL)+M9ROLLOFF)
00063 #define M10PITCHPOS (M10PITCHMRES*P$(M10PITCH)+M10PITCHOFF)
00064 #define M10ROLLPOS (M10ROLLMRES*P$(M10ROLL)+M10ROLLOFF)
00065 #define XEXIT     Q6
00066 #define YEXIT     Q7
00067 #define THETAEXIT Q8
00068 #define PHIEXIT   Q9
00069 ; Local variables
00070 #define THETAM9  Q128
00071 #define PHIM9    Q129
00072 #define THETAM10 Q130
00073 #define PHIM10   Q131
00074 OPEN FORWARD
00075 CLEAR
00076     ; get angles from actuators
00077     THETAM9 = M9PITCHPOS * 1000 / XM9
00078     PHIM9 = M9ROLLPOS * 1000 / YM9
00079     THETAM10 = M10PITCHPOS * 1000 / XM10
00080     PHIM10 = M10ROLLPOS * 1000 / YM10
00081     ; calculate exit values from angles
00082     XEXIT = (THETAM9 * LM9M10) / 1000
00083     YEXIT = (PHIM9 * LM9M10) / 1000
00084     THETAEXIT = 2 * (THETAM9 + THETAM10)
00085     PHIEXIT = 2 * (PHIM9 + PHIM10)
00086 CLOSE
00087 
00088 ; Calculate actuator positions from exit X, Y, Theta, and Phi
00089 ; Local variables
00090 #define THETAM9  Q228
00091 #define PHIM9    Q229
00092 #define THETAM10 Q230
00093 #define PHIM10   Q231
00094 #define M9PITCHPOS Q232
00095 #define M9ROLLPOS Q233
00096 #define M10PITCHPOS Q234
00097 #define M10ROLLPOS Q235
00098 OPEN INVERSE
00099 CLEAR
00100     ; get angles from exit
00101     THETAM9 = XEXIT * 1000 / LM9M10
00102     PHIM9 = YEXIT * 1000 / LM9M10
00103     THETAM10 = (THETAEXIT - 2 * THETAM9) / 2
00104     PHIM10 = (PHIEXIT - 2 * PHIM9) / 2
00105     ; calculate actuator values from angles in EGUs
00106     M9PITCHPOS = THETAM9 * XM9 / 1000
00107     M9ROLLPOS = PHIM9 * YM9 / 1000
00108     M10PITCHPOS = THETAM10 * XM10 / 1000
00109     M10ROLLPOS = PHIM10 * YM10 / 1000
00110     ; then in cts
00111     P$(M9PITCH) = (M9PITCHPOS - M9PITCHOFF) / M9PITCHMRES
00112     P$(M9ROLL) = (M9ROLLPOS - M9ROLLOFF) / M9ROLLMRES
00113     P$(M10PITCH) = (M10PITCHPOS - M10PITCHOFF) / M10PITCHMRES
00114     P$(M10ROLL) = (M10ROLLPOS - M10ROLLOFF) / M10ROLLMRES
00115 CLOSE
00116 
00117 ; A PLC(sx+15) needs to be made to do position reporting
00118 ; Readbacks should be in &{axisnum}Q81..89
00119 ; As forward kinematic, but with Px = mx62/(Ix08*32)
00120 #define M9PITCHPOS (M9PITCHMRES*m$(M9PITCH)62/(I$(M9PITCH)08*32)+M9PITCHOFF)
00121 #define M9ROLLPOS (M9ROLLMRES*m$(M9ROLL)62/(I$(M9ROLL)08*32)+M9ROLLOFF)
00122 #define M10PITCHPOS (M10PITCHMRES*m$(M10PITCH)62/(I$(M10PITCH)08*32)+M10PITCHOFF)
00123 #define M10ROLLPOS (M10ROLLMRES*m$(M10ROLL)62/(I$(M10ROLL)08*32)+M10ROLLOFF)
00124 #define XEXIT     Q86
00125 #define YEXIT     Q87
00126 #define THETAEXIT Q88
00127 #define PHIEXIT   Q89
00128 ; Local variables
00129 #define THETAM9  Q328
00130 #define PHIM9    Q329
00131 #define THETAM10 Q330
00132 #define PHIM10   Q331
00133 OPEN PLC $(PLC)
00134 CLEAR
00135     ADDRESS&$(COORD)
00136     ; get angles from actuators
00137     THETAM9 = M9PITCHPOS * 1000 / XM9
00138     PHIM9 = M9ROLLPOS * 1000 / YM9
00139     THETAM10 = M10PITCHPOS * 1000 / XM10
00140     PHIM10 = M10ROLLPOS * 1000 / YM10
00141     ; calculate exit values from angles
00142     XEXIT = (THETAM9 * LM9M10) / 1000
00143     YEXIT = (PHIM9 * LM9M10) / 1000
00144     THETAEXIT = 2 * (THETAM9 + THETAM10)
00145     PHIEXIT = 2 * (PHIM9 + PHIM10)
00146     ; If actuator motor record did the last move, set demands = readbacks
00147     if (M9PITCHMOVE = 1)
00148     or (M9ROLLMOVE = 1)
00149     or (M10PITCHMOVE = 1)
00150     or (M10ROLLMOVE = 1)
00151         M9PITCHMOVE = 0
00152         M9ROLLMOVE = 0
00153         M10PITCHMOVE = 0
00154         M10ROLLMOVE = 0
00155         Q76 = Q86
00156         Q77 = Q87
00157         Q78 = Q88
00158         Q79 = Q89
00159     endif
00160 CLOSE
00161 ENABLE PLC $(PLC)
00162 
00163 
 All Classes Files Functions Variables Typedefs Defines