From cc32ccf0177ab7ffc6e75a49c00e24d20748852d Mon Sep 17 00:00:00 2001 From: david mueller Date: Wed, 5 Aug 2026 13:28:23 +0100 Subject: [PATCH] Comp: Add modes with preceding homing to index to orient.comp orient.comp currently does not support using an index signal. This makes it unusable for setups where the spindle encoder position cannot be tracked due to the encoder reading being too slow. --- docs/src/gcode/m-code.adoc | 9 ++- src/emc/rs274ngc/interp_check.cc | 4 +- src/hal/components/orient.comp | 108 ++++++++++++++++++++++++------- 3 files changed, 93 insertions(+), 28 deletions(-) diff --git a/docs/src/gcode/m-code.adoc b/docs/src/gcode/m-code.adoc index 253358f25f1..f997fd43369 100644 --- a/docs/src/gcode/m-code.adoc +++ b/docs/src/gcode/m-code.adoc @@ -229,14 +229,19 @@ M19 R- Q- [P-] [$-] ** '0' rotate for smallest angular movement (default) ** '1' always rotate clockwise (same as M3 direction) ** '2' always rotate counterclockwise (same as M4 direction) -* '$' The spindle to orient (actually only determines which HAL pins + ** '3' same as '0' but with preceding rotation to an index signal + ** '4' same as '1' but with preceding rotation to an index signal + ** '5' same as '2' but with preceding rotation to an index signal + * '$' The spindle to orient (actually only determines which HAL pins carry the spindle position commands) M19 is a command of modal group 7, like M3, M4 and M5. M19 is cleared by any of M3,M4,M5. -Spindle orientation requires a quadrature encoder with an index to sense the +Spindle orientation with modes 0,1,2 requires a quadrature encoder to sense the spindle shaft position and direction of rotation. +Spindle orientation with modes 3,4,5 requires an additional index signal for +referencing the spindle before orientation. INI Settings in the [RS274NGC] section: diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index b5c9749c545..72a6a96db09 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -338,8 +338,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block CHKS(((motion == G_2 || motion == G_3 || (block->m_modes[7] == 19)) && fabs(p_value - block->p_number) > 0.001), _("P value not an integer with M19 G2 or G3")); - CHKS((block->m_modes[7] == 19) && ((p_value > 2) || p_value < 0), - _("P value must be 0,1,or 2 with M19")); + CHKS((block->m_modes[7] == 19) && ((p_value > 5) || p_value < 0), + _("P value must be 0,1,2,3,4 or 5 with M19")); CHKS(((motion == G_2 || motion == G_3) && round_to_int(block->p_number) < 1), _("P value should be 1 or greater with G2 or G3")); } diff --git a/src/hal/components/orient.comp b/src/hal/components/orient.comp index fdff8714f48..c26ab8f898c 100644 --- a/src/hal/components/orient.comp +++ b/src/hal/components/orient.comp @@ -1,16 +1,23 @@ component orient "Provide a PID command input for orientation mode based on current spindle position, target angle and orient mode"; pin in bool enable "enable angular output for orientation mode"; -pin in si32 mode "0: rotate - shortest move; 1: always rotate clockwise; 2: always rotate counterclockwise"; +pin in si32 mode "sets rotation direction and optional preceding reset to index signal"; pin in real position "spindle position input, unit 1 rev"; pin in real angle "orient target position in degrees, 0 ≤ angle < 360"; pin out real command "target spindle position, input to PID command"; pin out real poserr "in degrees - aid for PID tuning"; pin out bool is-oriented "This pin goes high when poserr < tolerance. Use to drive spindle.N.is-oriented"; -pin in real tolerance = 0.5 "The tolerance in degrees for considering the align completed"; +pin in real tolerance = 0.5 "The tolerance in degrees for considering the align completed"; +pin io bool index-enable "Connect to the spindle encoder counter to reset to index before orienting"; + +param r ui32 state "state machine for index homing"; + +variable int last_enable = 0; +variable int debounce = 0; // to prevent the in-position triggering with the spindle moving +variable int m = 0; +variable rtapi_real target_angle = 0; +variable rtapi_real latched_position = 0; -variable int last_enable = 0; -variable int debounce = 0; // to prevent the in-position triggering with the spindle moving option period no; @@ -36,6 +43,9 @@ The *mode* pin is interpreted as follows: which may be clockwise or counterclockwise. * 1: the spindle rotates always rotates clockwise to the new angle. * 2: the spindle rotates always rotates counterclockwise to the new angle. +* 3: same as mode 0 but with homing to index before orienting to the new angle. +* 4: same as mode 1 but with homing to index before orienting to the new angle. +* 5: same as mode 2 but with homing to index before orienting to the new angle. === HAL USAGE @@ -62,31 +72,81 @@ license "GPL"; FUNCTION(_) { - rtapi_real target_angle; - rtapi_real latched_position; + rtapi_bool en = enable; + + if (!en) { + if (state == 4) index_enable_set(0); + state_set(0); + } + is_oriented_set(0); // spindle.is-oriented inhibits spindle.orient - if (enable) { - if (enable ^ last_enable) { // positive edge on enable - is_oriented_set(0); + + switch (state){ + case 0: // waiting + if (en && !last_enable) { // positive edge on enable + m = mode; + if ((0 > m) || (5 < m)) { + rtapi_print_msg(RTAPI_MSG_ERR, "orient.comp: Unhandled mode %i\n", m); + } + if (m < 3){ + state_set(1); + } else { + state_set(3); + } + } + break; + + case 1: // normal orient - init debounce = 0; latched_position = position; // sample now target_angle = angle/360.0; - switch (mode) { - case 0: // shortest move - command_set(floor(latched_position+0.5-target_angle) + target_angle); - break; - case 1: // always cw - command_set(ceil(latched_position-target_angle) + target_angle); - break; - case 2: // always ccw - command_set(floor(latched_position-target_angle) + target_angle ); + state_set(2); + break; + + case 2: // orienting + switch (m % 3) { + case 0: // shortest move + command_set(floor(latched_position+0.5-target_angle) + target_angle); + break; + case 1: // always cw + command_set(ceil(latched_position-target_angle) + target_angle); + break; + case 2: // always ccw + command_set(floor(latched_position-target_angle) + target_angle); + break; + } + poserr_set((position - command) * 360.0); + debounce += (fabs(poserr) < tolerance && debounce <=100); + is_oriented_set(debounce > 100); + break; + + case 3: // index search - init + latched_position = position; // sample now + index_enable_set(1); + state_set(4); + break; + + case 4: // waiting for spindle index + if (! index_enable){ // index has reset + state_set(1); break; } - } - poserr_set((position - command) * 360.0); - debounce += (fabs(poserr) < tolerance && debounce <=100); - is_oriented_set((debounce > 100)); + switch (m) { + case 3: // shortest move to index + if (0.5 <= (latched_position - floor(latched_position))) { + command_set(position + 0.5); + } else { + command_set(position - 0.5); + } + break; + case 4: // always cw to index + command_set(position + 0.5); + break; + case 5: // always ccw to index + command_set(position - 0.5); + break; + } + break; } - last_enable = enable; + last_enable = en; } -