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