PROG_CS_motion_backlash.pmc

Go to the documentation of this file.
00001 CLOSE
00002 
00003 ;####################################################
00004 ; Generic program for moving any co-ordinate system
00005 ; Original Author: Tom Cobb
00006 ;
00007 ; Modified by Emma Shepherd, August 2010 to add support for
00008 ; backlash/hysteresis corrections for up to six motors.
00009 ;
00010 ; Important notes:
00011 ; * The NTM field of the co-ordinate system motor records MUST be
00012 ;   set to 'NO' for this program to work.
00013 ; * A separate program must be downloaded for each co-ordinate system.
00014 ; * The BDST macro should be set in the same way as you would setup the
00015 ;    .BDST field of the motor record.
00016 ; * The SLOWFEEDRATE and FASTFEEDRATE macros allow the velocity of the
00017 ;    final backlash take-out move to be slower than the rest of the move.
00018 ;    Default feedrate is 10000 so normally set FASTFEEDRATE to
00019 ;     this value and SLOWFEEDRATE an appropriate factor lower.
00020 ;
00021 ; Used variables: Q71..Q79, Q81..Q89, Q400..Q439
00022 ; Macros:
00023 ; PROG = $(PROG) ;Motion program number, e.g. 10
00024 ; MTR1 .. MTR6 ;Set to motor axis number on the PMAC/Geobrick requiring backlash correction, or 0 if unused.
00025 ; MTR1BDST .. MTR6BDST ;Set to motor backlash distance in motor record engineering units.
00026 ; SLOWFEEDRATE = $(SLOWFEEDRATE) ;Feedrate for backlash correction move, in Isx90 units, e.g. 2000.
00027 ; FASTFEEDRATE = $(FASTFEEDRATE) ;Feedrate for normal move, in Isx90 units, e.g. 10000.
00028 ;
00029 ; You should do a $$$ or type the following the first time you load this prog.
00030 ; If you type the commands in, ignore any errors. If you $$$, PLC1 will do it
00031 ; automatically for you at startup:
00032 ;&16 DEFINE LOOKAHEAD 50,10
00033 ;&15 DEFINE LOOKAHEAD 50,10
00034 ;&14 DEFINE LOOKAHEAD 50,10
00035 ;&13 DEFINE LOOKAHEAD 50,10
00036 ;&12 DEFINE LOOKAHEAD 50,10
00037 ;&11 DEFINE LOOKAHEAD 50,10
00038 ;&10 DEFINE LOOKAHEAD 50,10
00039 ;&9 DEFINE LOOKAHEAD 50,10
00040 ;&8 DEFINE LOOKAHEAD 50,10
00041 ;&7 DEFINE LOOKAHEAD 50,10
00042 ;&6 DEFINE LOOKAHEAD 50,10
00043 ;&5 DEFINE LOOKAHEAD 50,10
00044 ;&4 DEFINE LOOKAHEAD 50,10
00045 ;&3 DEFINE LOOKAHEAD 50,10
00046 ;&2 DEFINE LOOKAHEAD 50,10
00047 ;####################################################
00048 
00049 ;This program defines the program needed to do a move using the motor record
00050 ;on a CS axis
00051 ;to do a move we need to do &{CS}R
00052 ;to stop a move we need to do &{CS}A
00053 ;demand values are in &{CS}Q71..79
00054 ;The following are set by the motor record:
00055 ;Isx87 = Acceleration
00056 ;Isx88 = S curve
00057 ;Isx89 = Feedrate
00058 
00059 ; Set some defaults for all CS axes
00060 i5213,15,100=50   ; segmentation time (needed for lookahead)
00061 i5220,15,100=50   ; lookahead length (needed to limit max velocity to max set in CS)
00062 i5250,15,100=1    ; Enable kinematics
00063 
00064 #define BDST1 Q(400+$(MTR1))
00065 #define BDST2 Q(400+$(MTR2))
00066 #define BDST3 Q(400+$(MTR3))
00067 #define BDST4 Q(400+$(MTR4))
00068 #define BDST5 Q(400+$(MTR5))
00069 #define BDST6 Q(400+$(MTR6))
00070 #define NEED_CORRECTION Q433
00071 #define MTR1START Q434
00072 #define MTR2START Q435
00073 #define MTR3START Q436
00074 #define MTR4START Q437
00075 #define MTR5START Q438
00076 #define MTR6START Q439
00077 
00078 ; These are set by the motor records for the real motors:
00079 #define MTR1MRES P(4800+$(MTR1))
00080 #define MTR2MRES P(4800+$(MTR2))
00081 #define MTR3MRES P(4800+$(MTR3))
00082 #define MTR4MRES P(4800+$(MTR4))
00083 #define MTR5MRES P(4800+$(MTR5))
00084 #define MTR6MRES P(4800+$(MTR6))
00085 
00086 OPEN PROG $(PROG)
00087 CLEAR
00088 LINEAR
00089 ABS
00090 
00091 NEED_CORRECTION=0
00092 BDST1=0
00093 BDST2=0
00094 BDST3=0
00095 BDST4=0
00096 BDST5=0
00097 BDST6=0
00098 
00099 ; First record the starting motor positions
00100 MTR1START=m($(MTR1)*100+62)
00101 MTR2START=m($(MTR2)*100+62)
00102 MTR3START=m($(MTR3)*100+62)
00103 MTR4START=m($(MTR4)*100+62)
00104 MTR5START=m($(MTR5)*100+62)
00105 MTR6START=m($(MTR6)*100+62)
00106 
00107 ; Trigger the initial co-ordinate system move first at full speed
00108 FRAX(A,B,C,U,V,W,X,Y,Z)
00109 F$(FASTFEEDRATE)
00110 A(Q71)B(Q72)C(Q73)U(Q74)V(Q75)W(Q76)X(Q77)Y(Q78)Z(Q79)
00111 
00112 ; Check if a backlash correction is required on any
00113 ; of the motors (taking into account MRES for direction).
00114 ; Invert the backlash distance to turn it into an initial
00115 ; overshoot offset.
00116 
00117 if (MTR1MRES>0)
00118     if ($(MTR1BDST)<0 AND m($(MTR1)*100+62)>MTR1START OR $(MTR1BDST)>0 AND m($(MTR1)*100+62)<MTR1START)
00119         NEED_CORRECTION=1
00120         BDST1=-$(MTR1BDST)
00121     endif
00122 else
00123     if ($(MTR1BDST)<0 AND m($(MTR1)*100+62)<MTR1START OR $(MTR1BDST)>0 AND m($(MTR1)*100+62)>MTR1START)
00124         NEED_CORRECTION=1
00125         BDST1=-$(MTR1BDST)
00126     endif
00127 endif
00128 
00129 if (MTR2MRES>0)
00130     if ($(MTR2BDST)<0 AND m($(MTR2)*100+62)>MTR2START OR $(MTR2BDST)>0 AND m($(MTR2)*100+62)<MTR2START)
00131         NEED_CORRECTION=1
00132         BDST2=-$(MTR2BDST)
00133     endif
00134 else
00135     if ($(MTR2BDST)<0 AND m($(MTR2)*100+62)<MTR2START OR $(MTR2BDST)>0 AND m($(MTR2)*100+62)>MTR2START)
00136         NEED_CORRECTION=1
00137         BDST2=-$(MTR2BDST)
00138     endif
00139 endif
00140 
00141 if (MTR3MRES>0)
00142     if ($(MTR3BDST)<0 AND m($(MTR3)*100+62)>MTR3START OR $(MTR3BDST)>0 AND m($(MTR3)*100+62)<MTR3START)
00143         NEED_CORRECTION=1
00144         BDST3=-$(MTR3BDST)
00145     endif
00146 else
00147     if ($(MTR3BDST)<0 AND m($(MTR3)*100+62)<MTR3START OR $(MTR3BDST)>0 AND m($(MTR3)*100+62)>MTR3START)
00148         NEED_CORRECTION=1
00149         BDST3=-$(MTR3BDST)
00150     endif
00151 endif
00152 
00153 if (MTR4MRES>0)
00154     if ($(MTR4BDST)<0 AND m($(MTR4)*100+62)>MTR4START OR $(MTR4BDST)>0 AND m($(MTR4)*100+62)<MTR4START)
00155         NEED_CORRECTION=1
00156         BDST4=-$(MTR4BDST)
00157     endif
00158 else
00159     if ($(MTR4BDST)<0 AND m($(MTR4)*100+62)<MTR4START OR $(MTR4BDST)>0 AND m($(MTR4)*100+62)>MTR4START)
00160         NEED_CORRECTION=1
00161         BDST4=-$(MTR4BDST)
00162     endif
00163 endif
00164 
00165 if (MTR5MRES>0)
00166     if ($(MTR5BDST)<0 AND m($(MTR5)*100+62)>MTR5START OR $(MTR5BDST)>0 AND m($(MTR5)*100+62)<MTR5START)
00167         NEED_CORRECTION=1
00168         BDST5=-$(MTR5BDST)
00169     endif
00170 else
00171     if ($(MTR5BDST)<0 AND m($(MTR5)*100+62)<MTR5START OR $(MTR5BDST)>0 AND m($(MTR5)*100+62)>MTR5START)
00172         NEED_CORRECTION=1
00173         BDST5=-$(MTR5BDST)
00174     endif
00175 endif
00176 
00177 if (MTR6MRES>0)
00178     if ($(MTR6BDST)<0 AND m($(MTR6)*100+62)>MTR6START OR $(MTR6BDST)>0 AND m($(MTR6)*100+62)<MTR6START)
00179         NEED_CORRECTION=1
00180         BDST6=-$(MTR6BDST)
00181     endif
00182 else
00183     if ($(MTR6BDST)<0 AND m($(MTR6)*100+62)<MTR6START OR $(MTR6BDST)>0 AND m($(MTR6)*100+62)>MTR6START)
00184         NEED_CORRECTION=1
00185         BDST6=-$(MTR6BDST)
00186     endif
00187 endif
00188 
00189 
00190 if (NEED_CORRECTION=1)
00191     ; Trigger the co-ordinate system move using the backlash offset positions
00192     F$(FASTFEEDRATE)
00193     A(Q71)B(Q72)C(Q73)U(Q74)V(Q75)W(Q76)X(Q77)Y(Q78)Z(Q79)
00194 
00195     BDST1=0
00196     BDST2=0
00197     BDST3=0
00198     BDST4=0
00199     BDST5=0
00200     BDST6=0
00201 
00202     ; Trigger the co-ordinate system move now at slower speed to bring back to final position
00203     F$(SLOWFEEDRATE)
00204     A(Q71)B(Q72)C(Q73)U(Q74)V(Q75)W(Q76)X(Q77)Y(Q78)Z(Q79)
00205 endif
00206 
00207 DWELL0
00208 CLOSE
 All Classes Files Functions Variables Typedefs Defines