diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 69fda476554..a94f2478d41 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1015,6 +1015,18 @@ 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. + Mutually exclusive with `ROTARY_MODULO` (only one of the two can be set in a configuration). +* `ROTARY_MODULO = 1` - (bool) Continuous-modulo rotary mode. + Absolute moves on this axis take the shortest path: a `G0 A0` from a current position of A=50000 moves only the residual angle (40 deg in this example), not 138 full revolutions. + G-code may use absolute values of any magnitude (e.g. `A=200000` for helical-spiral CAM output) without the +/-360 limit imposed by `WRAPPED_ROTARY`. + System parameters `#5423`/`#5424`/`#5425` (and the named equivalents `#<_a>`/`#<_b>`/`#<_c>`, `#<_abs_a>`/`#<_abs_b>`/`#<_abs_c>`) report position wrapped to the range 0 up to but not including 360 degrees; probing parameters `#5064`/`#5065`/`#5066` likewise. + Internal commanded position and motion-side `motion.traj.position`/`joint..pos-fb` HAL pins remain accumulated to keep stepgens, encoders and PID loops consistent (no phantom unwind on wrap). + The accumulated physical position can still be read into G-code from the HAL pin via the `#<_hal[...]>` syntax (see <> and the `HAL_PIN_VARS` setting in <>); for example `#<_hal[joint.3.pos-fb]>` returns the unwrapped joint position. + Threading (`G33`, `G33.1`, `G76`) is rejected with an error if the modulo axis word is present in the block. + If the axis `MIN_LIMIT`/`MAX_LIMIT` range is not approximately 360 deg a startup warning is printed (typical of limited-range tilt axes where modulo is unusual). + Mutually exclusive with `WRAPPED_ROTARY` (only one of the two can be set in a configuration). + Mid-program restart-from-line and DRO visual wrap in GUIs are not handled by this flag; these are left to the existing restart machinery and to user-side HAL components if visual wrap is desired. + Path control codes `M26` (shortest, default) and `M27` (literal absolute) are modal: once programmed they persist for all subsequent absolute moves on modulo axes until the other code is programmed. Use `M27` to enter literal mode for a section of multi-turn motion, then `M26` to return to shortest-path. For incremental multi-turn moves, prefer `G91`. * `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/docs/src/gcode/m-code.adoc b/docs/src/gcode/m-code.adoc index a63bf47417b..87d4c708ecd 100644 --- a/docs/src/gcode/m-code.adoc +++ b/docs/src/gcode/m-code.adoc @@ -20,6 +20,7 @@ |<> | Tool Change |<> | Coolant Control |<> | Orient Spindle +|<> | Rotary Modulo Path: Shortest / Literal |<> | Feed & Spindle Overrides Enable/Disable |<> | Feed Override Control |<> | Spindle Override Control @@ -260,6 +261,47 @@ INI Settings in the [RS274NGC] section: ** 'spindle.N.locked' (out bit) Spindle orient complete pin. Cleared by any of M3,M4,M5. +[[mcode:m26-m27]] +== M26, M27 Rotary Modulo Path Control + +(((M26, M27 Rotary Modulo Path Control))) + +* 'M26' - on rotary axes flagged with `[AXIS_] ROTARY_MODULO = 1`, absolute + moves take the shortest path: a programmed value is reached by moving at most + 180 degrees in the shorter direction. This is the default when `ROTARY_MODULO` + is enabled. + +* 'M27' - on the same axes, force absolute moves to take the literal programmed + target. A programmed value of `A720` from a current position of `A0` produces + two full forward turns rather than no motion. Use this when a section of + G-code must execute the commanded angle literally, for example multi-turn + helical winding written in absolute mode. + +M26 and M27 are modal (modal group 3) and persist until the other code is +programmed or the program ends. They have no effect on rotary axes that are not +flagged `ROTARY_MODULO`, on incremental moves (`G91`), or on parameter readout +(`#5423`/`#5424`/`#5425` continue to report the wrapped value). + +Typical usage: + +[source,{ngc}] +---- +(at this point absolute A moves take the shortest path) +M27 (enter literal absolute mode for the section that follows) +G1 A720 (two full forward turns) +G1 A1080 (one more forward turn) +M26 (restore shortest path for the rest of the program) +G0 A0 (now resolves to the nearest equivalent angle) +---- + +For incremental multi-turn motion, prefer `G91` over `M27`; `M27` is intended +for the case where absolute G90 G-code must be honored literally. + +M26 and M27 are saved and restored by M70/M72/M73 alongside the other modal +M-codes. + +See also: `ROTARY_MODULO` in <] Section>>. + [[mcode:m48-m49]] == M48, M49 Speed and Feed Override Control diff --git a/src/emc/rs274ngc/interp_array.cc b/src/emc/rs274ngc/interp_array.cc index ba233a1d88b..372b5600e9d 100644 --- a/src/emc/rs274ngc/interp_array.cc +++ b/src/emc/rs274ngc/interp_array.cc @@ -137,6 +137,7 @@ group 5 = {m62,m63,m64,m65, - turn I/O point on/off group 6 = {m6,m61} - tool change group 7 = {m3,m4,m5,m19} - spindle turning, orient group 8 = {m7,m8,m9} - coolant +group 3 = {m26,m27} - rotary modulo absolute path: shortest / literal group 9 = {m48,m49, - feed and speed override switch bypass m50, - feed override switch bypass P1 to turn on, P0 to turn off m51, - spindle speed override switch bypass P1 to turn on, P0 to turn off @@ -148,7 +149,7 @@ group 10 = {m100..m199} - user-defined const int Interp::ems[] = { 4, 4, 4, 7, 7, 7, 6, 8, 8, 8, // 9 -1, -1, -1, -1, -1, -1, -1, -1, -1, 7, // 19 - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, // 29 + -1, -1, -1, -1, -1, -1, 3, 3, -1, -1, // 29 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, // 39 -1, -1, -1, -1, -1, -1, -1, -1, 9, 9, // 49 9, 9, 9, 9, -1, -1, -1, -1, -1, -1, // 59 diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 59171b7306e..c336a828880 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -3525,6 +3525,10 @@ int Interp::gen_m_codes(int *current, int *saved, std::string &cmd) MSG("------ gen_m_codes: index %d = -1!!\n",i); } break; + case 9: // rotary modulo path: -1 = default (M26), 27 = literal + // saved -1 with current 27 needs explicit M26 to restore + cmd += (val == 27) ? "M27\n" : "M26\n"; + break; } } } @@ -3774,7 +3778,7 @@ This handles four separate types of activity in order: 1. changing the tool (m6) - which also retracts and stops the spindle. 2. Turning the spindle on or off (m3, m4, and m5) 3. Turning coolant on and off (m7, m8, and m9) -4. turning a-axis clamping on and off (m26, m27) - commented out. +4. selecting rotary modulo absolute path (m26 = shortest, m27 = literal) 5. enabling or disabling feed and speed overrides (m49, m49). 6. changing the loaded toolnumber (m61). Within each group, only the first code encountered will be executed. @@ -4089,22 +4093,16 @@ int Interp::convert_m(block_pointer block, //!< pointer to a block of RS27 settings->flood = false; } -/* No axis clamps in this version - if (block->m_modes[2] == 26) - { -#ifdef DEBUG_EMC - COMMENT("interpreter: automatic A-axis clamping turned on"); -#endif - settings->a_axis_clamping = true; - } - else if (block->m_modes[2] == 27) - { -#ifdef DEBUG_EMC - COMMENT("interpreter: automatic A-axis clamping turned off"); -#endif - settings->a_axis_clamping = false; - } -*/ + /* M26 = rotary modulo absolute path takes shortest delta (default when + ROTARY_MODULO=1 is set in INI). M27 = take literal absolute target + so a programmed value over 180 deg from current produces a multi-turn + move. Modal group 3, persistent until reset by the other code. */ + if ((block->m_modes[3] == 26) && ONCE_M(3)) { + settings->rotary_modulo_literal = 0; + } else if ((block->m_modes[3] == 27) && ONCE_M(3)) { + settings->rotary_modulo_literal = 1; + } + if (is_user_defined_m_code(block, settings, 9) && ONCE_M(9)) { return convert_remapped_code(block, settings, STEP_M_9, 'm', block->m_modes[9]); @@ -5398,6 +5396,12 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 (_("Invalid spindle ($) number in G33 move"))); settings->active_spindle = (int)block->dollar_number; } + CHKS((block->a_flag && settings->a_rotary_modulo), + _("G33 incompatible with ROTARY_MODULO on axis A")); + CHKS((block->b_flag && settings->b_rotary_modulo), + _("G33 incompatible with ROTARY_MODULO on axis B")); + CHKS((block->c_flag && settings->c_rotary_modulo), + _("G33 incompatible with ROTARY_MODULO on axis C")); CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Spindle not turning in G33")); @@ -5413,6 +5417,12 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 (_("Invalid spindle ($) number in G33.1 move"))); settings->active_spindle = (int)block->dollar_number; } + CHKS((block->a_flag && settings->a_rotary_modulo), + _("G33.1 incompatible with ROTARY_MODULO on axis A")); + CHKS((block->b_flag && settings->b_rotary_modulo), + _("G33.1 incompatible with ROTARY_MODULO on axis B")); + CHKS((block->c_flag && settings->c_rotary_modulo), + _("G33.1 incompatible with ROTARY_MODULO on axis C")); CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Spindle not turning in G33.1")); @@ -5433,6 +5443,12 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 (_("Invalid D-number in G76 cycle"))); settings->active_spindle = (int)block->dollar_number; } + CHKS((block->a_flag && settings->a_rotary_modulo), + _("G76 incompatible with ROTARY_MODULO on axis A")); + CHKS((block->b_flag && settings->b_rotary_modulo), + _("G76 incompatible with ROTARY_MODULO on axis B")); + CHKS((block->c_flag && settings->c_rotary_modulo), + _("G76 incompatible with ROTARY_MODULO on axis C")); CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Chosen spindle (%i) not turning in G76"), settings->active_spindle); diff --git a/src/emc/rs274ngc/interp_find.cc b/src/emc/rs274ngc/interp_find.cc index 3eb92c6b06c..919e84a09d1 100644 --- a/src/emc/rs274ngc/interp_find.cc +++ b/src/emc/rs274ngc/interp_find.cc @@ -93,6 +93,17 @@ double Interp::find_arc_length(double x1, //!< X-coordinate of start poin of the axis are considered equivalent and we just need to find the nearest one. */ +// Modulo-rotary shortest-path target: returns current + delta where delta is +// (commanded - current) wrapped into (-180, 180]. Internal AA_current stays +// accumulated (matches motion-side traj.position) so stepgens see only small +// physical deltas; user-facing wrapping happens at #5423/canon read sites. +static double rotary_modulo_target(double commanded, double current) { + double delta = commanded - current; + double mod = fmod(delta + 180.0, 360.0); + if (mod < 0.0) mod += 360.0; + return current + mod - 180.0; +} + int Interp::unwrap_rotary(double *r, double sign_of, double commanded, double current, char axis) { double result; int neg = copysign(1.0, sign_of) < 0.0; @@ -201,9 +212,12 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(block->a_flag) { if(s->a_axis_wrapped) { - CHP(unwrap_rotary(AA_p, block->a_number, + CHP(unwrap_rotary(AA_p, block->a_number, block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a, s->AA_current, 'A')); + } else if (s->a_rotary_modulo && !s->rotary_modulo_literal) { + double cmd = block->a_number - s->AA_origin_offset - s->AA_axis_offset - s->tool_offset.a; + *AA_p = rotary_modulo_target(cmd, s->AA_current); } else { *AA_p = block->a_number - s->AA_origin_offset - s->AA_axis_offset; } @@ -213,9 +227,12 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(block->b_flag) { if(s->b_axis_wrapped) { - CHP(unwrap_rotary(BB_p, block->b_number, + CHP(unwrap_rotary(BB_p, block->b_number, block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b, s->BB_current, 'B')); + } else if (s->b_rotary_modulo && !s->rotary_modulo_literal) { + double cmd = block->b_number - s->BB_origin_offset - s->BB_axis_offset - s->tool_offset.b; + *BB_p = rotary_modulo_target(cmd, s->BB_current); } else { *BB_p = block->b_number - s->BB_origin_offset - s->BB_axis_offset; } @@ -225,9 +242,12 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(block->c_flag) { if(s->c_axis_wrapped) { - CHP(unwrap_rotary(CC_p, block->c_number, + CHP(unwrap_rotary(CC_p, block->c_number, block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c, s->CC_current, 'C')); + } else if (s->c_rotary_modulo && !s->rotary_modulo_literal) { + double cmd = block->c_number - s->CC_origin_offset - s->CC_axis_offset - s->tool_offset.c; + *CC_p = rotary_modulo_target(cmd, s->CC_current); } else { *CC_p = block->c_number - s->CC_origin_offset - s->CC_axis_offset; } @@ -297,6 +317,8 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(block->a_flag) { if(s->a_axis_wrapped) { CHP(unwrap_rotary(AA_p, block->a_number, block->a_number, s->AA_current, 'A')); + } else if (s->a_rotary_modulo && !s->rotary_modulo_literal) { + *AA_p = rotary_modulo_target(block->a_number, s->AA_current); } else { *AA_p = block->a_number; } @@ -307,6 +329,8 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(block->b_flag) { if(s->b_axis_wrapped) { CHP(unwrap_rotary(BB_p, block->b_number, block->b_number, s->BB_current, 'B')); + } else if (s->b_rotary_modulo && !s->rotary_modulo_literal) { + *BB_p = rotary_modulo_target(block->b_number, s->BB_current); } else { *BB_p = block->b_number; } @@ -317,6 +341,8 @@ int Interp::find_ends(block_pointer block, //!< pointer to a block of RS27 if(block->c_flag) { if(s->c_axis_wrapped) { CHP(unwrap_rotary(CC_p, block->c_number, block->c_number, s->CC_current, 'C')); + } else if (s->c_rotary_modulo && !s->rotary_modulo_literal) { + *CC_p = rotary_modulo_target(block->c_number, s->CC_current); } else { *CC_p = block->c_number; } @@ -436,6 +462,9 @@ int Interp::find_relative(double x1, //!< absolute x position CHP(unwrap_rotary(AA_2, AA_1, AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a, settings->AA_current, 'A')); + } else if (settings->a_rotary_modulo && !settings->rotary_modulo_literal) { + double cmd = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset - settings->tool_offset.a; + *AA_2 = rotary_modulo_target(cmd, settings->AA_current); } else { *AA_2 = AA_1 - settings->AA_origin_offset - settings->AA_axis_offset; } @@ -444,6 +473,9 @@ int Interp::find_relative(double x1, //!< absolute x position CHP(unwrap_rotary(BB_2, BB_1, BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b, settings->BB_current, 'B')); + } else if (settings->b_rotary_modulo && !settings->rotary_modulo_literal) { + double cmd = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset - settings->tool_offset.b; + *BB_2 = rotary_modulo_target(cmd, settings->BB_current); } else { *BB_2 = BB_1 - settings->BB_origin_offset - settings->BB_axis_offset; } @@ -452,6 +484,9 @@ int Interp::find_relative(double x1, //!< absolute x position CHP(unwrap_rotary(CC_2, CC_1, CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c, settings->CC_current, 'C')); + } else if (settings->c_rotary_modulo && !settings->rotary_modulo_literal) { + double cmd = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset - settings->tool_offset.c; + *CC_2 = rotary_modulo_target(cmd, settings->CC_current); } else { *CC_2 = CC_1 - settings->CC_origin_offset - settings->CC_axis_offset; } diff --git a/src/emc/rs274ngc/interp_internal.cc b/src/emc/rs274ngc/interp_internal.cc index 5d4156d983c..73f1b86bfce 100644 --- a/src/emc/rs274ngc/interp_internal.cc +++ b/src/emc/rs274ngc/interp_internal.cc @@ -448,23 +448,20 @@ int Interp::set_probe_data(setup_pointer settings) //!< pointer to machine settings->parameters[5063] = GET_EXTERNAL_PROBE_POSITION_Z(); a = GET_EXTERNAL_PROBE_POSITION_A(); - if(settings->a_axis_wrapped) { - a = fmod(a, 360.0); - if(a<0) a += 360.0; + if(settings->a_axis_wrapped || settings->a_rotary_modulo) { + a = wrap_rotary_to_360(a); } settings->parameters[5064] = a; b = GET_EXTERNAL_PROBE_POSITION_B(); - if(settings->b_axis_wrapped) { - b = fmod(b, 360.0); - if(b<0) b += 360.0; + if(settings->b_axis_wrapped || settings->b_rotary_modulo) { + b = wrap_rotary_to_360(b); } settings->parameters[5065] = b; c = GET_EXTERNAL_PROBE_POSITION_C(); - if(settings->c_axis_wrapped) { - c = fmod(c, 360.0); - if(c<0) c += 360.0; + if(settings->c_axis_wrapped || settings->c_rotary_modulo) { + c = wrap_rotary_to_360(c); } settings->parameters[5066] = c; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index bbcf9e926e7..988b2d66393 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -46,6 +46,12 @@ T D2R(T r) { return r * (M_PI / 180.); } template T SQ(T a) { return a*a; } +inline double wrap_rotary_to_360(double v) { + double m = fmod(v, 360.0); + if (m < 0.0) m += 360.0; + return m; +} + template inline int round_to_int(T x) { return (int)std::nearbyint(x); @@ -340,6 +346,7 @@ enum phases { STEP_SET_FEED_RATE, STEP_SET_SPINDLE_SPEED, STEP_PREPARE, + STEP_M_3, STEP_M_5, STEP_M_6, STEP_RETAIN_G43, @@ -801,6 +808,10 @@ struct setup int a_axis_wrapped; int b_axis_wrapped; int c_axis_wrapped; + int a_rotary_modulo; + int b_rotary_modulo; + int c_rotary_modulo; + int rotary_modulo_literal; // M26 = shortest path (default), M27 = literal absolute int a_indexer_jnum; int b_indexer_jnum; diff --git a/src/emc/rs274ngc/interp_namedparams.cc b/src/emc/rs274ngc/interp_namedparams.cc index 9fcc602a9c4..314753cf20b 100644 --- a/src/emc/rs274ngc/interp_namedparams.cc +++ b/src/emc/rs274ngc/interp_namedparams.cc @@ -703,15 +703,18 @@ int Interp::lookup_named_param(const char *nameBuf, break; case NP_A: // current position - *value = _setup.AA_current; + *value = _setup.a_rotary_modulo + ? wrap_rotary_to_360(_setup.AA_current) : _setup.AA_current; break; case NP_B: // current position - *value = _setup.BB_current; + *value = _setup.b_rotary_modulo + ? wrap_rotary_to_360(_setup.BB_current) : _setup.BB_current; break; case NP_C: // current position - *value = _setup.CC_current; + *value = _setup.c_rotary_modulo + ? wrap_rotary_to_360(_setup.CC_current) : _setup.CC_current; break; case NP_U: // current position @@ -751,18 +754,27 @@ int Interp::lookup_named_param(const char *nameBuf, break; case NP_ABS_A: // abs position - *value = _setup.AA_current + _setup.AA_axis_offset + - _setup.AA_origin_offset + _setup.tool_offset.a; + { + double v = _setup.AA_current + _setup.AA_axis_offset + + _setup.AA_origin_offset + _setup.tool_offset.a; + *value = _setup.a_rotary_modulo ? wrap_rotary_to_360(v) : v; + } break; case NP_ABS_B: // abs position - *value = _setup.BB_current + _setup.BB_axis_offset + - _setup.BB_origin_offset + _setup.tool_offset.b; + { + double v = _setup.BB_current + _setup.BB_axis_offset + + _setup.BB_origin_offset + _setup.tool_offset.b; + *value = _setup.b_rotary_modulo ? wrap_rotary_to_360(v) : v; + } break; case NP_ABS_C: // abs position - *value = _setup.CC_current + _setup.CC_axis_offset + - _setup.CC_origin_offset + _setup.tool_offset.c; + { + double v = _setup.CC_current + _setup.CC_axis_offset + + _setup.CC_origin_offset + _setup.tool_offset.c; + *value = _setup.c_rotary_modulo ? wrap_rotary_to_360(v) : v; + } break; // o-word subs may optionally have an diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 4bdc9ea5cc7..0369d9ba946 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -176,6 +176,10 @@ setup::setup() : a_axis_wrapped(0), b_axis_wrapped(0), c_axis_wrapped(0), + a_rotary_modulo(0), + b_rotary_modulo(0), + c_rotary_modulo(0), + rotary_modulo_literal(0), a_indexer_jnum(0), b_indexer_jnum(0), diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index 16b93c78ed0..4febde744e2 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -184,6 +184,9 @@ int Interp::write_m_codes(block_pointer block, //!< pointer to a block of RS27 settings->active_m_codes[8] = /* 8 overrides */ (settings->feed_hold) ? 53 : -1; + settings->active_m_codes[9] = /* 9 rotary modulo path */ + (settings->rotary_modulo_literal) ? 27 : -1; + return INTERP_OK; } diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index e2f798f161f..88fcd3c19fd 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -859,6 +859,10 @@ int Interp::init() _setup.a_axis_wrapped = 0; _setup.b_axis_wrapped = 0; _setup.c_axis_wrapped = 0; + _setup.a_rotary_modulo = 0; + _setup.b_rotary_modulo = 0; + _setup.c_rotary_modulo = 0; + _setup.rotary_modulo_literal = 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 +892,35 @@ 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_rotary_modulo = inifile.findBoolV("ROTARY_MODULO", "AXIS_A", false); + _setup.b_rotary_modulo = inifile.findBoolV("ROTARY_MODULO", "AXIS_B", false); + _setup.c_rotary_modulo = inifile.findBoolV("ROTARY_MODULO", "AXIS_C", false); + { + struct { const char *name; int *wrapped; int *modulo; } axes[] = { + {"AXIS_A", &_setup.a_axis_wrapped, &_setup.a_rotary_modulo}, + {"AXIS_B", &_setup.b_axis_wrapped, &_setup.b_rotary_modulo}, + {"AXIS_C", &_setup.c_axis_wrapped, &_setup.c_rotary_modulo}, + }; + for (auto &a : axes) { + if (*a.wrapped && *a.modulo) { + fprintf(stderr, + "%s: WRAPPED_ROTARY and ROTARY_MODULO are mutually exclusive; " + "ROTARY_MODULO disabled\n", a.name); + *a.modulo = 0; + } + if (*a.modulo) { + double lo = inifile.findRealV("MIN_LIMIT", a.name, 0.0); + double hi = inifile.findRealV("MAX_LIMIT", a.name, 0.0); + double range = hi - lo; + if (range > 0.0 && fabs(range - 360.0) > 0.01) { + fprintf(stderr, + "%s: ROTARY_MODULO=1 but MIN/MAX_LIMIT range is %.2f deg " + "(expected ~360); typical for limited-range rotaries (tilt) " + "where modulo is unusual\n", a.name, range); + } + } + } + } _setup.random_toolchanger = inifile.findBoolV("RANDOM_TOOLCHANGER", "EMCIO", false); _setup.num_spindles = inifile.findIntV("SPINDLES", "TRAJ", 1); @@ -1612,9 +1645,14 @@ 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; + // ROTARY_MODULO axes: present #5423-#5425 wrapped to [0,360); internal + // AA/BB/CC_current stay accumulated to keep sync with motion.traj.position. + _setup.parameters[5423] = _setup.a_rotary_modulo + ? wrap_rotary_to_360(_setup.AA_current) : _setup.AA_current; + _setup.parameters[5424] = _setup.b_rotary_modulo + ? wrap_rotary_to_360(_setup.BB_current) : _setup.BB_current; + _setup.parameters[5425] = _setup.c_rotary_modulo + ? wrap_rotary_to_360(_setup.CC_current) : _setup.CC_current; _setup.parameters[5426] = _setup.u_current; _setup.parameters[5427] = _setup.v_current; _setup.parameters[5428] = _setup.w_current;