Fix spindle position-sync jitter and catch infeasible thread pitches

docs/src/gcode/g-code.adoc

@@ -1071,6 +1071,16 @@ 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. The commanded position follows the
+spindle, but the axis lags it by an amount proportional to speed, so changing
+the speed part way through shifts the tool along the helix it is already
+cutting. A <<gcode:g76,G76>> cycle holds the override at the value the cycle
+started with rather than dropping it, because the whole cycle is one block; a
+G33 thread is one pass per line with the retract in between, so there is no
+span to hold.
+
 [[gcode:g33-tech-info]]
 .Technical Info
 At the beginning of each G33 pass, LinuxCNC uses the spindle speed and
@@ -1113,11 +1123,15 @@ 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.
+* The pitch and the spindle speed together ask an axis to move faster than
+  its maximum velocity.
 
 [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.
+In constant surface speed mode the spindle speed follows the radius, so the
+check uses the fastest speed the move can reach: the speed at its smallest
+cutting radius, limited by the G96 'D' word and by
+`[SPINDLE_n]MAX_FORWARD_VELOCITY`. If the move reaches the centre of rotation
+and neither limit is set the speed is unbounded and no check is made.
 
 [[gcode:g33.1]]
 == G33.1 Rigid Tapping(((G33.1 Rigid Tapping)))
@@ -1179,18 +1193,22 @@ 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 spindle speed together ask an axis to move faster than
+  its maximum velocity. See <<gcode:g33,G33>> for how the speed is determined
+  in constant surface speed mode.
 
 [[gcode:g38]]
 == G38._n_ Straight Probe(((G38.n Probe)))
@@ -2046,6 +2064,17 @@ 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 spindle speed together ask an axis to move faster than
+  its maximum velocity. See <<gcode:g33,G33>> for how the speed is determined
+  in constant surface speed mode.
+
+[NOTE]
+The spindle speed override is held for the whole G76 cycle at the value in
+effect when the cycle starts, capped at 100% because the pitch is checked
+against the programmed speed. Moving it while the cycle runs has no effect, so
+every pass cuts at one speed and they stay on the same helix. The previous
+setting is restored when the cycle ends. To thread at 80% of the programmed
+speed, set the override before the cycle starts.
 
 .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,11 @@ 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,
+and held at the value the cycle started with for the duration of a
+<<gcode:g76,G76>> cycle, whatever M48, M49 or M51 last selected. That selection
+is restored afterwards.
+
 [[mcode:m52]]
 == M52 Adaptive Feed Control
 

src/emc/motion/command.c

@@ -534,6 +534,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 	       does yet), and if in free mode, it disables the free mode traj
 	       planners which stops joint motion */
 	    rtapi_print_msg(RTAPI_MSG_DBG, "ABORT");
+	    /* the override release is queued behind the cycle, drop it here */
+	    emcmotStatus->enables_new &= ~SS_LOCKED;
 	    /* check for coord or free space motion active */
 	    if (GET_MOTION_TELEOP_FLAG()) {
                 axis_jog_abort_all(0);
@@ -1409,14 +1411,23 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 	    break;
 
 	case EMCMOT_SS_ENABLE:
-	    /* enable/disable overriding spindle speed */
+	    /* enable/disable/lock overriding spindle speed */
 	    /* can happen at any time */
-	    if ( emcmotCommand->mode != 0 ) {
+	    switch ( emcmotCommand->mode ) {
+	    case EMC_SO_OVERRIDE_LOCK:
+		/* hold whatever is in effect when the first locked move starts */
+		rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE: LOCK");
+		emcmotStatus->enables_new |= SS_LOCKED;
+		break;
+	    case EMC_SO_OVERRIDE_OFF:
+		rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE: OFF");
+		emcmotStatus->enables_new &= ~(SS_ENABLED | SS_LOCKED);
+		break;
+	    default:
 		rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE: ON");
 		emcmotStatus->enables_new |= SS_ENABLED;
-            } else {
-		rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE: OFF");
-		emcmotStatus->enables_new &= ~SS_ENABLED;
+		emcmotStatus->enables_new &= ~SS_LOCKED;
+		break;
 	    }
 	    break;
 

src/emc/motion/control.c

@@ -68,6 +68,10 @@ extern struct emcmot_status_t *emcmotStatus;
 //  etc.
 static double *pcmd_p[EMCMOT_MAX_AXIS];
 
+/* spindle override held while SS_LOCKED, see process_inputs */
+static double locked_spindle_scale[EMCMOT_MAX_SPINDLES];
+static unsigned char prev_enables = 0;
+
 /***********************************************************************
 *                      LOCAL FUNCTION PROTOTYPES                       *
 ************************************************************************/
@@ -371,6 +375,16 @@ static void process_inputs(void)
 	/* use the enables that are in effect right now */
 	enables = emcmotStatus->enables_new;
     }
+    /* Latch as the first locked move starts, so every pass of a G76 cycle
+       cuts at one speed.  Capped at 1.0: the pitch check used the programmed
+       speed. */
+    if ( (enables & SS_LOCKED) && !(prev_enables & SS_LOCKED) ) {
+	for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++) {
+	    double s = emcmotStatus->spindle_status[spindle_num].scale;
+	    locked_spindle_scale[spindle_num] = s < 1.0 ? s : 1.0;
+	}
+    }
+    prev_enables = enables;
     /* feed scaling first:  feed_scale, adaptive_feed, and feed_hold */
     scale = 1.0;
     if (   (emcmotStatus->motion_state != EMCMOT_MOTION_FREE)
@@ -427,7 +441,9 @@ static void process_inputs(void)
     for (spindle_num=0; spindle_num < emcmotConfig->numSpindles; spindle_num++){
 		scale = 1.0;
 		if ( enables & SS_ENABLED ) {
-			scale *= emcmotStatus->spindle_status[spindle_num].scale;
+			scale *= (enables & SS_LOCKED)
+				? locked_spindle_scale[spindle_num]
+				: emcmotStatus->spindle_status[spindle_num].scale;
 		}
 		/*non maskable (except during spindle synch move) spindle inhibit pin */
 		if ( enables & hal_get_bool(emcmot_hal_data->spindle[spindle_num].spindle_inhibit) ) {
@@ -1349,6 +1365,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

@@ -425,6 +425,7 @@ Suggestion: Split this in to an Error and a Status flag register..
 #define FS_ENABLED 0x02
 #define AF_ENABLED 0x04
 #define FH_ENABLED 0x08
+#define SS_LOCKED  0x10
 
 /* This structure contains all of the data associated with
    a single joint.  Note that this structure does not need
@@ -600,6 +601,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

@@ -734,6 +734,8 @@ extern void ENABLE_FEED_OVERRIDE();
 /* used to deactivate user control of spindle speed override */
 extern void DISABLE_SPEED_OVERRIDE(int spindle);
 extern void ENABLE_SPEED_OVERRIDE(int spindle);
+/* hold the override at whatever is in effect when the next move starts */
+extern void LOCK_SPEED_OVERRIDE(int spindle);
 
 /* used to deactivate user control of feed hold */
 extern void DISABLE_FEED_HOLD();
@@ -874,6 +876,14 @@ 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 maximum forward speed of one spindle, in RPM, or zero if that
+// limit is not available
+extern double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int spindle);
+
 // Returns the system feed rate
 extern double GET_EXTERNAL_FEED_RATE();
 

src/emc/nml_intf/emc.hh

@@ -334,6 +334,7 @@ extern int emcJointSetMaxJerk(int joint, double jerk);
 
 extern int emcSpindleSetParams(int spindle, double max_pos, double min_pos, double max_neg,
             double min_neg, double search_vel, double home_angle, int sequence, double increment);
+extern double emcSpindleGetMaxVelocity(int spindle);
 
 // implementation functions for EMC_TRAJ types
 

src/emc/nml_intf/emc_nml.hh

@@ -657,7 +657,7 @@ class EMC_TRAJ_SET_SO_ENABLE:public EMC_TRAJ_CMD_MSG {
     void update(CMS * cms);
 
     int spindle;
-    unsigned char mode; //mode=0, override off (will work with 100% SO), mode != 0, override on, user can change SO
+    unsigned char mode; //EMC_SO_OVERRIDE_OFF, _ON or _LOCK
 };
 
 class EMC_TRAJ_SET_FH_ENABLE:public EMC_TRAJ_CMD_MSG {

src/emc/nml_intf/motion_types.h

@@ -22,4 +22,9 @@
 #define EMC_MOTION_TYPE_PROBING 5
 #define EMC_MOTION_TYPE_INDEXROTARY 6
 
+/* modes of EMC_TRAJ_SET_SO_ENABLE, passed on as EMCMOT_SS_ENABLE */
+#define EMC_SO_OVERRIDE_OFF  0
+#define EMC_SO_OVERRIDE_ON   1
+#define EMC_SO_OVERRIDE_LOCK 2
+
 #endif

src/emc/rs274ngc/canonmodule.cc

@@ -118,6 +118,7 @@ BOOST_PYTHON_MODULE(emccanon) {
     def("ENABLE_FEED_HOLD",&ENABLE_FEED_HOLD);
     def("ENABLE_FEED_OVERRIDE",&ENABLE_FEED_OVERRIDE);
     def("ENABLE_SPEED_OVERRIDE",&ENABLE_SPEED_OVERRIDE);
+    def("LOCK_SPEED_OVERRIDE",&LOCK_SPEED_OVERRIDE);
     def("FINISH",&FINISH);
     def("ON_RESET", &ON_RESET);
     def("FLOOD_OFF",&FLOOD_OFF);
@@ -178,6 +179,8 @@ 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_SPINDLE_MAX_VELOCITY",&GET_EXTERNAL_SPINDLE_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

@@ -925,6 +925,7 @@ void ENABLE_FEED_HOLD() {}
 void DISABLE_SPEED_OVERRIDE(int /*spindle*/) {}
 void ENABLE_FEED_OVERRIDE() {}
 void ENABLE_SPEED_OVERRIDE(int /*spindle*/) {}
+void LOCK_SPEED_OVERRIDE(int /*spindle*/) {}
 void MIST_OFF() {}
 void FLOOD_OFF() {}
 void MIST_ON() {}
@@ -1064,6 +1065,8 @@ 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_SPINDLE_MAX_VELOCITY(int /*spindle*/) { return 0; }
 double GET_EXTERNAL_TRAVERSE_RATE() { return 0; }
 int GET_EXTERNAL_FLOOD() { return 0; }
 int GET_EXTERNAL_MIST() { return 0; }