CS_3jack_mirror.pmc

Go to the documentation of this file.
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 
 All Classes Files Functions Variables Typedefs Defines