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 sample-enable motion.digital-out-00 => sampler.0.enable +setp sampler.0.enable 1 + +start +loadusr -w halsampler -n 10000 diff --git a/tests/motion/heading/heading.ini b/tests/motion/heading/heading.ini new file mode 100644 index 00000000000..efe6ad7c814 --- /dev/null +++ b/tests/motion/heading/heading.ini @@ -0,0 +1,107 @@ +[EMC] +#DEBUG = 0xffffffff +DEBUG = 0 +VERSION = 1.1 + +[DISPLAY] +DISPLAY = test-ui.py +INTRO_GRAPHIC = linuxcnc.gif +INTRO_TIME = 0 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 +MDI_QUEUED_COMMANDS=10000 + +[RS274NGC] +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ./subs +LOG_LEVEL = 999 + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +BASE_PERIOD = 0 +SERVO_PERIOD = 1000000 + +[HAL] +HALUI = halui +HALFILE = LIB:core_sim.hal +HALFILE = sampler.hal +#POSTGUI_HALFILE = sampler.hal + +[TRAJ] +NO_FORCE_HOMING=1 +AXES = 3 +COORDINATES = X Y Z +HOME = 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 1.2 +MAX_LINEAR_VELOCITY = 4 + +[KINS] +JOINTS = 3 +KINEMATICS = trivkins + +[AXIS_X] +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 + +[AXIS_Y] +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 + +[AXIS_Z] +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +MIN_LIMIT = -4.0 +MAX_LIMIT = 4.0 + +[JOINT_0] +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[JOINT_1] +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[JOINT_2] +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 4 +MAX_ACCELERATION = 1000.0 +BACKLASH = 0.000 +INPUT_SCALE = 4000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -4.0 +MAX_LIMIT = 4.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[EMCIO] +#TOOL_CHANGE_QUILL_UP = 1 +#RANDOM_TOOLCHANGER = 0 +#TOOL_TABLE = simpockets.tbl diff --git a/tests/motion/heading/sampler.hal b/tests/motion/heading/sampler.hal new file mode 100644 index 00000000000..0c71780d0a2 --- /dev/null +++ b/tests/motion/heading/sampler.hal @@ -0,0 +1,16 @@ +loadrt sampler depth=7000 cfg=sffffffffb +#gcode took about 7 seconds on my PC + +net linenum motion.interp.line-number => sampler.0.pin.0 +net x-pos-fb joint.0.pos-fb => sampler.0.pin.1 +net y-pos-fb joint.1.pos-fb => sampler.0.pin.2 +net heading motion.interp.heading => sampler.0.pin.3 +net normal motion.interp.normal-heading => sampler.0.pin.4 +net radius motion.interp.arc-radius => sampler.0.pin.5 +net center-x motion.interp.arc-center-x => sampler.0.pin.6 +net center-y motion.interp.arc-center-y => sampler.0.pin.7 +net center-z motion.interp.arc-center-z => sampler.0.pin.8 +net iscircle motion.interp.iscircle => sampler.0.pin.9 + +#loadusr -w halsampler -n 10000 + diff --git a/tests/motion/heading/test-ui.py b/tests/motion/heading/test-ui.py new file mode 100755 index 00000000000..3525c0d470f --- /dev/null +++ b/tests/motion/heading/test-ui.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 + +import linuxcnc +import linuxcnc_util +import hal + +import time +import sys +import os + +print("Got to ui file") + +SAMPLER_KEY = 0x48534130 +# sampler.read() returns a tuple with the sample value +i = 0 +def read_samples(sampler): + global i + while sampler.readable: + print(i," : ",sampler.read()) # maybe this should do something else... + i = i + 1 + print(i," : records found") + +def read_sample(sampler): + # Just read one sample at a time and print it + # sampler.read() returns a tuple based on your format "sffffffffb" + data = sampler.read() + print(f"Sample: {data}") + +h = hal.component("python-ui") +h.ready() + +#sampler = h.stream(h, h.sampler_base, "sffffffffb") +#sampler = h.stream(h, h.sampler_base, "sffffffffb") +#sampler = hal.stream(h, "sampler_base", "sffffffffb") +sampler = hal.stream(h, SAMPLER_KEY, "sffffffffb") +# +# connect to LinuxCNC +# + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +# Wait for LinuxCNC to actually be ready +timeout = 5 +start = time.time() +while time.time() - start < timeout: + try: + s.poll() + break + except linuxcnc.error: + time.sleep(0.1) +else: + print("Timed out waiting for LinuxCNC status buffer") + sys.exit(1) + +#sampler = h.stream(h, h.sampler_base, "sffffffffb") +sampler = hal.stream(h, SAMPLER_KEY, "sffffffffb") +l = linuxcnc_util.LinuxCNC(command=c, status=s, error=e) +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +for joint in [0, 1, 2]: + c.home(joint) + # Wait for this specific joint to finish homing + while True: + s.poll() + if s.homed[joint]: + break + time.sleep(0.05) +l.wait_for_home([1, 1, 1, 0, 0, 0, 0, 0, 0]) + +c.mode(linuxcnc.MODE_AUTO) + +c.program_open("heading-test.ngc") + +# Enable sampling +hal.set_p("sampler.0.enable", "1") +#h["sampler.0.enable"] = True + +c.auto(linuxcnc.AUTO_RUN, 1) + +# wait for the interpreter to start running the heading-test.ngc program +while True: + 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("failed to start interpreter, interp_state is {}".format(e.s.interp_state)) + sys.exit(1) + +print("Collecting data...") +while True: + s.poll() + # Continuously drain the sampler buffer + while sampler.readable: + print("found some samples") + read_sample(sampler) + # Exit when the program finishes AND the buffer is empty + if s.interp_state == linuxcnc.INTERP_IDLE and not sampler.readable: + break + time.sleep(0.01) + +l.wait_for_interp_state(linuxcnc.INTERP_IDLE) + +hal.set_p("sampler.0.enable", "0") +time.sleep(0.1) +read_samples(sampler) # drain rest + + +print("done! it all worked") + +# 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..ab888829fc7 --- /dev/null +++ b/tests/motion/heading/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash +exec linuxcnc -r heading.ini