From ddadd47dac3219f3c4b98f0dc28a7dc7d0784c1d 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. --- src/emc/rs274ngc/interp_check.cc | 4 +- src/hal/components/orient.comp | 100 ++++++++++++++++++++++++------- 2 files changed, 80 insertions(+), 24 deletions(-) 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..cf6cbd4e9fe 100644 --- a/src/hal/components/orient.comp +++ b/src/hal/components/orient.comp @@ -1,16 +1,21 @@ 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"; 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 rtapi_real target_angle = 0; +variable rtapi_real latched_position = 0; option period no; @@ -36,6 +41,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 +70,79 @@ license "GPL"; FUNCTION(_) { - rtapi_real target_angle; - rtapi_real latched_position; + rtapi_bool en = enable; + + if (!en) 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 + if (mode > 2){ + index_enable_set(1); + state_set(3); + } else { + state_set(1); + } + } + 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 (mode % 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; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode % 3); + } + poserr_set((position - command) * 360.0); + debounce += (fabs(poserr) < tolerance && debounce <=100); + if (is_oriented_set(debounce > 100)) { + state_set(0); + } + break; + + case 3: // index search - init + latched_position = position; // sample now + 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 (mode) { + case 3: // shortest move to index + if (0 > 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; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode); + } + break; } - last_enable = enable; + last_enable = en; } -