diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index efb78a7d054..54c0eb25110 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -263,6 +263,52 @@ Note: feed-inhibit applies to G-code commands -- not jogs. *motion.tp-reverse* OUT BIT:: Trajectory planning is reversed (reverse run) +=== Interpreter Metadata Pins + +These pins provide geometric intent and interpreter state for the segment of motion currently being executed. Unlike standard position feedback, these values are synchronized with the trajectory planner's execution point. + +==== Interpreter Status Pins + +* `motion.interp.line-number` (s32, out) + + The current G-code line number being executed. + +* `motion.interp.motion-type` (s32, out) + + The type of motion currently in progress: +** 0: None +** 1: Rapid (G0) +** 2: Feed (G1) +** 3: Arc (G2, G3) +** 4: Tool Change/Other + +* `motion.interp.feedrate` (float, out) + + The interpreted feedrate for the current segment in units per minute. + +==== Interpreter Geometric Pins + +* `motion.interp.heading` (float, out) + + The XY plane heading of the current linear move in degrees. Measured counter-clockwise from the +X axis (0 to 360). This could be used to control a tangential knife. + +* `motion.interp.arc-radius` (float, out) + + The radius of the current circular move. This value is 0.0 during linear moves. This could be used to modify plasma cutting parameters based on the arc radius and when hole cutting. + +* `motion.interp.arc-center-x` (float, out) + + The absolute X-coordinate of the center point for the current arc. 0 if in YZ plane + +* `motion.interp.arc-center-y` (float, out) + + The absolute Y-coordinate of the center point for the current arc. 0 if in XZ plane + +* `motion.interp.arc-center-z` (float, out) + + The absolute Y-coordinate of the center point for the current arc if in XZ or YZ plane. + +* `motion.interp.normal-heading` (float, out) + + the heading from the current point back to the arc centre for the current arc. + + * `motion.interp.iscircle` (bit, out) + + True if the current arc is a full circle and not a helix. + +[NOTE] +These pins represent the *commanded geometric intent* from the interpreter and are updated in real-time as each motion segment is consumed by the trajectory planner. + == AXIS PINS (*L* is the axis letter, one of: *x y z a b c u v w*) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index c806deccf4f..11308c701a4 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -530,6 +530,15 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) SET_JOINT_FAULT_FLAG(joint, 0); } emcmotStatus->paused = 0; + // Clear pins on abort so tests see a clean state + if (emcmot_hal_data) { + *(emcmot_hal_data->interp_arc_radius) = 0.0; + *(emcmot_hal_data->interp_arc_center_x) = 0.0; + *(emcmot_hal_data->interp_arc_center_y) = 0.0; + *(emcmot_hal_data->interp_straight_heading) = 0.0; + *(emcmot_hal_data->interp_normal_heading) = 0.0; + *(emcmot_hal_data->iscircle) = 0.0; + } break; case EMCMOT_JOG_ABORT: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index fd42ac027fe..72976a573f9 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -26,7 +26,6 @@ #include #include //for kinematicsSwitchable() #include - #include "../tp/tp.h" #include "simple_tp.h" #include "motion.h" @@ -34,6 +33,8 @@ #include "config.h" #include "homing.h" #include "axis.h" +#include "state_tag.h" + // Mark strings for translation, but defer translation to userspace #define _(s) (s) @@ -1888,6 +1889,20 @@ static void output_to_hal(void) *(emcmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG(); *(emcmot_hal_data->on_soft_limit) = emcmotStatus->on_soft_limit; + /* Update the HAL Output Pins from the active tag */ + /* Geometric Metadata */ + *(emcmot_hal_data->interp_arc_radius) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_RADIUS]; + *(emcmot_hal_data->interp_arc_center_x) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X]; + *(emcmot_hal_data->interp_arc_center_y) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y]; + *(emcmot_hal_data->interp_straight_heading) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_STRAIGHT_HEADING]; + + /* Performance Metadata */ + *(emcmot_hal_data->interp_feedrate) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_FEED]; + + /* Line and Motion Type (Casting to int for s32 HAL pins) */ + *(emcmot_hal_data->interp_line_number) = (int)emcmotStatus->tag.fields[GM_FIELD_LINE_NUMBER]; + *(emcmot_hal_data->interp_motion_type) = (int)emcmotStatus->tag.fields[GM_FIELD_MOTION_MODE]; + *(emcmot_hal_data->iscircle) = (hal_bit_t)((emcmotStatus->tag.packed_flags & (1UL << GM_FLAG_IS_CIRCLE)) != 0); switch (emcmotStatus->motionType) { case EMC_MOTION_TYPE_FEED: //fall thru case EMC_MOTION_TYPE_ARC: @@ -2033,10 +2048,9 @@ static void output_to_hal(void) /* point to joint struct */ joint = &joints[joint_num]; joint_data = &(emcmot_hal_data->joint[joint_num]); - + /* apply backlash and motor offset to output */ - joint->motor_pos_cmd = - joint->pos_cmd + joint->backlash_filt + joint->motor_offset; + joint->motor_pos_cmd = joint->pos_cmd + joint->backlash_filt + joint->motor_offset; /* point to HAL data */ /* write to HAL pins */ *(joint_data->motor_offset) = joint->motor_offset; @@ -2131,6 +2145,9 @@ static void update_status(void) // and the state machine is still active. The homing status deassertion // must be delayed until the state machine is done. joint_status->homing = get_homing(joint_num); + /* check to see if we should pause in order to implement + single emcmotStatus->stepping */ + } joint_status->homed = get_homed(joint_num); joint_status->pos_cmd = joint->pos_cmd; @@ -2211,6 +2228,90 @@ static void update_status(void) emcmotStatus->stepping = 0; emcmotStatus->paused = 1; } + // State Tags handling + // Get the current executing trajectory component (the "Source of Truth") + /* Update the HAL Output Pins from the active tag */ + if (emcmot_hal_data) { + // Line and Motion Type + if (emcmot_hal_data->interp_line_number) { + *(emcmot_hal_data->interp_line_number) = (int)emcmotStatus->tag.fields[GM_FIELD_LINE_NUMBER]; + } + + // Performance Metadata + if (emcmot_hal_data->interp_feedrate) { + *(emcmot_hal_data->interp_feedrate) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_FEED]; + } + + // Geometric Metadata + + if (emcmot_hal_data->interp_arc_radius) { + *(emcmot_hal_data->interp_arc_radius) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_RADIUS]; + } + if (emcmot_hal_data->interp_arc_center_x) { + *(emcmot_hal_data->interp_arc_center_x) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X]; + } + if (emcmot_hal_data->interp_arc_center_y) { + *(emcmot_hal_data->interp_arc_center_y) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y]; + } + if (emcmot_hal_data->interp_arc_center_z) { + *(emcmot_hal_data->interp_arc_center_z) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Z]; + } + // Get the current motion type from the tag (1=G1, 2=G2, 3=G3) + int motion_type = (int)emcmotStatus->tag.fields[GM_FIELD_MOTION_MODE]; + if (motion_type == 10 || motion_type == 0) { + /* --- G1/G0 STATIC HEADING --- */ + // For linear moves, the heading doesn't change during the segment. + if (emcmot_hal_data->interp_straight_heading) { + *(emcmot_hal_data->interp_straight_heading) = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_STRAIGHT_HEADING]; + } + } + else if (motion_type == 20 || motion_type == 30) { + /* --- G2/G3: DYNAMIC ARC HEADING --- */ + + // 1. Get Static Center from Tag + double cx = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X]; + double cy = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y]; + double cz = emcmotStatus->tag.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Z]; + + // 2. Get Real-Time Feedback Deltas + double dx = emcmotStatus->carte_pos_fb.tran.x - cx; + double dy = emcmotStatus->carte_pos_fb.tran.y - cy; + double dz = emcmotStatus->carte_pos_fb.tran.z - cz; + + // 3. Determine Plane and Radial Angle + int plane = emcmotStatus->tag.fields[GM_FIELD_PLANE]; + double angle_rad = 0.0; // Initialize to prevent "uninitialized" error + + if (plane == 170) { // XY: X is Horizontal, Y is Vertical + angle_rad = atan2(dy, dx); + } + else if (plane == 180) { // XZ: Z is Horizontal, X is Vertical + angle_rad = atan2(dx, dz); + } + else if (plane == 190) { // YZ: Y is Horizontal, Z is Vertical + angle_rad = atan2(dz, dy); + } + // Optional: add an else here for a default plane if 170/180/190 aren't found + + // 4. Calculate Normal Heading (Tool-to-Center) + double normal_deg = (angle_rad * (180.0 / M_PI)) + 180.0; + while (normal_deg < 0) normal_deg += 360.0; + while (normal_deg >= 360.0) normal_deg -= 360.0; + *(emcmot_hal_data->interp_normal_heading) = normal_deg; + + // 5. Calculate Tangent Heading (Direction of Travel) + double tangent_rad = (motion_type == 30) ? (angle_rad + (M_PI / 2.0)) : (angle_rad - (M_PI / 2.0)); + double heading_deg = tangent_rad * (180.0 / M_PI); + + // 6. Final Normalization and Assignment + while (heading_deg < 0) heading_deg += 360.0; + while (heading_deg >= 360.0) heading_deg -= 360.0; + + if (emcmot_hal_data->interp_straight_heading) { + *(emcmot_hal_data->interp_straight_heading) = heading_deg; + } + } + } #ifdef WATCH_FLAGS /*! \todo FIXME - this is for debugging */ if ( old_motion_flag != emcmotStatus->motionFlag ) { diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index 4ecc10bf87f..18b48134dd6 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -199,6 +199,19 @@ typedef struct { hal_float_t *feed_mm_per_second; /* feed mm per second*/ hal_float_t *switchkins_type; + /* Interp State Pins */ + hal_s32_t *interp_line_number; + hal_s32_t *interp_motion_type; + hal_float_t *interp_feedrate; + + /* New Geometric Metadata Pins */ + hal_float_t *interp_arc_radius; + hal_float_t *interp_arc_center_x; + hal_float_t *interp_arc_center_y; + hal_float_t *interp_arc_center_z; + hal_float_t *interp_straight_heading; + hal_float_t *interp_normal_heading; + hal_bit_t *iscircle; } emcmot_hal_data_t; /*********************************************************************** diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 196007388c3..4933a38c745 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -551,6 +551,22 @@ static int init_hal_io(void) CALL_CHECK(hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id, "motion.program-line")); CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->jog_is_active), mot_comp_id, "motion.jog-is-active")); + /* Standard Interp State Pins */ + CALL_CHECK(hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->interp_line_number), mot_comp_id, "motion.interp.line-number")); + CALL_CHECK(hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->interp_motion_type), mot_comp_id, "motion.interp.motion-type")); + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_feedrate), mot_comp_id, "motion.interp.feedrate")); + + /* New Geometric Metadata Pins */ + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_radius), mot_comp_id, "motion.interp.arc-radius")); + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_center_x), mot_comp_id, "motion.interp.arc-center-x")); + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_center_y), mot_comp_id, "motion.interp.arc-center-y")); + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_arc_center_z), mot_comp_id, "motion.interp.arc-center-z")); + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_straight_heading), mot_comp_id, "motion.interp.heading")); + CALL_CHECK(hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->interp_normal_heading), mot_comp_id, "motion.interp.normal-heading")); + CALL_CHECK(hal_pin_bit_newf(HAL_OUT, &emcmot_hal_data->iscircle, mot_comp_id, "motion.interp.iscircle")); + + + /* export debug parameters */ /* these can be used to view any internal variable, simply change a line in control.c:output_to_hal() and recompile */ diff --git a/src/emc/motion/state_tag.h b/src/emc/motion/state_tag.h index ff0dcfe7313..4d61b3c87a9 100644 --- a/src/emc/motion/state_tag.h +++ b/src/emc/motion/state_tag.h @@ -57,6 +57,7 @@ typedef enum { GM_FLAG_IN_REMAP, GM_FLAG_IN_SUB, GM_FLAG_EXTERNAL_FILE, + GM_FLAG_IS_CIRCLE, GM_FLAG_MAX_FLAGS } StateFlag; @@ -67,7 +68,7 @@ typedef enum { * WARNING: * * 1) Since these are used as array indices, they have to start at 0, - * be monotonic, and the MAX_FIELDS enum MUST be last in the list. + * be monotonic, and the GM_FIELD_MAX_FIELDS enum MUST be last in the list. * * 2) If your application needs to pass state tags through NML, then * you MUST update the corresponding cms->update function for state @@ -98,6 +99,12 @@ typedef enum { GM_FIELD_FLOAT_SPEED, GM_FIELD_FLOAT_PATH_TOLERANCE, GM_FIELD_FLOAT_NAIVE_CAM_TOLERANCE, + GM_FIELD_FLOAT_ARC_RADIUS, + GM_FIELD_FLOAT_ARC_CENTER_X, + GM_FIELD_FLOAT_ARC_CENTER_Y, + GM_FIELD_FLOAT_ARC_CENTER_Z, + GM_FIELD_FLOAT_STRAIGHT_HEADING, + GM_FIELD_FLOAT_NORMAL_HEADING, GM_FIELD_FLOAT_MAX_FIELDS } StateFieldFloat; diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 59171b7306e..708f100495e 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -1148,6 +1148,38 @@ int Interp::convert_arc2(int move, //!< either G_2 (cw arc) or G_3 (ccw ar inverse_time_rate_arc(*current1, *current2, *current3, center1, center2, turn, end1, end2, end3, block, settings); + // We need to determine which 'center' is X and which is Y + double abs_x = 0, abs_y = 0, abs_z =0; + double abs_cx = 0, abs_cy = 0,abs_cz=0; + + if (settings->plane == CANON_PLANE::XY) { + // Plane 1=X, 2=Y, 3=Z + abs_x = end1; + abs_y = end2; + abs_z = end3; + abs_cx = center1; + abs_cy = center2; + abs_cz = *current3; + } else if (settings->plane == CANON_PLANE::XZ) { + // Plane 1=X, 2=Z, 3=Y + abs_x = end1; + abs_y = end3; // Or whatever your system expects for non-active axes + abs_z = end2; + abs_cx = center1; + abs_cy = *current3; + abs_cz = center2; + } else { // YZ plane + // Plane 1=Y, 2=Z, 3=X + abs_x = end3; + abs_y = end1; + abs_z = end2; + abs_cx = *current3; + abs_cy = center1; + abs_cz = center2; + } + // Call tagger with the resolved X, Y, CX, and CY + tag_arc(block, abs_x, abs_y, abs_z, abs_cx, abs_cy, abs_cz, move, settings->plane ); + ARC_FEED(block->line_number, end1, end2, center1, center2, turn, end3, AA_end, BB_end, CC_end, u, v, w); *current1 = end1; @@ -5379,6 +5411,7 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 } else ERS("BUG: Invalid plane for cutter compensation"); CHP(status); } else if (move == G_0) { + tag_straight(block,end_x, end_y); // Update the heading and clear arc data for ANY straight move STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, u_end, v_end, w_end); @@ -5386,6 +5419,7 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 settings->current_y = end_y; settings->current_z = end_z; } else if (move == G_1) { + tag_straight(block,end_x, end_y); // Update the heading and clear arc data for ANY straight move STRAIGHT_FEED(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, u_end, v_end, w_end); @@ -6352,3 +6386,135 @@ int Interp::update_tag(StateTag &tag) UPDATE_TAG(tag); return INTERP_OK; } + +/****************************************************************************/ + +/*! tag_straight + +Returned Value: int + Applies new geometric tags for straight moves (G0 and G1 segments) + Called from convert_arc2 +*/ + +int Interp::tag_straight(block_pointer block, double x, double y) +{ + // Use a static variable to remember the heading between function calls + // Initialize to 0.0 or your preferred default start angle + static double last_valid_heading = 0.0; + + double start_x = _setup.current_x; + double start_y = _setup.current_y; + + double dx = x - start_x; + double dy = y - start_y; + + // Only update the heading if there is actual XY motion + if (hypot(dx, dy) > 0.0001) + { + double heading = atan2(dy, dx) * (180.0 / M_PI); + + // Normalization + heading = fmod(heading, 360.0); + if (heading < 0.0) heading += 360.0; + + // Store it for this block AND for the next Z-only move + block->arc_heading = heading; + last_valid_heading = heading; + } + else + { + // For Z-only moves, reuse the last calculated XY heading + block->arc_heading = last_valid_heading; + } + + // Reset Arc data for safety + block->radius = 0.0; + block->arc_center_x = 0.0; + block->arc_center_y = 0.0; + block->iscircle = false; + + // Ship the data to the status registers + write_canon_state_tag(block, &_setup); + + return INTERP_OK; +} + +/****************************************************************************/ + +/*! tag_arcs + +Returned Value: int + Applies new tags for Arcs (G2 and G3 segments) + Called from convert_arc2 +*/ + +int Interp::tag_arc(block_pointer block, double x, double y, double z, double center_x, double center_y, double center_z, int move, CANON_PLANE plane) + { + + // Initialize variables to be populated by the plane logic + double dx = 0, dy = 0; + bool is_helix = false; + bool is_360 = false; + + // Resolve Plane-Specific Deltas (Heading) and Helix (Perpendicular Move) + if (plane == CANON_PLANE::XY) { + // Heading axes: X=Horiz, Y=Vert + dx = center_x - _setup.current_x; + dy = center_y - _setup.current_y; + + // Helix axis: Z + if (fabs(z - _setup.current_z) > TOLERANCE_EQUAL) + is_helix = true; + + // 360 Check: Do planar endpoints match start? + if (fabs(x - _setup.current_x) < TOLERANCE_EQUAL && fabs(y - _setup.current_y) < TOLERANCE_EQUAL) + is_360 = true; + } + else if (plane == CANON_PLANE::XZ) { + // Heading axes: Z=Horiz, X=Vert (Standard G18 orientation) + dx = center_z - _setup.current_z; + dy = center_x - _setup.current_x; + + // Helix axis: Y + if (fabs(y - _setup.current_y) > TOLERANCE_EQUAL) + is_helix = true; + + if (fabs(x - _setup.current_x) < TOLERANCE_EQUAL && fabs(z - _setup.current_z) < TOLERANCE_EQUAL) + is_360 = true; + } + else { // plane == CANON_PLANE::YZ + // Heading axes: Y=Horiz, Z=Vert + dx = center_y - _setup.current_y; + dy = center_z - _setup.current_z; + + // Helix axis: X + if (fabs(x - _setup.current_x) > TOLERANCE_EQUAL) + is_helix = true; + + if (fabs(y - _setup.current_y) < TOLERANCE_EQUAL && fabs(z - _setup.current_z) < TOLERANCE_EQUAL) + is_360 = true; + } + + // Heading Calculation (Using the dx/dy resolved above) + double radial_angle = atan2(dy, dx); + double tangent_angle = (move == G_3) ? (radial_angle + (M_PI / 2.0)) : (radial_angle - (M_PI / 2.0)); + double heading = tangent_angle * (180.0 / M_PI); + + // Normalise 0-360 + while (heading < 0) heading += 360.0; + while (heading >= 360.0) heading -= 360.0; + + // Final Assignments + block->iscircle = (is_360 && !is_helix) ? 1 : 0; + block->arc_center_x = center_x; + block->arc_center_y = center_y; + block->arc_center_z = center_z; + block->arc_radius = hypot(dx, dy); // Radius in the active plane + block->arc_heading = heading; + + write_canon_state_tag(block, &_setup); + //rtapi_print("DEBUG ARC: Plane=%d Move=%d | DX=%.4f DY=%.4f | Radius=%.4f Heading=%.2f | CX=%.4f CY=%.4f CZ=%.4f\n", + // (int)plane, move, dx, dy, block->arc_radius, block->arc_heading, + // block->arc_center_x, block->arc_center_y, block->arc_center_z); + return INTERP_OK; + } diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index bbcf9e926e7..17e8877ac75 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -495,6 +495,15 @@ struct block_struct long offset{}; // start of line in file int o_type{}; int call_type{}; // oword-sub, python oword-sub, remap + + // Add Geometic fields + double arc_center_x{}; + double arc_center_y{}; + double arc_center_z{}; + double arc_radius{}; + double arc_heading{}; + double normal_heading{}; + bool iscircle{}; const char *o_name{}; // !!!KL be sure to free this double params[INTERP_SUB_PARAMS]{}; int param_cnt{}; @@ -781,7 +790,6 @@ struct setup context sub_context[INTERP_SUB_ROUTINE_LEVELS]; int call_state; // enum call_states - indicate Py handler reexecution offset_map_type offset_map; // store label x name, file, line - bool adaptive_feed; // adaptive feed is enabled bool feed_hold; // feed hold is enabled int loggingLevel; // 0 means logging is off @@ -817,6 +825,15 @@ struct setup int disable_g92_persistence; +// add new geometric fields for our new tags + double heading; + double radius; + double center_x; + double center_y; + double center_z; + double normal_heading; + bool iscircle; + #define FEATURE(x) (_setup.feature_set & FEATURE_ ## x) #define FEATURE_RETAIN_G43 0x00000001 #define FEATURE_OWORD_N_ARGS 0x00000002 diff --git a/src/emc/rs274ngc/interp_write.cc b/src/emc/rs274ngc/interp_write.cc index 16b93c78ed0..b451a4d41ba 100644 --- a/src/emc/rs274ngc/interp_write.cc +++ b/src/emc/rs274ngc/interp_write.cc @@ -333,6 +333,16 @@ int Interp::write_state_tag(block_pointer block, state.fields_float[GM_FIELD_FLOAT_FEED] = settings->feed_rate; state.fields_float[GM_FIELD_FLOAT_SPEED] = settings->speed[0]; + + // Pack new geometric data. block is NULL on the M70 save path + // (save_settings() in interp_convert.cc), so guard against null deref. + state.fields_float[GM_FIELD_FLOAT_STRAIGHT_HEADING] = (block == NULL) ? 0.0f : (float)block->arc_heading; + state.fields_float[GM_FIELD_FLOAT_ARC_RADIUS] = (block == NULL) ? 0.0f : (float)block->arc_radius; + state.fields_float[GM_FIELD_FLOAT_ARC_CENTER_X] = (block == NULL) ? 0.0f : (float)block->arc_center_x; + state.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Y] = (block == NULL) ? 0.0f : (float)block->arc_center_y; + state.fields_float[GM_FIELD_FLOAT_ARC_CENTER_Z] = (block == NULL) ? 0.0f : (float)block->arc_center_z; + state.fields_float[GM_FIELD_FLOAT_NORMAL_HEADING] = (block == NULL) ? 0.0f : (float)block->normal_heading; + state.flags[GM_FLAG_IS_CIRCLE] = (block == NULL) ? false : block->iscircle; return 0; } diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index c96ce99be45..f02d334a810 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -657,6 +657,10 @@ int read_inputs(setup_pointer settings); void doLog(unsigned int flags, const char *file, int line, const char *fmt, ...) __attribute__((format(printf,5,6))); + /* State Tags Helpers */ + int tag_straight(block_pointer block, double x, double y); + int tag_arc(block_pointer block, double x, double y, double z, double center_x, double center_y, double center_z, int move, CANON_PLANE plane); + const char *interp_status(int status); //technically this violates encapsulation rules but is needed for diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index e2f798f161f..fe2f81788e1 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -2228,8 +2228,13 @@ int Interp::active_modes(int *g_codes, tag.flags[GM_FLAG_FEED_HOLD] ? 53 : -1; - // Copy float-type state - for (i=0; i sampler.0.pin.0 +net motion-type motion.interp.motion-type => sampler.0.pin.1 +net feedrate motion.interp.feedrate => sampler.0.pin.2 +net arc-radius motion.interp.arc-radius => sampler.0.pin.3 +net arc-center-x motion.interp.arc-center-x => sampler.0.pin.4 +net arc-center-y motion.interp.arc-center-y => sampler.0.pin.5 +net arc-center-z motion.interp.arc-center-z => sampler.0.pin.6 +net heading motion.interp.heading => sampler.0.pin.7 +net normal-heading motion.interp.normal-heading => sampler.0.pin.8 +net iscircle motion.interp.iscircle => sampler.0.pin.9 +net x-pos-fb joint.0.pos-fb => sampler.0.pin.10 +net y-pos-fb joint.1.pos-fb => sampler.0.pin.11 diff --git a/tests/motion/heading/heading.ini b/tests/motion/heading/heading.ini new file mode 100644 index 00000000000..d96e5b0987d --- /dev/null +++ b/tests/motion/heading/heading.ini @@ -0,0 +1,96 @@ +[EMC] +DEBUG = 0 +VERSION = 1.1 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[HAL] +HALFILE = LIB:core_sim.hal +HALFILE = heading.hal + +[TRAJ] +NO_FORCE_HOMING=1 +AXES = 3 +COORDINATES = X Y Z +HOME = 0 0 0 +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 100 +MAX_LINEAR_VELOCITY = 500 + +[KINS] +KINEMATICS = trivkins +JOINTS = 3 + +[AXIS_X] +MIN_LIMIT = -1000.0 +MAX_LIMIT = 1000.0 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 3000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 1000 +MAX_ACCELERATION = 3000 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -1000.0 +MAX_LIMIT = 1000.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_SEQUENCE = 0 + +[AXIS_Y] +MIN_LIMIT = -1000.0 +MAX_LIMIT = 1000.0 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 3000 + +[JOINT_1] +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 3000 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -1000.0 +MAX_LIMIT = 1000.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_SEQUENCE = 0 + +[AXIS_Z] +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 3000 + +[JOINT_2] +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 500 +MAX_ACCELERATION = 3000 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -500.0 +MAX_LIMIT = 500.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_SEQUENCE = 0 diff --git a/tests/motion/heading/test-ui.py b/tests/motion/heading/test-ui.py new file mode 100755 index 00000000000..002c0050848 --- /dev/null +++ b/tests/motion/heading/test-ui.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python3 + +import linuxcnc +import linuxcnc_util +import hal + +import time +import sys +import ast +import math + + +INTERPTIMEOUT = 5 # Max 5 seconds to wait for the interpreter +GCODETIMEOUT = 120 # Max 60 seconds for the gcode file to run12 +SAMPLEN = 1 # Sample ewvery n'th record + +# +# Sample collection routine +# +nsamples = 0 +lastsample = None +lastrecnum = 0 +ctr = 0 + +def is_arc_valid(sample, tolerance=0.11): + """ + Validates circularity, radius accuracy, and G2/G3 heading consistency. + Returns True on pass, False on failure. + """ + global ctr + # Extract Fields + motion_type = sample[1] # 20=G2, 30=G3 + expected_radius = sample[3] + cx, cy = sample[4], sample[5] + heading = sample[7] + normal_heading = sample[8] + x = sample[10] + y = sample[11] + # 2. Radius Check: Distance from tool (x,y) to center (cx,cy) + actual_radius = math.hypot(x - cx, y - cy) + if abs(actual_radius - expected_radius) > tolerance: + ctr = ctr + 1 + if ctr > 40: + print(f"FAIL Radius after 40 times: Expected {expected_radius}, Got {actual_radius:.4f}") + ctr = 0 + return False + ctr = 0 + # 3. Normal Direction Check: Normal must point from tool to center + angle_to_center = math.degrees(math.atan2(cy - y, cx - x)) % 360 + norm_diff = abs(angle_to_center - (normal_heading % 360)) + if norm_diff > 180: norm_diff = 360 - norm_diff + + if norm_diff > tolerance: + print(f"FAIL Normal: Expected {angle_to_center:.4f}, Got {normal_heading:.4f}") + return False + # 4. Tangency/Direction Check: (Heading vs Normal) + #G2 (20): Heading should be Normal + 90 + #G3 (30): Heading should be Normal - 90 (or + 270) + diff = (heading - normal_heading) % 360 + + if motion_type == 20: # G2 + if abs(diff - 90) > tolerance: + print(f"FAIL: G2 normal heading outside of tolerance",diff) + return False + elif motion_type == 30: # G3 + if abs(diff - 270) > tolerance: + print(f"FAIL: G3 normal heading outside of tolerance",diff) + return False + return True + + +def is_heading_valid(sample1, sample2, tolerance=0.1): + """ + Returns True if the travel direction between two samples + matches the reported heading within a given tolerance. + """ + global ctr + if sample1[0] != sample2[0]: + ctr = ctr + 1 + if ctr < 5: #allow to settle on change of segment + return True + else: + ctr = 0 #Reset + # Extract coords and reported heading + x1, y1 = sample1[10], sample1[11] + x2, y2 = sample2[10], sample2[11] + reported_heading = sample2[7] + + dx = x2 - x1 + dy = y2 - y1 + + # Avoid calculation if there is no movement (prevents jitter errors) + if math.hypot(dx, dy) < 1e-9: + return True + + # Calculate angle: atan2(y, x) -> degrees + calculated_heading = math.degrees(math.atan2(dy, dx)) + + # Normalize to 0-360 if your system doesn't use negative degrees + if calculated_heading < 0: + calculated_heading += 360 + + # Standardize reported_heading to same 0-360 range if needed + if reported_heading < 0: + reported_heading += 360 + + # Handle the "wrap-around" case (e.g., 359.9 is near 0.1) + diff = abs(calculated_heading - reported_heading) + if diff > 180: + diff = 360 - diff + return(diff <= tolerance) + +def parse_sample(sample): + """ + Parses a tuple of sampler data for tests + Fields: line-number, motion-type, feedrate, arc-radius, arc-center-x, arc-center-y, arc-center-z, + heading, normal-heading, iscircle, x-pos-fb, y-pos-fb + """ + global lastsample + global lastrecnum + + #print("{:6d} {}".format(nsamples, sample)) + if sample[0] != lastsample[0]: + return True + if sample[0] not in (4, 5, 6): + # We are only interested in lines 4,5,6. Do not change gcode file + print("Gcode line number is not 4,5,or 6") + return False + if sample[0] == 4 and sample[1] != 0: + # gcode Line 4 should be G0 + print("Gcode for line 4: Expected G0") + return False + if sample[0] == 5 and (sample[1] != 20 or sample[1] != 30): + # gcode Line 5 should be G2 but G3 is supported + if not is_arc_valid(sample, 0.001): + print("Line 5 G2/G3 tests are invalid at record ", lastrecnum) + return False + + if sample[0] == 6 and sample[1] != 10: + # gcode Line 6 should be G1 + print("Gcode for line 6: Expected G1") + return False + if lastrecnum >= 2 and (lastsample[0] == sample[0] == 4): # we have at least 2 G0 records + if not is_heading_valid(lastsample, sample, tolerance=0.001): + print("Line 4 heading does not agree with expected value at record ",lastrecnum) + return False + if lastrecnum >= 2 and (lastsample[0] == sample[0] == 6): # we have at least 2 G0 records + if not is_heading_valid(lastsample, sample, tolerance=0.001): + print("Line 6 heading or other values does not agree with expected value at record ",lastrecnum) + return False + lastrecnum = lastrecnum + 1 + lastsample = sample + +# +# Sample collection routine +# + +def read_sample(): + global nsamples, lastsample + sample = sampler.read() + if sample != lastsample: # Don't print duplicates + if (nsamples % SAMPLEN == 0): + if (sample[0] != 0): + test_result = parse_sample(sample) # every 1th record + if test_result == False: + print("Test Failed") + nsamples += 1 + lastsample = sample + +# +# Setup a hal component and connect to the sampler stream +# +h = hal.component("python-ui") +sampler = hal.stream(h, hal.sampler_base, "ssfffffffbff") +h.ready() + +# +# connect to LinuxCNC +# + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +l = linuxcnc_util.LinuxCNC(command=c, status=s, error=e) +# Enable machine +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) + +# Home all joints +c.home(-1) +c.wait_complete() +l.wait_for_home([1, 1, 1, 0, 0, 0, 0, 0, 0]) + +# +# Setup to run GCode program +# +c.mode(linuxcnc.MODE_AUTO) +c.program_open("heading-test.ngc") + +# Enable sampling +hal.set_p("sampler.0.enable", "1") +read_sample() +# Start the GCode +c.auto(linuxcnc.AUTO_RUN, 1) + +# Wait for the interpreter to start running the program +start = time.time() +while time.time() - start < INTERPTIMEOUT: + s.poll() + if s.interp_state != linuxcnc.INTERP_IDLE: + print("Interpreter active...") + break + time.sleep(0.01) +if s.interp_state == linuxcnc.INTERP_IDLE: + print("Timed out starting interpreter") + sys.exit(1) + +# +# Loop until the program finishes while collecting data +# +start = time.time() +s.poll() +while s.interp_state != linuxcnc.INTERP_IDLE and time.time() - start < GCODETIMEOUT: + # Continuously drain the sampler buffer + while sampler.readable: + read_sample() + time.sleep(0.01) + s.poll() + +if s.interp_state != linuxcnc.INTERP_IDLE: + print("Timed out running the GCode program") + sys.exit(1) + +# Disable the sampler +hal.set_p("sampler.0.enable", "0") + +# Give RT time to finalize the last sample(s) +time.sleep(0.01) + +# Drain any remaining samples +while sampler.readable: + read_sample() + +# if we get here it all worked! +sys.exit(0) + diff --git a/tests/motion/heading/test.sh b/tests/motion/heading/test.sh new file mode 100755 index 00000000000..db77e154b63 --- /dev/null +++ b/tests/motion/heading/test.sh @@ -0,0 +1,8 @@ +#!/bin/bash +#exec linuxcnc -r heading.ini +if linuxcnc -r heading.ini; then + echo "Completed successfully" > result +else + echo "Test failed" > result +fi +