Add spindle angle offset (D word) for G33 and G76 threading

docs/src/gcode/g-code.adoc

@@ -1041,10 +1041,11 @@ It is an error if :
 
 [source,ngc]
 ----
-G33 X- Y- Z- K- $-
+G33 X- Y- Z- K- $- D-
 ----
 
 * 'K' - distance per revolution
+* 'D' - (optional) start angle offset from the spindle index pulse, in degrees
 
 For spindle-synchronized motion in one direction, code 'G33 X- Y- Z- K-'
 where K gives the distance moved in XYZ for each revolution of the
@@ -1064,6 +1065,20 @@ speed pins, so multiple passes line up. 'G33' moves end at the programmed
 endpoint.
 G33 could be used to cut tapered threads or a fusee.
 
+The (optional) 'D' argument delays the start of the motion until the spindle
+has turned 'D' degrees past the index pulse, instead of starting at the index
+pulse itself. If 'D' is omitted the default is zero, which starts at the index
+pulse as before. This is how successive starts of a multi-start thread are cut:
+the same thread program is run once per start, each with a different 'D'.
+For example, a two-start thread is cut with 'D0' and then 'D180'; a three-start
+thread with 'D0', 'D120' and 'D240'.
+
+[NOTE]
+'D' is an angle past the index pulse and so is only meaningful as a positive
+number. A negative value is not an error: its magnitude is used, so 'D-90'
+behaves exactly as 'D90'. Values of 360 or more are not reduced, so 'D450'
+waits a full turn longer than 'D90'.
+
 All the axis words are optional, except that at least one must be used.
 
 [NOTE]
@@ -1082,6 +1097,10 @@ angle. That means that Z will reach the correct position just as it
 finishes accelerating to the proper speed, and can immediately begin
 cutting a good thread.
 
+When a 'D' offset is programmed the axis is instead held at rest until the
+spindle reaches the requested angle, and tracking begins from there. Allow
+enough clearance before the thread for the axis to accelerate from that point.
+
 .HAL Connections
 The pin 'spindle.N.at-speed' must be set or driven true for the motion to
 start. Additionally spindle.N.revs must increase by 1 for each revolution
@@ -1960,7 +1979,7 @@ In example S100 with 1.25MM per revolution thread pitch gives a feed of F125.
 
 [source,ngc]
 ----
-G76 P- Z- I- J- R- K- Q- H- E- L- $-
+G76 P- Z- I- J- R- K- Q- H- E- L- $- D-
 ----
 
 .G76 Threading
@@ -2001,6 +2020,20 @@ Unnecessarily high degression values will produce an unnecessarily high number o
 * 'H-' - The number of 'spring passes'.
   Spring passes are additional passes at full thread depth.
   If no additional passes are desired, program 'H0'.
+* 'D-' - The 'start angle offset' in degrees from the spindle index pulse.
+  Every pass of the cycle begins once the spindle has turned 'D' degrees past
+  the index pulse, instead of at the index pulse itself.
+  If 'D' is omitted the default is zero, which starts every pass at the index
+  pulse as before.
+  This cuts one start of a multi-start thread: run the same 'G76' cycle once per
+  start, each with a different 'D'. A two-start thread is cut with 'D0' and then
+  'D180', a three-start thread with 'D0', 'D120' and 'D240'.
+
+[NOTE]
+'D' is an angle past the index pulse and so is only meaningful as a positive
+number. A negative value is not an error: its magnitude is used, so 'D-90'
+behaves exactly as 'D90'. Values of 360 or more are not reduced, so 'D450'
+waits a full turn longer than 'D90'.
 
 Thread entries and exits can be programmed tapered with the 'E' and 'L' values.
 

docs/src/gcode/overview.adoc

@@ -116,7 +116,8 @@ The table includes N and O for completeness, even though, as defined above, line
 |A | A axis of machine
 |B | B axis of machine
 |C | C axis of machine
-|D | Tool radius compensation number
+.2+|D | Tool radius compensation number.
+<| Start angle offset from the spindle index pulse, in degrees, for G33 and G76 threading.
 |F | Feed rate
 |G | General function (See table  <<cap:modal-groups,G-code Modal Groups>>)
 |H | Tool length offset index

src/emc/motion/command.c

@@ -1051,7 +1051,7 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 	    break;
 
 	case EMCMOT_SET_SPINDLESYNC:
-		tpSetSpindleSync(&emcmotInternal->coord_tp, emcmotCommand->spindle, emcmotCommand->spindlesync, emcmotCommand->flags);
+		tpSetSpindleSync(&emcmotInternal->coord_tp, emcmotCommand->spindle, emcmotCommand->spindlesync, emcmotCommand->flags, emcmotCommand->angular_offset_degrees);
 		break;
 
 	case EMCMOT_SET_LINE:

src/emc/motion/motion.h

@@ -219,6 +219,7 @@ extern "C" {
                                    constraints (the INI file) */
         int motion_type;        /* this move is because of traverse, feed, arc, or toolchange */
         double spindlesync;     /* user units per spindle revolution, 0 = no sync */
+        double angular_offset_degrees; /* spindle angle offset for threading start (D word) */
 	double acc;		/* max acceleration */
 	double jerk;			/* jerk for traj */
     double ini_maxjerk;

src/emc/nml_intf/canon.hh

@@ -432,7 +432,7 @@ extern void STOP_CUTTER_RADIUS_COMPENSATION();
 translation commands. */
 
 /* used for threading */
-extern void START_SPEED_FEED_SYNCH(int spindle, double feed_per_revolution, bool velocity_mode);
+extern void START_SPEED_FEED_SYNCH(int spindle, double feed_per_revolution, bool velocity_mode, double angle_degrees = 0.0);
 extern void STOP_SPEED_FEED_SYNCH();
 
 

src/emc/nml_intf/emc.cc

@@ -1115,6 +1115,7 @@ void EMC_TRAJ_SET_SPINDLESYNC::update(CMS * cms)
     EMC_TRAJ_CMD_MSG::update(cms);
     cms->update(feed_per_revolution);
     cms->update(velocity_mode);
+    cms->update(angular_offset_degrees);
 }
 
 /*

src/emc/nml_intf/emc.hh

@@ -374,7 +374,7 @@ extern int emcTrajLinearMove(const EmcPose& end, int type, double vel,
 extern int emcTrajCircularMove(const EmcPose& end, const PM_CARTESIAN& center, const PM_CARTESIAN&
         normal, int turn, int type, double vel, double ini_maxvel, double acc, double ini_maxjerk);
 extern int emcTrajSetTermCond(int cond, double tolerance);
-extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index);
+extern int emcTrajSetSpindleSync(int spindle, double feed_per_revolution, bool wait_for_index, double angular_offset_degrees = 0.0);
 extern int emcTrajSetOffset(const EmcPose& tool_offset);
 extern int emcTrajSetHome(const EmcPose& home);
 extern int emcTrajClearProbeTrippedFlag();

src/emc/nml_intf/emc_nml.hh

@@ -815,7 +815,8 @@ class EMC_TRAJ_SET_SPINDLESYNC:public EMC_TRAJ_CMD_MSG {
       : EMC_TRAJ_CMD_MSG(EMC_TRAJ_SET_SPINDLESYNC_TYPE, sizeof(EMC_TRAJ_SET_SPINDLESYNC)),
         spindle(0),
         feed_per_revolution(0.0),
-        velocity_mode(false)
+        velocity_mode(false),
+        angular_offset_degrees(0.0)
     {};
 
     // Sub-class update() calls base-class update()
@@ -825,6 +826,7 @@ class EMC_TRAJ_SET_SPINDLESYNC:public EMC_TRAJ_CMD_MSG {
     int spindle;
     double feed_per_revolution;
     bool velocity_mode;
+    double angular_offset_degrees; // spindle angle offset for threading start (D word, degrees)
 };
 
 class EMC_TRAJ_SET_OFFSET:public EMC_TRAJ_CMD_MSG {

src/emc/rs274ngc/canonmodule.cc

@@ -18,6 +18,7 @@
  */
 #define BOOST_PYTHON_MAX_ARITY 13
 #include <boost/python/def.hpp>
+#include <boost/python/args.hpp>
 #include <boost/python/module.hpp>
 #include <boost/python/scope.hpp>
 #include <boost/python/enum.hpp>
@@ -226,7 +227,12 @@ BOOST_PYTHON_MODULE(emccanon) {
     def("SPINDLE_RETRACT",&SPINDLE_RETRACT);
     def("SPINDLE_RETRACT_TRAVERSE",&SPINDLE_RETRACT_TRAVERSE);
     def("START_CUTTER_RADIUS_COMPENSATION",&START_CUTTER_RADIUS_COMPENSATION);
-    def("START_SPEED_FEED_SYNCH",&START_SPEED_FEED_SYNCH);
+    // spell the arguments out so the C++ default for angle_degrees survives into
+    // Python: a bare function pointer would make the new argument mandatory and
+    // break every existing three-argument caller
+    def("START_SPEED_FEED_SYNCH",&START_SPEED_FEED_SYNCH,
+	(arg("spindle"), arg("feed_per_revolution"), arg("velocity_mode"),
+	 arg("angle_degrees")=0.0));
     def("START_SPINDLE_CLOCKWISE",&START_SPINDLE_CLOCKWISE);
     def("START_SPINDLE_COUNTERCLOCKWISE",&START_SPINDLE_COUNTERCLOCKWISE);
     def("STOP_CUTTER_RADIUS_COMPENSATION",&STOP_CUTTER_RADIUS_COMPENSATION);

src/emc/rs274ngc/gcodemodule.cc

@@ -869,7 +869,7 @@ 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(int /*spindle*/, double /*sync*/, bool /*vel*/) {}
+void START_SPEED_FEED_SYNCH(int /*spindle*/, double /*sync*/, bool /*vel*/, double /*angle*/) {}
 void STOP_SPEED_FEED_SYNCH() {}
 void START_SPINDLE_COUNTERCLOCKWISE(int /*spindle*/, int /*wait_for_at_speed*/) {}
 void START_SPINDLE_CLOCKWISE(int /*spindle*/, int /*wait_for_at_speed*/) {}

src/emc/rs274ngc/interp_check.cc

@@ -257,8 +257,9 @@ int Interp::check_other_codes(block_pointer block)       //!< pointer to a block
         (motion != G_70) && (motion != G_71) && (motion != G_71_1) &&
         (motion != G_71_2) && (motion != G_72) && (motion != G_72_1) &&
         (motion != G_72_2) && (motion != G_73) && (motion != G_83) &&
-        (block->g_modes[14] != G_96)),
-        _("D word with no G41, G41.1, G42, G42.1, G71, G71.1, G71.2 G73, G83 or G96 to use it"));
+        (block->g_modes[14] != G_96) &&
+        (motion != G_33) && (motion != G_76)),
+        _("D word with no G41, G41.1, G42, G42.1, G71, G71.1, G71.2 G73, G83, G96, G33 or G76 to use it"));
   }
 
   if (block->dollar_flag) {

src/emc/rs274ngc/interp_convert.cc

@@ -5528,7 +5528,10 @@ 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"));
-    START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0);
+    // the offset is a direction-less angle past the index pulse, so a negative
+    // D is taken as its magnitude rather than rejected
+    double g33_angle = block->d_flag ? fabs(block->d_number_float) : 0.0;
+    START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0, g33_angle);
     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();
     settings->current_x = end_x;
@@ -5660,26 +5663,27 @@ threading_pass(setup_pointer settings, block_pointer block,
 	       int boring, double safe_x, double depth, double end_depth,
 	       double start_y, double start_z, double zoff, double taper_dist,
 	       int entry_taper, int exit_taper, double taper_pitch,
-	       double pitch, double full_threadheight, double target_z) {
+	       double pitch, double full_threadheight, double target_z,
+	       double angle_offset) {
     STRAIGHT_TRAVERSE(block->line_number, boring?
 		      safe_x + depth - end_depth:
 		      safe_x - depth + end_depth,
 		      start_y, start_z - zoff, AABBCC); //back
     if(taper_dist && entry_taper) {
 	DISABLE_FEED_OVERRIDE();
-	START_SPEED_FEED_SYNCH(settings->active_spindle, taper_pitch, 0);
+	START_SPEED_FEED_SYNCH(settings->active_spindle, taper_pitch, 0, angle_offset);
 	STRAIGHT_FEED(block->line_number, boring?
 		      safe_x + depth - full_threadheight:
 		      safe_x - depth + full_threadheight,
 		      start_y, start_z - zoff, AABBCC); //in
 	STRAIGHT_FEED(block->line_number, boring? safe_x + depth: safe_x - depth, //angled in
 		      start_y, start_z - zoff - taper_dist, AABBCC);
-	START_SPEED_FEED_SYNCH(settings->active_spindle, pitch, 0);
+	START_SPEED_FEED_SYNCH(settings->active_spindle, pitch, 0, angle_offset);
     } else {
 	STRAIGHT_TRAVERSE(block->line_number, boring? safe_x + depth: safe_x - depth,
 			  start_y, start_z - zoff, AABBCC); //in
 	DISABLE_FEED_OVERRIDE();
-	START_SPEED_FEED_SYNCH(settings->active_spindle, pitch, 0);
+	START_SPEED_FEED_SYNCH(settings->active_spindle, pitch, 0, angle_offset);
     }
 
     if(taper_dist && exit_taper) {
@@ -5768,6 +5772,9 @@ int Interp::convert_threading_cycle(block_pointer block,
     int entry_taper = taper_flags & 1;
     int exit_taper = taper_flags & 2;
 
+    // as in G33: a negative D is taken as its magnitude
+    double angle_offset = block->d_flag ? fabs(block->d_number_float) : 0.0;
+
     double depth, zoff;
     int pass = 1;
 
@@ -5778,7 +5785,7 @@ int Interp::convert_threading_cycle(block_pointer block,
     while (depth < end_depth) {
 	threading_pass(settings, block, boring, safe_x, depth, end_depth, start_y,
 		       start_z, zoff, taper_dist, entry_taper, exit_taper,
-		       taper_pitch, pitch, full_threadheight, target_z);
+		       taper_pitch, pitch, full_threadheight, target_z, angle_offset);
         depth = full_dia_depth + cut_increment * pow(++pass, 1.0/degression);
         zoff = (depth - full_dia_depth) * tan(compound_angle);
     }
@@ -5789,7 +5796,7 @@ int Interp::convert_threading_cycle(block_pointer block,
     for(int i = 0; i<spring_cuts+1; i++) {
 	threading_pass(settings, block, boring, safe_x, depth, end_depth, start_y,
 		       start_z, zoff, taper_dist, entry_taper, exit_taper,
-		       taper_pitch, pitch, full_threadheight, target_z);
+		       taper_pitch, pitch, full_threadheight, target_z, angle_offset);
     }
     STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AABBCC);
     settings->current_x = end_x;

src/emc/sai/saicanon.cc

@@ -737,8 +737,15 @@ int GET_EXTERNAL_AXIS_MASK() {return 0x3f;} // XYZABC machine
 double GET_EXTERNAL_ANGLE_UNITS() {return 1.0;}
 int GET_EXTERNAL_SELECTED_TOOL_SLOT() { return 0; }
 int GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE(int /*spindle*/) {return so_enable;}
-void START_SPEED_FEED_SYNCH(int /*spindle*/, double sync, bool vel)
-{PRINT("START_SPEED_FEED_SYNC(%f,%d)\n", sync, vel);}
+void START_SPEED_FEED_SYNCH(int /*spindle*/, double sync, bool vel, double angle_degrees)
+{
+    // only print the angle when one was asked for, so that the expected output
+    // of tests predating the D word stays valid
+    if (angle_degrees != 0.0)
+        PRINT("START_SPEED_FEED_SYNC(%f,%d,%f)\n", sync, vel, angle_degrees);
+    else
+        PRINT("START_SPEED_FEED_SYNC(%f,%d)\n", sync, vel);
+}
 CANON_MOTION_MODE motion_mode;
 
 int GET_EXTERNAL_DIGITAL_INPUT(int /*index*/, int def) { return def; }

src/emc/task/emccanon.cc

@@ -1503,13 +1503,14 @@ void STOP_CUTTER_RADIUS_COMPENSATION()
 
 
 
-void START_SPEED_FEED_SYNCH(int spindle, double feed_per_revolution, bool velocity_mode)
+void START_SPEED_FEED_SYNCH(int spindle, double feed_per_revolution, bool velocity_mode, double angle_degrees)
 {
     flush_segments();
     auto spindleSyncMsg = std::make_unique<EMC_TRAJ_SET_SPINDLESYNC>();
     spindleSyncMsg->spindle = spindle;
     spindleSyncMsg->feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution));
     spindleSyncMsg->velocity_mode = velocity_mode;
+    spindleSyncMsg->angular_offset_degrees = angle_degrees;
     interp_list.append(std::move(spindleSyncMsg));
     canon.spindle[spindle].synched = 1;
 }

src/emc/task/emctaskmain.cc

@@ -1887,7 +1887,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd)
 
     case EMC_TRAJ_SET_SPINDLESYNC_TYPE:
         emcTrajSetSpindlesyncMsg = reinterpret_cast<EMC_TRAJ_SET_SPINDLESYNC *>(cmd);
-        retval = emcTrajSetSpindleSync(emcTrajSetSpindlesyncMsg->spindle, emcTrajSetSpindlesyncMsg->feed_per_revolution, emcTrajSetSpindlesyncMsg->velocity_mode);
+        retval = emcTrajSetSpindleSync(emcTrajSetSpindlesyncMsg->spindle, emcTrajSetSpindlesyncMsg->feed_per_revolution, emcTrajSetSpindlesyncMsg->velocity_mode, emcTrajSetSpindlesyncMsg->angular_offset_degrees);
         break;
 
     case EMC_TRAJ_SET_OFFSET_TYPE:

src/emc/task/taskintf.cc

@@ -1505,12 +1505,13 @@ int emcTrajSetOffset(const EmcPose& tool_offset)
     return usrmotWriteEmcmotCommand(&emcmotCommand);
 }
 
-int emcTrajSetSpindleSync(int spindle, double fpr, bool wait_for_index)
+int emcTrajSetSpindleSync(int spindle, double fpr, bool wait_for_index, double angular_offset_degrees)
 {
     emcmotCommand.command = EMCMOT_SET_SPINDLESYNC;
     emcmotCommand.spindle = spindle;
     emcmotCommand.spindlesync = fpr;
     emcmotCommand.flags = wait_for_index;
+    emcmotCommand.angular_offset_degrees = angular_offset_degrees;
     return usrmotWriteEmcmotCommand(&emcmotCommand);
 }
 

src/emc/tp/tc.c

@@ -792,6 +792,7 @@ int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp)
     tc->tolerance = tp->tolerance;
     tc->synchronized = tp->synchronized;
     tc->uu_per_rev = tp->uu_per_rev;
+    tc->angle_offset = tp->spindle.pending_offset;
     return TP_ERR_OK;
 }
 

src/emc/tp/tc_types.h

@@ -176,6 +176,7 @@ typedef struct {
                             // stay within this distance from the path.
     int synchronized;       // spindle sync state
     double uu_per_rev;      // for sync, user units per rev (e.g. 0.0625 for 16tpi)
+    double angle_offset;    // spindle angle offset from index (revolutions) for D word
     double vel_at_blend_start;
     int sync_accel;         // we're accelerating up to sync with the spindle
     unsigned char enables;  // Feed scale, etc, enable bits for this move