Skip to content

Commit bf7aa25

Browse files
committed
feat(rs274ngc): MAX_UNWIND_TURNS auto rotary G0 rebase via G92
Add [AXIS_<L>] 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).
1 parent f108dbd commit bf7aa25

4 files changed

Lines changed: 152 additions & 3 deletions

File tree

docs/src/config/ini-config.adoc

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1017,6 +1017,30 @@ The _<letter>_ specifies one of: X Y Z A B C U V W
10171017
For a rotary axis (A,B,C typ) with unlimited rotation having no `MAX_LIMIT` for that axis in the `[AXIS_`<letter>`]` section a value of 1e99 is used.
10181018
* `WRAPPED_ROTARY = 1` - (bool) When this is set to 1 for an ANGULAR axis the axis will move 0-359.999 degrees.
10191019
Positive Numbers will move the axis in a positive direction and negative numbers will move the axis in the negative direction.
1020+
* `MAX_UNWIND_TURNS = 10` - (real) When set to a positive value for a rotary axis (A, B, or C), enables automatic
1021+
unwind avoidance on G0 absolute moves. When a G0 with the rotary word would otherwise unwind more than
1022+
this many full turns from the current programmed position, the interpreter folds the whole turns into the
1023+
axis (G92) offset so the motor takes the shortest path (within +/- 180 degrees) to the target's angular
1024+
position while the programmed (work-frame) position still reaches the commanded target. Subsequent
1025+
G1/G0 absolute moves use the rebased frame. The motion-side traj.position remains accumulated, so
1026+
stepgens, encoders, and PID see no discontinuity.
1027+
+
1028+
This is intended for rotational-cutting workflows (helical winding, spiral carving) where a G1 must
1029+
honor literal multi-turn absolute targets but a terminal G0 should not unwind. CAM output and existing
1030+
G-code do not need to change.
1031+
+
1032+
Because the unwind rides the G92 offset, the DRO, `#5423`/`#5424`/`#5425` and `_a`/`_b`/`_c` named
1033+
parameters all report the programmed value with no special handling, and the accumulated turns show up
1034+
as a G92 offset (`#5214`/`#5215`/`#5216`). Consequences: the feature reserves G92 on a managed axis, so
1035+
do not set G92 manually on an axis that has `MAX_UNWIND_TURNS` (use it freely on other axes); a `G92.1`
1036+
in the program clears the accumulated unwind, after which the next qualifying G0 simply rebases again; and
1037+
the offset is cleared automatically when the axis is re-homed or estop-reset, so a fresh home starts clean.
1038+
+
1039+
The feature is gated to trivkins-style 1:1 axis-to-joint mapping; behavior in coupled kinematics
1040+
(5-axis TCP, gantry rotary) is undefined and not supported. Mutually exclusive with `WRAPPED_ROTARY`;
1041+
if both are set on the same axis, `MAX_UNWIND_TURNS` is ignored with a startup warning.
1042+
+
1043+
Default: 0 (feature disabled).
10201044
* `LOCKING_INDEXER_JOINT = 4` - (int) This value selects a joint to use for a locking indexer for the specified axis _<letter>_.
10211045
In this example, the joint is 4 which would correspond to the B axis for a XYZAB system with trivkins (identity) kinematics.
10221046
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.

src/emc/rs274ngc/interp_convert.cc

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5399,6 +5399,53 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1
53995399
}
54005400

54015401
settings->motion_mode = move;
5402+
5403+
// Auto-rotary-rebase: on a G0 with a rotary word, if the programmed
5404+
// (work-frame) delta exceeds MAX_UNWIND_TURNS, fold the whole turns of
5405+
// unwind into the axis (G92) offset so the motor takes the shortest path
5406+
// to the target's angular position (within +/- 180 deg) while the work-frame
5407+
// position still reaches the programmed target. Riding the existing G92
5408+
// plumbing means the DRO, #<_a> and #5423 all report the programmed value
5409+
// with no special-casing, and the motion-side accumulated position is left
5410+
// alone so stepgens, encoders and PID see no discontinuity.
5411+
// Only applies in absolute mode (G90); G53 explicitly bypasses; incremental
5412+
// (G91) does not need it.
5413+
if (move == G_0 &&
5414+
block->g_modes[GM_MODAL_0] != G_53 &&
5415+
settings->distance_mode == DISTANCE_MODE::ABSOLUTE) {
5416+
auto rebase = [](double cur, double *off, double max_turns, int flag,
5417+
double bnum) -> bool {
5418+
if (!flag || max_turns <= 0.0) return false;
5419+
double delta = bnum - cur;
5420+
if (fabs(delta) / 360.0 <= max_turns) return false;
5421+
double mod = fmod(delta + 180.0, 360.0);
5422+
if (mod < 0.0) mod += 360.0;
5423+
double delta_short = mod - 180.0;
5424+
// Absorb the whole-turn remainder into the existing axis offset; the
5425+
// sub-turn delta_short is the only physical motion that results.
5426+
*off += (cur + delta_short) - bnum;
5427+
return true;
5428+
};
5429+
bool rebased = false;
5430+
rebased |= rebase(settings->AA_current, &settings->AA_axis_offset,
5431+
settings->a_max_unwind_turns, block->a_flag, block->a_number);
5432+
rebased |= rebase(settings->BB_current, &settings->BB_axis_offset,
5433+
settings->b_max_unwind_turns, block->b_flag, block->b_number);
5434+
rebased |= rebase(settings->CC_current, &settings->CC_axis_offset,
5435+
settings->c_max_unwind_turns, block->c_flag, block->c_number);
5436+
if (rebased) {
5437+
SET_G92_OFFSET(settings->axis_offset_x, settings->axis_offset_y,
5438+
settings->axis_offset_z, settings->AA_axis_offset,
5439+
settings->BB_axis_offset, settings->CC_axis_offset,
5440+
settings->u_axis_offset, settings->v_axis_offset,
5441+
settings->w_axis_offset);
5442+
settings->parameters[G92_APPLIED] = 1.0;
5443+
settings->parameters[5214] = PROGRAM_TO_USER_ANG(settings->AA_axis_offset);
5444+
settings->parameters[5215] = PROGRAM_TO_USER_ANG(settings->BB_axis_offset);
5445+
settings->parameters[5216] = PROGRAM_TO_USER_ANG(settings->CC_axis_offset);
5446+
}
5447+
}
5448+
54025449
CHP(find_ends(block, settings, &end_x, &end_y, &end_z,
54035450
&AA_end, &BB_end, &CC_end, &u_end, &v_end, &w_end));
54045451

src/emc/rs274ngc/interp_internal.hh

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -811,6 +811,15 @@ struct setup
811811
int b_axis_wrapped;
812812
int c_axis_wrapped;
813813

814+
// Per-axis automatic G0 unwind threshold, in turns.
815+
// 0 disables; >0 makes a G0 that would otherwise unwind more than the
816+
// threshold fold the whole turns into the axis (G92) offset, so the motor
817+
// stays put while the work frame still reaches the programmed target.
818+
// Mutually exclusive with WRAPPED_ROTARY.
819+
double a_max_unwind_turns;
820+
double b_max_unwind_turns;
821+
double c_max_unwind_turns;
822+
814823
int a_indexer_jnum;
815824
int b_indexer_jnum;
816825
int c_indexer_jnum;

src/emc/rs274ngc/rs274ngc_pre.cc

Lines changed: 72 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -859,6 +859,9 @@ int Interp::init()
859859
_setup.a_axis_wrapped = 0;
860860
_setup.b_axis_wrapped = 0;
861861
_setup.c_axis_wrapped = 0;
862+
_setup.a_max_unwind_turns = 0.0;
863+
_setup.b_max_unwind_turns = 0.0;
864+
_setup.c_max_unwind_turns = 0.0;
862865
_setup.random_toolchanger = 0;
863866
_setup.a_indexer_jnum = -1; // -1 means not used
864867
_setup.b_indexer_jnum = -1; // -1 means not used
@@ -888,6 +891,28 @@ int Interp::init()
888891
_setup.a_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_A", false);
889892
_setup.b_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_B", false);
890893
_setup.c_axis_wrapped = inifile.findBoolV("WRAPPED_ROTARY", "AXIS_C", false);
894+
895+
_setup.a_max_unwind_turns = inifile.findRealV("MAX_UNWIND_TURNS", "AXIS_A", 0.0);
896+
_setup.b_max_unwind_turns = inifile.findRealV("MAX_UNWIND_TURNS", "AXIS_B", 0.0);
897+
_setup.c_max_unwind_turns = inifile.findRealV("MAX_UNWIND_TURNS", "AXIS_C", 0.0);
898+
899+
struct UnwindCheck { const char *axis_name; int wrapped; double turns; };
900+
UnwindCheck uw_checks[] = {
901+
{"AXIS_A", _setup.a_axis_wrapped, _setup.a_max_unwind_turns},
902+
{"AXIS_B", _setup.b_axis_wrapped, _setup.b_max_unwind_turns},
903+
{"AXIS_C", _setup.c_axis_wrapped, _setup.c_max_unwind_turns},
904+
};
905+
for (auto &uw : uw_checks) {
906+
if (uw.turns > 0.0 && uw.wrapped) {
907+
fprintf(stderr,
908+
"%s: MAX_UNWIND_TURNS is mutually exclusive with WRAPPED_ROTARY; "
909+
"MAX_UNWIND_TURNS ignored.\n", uw.axis_name);
910+
if (uw.axis_name[5] == 'A') _setup.a_max_unwind_turns = 0.0;
911+
if (uw.axis_name[5] == 'B') _setup.b_max_unwind_turns = 0.0;
912+
if (uw.axis_name[5] == 'C') _setup.c_max_unwind_turns = 0.0;
913+
}
914+
}
915+
891916
_setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false);
892917
_setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1);
893918

@@ -2033,9 +2058,53 @@ int Interp::synch()
20332058
_setup.control_mode = GET_EXTERNAL_MOTION_CONTROL_MODE();
20342059
_setup.tolerance = GET_EXTERNAL_MOTION_CONTROL_TOLERANCE();
20352060
_setup.naivecam_tolerance = GET_EXTERNAL_MOTION_CONTROL_NAIVECAM_TOLERANCE();
2036-
_setup.AA_current = GET_EXTERNAL_POSITION_A();
2037-
_setup.BB_current = GET_EXTERNAL_POSITION_B();
2038-
_setup.CC_current = GET_EXTERNAL_POSITION_C();
2061+
// Invalidate any MAX_UNWIND_TURNS auto-unwind G92 offset when the machine
2062+
// frame is re-anchored under us (homing, estop reset). Such an event snaps
2063+
// the external (work-frame) position by far more than any blend rounding
2064+
// while the interp was not commanding motion, so a jump over half a turn
2065+
// with a live offset on a managed axis means the accumulated frame is gone
2066+
// and the unwind offset is now stale. Collapse the offset back into the work
2067+
// frame and re-emit the G92 so canon agrees; otherwise the next absolute move
2068+
// would apply the stale offset and run onto the wrong path. A plain jog keeps
2069+
// machine and work frame moving together (sub-turn), so the offset stays
2070+
// valid and is left alone. Only managed axes (MAX_UNWIND_TURNS > 0) are
2071+
// touched, so a manually set G92 on an unmanaged axis is never disturbed.
2072+
{
2073+
double ext_a = GET_EXTERNAL_POSITION_A();
2074+
double ext_b = GET_EXTERNAL_POSITION_B();
2075+
double ext_c = GET_EXTERNAL_POSITION_C();
2076+
bool unwind_reset = false;
2077+
if (_setup.a_max_unwind_turns > 0.0 && _setup.AA_axis_offset != 0.0 &&
2078+
fabs(ext_a - _setup.AA_current) > 180.0) {
2079+
ext_a += _setup.AA_axis_offset;
2080+
_setup.AA_axis_offset = 0.0;
2081+
_setup.parameters[5214] = 0.0;
2082+
unwind_reset = true;
2083+
}
2084+
if (_setup.b_max_unwind_turns > 0.0 && _setup.BB_axis_offset != 0.0 &&
2085+
fabs(ext_b - _setup.BB_current) > 180.0) {
2086+
ext_b += _setup.BB_axis_offset;
2087+
_setup.BB_axis_offset = 0.0;
2088+
_setup.parameters[5215] = 0.0;
2089+
unwind_reset = true;
2090+
}
2091+
if (_setup.c_max_unwind_turns > 0.0 && _setup.CC_axis_offset != 0.0 &&
2092+
fabs(ext_c - _setup.CC_current) > 180.0) {
2093+
ext_c += _setup.CC_axis_offset;
2094+
_setup.CC_axis_offset = 0.0;
2095+
_setup.parameters[5216] = 0.0;
2096+
unwind_reset = true;
2097+
}
2098+
if (unwind_reset)
2099+
SET_G92_OFFSET(_setup.axis_offset_x, _setup.axis_offset_y,
2100+
_setup.axis_offset_z, _setup.AA_axis_offset,
2101+
_setup.BB_axis_offset, _setup.CC_axis_offset,
2102+
_setup.u_axis_offset, _setup.v_axis_offset,
2103+
_setup.w_axis_offset);
2104+
_setup.AA_current = ext_a;
2105+
_setup.BB_current = ext_b;
2106+
_setup.CC_current = ext_c;
2107+
}
20392108
_setup.u_current = GET_EXTERNAL_POSITION_U();
20402109
_setup.v_current = GET_EXTERNAL_POSITION_V();
20412110
_setup.w_current = GET_EXTERNAL_POSITION_W();

0 commit comments

Comments
 (0)