(X 2,Y 2 ) (X 1,Y 1 ) (X 0,Y 0 ) (X c,y c ) (X 3,Y 3 )

Transcription

1 Application Note Nov-2004 Probing for Dimensional Analysis This example shows how you can use Turbo PMAC s move-until-trigger function with the super-fast hardware position capture to find the exact location and size of a feature in this example, the center and radius of a circular hole. System Description In this system, we have a flat object in the XY-plane with a circular hole in it. We know the approximate location and size of this hole, but we need to know it precisely. We have a Cartesian (XYZ) actuator with a touch probe along the Z-axis. As the probe tip is moved in the XY-plane, when it makes contact with an object, a contact in the probe is immediately opened. This probe can be used to find several points on the edge of the hole, and from these points, the center and radius of the circle can be calculated. Strategy From the nominal center (X 0, Y 0 ), the probe moves out in each of three directions separated by equal angles in the XY-plane until it hits the edge of the hole, triggering the probe input and capturing the positions of the XY encoders. (X 2,Y 2 ) (X 0,Y 0 ) (X c,y c ) (X 1,Y 1 ) (X 3,Y 3 ) The Turbo PMAC feature that makes this easy is the programmed move until trigger. This move type is a variant of Turbo PMAC s RAPID move mode. In a move until trigger, the destination each axis in the absence of a trigger is specified. If no trigger is found before that destination is reached, the move is exactly like a regular RAPID move. However, if a pre-defined trigger condition occurs before the initial destination is reached, the actual position at the trigger is captured and stored, and a post-trigger move to an end-point of a specified distance from the trigger point is calculated and executed. The motion command ABS X10^1 Y20^1 tells the X-axis to move to a position of 10 user units and the Y-axis to a position of 20 user units if no trigger is found. However, if a trigger is found, each axis is to come to a stop at a position +1 user unit from the trigger position. If the final quantity is a 0, the axis will return to the exact trigger position and stop. Probing For Dimensions Analysis 1

2 Nov-2004 Application Note Turbo PMAC s hardware position-capture functions latch the encoder position to the exact count, regardless of the speed of motion. The captured raw encoder count values are stored in dedicated registers and could be used for subsequent calculations, but it is typically easier to use the scaled position values for the end of the post-trigger move, which can be set to be the triggered position itself, as they are in this case. From the three captured XY trigger positions, the X and Y coordinates of the real center and the circle s actual radius can easily be calculated by the Turbo PMAC. The equations for this are: y 23 ( x 12 x 12 + y 12 y 12 ) y 12 ( x 23 x 23 + y 23 y 23 ) xc = x 12 y 23 x 23 y 12 x 12 ( x 23 x 23 + y 23 y 23 ) x 23 ( x 12 x 12 + y 12 y 12 ) yc = x 12 y 23 x 23 y 12 where x c and y c are the center coordinates, and: xij = x j x i xi + x j xij = 2 with comparable definitions for y. The radius can then simply be computed as: r = ( ) 2 + ( ) 2 x 1 xc Setup and Program The following setup and program use Motors 1 and 2 on a Turbo PMAC2 board as the X and Y-axes using the on-board Servo IC 0 Channels 1 and 2, with the USER1 and USER2 input flags as the capturing trigger signals for these encoders. The probe s trigger would be wired into both inputs. Note that the post-trigger distance in the move until trigger command is zero (^0). This makes the target end-of-move axis registers, in user units, equal to the trigger position. In this way the algorithm can just store these values as representing the trigger positions. The X-coordinates and the Y-coordinates for the three triggered moves are each stored in three consecutive Q-variables. This permits the program to use Turbo PMAC s array capabilities to set these variables. However, in using the stored values to calculate the center and radius, it is easier to access these variables directly and not in an array; Turbo PMAC permits this as well. Substitutions and Definitions #define XnomCent Q201 ; Initial est, X coord of center #define YnomCent Q202 ; Initial est, Y coord of center #define NomRadius Q203 ; Initial est, radius #define Xdest Q204 ; Programmed X move destination #define Ydest Q205 ; Programmed Y move destination #define Move Q206 ; Move counter #define X1 Q211 ; Captured X on first move #define X2 Q212 ; Captured X on second move #define X3 Q213 ; Captured X on third move #define DX12 Q214 ; X2-X1 #define DX23 Q215 ; X3-X2 #define AX12 Q216 ; (X1+X2)/2 #define AX23 Q217 ; (X2+X3)/2 #define Xcent Q218 ; Calculated X center #define Y1 Q221 ; Captured Y on first move #define Y2 Q222 ; Captured Y on second move #define Y3 Q223 ; Captured Y on third move #define DY12 Q224 ; Y2-Y1 #define DY23 Q225 ; Y3-Y2 x 1 xc 2 Probing For Dimensions Analysis

3 Application Note Nov-2004 #define AY12 Q226 ; (Y1+Y2)/2 #define AY23 Q227 ; (Y2+Y3)/2 #define Ycent Q228 ; Calculated Y center #define Denom Q229 ; Denominator term #define Temp1 Q231 ; Intermediate value #define Temp2 Q232 ; Intermediate value #define Radius Q233 ; Calculated radius #define XtargPos M5147 ; X end-of-move position XTargPos->L:$ ; CS1 X target position #define YtargPos M5148 ; Y end-of-move position YTargPos->L:$ ; CS1 Y target position Setup Variables for Trigger (* indicates default value) I125=$ ; #1 use IC0 Chan 1 flags* I197=0 ; #1 HW capture on flag trig* I7012=2 ; IC0 Chan 1 rising flag capture I7013=3 ; Capture with USER1 flag I225=$ ; #2 use IC0 Chan 2 flags* I297=0 ; #2 HW capture on flag trig* I7022=2 ; IC0 Chan 2 rising flag capture I7023=3 ; Capture with USER2 flag Define the Coordinate System &1 ; Address Coordinate system 1 #1->400X ; Program in mm, 2.5um res #2->400Y ; Program in mm, 2.5um res #3->200Z ; Program in mm, 5um res Motion Program to Accomplish Task OPEN PROG 72 CLEAR ABS ; Specify positions RAPID ; Minimum-time point-to-point X(XNomCent) Y(YNomCent) ; Move to nominal center Z-5 ; Lower probe Move=1 ; Start move counter WHILE (Move<4) ; Loop three times XDest=XNomCent+1.25*NomRadius*COS(120*(Move-1)) YDest=YNomCent+1.25*NomRadius*SIN(120*(Move-1)) X(XDest)^0 Y(YDest)^0 ; Move until trigger Q(210+Move)=XTargPos ; Store X trigger position Q(220+Move)=YTargPos ; Store Y trigger position X(XNomCent) Y(YNomCent) ; Move back to nominal center Move=Move+1 ENDWHILE Compute Center from the 3 Trigger Positions DX12=X2-X1 DX23=X3-X2 AX12=(X1+X2)/2 AX23=(X2+X3)/2 DY12=Y2-Y1 DY23=Y3-Y2 AY12=(Y1+Y2)/2 AY23=(Y2+Y3)/2 Denom=DX12*DY23-DY12*DX23 Temp1=DY23*(DX12*AX12+DY12*AY12) Temp2=DY12*(DX23*AX23+DY23*AY23) Probing For Dimensions Analysis 3

4 Nov-2004 Application Note XCent=(Temp1-Temp2)/Denom Temp1=DX12*(DX23*AX23+DY23*AY23) Temp2=DX23*(DX12*AX12+DY12*AY12) YCent=(Temp1-Temp2)/Denom Temp1=(X1-XCent)*(X1-XCent) Temp2=(Y1-YCent)*(Y1-YCent) Radius=SQRT(Temp1+Temp2) CLOSE Variant 1: Capturing Sinusoidal Encoder Position The above example was configured on a system with quadrature encoders of 10-micron pitch on each motor. The default times-4 decode yielded a 2.5-micron count resolution, and the position-capture function just needed to latch the counter value on the input trigger. In this variant, the motors have sinusoidal encoders of 10-micron pitch processed through an Acc-51 high-resolution interpolator board, which produces 4096 states per line (pitch) of the encoder, scaled as 128 software counts per line with 32 sub-counts per software count. The software will consider the motors to have 12,800 counts per millimeter; the fundamental resolution will be about 2.5 nanometers. This type of feedback poses several challenges for software that uses hardware position capture to obtain a precise trigger position: The full-resolution data from the sine and cosine encoder inputs is only latched and calculated once per servo cycle, and so does not exist for an asynchronous trigger. Another method must be used to obtain high-resolution data. The units of the hardware counter (with 4 counts per line) are now different from software counts used to scale the axes. The capture trigger comes in on a different hardware channel (one on the interpolator board) than the amplifier and limit flags, requiring more flexibility in motor setup. Starting with Revision D of the PMAC2-style Servo IC (released beginning of 2003) and Turbo PMAC firmware revision V1.940 (released June 2003), Turbo PMAC systems have the hardware and software features necessary to meet these challenges. The Servo IC is used on the Acc-51E UMAC interpolator, the Acc-51C Compact UMAC interpolator, and the Acc-51P2 PCI-format interpolator. (The Acc-51P PCI-format interpolator, which uses a PMAC-style Servo IC, can support capture only to the nearest ¼ of a line only). The Revision D PMAC2-style Servo IC has hardware 1/T count extension capability that continually estimates the fractional position data between zero crossings of the sine and cosine signals. This fractional estimation can then be latched by the capture trigger so it can be combined in software with the captured counter value. This feature is enabled by setting I-variable I7mn9 (for Servo IC m Channel n) to 1. In Turbo PMAC firmware revision V1.940, two new control bits were added to the Ixx24 Flag Mode Control setup variable. Bit 11, when set to 1, has the triggered-move routines recognize that there is additional fractional count resolution. Bit 12, when set to 1, has the triggered-move routines use the captured fractional-count value from the hardware 1/T circuitry. In Turbo PMAC firmware revision V1.940, two new flag-address I-variables were added to the existing Ixx25 variable, permitting different sources for different types of flags for the motor. If Ixx42 is set to a value greater than 0, it specifies the address of the amplifier flags; if Ixx43 is set to a value greater than 0, it specifies the address of the limit flags. This allows Ixx25 to specify the address of the capture flags alone, so they can come from the interpolator channel, and the others from the channel of the general axis interface. 4 Probing For Dimensions Analysis

5 Application Note Nov-2004 In this example, we use a UMAC Turbo with the axes controlled from Channels 1 and 2 an Acc-24E2A board using Servo IC 2, but with position feedback from Channels 1 and 2 of an Acc-51E board using Servo IC 3. The probe trigger signal is wired into the HOME1 and HOME2 flag inputs on the Acc-51E board. Setup Variables for Trigger Condition I142=$ ; #1 use IC2 Ch 1 amp flags I143=$ ; #1 use IC2 Ch 1 limit flags I125=$ ; #1 use IC3 Ch 1 capture flags I124=$ ; Sub-count, hi-res, PMAC2 IC I197=0 ; #1 HW capture on flag trig I7312=2 ; IC3 Ch 1 rising flag capture I7313=0 ; IC3 Ch 1 capture w/ HOME1 flag I7319=1 ; IC3 Ch 1 HW 1/T sub-count on I242=$ ; #2 use IC2 Ch 2 amp flags I243=$ ; #2 use IC2 Ch 2 limit flags I225=$ ; #2 use IC3 Ch 2 capture flags I224=$ ; Sub-count, hi-res, PMAC2 IC I297=0 ; #2 HW capture on flag trig I7322=2 ; IC3 Ch 2 rising flag capture I7323=0 ; IC3 Ch 2 capture w/ HOME2 flag I7329=1 ; IC3 Ch 2 HW 1/T sub-count on Define the Coordinate System &1 ; Address Coordinate system 1 #1->12800X ; Program in mm, ~80nm per SWct #2->12800Y ; Program in mm, ~80nm per SWct #3->6400Z ; Program in mm, ~160nm per SWct Once this setup for the trigger and axis definitions has been made, the example program can be used unchanged. Variant 2: Trapping Errors The above example program did not take into account what would happen if any of the three triggered moves failed to find its trigger. It would use the end position of the untriggered move as a trigger point. To catch such an event, we can use the triggered move status bit for each motor, which is set from the start of a triggered move until the trigger is found. If no trigger is found, it stays set at the end of the move. In our case, seeing that this bit is set after the move completes indicates a failure. In this example, we check at the end of each probing move to see if the trigger was found. If it was, we continue with calculations and any subsequent moves. If it was not, we stop the procedure and set a flag to notify whatever algorithm would use the results. Additional Substitutions and Definitions #define Mtr1LookForTrig M136 Mtr1LookForTrig->X:$0000C0,7,1 ; Triggered move status bit #define Failed Q207 ; Set to 1 if no trigger found Motion Program to Accomplish Task OPEN PROG 72 CLEAR ABS ; Specify positions RAPID ; Minimum-time point-to-point X(XNomCent) Y(YNomCent) ; Move to nominal center Z-5 ; Lower probe Move=1 ; Start move counter Failed=0 ; No missed triggers yet WHILE (Move<4 AND Failed=0) ; Loop three times Probing For Dimensions Analysis 5

6 Nov-2004 Application Note XDest=XNomCent+1.25*NomRadius*COS(120*(Move-1)) YDest=YNomCent+1.25*NomRadius*SIN(120*(Move-1)) X(XDest)^0 Y(YDest)^0 ; Move until trigger IF (Mtr1LookForTrig=1) ; Did not find trigger Failed=1 ; Procedure has failed ELSE Q(210+Move)=XTargPos ; Store X trigger position Q(220+Move)=YTargPos ; Store Y trigger position ENDIF X(XNomCent) Y(YNomCent) ; Move back to nominal center Move=Move+1 ENDWHILE IF (Failed=0) ; Found all three points Compute Center from the 3 Trigger Positions DX12=X2-X1 DX23=X3-X2 AX12=(X1+X2)/2 AX23=(X2+X3)/2 DY12=Y2-Y1 DY23=Y3-Y2 AY12=(Y1+Y2)/2 AY23=(Y2+Y3)/2 Denom=DX12*DY23-DY12*DX23 Temp1=DY23*(DX12*AX12+DY12*AY12) Temp2=DY12*(DX23*AX23+DY23*AY23) XCent=(Temp1-Temp2)/Denom Temp1=DX12*(DX23*AX23+DY23*AY23) Temp2=DX23*(DX12*AX12+DY12*AY12) YCent=(Temp1-Temp2)/Denom Temp1=(X1-XCent)*(X1-XCent) Temp2=(Y1-YCent)*(Y1-YCent) Radius=SQRT(Temp1+Temp2) ELSE ; At least one move failed Radius=0 ; Set to zero as flag ENDIF CLOSE 6 Probing For Dimensions Analysis