diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 6ffc2b06906..a224894db79 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1018,6 +1018,25 @@ 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 accumulated position, the interpreter shifts an internal user-frame + offset so the motor stays put while the user-frame position jumps to the programmed 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. ++ +Side effects to be aware of: `#5423`/`#5424`/`#5425` and `_a`/`_b`/`_c` named parameters report +user-frame values (the value the programmer commanded). The absolute machine-frame position drifts by +one rebase increment per program run; eventually soft-limit `MAX_LIMIT` may be reached on very long runs. +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 59171b7306e..05043252c5c 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -5343,6 +5343,39 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } settings->motion_mode = move; + + // Auto-rotary-rebase: on G0 with a rotary word, if the user-frame delta + // exceeds MAX_UNWIND_TURNS, shift the modulo offset so the motor takes the + // shortest path to the target's angular position (within +/- 180 deg) + // while the user-frame position jumps to the programmed target. The whole + // turns of unwind are absorbed by the offset; only the sub-turn part is + // physical motion. + // 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) { + if (!flag || max_turns <= 0.0) return; + double user_pos = cur - *off; + double delta = bnum - user_pos; + if (fabs(delta) / 360.0 > max_turns) { + double mod = fmod(delta + 180.0, 360.0); + if (mod < 0.0) mod += 360.0; + double delta_short = mod - 180.0; + double target_machine = cur + delta_short; + *off = target_machine - bnum; + } + }; + rebase(settings->AA_current, &settings->AA_modulo_offset, + settings->a_max_unwind_turns, block->a_flag, block->a_number); + rebase(settings->BB_current, &settings->BB_modulo_offset, + settings->b_max_unwind_turns, block->b_flag, block->b_number); + rebase(settings->CC_current, &settings->CC_modulo_offset, + settings->c_max_unwind_turns, block->c_flag, block->c_number); + } + 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_find.cc b/src/emc/rs274ngc/interp_find.cc index 3eb92c6b06c..840d4e605c8 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -298,7 +298,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(s->a_axis_wrapped) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); } else { - *AA_p = block->a_number; + *AA_p = block->a_number + s->AA_modulo_offset; } } else { *AA_p = s->AA_current; @@ -308,7 +308,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(s->b_axis_wrapped) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); } else { - *BB_p = block->b_number; + *BB_p = block->b_number + s->BB_modulo_offset; } } else { *BB_p = s->BB_current; @@ -318,7 +318,7 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(s->c_axis_wrapped) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); } else { - *CC_p = block->c_number; + *CC_p = block->c_number + s->CC_modulo_offset; } } else { *CC_p = s->CC_current; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index bbcf9e926e7..18012206b09 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -802,6 +802,22 @@ struct setup int b_axis_wrapped; int c_axis_wrapped; + // Per-axis automatic G0 unwind threshold, in turns. + // 0 disables; >0 triggers an interp-side rebase when a G0 with the rotary + // word would otherwise unwind more than the threshold, leaving the motor + // stationary and shifting the user-frame so the target is reached without + // physical motion. Mutually exclusive with WRAPPED_ROTARY and ROTARY_MODULO. + double a_max_unwind_turns; + double b_max_unwind_turns; + double c_max_unwind_turns; + + // Persistent per-axis offset between motion-side accumulated position and + // user-frame position. Bumped by the auto-rebase logic. user_pos = + // AA_current - AA_modulo_offset. + double AA_modulo_offset; + double BB_modulo_offset; + double CC_modulo_offset; + int a_indexer_jnum; int b_indexer_jnum; int c_indexer_jnum; diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 9fcc602a9c4..060c8ecbc25 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -702,16 +702,16 @@ int Interp::lookup_named_param(const char *nameBuf, *value = _setup.current_z; break; - case NP_A: // current position - *value = _setup.AA_current; + case NP_A: // current position (user-frame; subtracts auto-rebase offset) + *value = _setup.AA_current - _setup.AA_modulo_offset; break; - case NP_B: // current position - *value = _setup.BB_current; + case NP_B: // current position (user-frame; subtracts auto-rebase offset) + *value = _setup.BB_current - _setup.BB_modulo_offset; break; - case NP_C: // current position - *value = _setup.CC_current; + case NP_C: // current position (user-frame; subtracts auto-rebase offset) + *value = _setup.CC_current - _setup.CC_modulo_offset; break; case NP_U: // current position diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index e2f798f161f..8e51f87c41a 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -859,6 +859,12 @@ 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.AA_modulo_offset = 0.0; + _setup.BB_modulo_offset = 0.0; + _setup.CC_modulo_offset = 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 +894,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); @@ -1612,9 +1640,12 @@ int Interp::_read(const char *command) //!< may be NULL or a string to read _setup.parameters[5420] = _setup.current_x; _setup.parameters[5421] = _setup.current_y; _setup.parameters[5422] = _setup.current_z; - _setup.parameters[5423] = _setup.AA_current; - _setup.parameters[5424] = _setup.BB_current; - _setup.parameters[5425] = _setup.CC_current; + // Subtract the auto-rotary-rebase offset so #5423-#5425 report user-frame + // (matches the value the programmer commanded). Offset is 0 unless + // MAX_UNWIND_TURNS is in effect for the axis. + _setup.parameters[5423] = _setup.AA_current - _setup.AA_modulo_offset; + _setup.parameters[5424] = _setup.BB_current - _setup.BB_modulo_offset; + _setup.parameters[5425] = _setup.CC_current - _setup.CC_modulo_offset; _setup.parameters[5426] = _setup.u_current; _setup.parameters[5427] = _setup.v_current; _setup.parameters[5428] = _setup.w_current;