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