From 91d22c53edcc223a0138242f67e2cce1a30381a4 Mon Sep 17 00:00:00 2001 From: david mueller Date: Sat, 18 Jul 2026 20:47:27 +0100 Subject: [PATCH] orient.comp: Add modes with preceding referencing to index signal --- src/emc/rs274ngc/interp_check.cc | 4 +- src/hal/components/orient.comp | 84 +++++++++++++++++++++++++++----- 2 files changed, 74 insertions(+), 14 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 230f66fd6b4..a3284cb2090 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 bit enable "enable angular output for orientation mode"; -pin in s32 mode "0: rotate - shortest move; 1: always rotate clockwise; 2: always rotate counterclockwise"; +pin in s32 mode "sets rotation direction"; pin in float position "spindle position input, unit 1 rev"; pin in float angle "orient target position in degrees, 0 ≤ angle < 360"; pin out float command "target spindle position, input to PID command"; pin out float poserr "in degrees - aid for PID tuning"; pin out bit is-oriented "This pin goes high when poserr < tolerance. Use to drive spindle.N.is-oriented"; pin in float tolerance = 0.5 "The tolerance in degrees for considering the align completed"; +pin io bit index-enable "Connect to the spindle encoder counter to reset to index before orienting"; + +param r unsigned 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 double target_angle = 0; +variable double latched_position = 0; option fp yes; option period no; @@ -37,6 +42,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 @@ -63,16 +71,35 @@ license "GPL"; FUNCTION(_) { - double target_angle; - double latched_position; is_oriented = 0; // spindle.is-oriented inhibits spindle.orient - if (enable) { - if (enable ^ last_enable) { // positive edge on enable - is_oriented = 0; + + switch (state){ + case 0: // waiting + if (enable) { + index_enable = 0; + if (enable ^ last_enable) { // positive edge on enable + is_oriented = 0; + if (mode > 2){ + index_enable = 1; + state = 3; + } else { + state = 1; + } + } + } + break; + + case 1: // normal orient - init + if (! enable) state = 0; debounce = 0; latched_position = position; // sample now target_angle = angle/360.0; - switch (mode) { + state = 2; + break; + + case 2: // orienting + if (! enable) state = 0; + switch (mode % 3) { case 0: // shortest move command = floor(latched_position+0.5-target_angle) + target_angle; break; @@ -82,12 +109,45 @@ FUNCTION(_) { case 2: // always ccw command = floor(latched_position-target_angle) + target_angle ; break; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode % 3); + } + poserr = (position - command) * 360.0; + debounce += (fabs(poserr) < tolerance && debounce <=100); + is_oriented = (debounce > 100); + if (is_oriented) state = 0; + break; + + case 3: // index search - init + if (! enable) state = 0; + latched_position = position; // sample now + state = 4; + break; + + case 4: // waiting for spindle index + if (! enable) state = 0; + if (! index_enable){ // index has reset) + state = 1; + break; + } + switch (mode) { + case 3: // shortest move to index + if (0 > latched_position) { + command = position + 0.5; + } else { + command = position - 0.5; + } + break; + case 4: // always cw to index + command = position + 0.5; + break; + case 5: // always ccw to index + command = position - 0.5; + break; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode); } - } - poserr = (position - command) * 360.0; - debounce += (fabs(poserr) < tolerance && debounce <=100); - is_oriented = (debounce > 100); + break; } last_enable = enable; } -