From a49fda0551723a126fa6929ff49dfa2b1c65ab33 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 1 May 2026 14:19:52 +0800 Subject: [PATCH] feat(rs274ngc): MAX_UNWIND_TURNS auto rotary G0 rebase via G92 Add [AXIS_] MAX_UNWIND_TURNS = N. On a G0 absolute move with the rotary word, if the programmed (work-frame) delta exceeds N full turns, fold the whole turns into the axis (G92) offset so the motor takes the shortest path (within +/- 180 deg) to the target's angular position while the work-frame position still reaches the commanded target. The motion-side accumulated position is unchanged, so stepgens, encoders and PID see no discontinuity. Riding the existing G92 plumbing means the DRO, #<_a> and #5423 all report the programmed value with no special-casing; the accumulated turns appear as a G92 offset (#5214-#5216). On re-home or estop reset the interpreter resynchronizes and detects the frame re-anchor (a >180 deg work-frame jump on a managed axis with a live offset); it collapses the stale offset back into the work frame and re-emits the G92 so canon agrees, so a fresh home starts clean. Gated to trivkins 1:1 axis-to-joint mapping; coupled kinematics (5-axis TCP, gantry rotary) unsupported. Mutually exclusive with WRAPPED_ROTARY (startup warning if both set on the same axis). Default 0 (disabled). --- docs/src/config/ini-config.adoc | 24 +++++++++ src/emc/rs274ngc/interp_convert.cc | 47 ++++++++++++++++++ src/emc/rs274ngc/interp_internal.hh | 9 ++++ src/emc/rs274ngc/interp_setup.cc | 4 ++ src/emc/rs274ngc/rs274ngc_pre.cc | 75 +++++++++++++++++++++++++++-- 5 files changed, 156 insertions(+), 3 deletions(-) diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 8f1e8d09d56..7b3470fae85 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1017,6 +1017,30 @@ The __ specifies one of: X Y Z A B C U V W For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_``]` section a value of 1e99 is used. * `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees. Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction. +* `MAX_UNWIND_TURNS = 10` - (real) When set to a positive value for a rotary axis (A, B, or C), enables automatic + unwind avoidance on G0 absolute moves. When a G0 with the rotary word would otherwise unwind more than + this many full turns from the current programmed position, the interpreter folds the whole turns into the + axis (G92) offset so the motor takes the shortest path (within +/- 180 degrees) to the target's angular + position while the programmed (work-frame) position still reaches the commanded target. Subsequent + G1/G0 absolute moves use the rebased frame. The motion-side traj.position remains accumulated, so + stepgens, encoders, and PID see no discontinuity. ++ +This is intended for rotational-cutting workflows (helical winding, spiral carving) where a G1 must +honor literal multi-turn absolute targets but a terminal G0 should not unwind. CAM output and existing +G-code do not need to change. ++ +Because the unwind rides the G92 offset, the DRO, `#5423`/`#5424`/`#5425` and `_a`/`_b`/`_c` named +parameters all report the programmed value with no special handling, and the accumulated turns show up +as a G92 offset (`#5214`/`#5215`/`#5216`). Consequences: the feature reserves G92 on a managed axis, so +do not set G92 manually on an axis that has `MAX_UNWIND_TURNS` (use it freely on other axes); a `G92.1` +in the program clears the accumulated unwind, after which the next qualifying G0 simply rebases again; and +the offset is cleared automatically when the axis is re-homed or estop-reset, so a fresh home starts clean. ++ +The feature is gated to trivkins-style 1:1 axis-to-joint mapping; behavior in coupled kinematics +(5-axis TCP, gantry rotary) is undefined and not supported. Mutually exclusive with `WRAPPED_ROTARY`; +if both are set on the same axis, `MAX_UNWIND_TURNS` is ignored with a startup warning. ++ +Default: 0 (feature disabled). * `LOCKING_INDEXER_JOINT = 4` - (int) This value selects a joint to use for a locking indexer for the specified axis __. In this example, the joint is 4 which would correspond to the B axis for a XYZAB system with trivkins (identity) kinematics. When set, a G0 move for this axis will initiate an unlock with the `joint.4.unlock pin` then wait for the `joint.4.is-unlocked` pin then move the joint at the rapid rate for that joint. diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 8f8573cab2f..b3c54841aa5 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -5399,6 +5399,53 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + + // Auto-rotary-rebase: on a G0 with a rotary word, if the programmed + // (work-frame) delta exceeds MAX_UNWIND_TURNS, fold the whole turns of + // unwind into the axis (G92) offset so the motor takes the shortest path + // to the target's angular position (within +/- 180 deg) while the work-frame + // position still reaches the programmed target. Riding the existing G92 + // plumbing means the DRO, #<_a> and #5423 all report the programmed value + // with no special-casing, and the motion-side accumulated position is left + // alone so stepgens, encoders and PID see no discontinuity. + // Only applies in absolute mode (G90); G53 explicitly bypasses; incremental + // (G91) does not need it. + if (move == G_0 && + block->g_modes[GM_MODAL_0] != G_53 && + settings->distance_mode == DISTANCE_MODE::ABSOLUTE) { + auto rebase = [](double cur, double *off, double max_turns, int flag, + double bnum) -> bool { + if (!flag || max_turns <= 0.0) return false; + double delta = bnum - cur; + if (fabs(delta) / 360.0 <= max_turns) return false; + double mod = fmod(delta + 180.0, 360.0); + if (mod < 0.0) mod += 360.0; + double delta_short = mod - 180.0; + // Absorb the whole-turn remainder into the existing axis offset; the + // sub-turn delta_short is the only physical motion that results. + *off += (cur + delta_short) - bnum; + return true; + }; + bool rebased = false; + rebased |= rebase(settings->AA_current, &settings->AA_axis_offset, + settings->a_max_unwind_turns, block->a_flag, block->a_number); + rebased |= rebase(settings->BB_current, &settings->BB_axis_offset, + settings->b_max_unwind_turns, block->b_flag, block->b_number); + rebased |= rebase(settings->CC_current, &settings->CC_axis_offset, + settings->c_max_unwind_turns, block->c_flag, block->c_number); + if (rebased) { + SET_G92_OFFSET(settings->axis_offset_x, settings->axis_offset_y, + settings->axis_offset_z, settings->AA_axis_offset, + settings->BB_axis_offset, settings->CC_axis_offset, + settings->u_axis_offset, settings->v_axis_offset, + settings->w_axis_offset); + settings->parameters[G92_APPLIED] = 1.0; + settings->parameters[5214] = PROGRAM_TO_USER_ANG(settings->AA_axis_offset); + settings->parameters[5215] = PROGRAM_TO_USER_ANG(settings->BB_axis_offset); + settings->parameters[5216] = PROGRAM_TO_USER_ANG(settings->CC_axis_offset); + } + } + CHP(find_ends(block, settings, &end_x, &end_y, &end_z, &AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end)); diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 22268854211..4a27b1bc3eb 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -811,6 +811,15 @@ struct setup int b_axis_wrapped; int c_axis_wrapped; + // Per-axis automatic G0 unwind threshold, in turns. + // 0 disables; >0 makes a G0 that would otherwise unwind more than the + // threshold fold the whole turns into the axis (G92) offset, so the motor + // stays put while the work frame still reaches the programmed target. + // Mutually exclusive with WRAPPED_ROTARY. + double a_max_unwind_turns; + double b_max_unwind_turns; + double c_max_unwind_turns; + int a_indexer_jnum; int b_indexer_jnum; int c_indexer_jnum; diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 365e4682d6c..f9656742e04 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -177,6 +177,10 @@ setup::setup() : b_axis_wrapped(0), c_axis_wrapped(0), + a_max_unwind_turns(0.0), + b_max_unwind_turns(0.0), + c_max_unwind_turns(0.0), + a_indexer_jnum(0), b_indexer_jnum(0), c_indexer_jnum(0), diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index f843fc82985..ff938b3438f 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -859,6 +859,9 @@ int Interp::init() _setup.a_axis_wrapped = 0; _setup.b_axis_wrapped = 0; _setup.c_axis_wrapped = 0; + _setup.a_max_unwind_turns = 0.0; + _setup.b_max_unwind_turns = 0.0; + _setup.c_max_unwind_turns = 0.0; _setup.random_toolchanger = 0; _setup.a_indexer_jnum = -1; // -1 means not used _setup.b_indexer_jnum = -1; // -1 means not used @@ -888,6 +891,28 @@ int Interp::init() _setup.a_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_A", false); _setup.b_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_B", false); _setup.c_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_C", false); + + _setup.a_max_unwind_turns = inifile.findRealV("MAX_UNWIND_TURNS", "AXIS_A", 0.0); + _setup.b_max_unwind_turns = inifile.findRealV("MAX_UNWIND_TURNS", "AXIS_B", 0.0); + _setup.c_max_unwind_turns = inifile.findRealV("MAX_UNWIND_TURNS", "AXIS_C", 0.0); + + struct UnwindCheck { const char *axis_name; int wrapped; double turns; }; + UnwindCheck uw_checks[] = { + {"AXIS_A", _setup.a_axis_wrapped, _setup.a_max_unwind_turns}, + {"AXIS_B", _setup.b_axis_wrapped, _setup.b_max_unwind_turns}, + {"AXIS_C", _setup.c_axis_wrapped, _setup.c_max_unwind_turns}, + }; + for (auto &uw : uw_checks) { + if (uw.turns > 0.0 && uw.wrapped) { + fprintf(stderr, + "%s: MAX_UNWIND_TURNS is mutually exclusive with WRAPPED_ROTARY; " + "MAX_UNWIND_TURNS ignored.\n", uw.axis_name); + if (uw.axis_name[5] == 'A') _setup.a_max_unwind_turns = 0.0; + if (uw.axis_name[5] == 'B') _setup.b_max_unwind_turns = 0.0; + if (uw.axis_name[5] == 'C') _setup.c_max_unwind_turns = 0.0; + } + } + _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); @@ -2033,9 +2058,53 @@ int Interp::synch() _setup.control_mode = GET_EXTERNAL_MOTION_CONTROL_MODE(); _setup.tolerance = GET_EXTERNAL_MOTION_CONTROL_TOLERANCE(); _setup.naivecam_tolerance = GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE(); - _setup.AA_current = GET_EXTERNAL_POSITION_A(); - _setup.BB_current = GET_EXTERNAL_POSITION_B(); - _setup.CC_current = GET_EXTERNAL_POSITION_C(); + // Invalidate any MAX_UNWIND_TURNS auto-unwind G92 offset when the machine + // frame is re-anchored under us (homing, estop reset). Such an event snaps + // the external (work-frame) position by far more than any blend rounding + // while the interp was not commanding motion, so a jump over half a turn + // with a live offset on a managed axis means the accumulated frame is gone + // and the unwind offset is now stale. Collapse the offset back into the work + // frame and re-emit the G92 so canon agrees; otherwise the next absolute move + // would apply the stale offset and run onto the wrong path. A plain jog keeps + // machine and work frame moving together (sub-turn), so the offset stays + // valid and is left alone. Only managed axes (MAX_UNWIND_TURNS > 0) are + // touched, so a manually set G92 on an unmanaged axis is never disturbed. + { + double ext_a = GET_EXTERNAL_POSITION_A(); + double ext_b = GET_EXTERNAL_POSITION_B(); + double ext_c = GET_EXTERNAL_POSITION_C(); + bool unwind_reset = false; + if (_setup.a_max_unwind_turns > 0.0 && _setup.AA_axis_offset != 0.0 && + fabs(ext_a - _setup.AA_current) > 180.0) { + ext_a += _setup.AA_axis_offset; + _setup.AA_axis_offset = 0.0; + _setup.parameters[5214] = 0.0; + unwind_reset = true; + } + if (_setup.b_max_unwind_turns > 0.0 && _setup.BB_axis_offset != 0.0 && + fabs(ext_b - _setup.BB_current) > 180.0) { + ext_b += _setup.BB_axis_offset; + _setup.BB_axis_offset = 0.0; + _setup.parameters[5215] = 0.0; + unwind_reset = true; + } + if (_setup.c_max_unwind_turns > 0.0 && _setup.CC_axis_offset != 0.0 && + fabs(ext_c - _setup.CC_current) > 180.0) { + ext_c += _setup.CC_axis_offset; + _setup.CC_axis_offset = 0.0; + _setup.parameters[5216] = 0.0; + unwind_reset = true; + } + if (unwind_reset) + SET_G92_OFFSET(_setup.axis_offset_x, _setup.axis_offset_y, + _setup.axis_offset_z, _setup.AA_axis_offset, + _setup.BB_axis_offset, _setup.CC_axis_offset, + _setup.u_axis_offset, _setup.v_axis_offset, + _setup.w_axis_offset); + _setup.AA_current = ext_a; + _setup.BB_current = ext_b; + _setup.CC_current = ext_c; + } _setup.u_current = GET_EXTERNAL_POSITION_U(); _setup.v_current = GET_EXTERNAL_POSITION_V(); _setup.w_current = GET_EXTERNAL_POSITION_W();