CS_3jack.pmc

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