Before run, warn if spindle-synched move violates axis limits

During a spindle-synched move, too high a spindle speed may require an
axis to move faster than it is able.  This patch looks for that
situation using programmed spindle speed and INI per-axis max velocity
settings.

See LinuxCNC issue LinuxCNC#167 for more information.

Signed-off-by: John Morris <[email protected]>

NOTE: Slight adjustments were made to rebase onto LinuxCNC 2.7 (mostly
due to incompatibilities w/ state tags)

Signed-off-by: Robert W. Ellenberg <[email protected]>

lib/python/rs274/glcanon.py

@@ -49,6 +49,8 @@ def __init__(self, colors, geometry, is_foam=0):
         self.arcfeed = []; self.arcfeed_append = self.arcfeed.append
         # dwell list - [line number, color, pos x, pos y, pos z, plane]
         self.dwells = []; self.dwells_append = self.dwells.append
+        # spindle-synched feed list - (line number, (position deltas), S, fpr)
+        self.feed_synched = []
         self.choice = None
         self.feedrate = 1
         self.lo = (0,) * 9
@@ -162,6 +164,32 @@ def calc_extents(self):
                 self.min_extents_notool[0], self.min_extents_notool[1], min_z
             self.max_extents_notool = \
                 self.max_extents_notool[0], self.max_extents_notool[1], max_z
+    def calc_velocity(self, delta, axis_max_vel):
+        """Using techniques from getStraightVelocity() in emccanon.cc, given 9
+        axis deltas and velocity limits, calculate max velocity of a
+        straight move; deltas should be absolute; invalid axes should be 0
+        """
+        # Clean up tiny values
+        delta = tuple([(0.0 if i<1e-7 else i) for i in delta])
+        # Fastest time of coordinated move is the maximum time of any
+        # one axis to perform move at axis max velocity
+        tmax = max([(i[0]/i[1] if i[1] else 0.0)
+                     for i in zip(delta, axis_max_vel)])
+        # Total distance is the hypotenuse of a set of three axes;
+        # which set depends on the type of move
+        if sum(delta[0:3]) > 0:
+            # Linear XYZ with or without ABC or UVW
+            dtot = math.sqrt(sum(i*i for i in delta[0:3]))
+        elif sum(delta[6:9]) > 0:
+            # Linear UVW without XYZ and with or without ABC
+            dtot = math.sqrt(sum(i*i for i in delta[6:9]))
+        else:
+            # Angular-only
+            dtot = math.sqrt(sum(i*i for i in delta[3:6]))
+        # Max velocity = total distance / fastest time
+        max_vel = dtot/tmax
+        return max_vel
+
     def tool_offset(self, xo, yo, zo, ao, bo, co, uo, vo, wo):
         self.first_move = True
         x, y, z, a, b, c, u, v, w = self.lo
@@ -230,6 +258,18 @@ def straight_feed(self, x,y,z, a,b,c, u, v, w):
         self.lo = l
     straight_probe = straight_feed
 
+    def straight_feed_synched(self, lineno, x,y,z, a,b,c, u,v,w, s, fpr):
+        """For spindle-synched straight feeds, also collect data needed to
+        check if the commanded spindle rate and feed per revolution
+        will violate any axis MAX_VELOCITY constraints"""
+        if self.suppress > 0: return
+        # save segment start and record straight feed segment
+        lo = self.lo
+        self.straight_feed(x,y,z, a,b,c, u, v, w)
+        # record axis distances, spindle speed and feed per revolution
+        delta = tuple([abs(i[0]-i[1]) for i in zip(lo, self.lo)])
+        self.feed_synched.append((lineno, delta, s, fpr))
+
     def user_defined_function(self, i, p, q):
         if self.suppress > 0: return
         color = self.colors['m1xx']

src/emc/rs274ngc/gcodemodule.cc

@@ -144,6 +144,11 @@ static bool metric;
 static double _pos_x, _pos_y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w;
 EmcPose tool_offset;
 
+// Track spindle synched motion
+static int spindle_synched_motion = 0;
+static double spindle_synched_feed_per_revolution;
+static double spindle_speed_programmed = 0.0;
+
 static InterpBase *pinterp;
 #define interp_new (*pinterp)
 
@@ -225,11 +230,15 @@ void STRAIGHT_FEED(int line_number,
     if(metric) { x /= 25.4; y /= 25.4; z /= 25.4; u /= 25.4; v /= 25.4; w /= 25.4; }
     maybe_new_line(line_number);
     if(interp_error) return;
-    PyObject *result =
+	PyObject *result = spindle_synched_motion ?
+	    callmethod(callback, "straight_feed_synched", "ifffffffffff",
+		       line_number, x, y, z, a, b, c, u, v, w,
+		       spindle_speed_programmed,
+		       spindle_synched_feed_per_revolution / (metric?25.4:1.0)) :
         callmethod(callback, "straight_feed", "fffffffff",
                             x, y, z, a, b, c, u, v, w);
-    if(result == NULL) interp_error ++;
-    Py_XDECREF(result);
+        if(result == NULL) interp_error ++;
+        Py_XDECREF(result);
 }
 
 void STRAIGHT_TRAVERSE(int line_number,
@@ -398,14 +407,23 @@ void SET_FEED_REFERENCE(double reference) { }
 void SET_CUTTER_RADIUS_COMPENSATION(double radius) {}
 void START_CUTTER_RADIUS_COMPENSATION(int direction) {}
 void STOP_CUTTER_RADIUS_COMPENSATION(int direction) {}
-void START_SPEED_FEED_SYNCH() {}
-void START_SPEED_FEED_SYNCH(double sync, bool vel) {}
-void STOP_SPEED_FEED_SYNCH() {}
+void START_SPEED_FEED_SYNCH() {
+    spindle_synched_motion = 1;
+}
+void START_SPEED_FEED_SYNCH(double sync, bool vel) {
+    spindle_synched_motion = 1;
+    spindle_synched_feed_per_revolution = sync;
+}
+void STOP_SPEED_FEED_SYNCH() {
+    spindle_synched_motion = 0;
+}
 void START_SPINDLE_COUNTERCLOCKWISE() {}
 void START_SPINDLE_CLOCKWISE() {}
 void SET_SPINDLE_MODE(double) {}
 void STOP_SPINDLE_TURNING() {}
-void SET_SPINDLE_SPEED(double rpm) {}
+void SET_SPINDLE_SPEED(double rpm) {
+    spindle_speed_programmed = rpm;
+}
 void ORIENT_SPINDLE(double d, int i) {}
 void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout) {}
 void PROGRAM_STOP() {}
@@ -707,6 +725,8 @@ static PyObject *parse_file(PyObject *self, PyObject *args) {
     metric=false;
     interp_error = 0;
     last_sequence_number = -1;
+    spindle_synched_motion = 0;
+    spindle_speed_programmed = 0.0;
 
     _pos_x = _pos_y = _pos_z = _pos_a = _pos_b = _pos_c = 0;
     _pos_u = _pos_v = _pos_w = 0;

src/emc/usr_intf/axis/scripts/axis.py

@@ -1689,6 +1689,19 @@ def run_warn():
             if o.canon.max_extents_notool[i] > machine_limit_max[i]:
                 warnings.append(_("Program exceeds machine maximum on axis %s")
                     % "XYZABCUVW"[i])
+        # Warn user if spindle-synched feeds violate axis limits
+        axis_max_vel = tuple([
+            float(inifile.find("AXIS_%d" % i,"MAX_VELOCITY") or 0.0) * 60 
+            for i in range(9)])
+        for line_no, delta, rpm, fpr in o.canon.feed_synched:
+            fpm = rpm * fpr  # feed per minute
+            max_fpm = o.canon.calc_velocity(delta, axis_max_vel) * 0.95
+            if fpm > max_fpm:
+                warnings.append(_(
+                    "Spindle speed %(rpm_set).1f RPM exceeds maximum "
+                    "%(rpm_max).1f RPM for spindle-synched motion "
+                    "on line %(line_no)d" %
+                    dict(rpm_set=rpm, rpm_max=max_fpm/fpr, line_no=line_no)))
     if warnings:
         text = "\n".join(warnings)
         return int(root_window.tk.call("nf_dialog", ".error",

src/emc/usr_intf/gremlin/gremlin.py

@@ -267,6 +267,19 @@ def load(self,filename = None):
             unitcode = "G%d" % (20 + (s.linear_units == 1))
             initcode = self.inifile.find("RS274NGC", "RS274NGC_STARTUP_CODE") or ""
             result, seq = self.load_preview(filename, canon, unitcode, initcode)
+            # Warn user if spindle-synched feeds violate axis limits
+            axis_max_vel = tuple([
+                float(self.inifile.find("AXIS_%d" % i,"MAX_VELOCITY") or 0.0)*60
+                for i in range(9)])
+            for line_no, delta, rpm, fpr in canon.feed_synched:
+                fpm = rpm * fpr  # feed per minute
+                max_fpm = canon.calc_velocity(delta, axis_max_vel) * 0.95
+                if fpm > max_fpm:
+                    warnings.append(
+                        "Spindle speed %(rpm_set).1f RPM exceeds maximum "
+                        "%(rpm_max).1f RPM for spindle-synched motion "
+                        "on line %(line_no)d" %
+                        dict(rpm_set=rpm, rpm_max=max_fpm/fpr, line_no=line_no))
             if result > gcode.MIN_ERROR:
                 self.report_gcode_error(result, seq, filename)