00001 CLOSE 00002 00003 ;############################################### 00004 ;; \file 00005 ;; Define motion of a generic 3 jack system with a mirror on top. Jacks should 00006 ;; be numbered in a clockwise order looking from the top with J1 having the 00007 ;; smallest Z value 00008 ;; 00009 ;; Original Author: Tom Cobb 00010 ;; 00011 ;; Defined axes: 00012 ;; - X (addr 6) = Pitch in i15 EGUs 00013 ;; - Y (addr 7) = Roll in i15 EGUs 00014 ;; - Z (addr 8) = Height in same EGUs as jacks 00015 ;; 00016 ;; Macros (and example values): 00017 ;; - COORD = $(COORD) CS number (only works for CS 1..9), e.g. 2 00018 ;; - PLC = $(PLC) PLC number, should be CS number+15, e.g. 17 00019 ;; - J1 = $(J1) Axisnum for jack 1, e.g. 2 00020 ;; - J1X = $(J1X) global X co-ord of J1 base in mm, e.g. 0 00021 ;; - J1Z = $(J1Z) global Z co-ord of J1 base in mm, e.g. -100 00022 ;; - J2 = $(J2) Axisnum for jack 2, e.g. 3 00023 ;; - J2X = $(J2X) global X co-ord of J2 base in mm, e.g. 50 00024 ;; - J2Z = $(J2Z) global Z co-ord of J2 base in mm, e.g. 100 00025 ;; - J3 = $(J3) Axisnum for jack 3, e.g. 4 00026 ;; - J3X = $(J3X) global X co-ord of J3 base in mm, e.g. -50 00027 ;; - J3Z = $(J3Z) global Z co-ord of J3 base in mm, e.g. 100 00028 ;; - MD = $(MD) depth of surface in mm, e.g. 10 00029 ;; - MCX = $(MCX) global X co-ord of measure point in mm, e.g. 0 00030 ;; - MCZ = $(MCZ) global Z co-ord of measure in mm, e.g. 10 00031 ;; - JTX = $(JTX) jack plane X co-ord of J1 top in mm, e.g. 20 00032 ;; - JTZ = $(JTZ) jack plane Z co-ord of J1 top in mm, e.g. 20 00033 ;; - MP = $(MP) pitch of mirror support in deg, e.g. 45 00034 ;; - MR = $(MR) roll of mirror support in deg, e.g. 45 00035 ;################################################ 00036 00037 ; Change to CS$(COORD) 00038 &$(COORD) 00039 00040 ; Set relevant axes to use kinematics 00041 #$(J1)->I 00042 #$(J2)->I 00043 #$(J3)->I 00044 00045 ; These are set by motor_in_cs.template 00046 #define J1MOVE P(4700+$(J1)) 00047 #define J1MRES P(4800+$(J1)) 00048 #define J1OFF P(4900+$(J1)) 00049 #define J2MOVE P(4700+$(J2)) 00050 #define J2MRES P(4800+$(J2)) 00051 #define J2OFF P(4900+$(J2)) 00052 #define J3MOVE P(4700+$(J3)) 00053 #define J3MRES P(4800+$(J3)) 00054 #define J3OFF P(4900+$(J3)) 00055 00056 ; Setup the inputs, all are in i15 EGUs or mm 00057 ; These are the global co-ordinates of J1 00058 #define J1X Q20 00059 J1X = $(J1X) 00060 #define J1Z Q21 00061 J1Z = $(J1Z) 00062 ; These are the global co-ordinates of J2 00063 #define J2X Q22 00064 J2X = $(J2X) 00065 #define J2Z Q23 00066 J2Z = $(J2Z) 00067 ; These are the global co-ordinates of J3 00068 #define J3X Q24 00069 J3X = $(J3X) 00070 #define J3Z Q25 00071 J3Z = $(J3Z) 00072 ; The mirror support is mounted at (0,0) relative to these co-ordinates 00073 #define JTX Q26 00074 JTX = $(JTX) 00075 #define JTZ Q27 00076 JTZ = $(JTZ) 00077 ; This is the pitch of the mirror support mounted on the jack plane 00078 #define MP Q28 00079 MP = $(MP) 00080 ; This is the roll of the mirror support 00081 #define MR Q29 00082 MR = $(MR) 00083 ; This is the surface depth 00084 #define MD Q30 00085 MD = $(MD) 00086 ; This is the measure point for pitch, roll, Y in global co-ordinates 00087 #define MCX Q31 00088 MCX = $(MCX) 00089 #define MCZ Q32 00090 MCZ = $(MCZ) 00091 00092 00093 ; Calculate Pitch, Roll and Height from jack positions 00094 #define J1Y (J1MRES*P$(J1)+J1OFF) 00095 #define J2Y (J2MRES*P$(J2)+J2OFF) 00096 #define J3Y (J3MRES*P$(J3)+J3OFF) 00097 #define PITCH Q7 00098 #define ROLL Q8 00099 #define HEIGHT Q9 00100 ; Local variables 00101 #define JP Q128 00102 #define JR Q129 00103 #define UX Q130 00104 #define UY Q131 00105 #define UZ Q132 00106 #define VX Q133 00107 #define VY Q134 00108 #define VZ Q135 00109 #define WX Q136 00110 #define WY Q137 00111 #define WZ Q138 00112 OPEN FORWARD 00113 CLEAR 00114 ; We calculate vectors U = J3 - J1, V = J2 - J1 00115 UX = J3X - J1X 00116 UY = J3Y - J1Y 00117 UZ = J3Z - J1Z 00118 VX = J2X - J1X 00119 VY = J2Y - J1Y 00120 VZ = J2Z - J1Z 00121 ; The cross product of these gives W, the normal to the jack plane 00122 WX = UY*VZ-UZ*VY 00123 WY = UZ*VX-UX*VZ 00124 WZ = UX*VY-UY*VX 00125 ; We can calculate the pitch and roll of this normal from the Y+ vector 00126 ; WY != 0 as jacks are all in Y direction 00127 JP = atan(WZ/WY) 00128 JR = -atan(WX/WY) 00129 ; Add the pitch and roll of the mirror support in and we have pitch and roll 00130 PITCH = JP + MP 00131 ROLL = JR + MR 00132 ; Now we work out Y 00133 ; First we calculate U, mirror plane normal 00134 UX = -tan(ROLL) 00135 UY = 1.0 00136 UZ = tan(PITCH) 00137 ; Now work out V, the origin of the jack plane 00138 ; Start at the top of jack1, then move to the origin of the jack plane 00139 ; Then up by the depth of the mirror plane 00140 VX = J1X - cos(JR)*JTX 00141 VY = J1Y + sin(JP)*JTZ - sin(JR)*JTX + MD/(cos(ROLL)*cos(PITCH)) 00142 VZ = J1Z - cos(JP)*JTZ 00143 ; Now we can calculate the equation of the mirror plane, and so Y 00144 HEIGHT = UX*(VX-MCX)/UY + VY + UZ*(VZ-MCZ)/UY 00145 CLOSE 00146 00147 00148 ; Calculate jack positions from Pitch, Roll and Height 00149 ; Local variables 00150 #define JP Q228 00151 #define JR Q229 00152 #define UX Q230 00153 #define UY Q231 00154 #define UZ Q232 00155 #define VX Q233 00156 #define VY Q234 00157 #define VZ Q235 00158 #define WX Q236 00159 #define WY Q237 00160 #define WZ Q238 00161 #define J1Y Q239 00162 #define J2Y Q240 00163 #define J3Y Q241 00164 OPEN INVERSE 00165 CLEAR 00166 ; get the total pitch and roll 00167 JP = PITCH - MP 00168 JR = ROLL - MR 00169 ; define U, normal to the mirror plane 00170 UX = -tan(ROLL) 00171 UY = 1.0 00172 UZ = tan(PITCH) 00173 ; define V, the measurement point on the mirror plane, moved down by the depth of the mirror 00174 VX = MCX 00175 VY = HEIGHT - MD/(cos(ROLL)*cos(PITCH)) 00176 VZ = MCZ 00177 ; Now work out W, the origin of the jack plane 00178 ; Start at the top of jack1, then move to the origin of the jack plane 00179 WX = J1X - cos(JR)*JTX 00180 WZ = J1Z - cos(JP)*JTZ 00181 ; work out VY using the equation of the lower mirror plane 00182 WY = UX*(VX-WX)/UY + VY + UZ*(VZ-WZ)/UY 00183 ; define V, normal to jack plane 00184 VX = -tan(JR) 00185 VY = 1.0 00186 VZ = tan(JP) 00187 ; we now have W, a point on the jack plane, and a V, the normal, so use equation to work out jack heights in EGUs 00188 J1Y = VX*(WX-J1X)/VY + WY + VZ*(WZ-J1Z)/VY 00189 J2Y = VX*(WX-J2X)/VY + WY + VZ*(WZ-J2Z)/VY 00190 J3Y = VX*(WX-J3X)/VY + WY + VZ*(WZ-J3Z)/VY 00191 ; and now in cts 00192 P$(J1) = (J1Y - J1OFF) / J1MRES 00193 P$(J2) = (J2Y - J2OFF) / J2MRES 00194 P$(J3) = (J3Y - J3OFF) / J3MRES 00195 CLOSE 00196 00197 00198 ; A PLC(sx+15) needs to be made to do position reporting 00199 ; Readbacks should be in &{axisnum}Q81..89 00200 ; As forward kinematic, but with Px = mx62/(Ix08*32) 00201 #define J1Y (J1MRES*m$(J1)62/(I$(J1)08*32)+J1OFF) 00202 #define J2Y (J2MRES*m$(J2)62/(I$(J2)08*32)+J2OFF) 00203 #define J3Y (J3MRES*m$(J3)62/(I$(J3)08*32)+J3OFF) 00204 #define PITCH Q87 00205 #define ROLL Q88 00206 #define HEIGHT Q89 00207 ; Local variables 00208 #define JP Q328 00209 #define JR Q329 00210 #define UX Q330 00211 #define UY Q331 00212 #define UZ Q332 00213 #define VX Q333 00214 #define VY Q334 00215 #define VZ Q335 00216 #define WX Q336 00217 #define WY Q337 00218 #define WZ Q338 00219 OPEN PLC $(PLC) 00220 CLEAR 00221 ADDRESS&$(COORD) 00222 ; We calculate vectors U = J3 - J1, V = J2 - J1 00223 UX = J3X - J1X 00224 UY = J3Y - J1Y 00225 UZ = J3Z - J1Z 00226 VX = J2X - J1X 00227 VY = J2Y - J1Y 00228 VZ = J2Z - J1Z 00229 ; The cross product of these gives W, the normal to the jack plane 00230 WX = UY*VZ-UZ*VY 00231 WY = UZ*VX-UX*VZ 00232 WZ = UX*VY-UY*VX 00233 ; We can calculate the pitch and roll of this normal from the Y+ vector 00234 ; WY != 0 as jacks are all in Y direction 00235 JP = atan(WZ/WY) 00236 JR = -atan(WX/WY) 00237 ; Add the pitch and roll of the mirror support in and we have pitch and roll 00238 PITCH = JP + MP 00239 ROLL = JR + MR 00240 ; Now we work out Y 00241 ; First we calculate U, mirror plane normal 00242 UX = -tan(ROLL) 00243 UY = 1.0 00244 UZ = tan(PITCH) 00245 ; Now work out V, the origin of the jack plane 00246 ; Start at the top of jack1, then move to the origin of the jack plane 00247 ; Then up by the depth of the mirror plane 00248 VX = J1X - cos(JR)*JTX 00249 VY = J1Y + sin(JP)*JTZ - sin(JR)*JTX + MD/(cos(ROLL)*cos(PITCH)) 00250 VZ = J1Z - cos(JP)*JTZ 00251 ; Now we can calculate the equation of the mirror plane, and so Y 00252 HEIGHT = UX*(VX-MCX)/UY + VY + UZ*(VZ-MCZ)/UY 00253 ; If jack motor record did the last move, set demands = readbacks 00254 if (J1MOVE = 1) 00255 or (J2MOVE = 1) 00256 or (J3MOVE = 1) 00257 J1MOVE = 0 00258 J2MOVE = 0 00259 J3MOVE = 0 00260 Q77 = Q87 00261 Q78 = Q88 00262 Q79 = Q89 00263 endif 00264 CLOSE 00265 ENABLE PLC $(PLC) 00266 00267