Fix spindle position-sync jitter and catch infeasible thread pitches

docs/src/gcode/g-code.adoc

@@ -1071,6 +1071,12 @@ K follows the drive line described by 'X- Y- Z-'. K is not parallel to
 the Z axis if X or Y endpoints are used for example when cutting tapered
 threads.
 
+[NOTE]
+The spindle speed override has no effect during a G33 move, and the previous
+setting is restored when the move ends. A thread is cut in several passes over
+the same helix, so changing the spindle speed part way through would change the
+lead and spoil the thread.
+
 [[gcode:g33-tech-info]]
 .Technical Info
 At the beginning of each G33 pass, LinuxCNC uses the spindle speed and
@@ -1113,11 +1119,8 @@ It is an error if:
 * An F word is given (the feed follows from K and the spindle speed).
 * The selected spindle is not commanded to turn (M3 or M4 active) when
   this command is executed.
-
-[NOTE]
-The pitch and the spindle speed together set the axis feed. If they ask for
-more than the machine can deliver, the program is not rejected: there is no
-interpreter check and no error message for that case.
+* The pitch and the commanded spindle speed together ask an axis to move
+  faster than its maximum velocity.
 
 [[gcode:g33.1]]
 == G33.1 Rigid Tapping(((G33.1 Rigid Tapping)))
@@ -1179,18 +1182,21 @@ M2 (end program)
 * See <<gcode:g90-g91,G90>> & <<gcode:g0,G0>> & <<mcode:m2-m30,M2>>
   sections for more information.
 
+[NOTE]
+Unlike G33, the spindle speed override stays active during a G33.1 move and the
+feed follows whatever the spindle actually does. A tap guides itself in its own
+hole, so there is no thread lead to spoil, and being able to slow the spindle
+while the tap is in the work is useful.
+
 It is an error if:
 
 * All axis words are omitted.
 * No K word is given.
 * An F word is given (the feed follows from K and the spindle speed).
 * The selected spindle is not commanded to turn (M3 or M4 active) when
   this command is executed.
-
-[NOTE]
-The pitch and the spindle speed together set the axis feed. If they ask for
-more than the machine can deliver, the program is not rejected: there is no
-interpreter check and no error message for that case.
+* The pitch and the commanded spindle speed together ask an axis to move
+  faster than its maximum velocity.
 
 [[gcode:g38]]
 == G38._n_ Straight Probe(((G38.n Probe)))
@@ -2046,6 +2052,12 @@ It is an error if:
 * All the required words are not specified.
 * 'P-', 'J-', 'K-' or 'H-' is negative.
 * 'E-' is greater than half the drive line length.
+* The pitch and the commanded spindle speed together ask an axis to move
+  faster than its maximum velocity.
+
+[NOTE]
+The spindle speed override has no effect during the G76 cycle, for the same
+reason as G33, and the previous setting is restored when the cycle ends.
 
 .HAL Connections
 The pins 'spindle.N.at-speed' and the 'encoder._n_.phase-Z' for the

docs/src/gcode/m-code.adoc

@@ -307,6 +307,10 @@ no influence, and the spindle speed will have the
 exact program specified value of the S-word
 (described in the <<sec:set-spindle-speed,Spindle Speed>> section).
 
+The override is also suspended for the duration of a
+<<gcode:g33,G33>> move or a <<gcode:g76,G76>> cycle, whatever M48, M49 or M51
+last selected, and that selection is restored afterwards.
+
 [[mcode:m52]]
 == M52 Adaptive Feed Control
 

src/emc/motion/control.c

@@ -1350,6 +1350,17 @@ static void get_pos_cmds(long period)
 	    /* run coordinated trajectory planning cycle */
 
 	    tpRunCycle(&emcmotInternal->coord_tp, period);
+
+            if (emcmotStatus->syncOverrunSpindle) {
+                tpAbort(&emcmotInternal->coord_tp);
+                reportError(_("spindle-synchronized move exceeds axis limits: "
+                              "spindle %d is outrunning the axis by %f per "
+                              "second, reduce the spindle speed or the pitch"),
+                            emcmotStatus->syncOverrunSpindle - 1,
+                            emcmotStatus->syncOverrunError);
+                emcmotStatus->syncOverrunSpindle = 0;
+                SET_MOTION_ERROR_FLAG(1);
+            }
             /* get new commanded traj pos */
             tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd);
 

src/emc/motion/motion.c

@@ -890,6 +890,8 @@ static int init_comm_buffers(void)
     ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd);
     ZERO_EMC_POSE(emcmotStatus->carte_pos_fb);
     emcmotStatus->vel = 0.0;
+    emcmotStatus->syncOverrunSpindle = 0;
+    emcmotStatus->syncOverrunError = 0.0;
     emcmotConfig->limitVel = 0.0;
     emcmotStatus->acc = 0.0;
     emcmotStatus->feed_scale = 1.0;

src/emc/motion/motion.h

@@ -600,6 +600,10 @@ Suggestion: Split this in to an Error and a Status flag register..
 	emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS];	/* all joint status data */
     emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS];	/* all axis status data */
     int spindleSync;    /* spindle used for synchronised moves. -1 = none */
+    int syncOverrunSpindle; /* spindle that outran the axis in a synced move,
+                               plus one; 0 = none.  Set by the planner, raised
+                               by the motion controller. */
+    double syncOverrunError; /* by how much per second */
     spindle_status_t spindle_status[EMCMOT_MAX_SPINDLES]; /* all spindle data */
 
 

src/emc/nml_intf/canon.hh

@@ -874,6 +874,10 @@ below.
 extern double GET_EXTERNAL_ANGLE_UNIT_FACTOR();
 */
 
+// Returns the maximum velocity of one axis, indexed 0-8 as XYZABCUVW, in
+// program units per minute, or zero if that limit is not available
+extern double GET_EXTERNAL_AXIS_MAX_VELOCITY(int axis);
+
 // Returns the system feed rate
 extern double GET_EXTERNAL_FEED_RATE();
 

src/emc/rs274ngc/canonmodule.cc

@@ -178,6 +178,7 @@ BOOST_PYTHON_MODULE(emccanon) {
     def("GET_EXTERNAL_TOOL_LENGTH_ZOFFSET",&GET_EXTERNAL_TOOL_LENGTH_ZOFFSET);
     def("GET_EXTERNAL_TOOL_SLOT",&GET_EXTERNAL_TOOL_SLOT);
     def("GET_EXTERNAL_TOOL_TABLE",&GET_EXTERNAL_TOOL_TABLE);
+    def("GET_EXTERNAL_AXIS_MAX_VELOCITY",&GET_EXTERNAL_AXIS_MAX_VELOCITY);
     def("GET_EXTERNAL_TRAVERSE_RATE",&GET_EXTERNAL_TRAVERSE_RATE);
     def("GET_OPTIONAL_PROGRAM_STOP",&GET_OPTIONAL_PROGRAM_STOP);
     def("INIT_CANON",&INIT_CANON);

src/emc/rs274ngc/gcodemodule.cc

@@ -1066,6 +1066,7 @@ CANON_DIRECTION GET_EXTERNAL_SPINDLE(int) { return CANON_STOPPED; }
 int GET_EXTERNAL_TOOL_SLOT() { return 0; }
 int GET_EXTERNAL_SELECTED_TOOL_SLOT() { return 0; }
 double GET_EXTERNAL_FEED_RATE() { return 1; }
+double GET_EXTERNAL_AXIS_MAX_VELOCITY(int /*axis*/) { return 0; }
 double GET_EXTERNAL_TRAVERSE_RATE() { return 0; }
 int GET_EXTERNAL_FLOOD() { return 0; }
 int GET_EXTERNAL_MIST() { return 0; }

src/emc/rs274ngc/interp_check.cc

@@ -393,3 +393,80 @@ int Interp::check_other_codes(block_pointer block)       //!< pointer to a block
 
   return INTERP_OK;
 }
+
+/****************************************************************************/
+
+/*! check_spindle_sync_feed
+
+Returned Value: int
+   Returns an error if any axis of the move would have to run faster than its
+   maximum velocity to hold the commanded pitch at the commanded spindle speed.
+   Otherwise returns INTERP_OK.
+
+Side effects: none
+
+Called by:
+   Interp::convert_straight (G33, G33.1)
+   Interp::convert_threading_cycle (G76)
+
+Nothing downstream rejects a feed the machine cannot deliver: the planner
+clamps the velocity, the axis falls behind, and the thread is cut wrong.
+
+The bound is the per-axis maximum, not the traj maximum, because the max
+velocity slider is deliberately not applied to position-synchronized moves (see
+tpGetMaxTargetVel).  The feed is projected onto each axis by its share of the
+move length, as the planner distributes it.  Rotary axes are ignored: a pitch
+is a linear distance per revolution.
+
+Using the commanded S word means the error names the offending line and does
+not depend on the spindle already running.  Skipped in constant surface speed
+mode, and for any axis whose limit is unavailable (the standalone interpreter
+reports zero).
+
+*/
+
+int Interp::check_spindle_sync_feed(setup_pointer settings,  //!< pointer to machine settings
+                                    double pitch,            //!< program units per revolution
+                                    const char *code,        //!< G code name, for the message
+                                    const double delta[9])   //!< move, program units, XYZABCUVW
+{
+  static const char axis_name[] = "XYZABCUVW";
+  int spindle = settings->active_spindle;
+
+  if (settings->spindle_mode[spindle] != SPINDLE_MODE::CONSTANT_RPM)
+    return INTERP_OK;
+
+  double speed = settings->speed[spindle];
+  if (speed <= 0.0 || pitch == 0.0)
+    return INTERP_OK;
+
+  double length = 0.0;
+  for (int ax = 0; ax < 9; ax++) {
+    if (ax >= 3 && ax <= 5)
+      continue;                 /* rotary */
+    length += delta[ax] * delta[ax];
+  }
+  length = sqrt(length);
+  if (length <= 0.0)
+    return INTERP_OK;
+
+  /* program units per minute along the path */
+  double required_rate = fabs(pitch) * speed;
+
+  for (int ax = 0; ax < 9; ax++) {
+    if (ax >= 3 && ax <= 5)
+      continue;
+    if (delta[ax] == 0.0)
+      continue;
+    double max_rate = GET_EXTERNAL_AXIS_MAX_VELOCITY(ax);
+    if (max_rate <= 0.0)
+      continue;
+    double axis_rate = required_rate * fabs(delta[ax]) / length;
+    CHKS((axis_rate > max_rate),
+         _("%s pitch %g at spindle speed %g needs %g per minute on the %c axis, "
+           "which exceeds its maximum velocity of %g"),
+         code, fabs(pitch), speed, axis_rate, axis_name[ax], max_rate);
+  }
+
+  return INTERP_OK;
+}

src/emc/rs274ngc/interp_convert.cc

@@ -5114,6 +5114,43 @@ int Interp::convert_spindle_mode(int dollar_number, block_pointer block, setup_p
 	}
     return INTERP_OK;
 }
+
+/* Thread cutting re-enters the same helix each pass, so moving the spindle
+   speed part way through shifts the lead.  G33.1 deliberately keeps the
+   override: a tap is self-guiding and slowing down is useful. */
+
+static void suspend_speed_override(setup_pointer settings)
+{
+    DISABLE_SPEED_OVERRIDE(settings->active_spindle);
+}
+
+static void restore_speed_override(setup_pointer settings)
+{
+    /* back to what the program asked for, so an M49 or M51 P0 still holds */
+    if (settings->speed_override[settings->active_spindle]) {
+        ENABLE_SPEED_OVERRIDE(settings->active_spindle);
+    }
+}
+
+/* Displacement of a move, ordered XYZABCUVW. */
+
+static void sync_move_delta(setup_pointer settings,
+                            double end_x, double end_y, double end_z,
+                            double AA_end, double BB_end, double CC_end,
+                            double u_end, double v_end, double w_end,
+                            double delta[9])
+{
+    delta[0] = end_x - settings->current_x;
+    delta[1] = end_y - settings->current_y;
+    delta[2] = end_z - settings->current_z;
+    delta[3] = AA_end - settings->AA_current;
+    delta[4] = BB_end - settings->BB_current;
+    delta[5] = CC_end - settings->CC_current;
+    delta[6] = u_end - settings->u_current;
+    delta[7] = v_end - settings->v_current;
+    delta[8] = w_end - settings->w_current;
+}
+
 /****************************************************************************/
 
 /*! convert_stop
@@ -5526,9 +5563,15 @@ int Interp::convert_straight(int move,   //!< either G_0 or G_1
     CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) &&
            (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)),
           _("Spindle not turning in G33"));
+    double delta[9];
+    sync_move_delta(settings, end_x, end_y, end_z, AA_end, BB_end, CC_end,
+                    u_end, v_end, w_end, delta);
+    CHP(check_spindle_sync_feed(settings, block->k_number, "G33", delta));
+    suspend_speed_override(settings);
     START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0);
     STRAIGHT_FEED(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, u_end, v_end, w_end);
     STOP_SPEED_FEED_SYNCH();
+    restore_speed_override(settings);
     settings->current_x = end_x;
     settings->current_y = end_y;
     settings->current_z = end_z;
@@ -5541,14 +5584,20 @@ int Interp::convert_straight(int move,   //!< either G_0 or G_1
     CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) &&
            (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)),
           _("Spindle not turning in G33.1"));
-    START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0);
     double scale = 1;
     if(block->i_flag){
         scale = block->i_number;
         if(scale < 1){
             scale = 1;
         }
     }
+    double delta[9];
+    sync_move_delta(settings, end_x, end_y, end_z, AA_end, BB_end, CC_end,
+                    u_end, v_end, w_end, delta);
+    // I multiplies the spindle speed for the retract
+    CHP(check_spindle_sync_feed(settings, block->k_number * scale, "G33.1",
+                                delta));
+    START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0);
     RIGID_TAP(block->line_number, end_x, end_y, end_z, scale);
     STOP_SPEED_FEED_SYNCH();
     // after the RIGID_TAP cycle we'll be in the same spot
@@ -5771,6 +5820,18 @@ int Interp::convert_threading_cycle(block_pointer block,
 
     double target_z = end_z + fabs(k_number) * tan(compound_angle);
 
+    // A taper also moves X by the thread height over the taper distance, at
+    // the correspondingly larger pitch.
+    double plain_pass[9] = {0.0, 0.0, target_z - start_z, 0, 0, 0, 0, 0, 0};
+    CHP(check_spindle_sync_feed(settings, pitch, "G76", plain_pass));
+    if (taper_dist != 0.0 && (entry_taper || exit_taper)) {
+        double taper_pass[9] = {full_threadheight, 0.0, taper_dist,
+                                0, 0, 0, 0, 0, 0};
+        CHP(check_spindle_sync_feed(settings, taper_pitch, "G76", taper_pass));
+    }
+
+    suspend_speed_override(settings);
+
     depth = start_depth;
     zoff = (depth - full_dia_depth) * tan(compound_angle);
     while (depth < end_depth) {
@@ -5789,6 +5850,7 @@ int Interp::convert_threading_cycle(block_pointer block,
 		       start_z, zoff, taper_dist, entry_taper, exit_taper,
 		       taper_pitch, pitch, full_threadheight, target_z);
     }
+    restore_speed_override(settings);
     STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AABBCC);
     settings->current_x = end_x;
     settings->current_y = end_y;

src/emc/rs274ngc/rs274ngc_interp.hh

@@ -223,6 +223,8 @@ public:
  int check_items(block_pointer block, setup_pointer settings);
  int check_m_codes(block_pointer block);
  int check_other_codes(block_pointer block);
+ int check_spindle_sync_feed(setup_pointer settings, double pitch,
+                             const char *code, const double delta[9]);
  int close_and_downcase(char *line);
  void nurbs_reset_global_variables(void);
  int convert_nurbs(int move, block_pointer block, setup_pointer settings);

src/emc/sai/saicanon.cc

@@ -969,6 +969,12 @@ extern CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE(int idx)
 #endif //}
 }
 
+/* The standalone interpreter has no machine, so no axis limits */
+double GET_EXTERNAL_AXIS_MAX_VELOCITY(int /*axis*/)
+{
+  return 0.0;
+}
+
 /* Returns the system traverse rate */
 double GET_EXTERNAL_TRAVERSE_RATE()
 {

src/emc/task/emccanon.cc

@@ -3815,6 +3815,21 @@ double GET_EXTERNAL_FEED_RATE()
     return feed;
 }
 
+// maximum velocity of one axis, in program units per minute
+double GET_EXTERNAL_AXIS_MAX_VELOCITY(int axis)
+{
+    if (axis < 0 || axis > 8 || !axis_valid(axis)) {
+	return 0.0;
+    }
+
+    double vel = emcAxisGetMaxVelocity(axis);
+
+    if (axis >= 3 && axis <= 5) {
+	return TO_PROG_ANG(FROM_EXT_ANG(vel)) * 60.0;
+    }
+    return TO_PROG_LEN(FROM_EXT_LEN(vel)) * 60.0;
+}
+
 // traverse rate wanted is in program units per minute
 double GET_EXTERNAL_TRAVERSE_RATE()
 {