diff --git a/docs/src/gcode/g-code.adoc b/docs/src/gcode/g-code.adoc index 6253e57afb2..825d5f84d28 100644 --- a/docs/src/gcode/g-code.adoc +++ b/docs/src/gcode/g-code.adoc @@ -1071,6 +1071,12 @@ K follows the drive line described by 'X- Y- Z-'. K is not parallel to the Z axis if X or Y endpoints are used for example when cutting tapered threads. +[NOTE] +The spindle speed override has no effect during a G33 move, and the previous +setting is restored when the move ends. A thread is cut in several passes over +the same helix, so changing the spindle speed part way through would change the +lead and spoil the thread. + [[gcode:g33-tech-info]] .Technical Info At the beginning of each G33 pass, LinuxCNC uses the spindle speed and @@ -1113,11 +1119,15 @@ It is an error if: * An F word is given (the feed follows from K and the spindle speed). * The selected spindle is not commanded to turn (M3 or M4 active) when this command is executed. +* The pitch and the spindle speed together ask an axis to move faster than + its maximum velocity. [NOTE] -The pitch and the spindle speed together set the axis feed. If they ask for -more than the machine can deliver, the program is not rejected: there is no -interpreter check and no error message for that case. +In constant surface speed mode the spindle speed follows the radius, so the +check uses the fastest speed the move can reach: the speed at its smallest +cutting radius, limited by the G96 'D' word and by +`[SPINDLE_n]MAX_FORWARD_VELOCITY`. If the move reaches the centre of rotation +and neither limit is set the speed is unbounded and no check is made. [[gcode:g33.1]] == G33.1 Rigid Tapping(((G33.1 Rigid Tapping))) @@ -1179,6 +1189,12 @@ M2 (end program) * See <> & <> & <> sections for more information. +[NOTE] +Unlike G33, the spindle speed override stays active during a G33.1 move and the +feed follows whatever the spindle actually does. A tap guides itself in its own +hole, so there is no thread lead to spoil, and being able to slow the spindle +while the tap is in the work is useful. + It is an error if: * All axis words are omitted. @@ -1186,11 +1202,9 @@ It is an error if: * An F word is given (the feed follows from K and the spindle speed). * The selected spindle is not commanded to turn (M3 or M4 active) when this command is executed. - -[NOTE] -The pitch and the spindle speed together set the axis feed. If they ask for -more than the machine can deliver, the program is not rejected: there is no -interpreter check and no error message for that case. +* The pitch and the spindle speed together ask an axis to move faster than + its maximum velocity. See <> for how the speed is determined + in constant surface speed mode. [[gcode:g38]] == G38._n_ Straight Probe(((G38.n Probe))) @@ -2046,6 +2060,13 @@ It is an error if: * All the required words are not specified. * 'P-', 'J-', 'K-' or 'H-' is negative. * 'E-' is greater than half the drive line length. +* The pitch and the spindle speed together ask an axis to move faster than + its maximum velocity. See <> for how the speed is determined + in constant surface speed mode. + +[NOTE] +The spindle speed override has no effect during the G76 cycle, for the same +reason as G33, and the previous setting is restored when the cycle ends. .HAL Connections The pins 'spindle.N.at-speed' and the 'encoder._n_.phase-Z' for the diff --git a/docs/src/gcode/m-code.adoc b/docs/src/gcode/m-code.adoc index 21a3da5d6a9..ffd215f6a48 100644 --- a/docs/src/gcode/m-code.adoc +++ b/docs/src/gcode/m-code.adoc @@ -307,6 +307,10 @@ no influence, and the spindle speed will have the exact program specified value of the S-word (described in the <> section). +The override is also suspended for the duration of a +<> move or a <> cycle, whatever M48, M49 or M51 +last selected, and that selection is restored afterwards. + [[mcode:m52]] == M52 Adaptive Feed Control diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 2ddf587b484..5bc132f9b37 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1349,6 +1349,17 @@ static void get_pos_cmds(long period) /* run coordinated trajectory planning cycle */ tpRunCycle(&emcmotInternal->coord_tp, period); + + if (emcmotStatus->syncOverrunSpindle) { + tpAbort(&emcmotInternal->coord_tp); + reportError(_("spindle-synchronized move exceeds axis limits: " + "spindle %d is outrunning the axis by %f per " + "second, reduce the spindle speed or the pitch"), + emcmotStatus->syncOverrunSpindle - 1, + emcmotStatus->syncOverrunError); + emcmotStatus->syncOverrunSpindle = 0; + SET_MOTION_ERROR_FLAG(1); + } /* get new commanded traj pos */ tpGetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd); diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index d2cb7615958..f3eef62aae4 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -890,6 +890,8 @@ static int init_comm_buffers(void) ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd); ZERO_EMC_POSE(emcmotStatus->carte_pos_fb); emcmotStatus->vel = 0.0; + emcmotStatus->syncOverrunSpindle = 0; + emcmotStatus->syncOverrunError = 0.0; emcmotConfig->limitVel = 0.0; emcmotStatus->acc = 0.0; emcmotStatus->feed_scale = 1.0; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1312b5e45dd..f31d4dedc0e 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -600,6 +600,10 @@ Suggestion: Split this in to an Error and a Status flag register.. emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */ emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */ int spindleSync; /* spindle used for synchronised moves. -1 = none */ + int syncOverrunSpindle; /* spindle that outran the axis in a synced move, + plus one; 0 = none. Set by the planner, raised + by the motion controller. */ + double syncOverrunError; /* by how much per second */ spindle_status_t spindle_status[EMCMOT_MAX_SPINDLES]; /* all spindle data */ diff --git a/src/emc/nml_intf/canon.hh b/src/emc/nml_intf/canon.hh index 916b3e92971..324dcb835cc 100644 --- a/src/emc/nml_intf/canon.hh +++ b/src/emc/nml_intf/canon.hh @@ -874,6 +874,14 @@ below. extern double GET_EXTERNAL_ANGLE_UNIT_FACTOR(); */ +// Returns the maximum velocity of one axis, indexed 0-8 as XYZABCUVW, in +// program units per minute, or zero if that limit is not available +extern double GET_EXTERNAL_AXIS_MAX_VELOCITY(int axis); + +// Returns the maximum forward speed of one spindle, in RPM, or zero if that +// limit is not available +extern double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int spindle); + // Returns the system feed rate extern double GET_EXTERNAL_FEED_RATE(); diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 2738b34144b..10e4bb67a5e 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -334,6 +334,7 @@ extern int emcJointSetMaxJerk(int joint, double jerk); extern int emcSpindleSetParams(int spindle, double max_pos, double min_pos, double max_neg, double min_neg, double search_vel, double home_angle, int sequence, double increment); +extern double emcSpindleGetMaxVelocity(int spindle); // implementation functions for EMC_TRAJ types diff --git a/src/emc/rs274ngc/canonmodule.cc b/src/emc/rs274ngc/canonmodule.cc index 3399fb2fc91..d25924235b8 100644 --- a/src/emc/rs274ngc/canonmodule.cc +++ b/src/emc/rs274ngc/canonmodule.cc @@ -178,6 +178,8 @@ BOOST_PYTHON_MODULE(emccanon) { def("GET_EXTERNAL_TOOL_LENGTH_ZOFFSET",&GET_EXTERNAL_TOOL_LENGTH_ZOFFSET); def("GET_EXTERNAL_TOOL_SLOT",&GET_EXTERNAL_TOOL_SLOT); def("GET_EXTERNAL_TOOL_TABLE",&GET_EXTERNAL_TOOL_TABLE); + def("GET_EXTERNAL_AXIS_MAX_VELOCITY",&GET_EXTERNAL_AXIS_MAX_VELOCITY); + def("GET_EXTERNAL_SPINDLE_MAX_VELOCITY",&GET_EXTERNAL_SPINDLE_MAX_VELOCITY); def("GET_EXTERNAL_TRAVERSE_RATE",&GET_EXTERNAL_TRAVERSE_RATE); def("GET_OPTIONAL_PROGRAM_STOP",&GET_OPTIONAL_PROGRAM_STOP); def("INIT_CANON",&INIT_CANON); diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 3b15edea612..45c99a05c99 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -1064,6 +1064,8 @@ CANON_DIRECTION GET_EXTERNAL_SPINDLE(int) { return CANON_STOPPED; } int GET_EXTERNAL_TOOL_SLOT() { return 0; } int GET_EXTERNAL_SELECTED_TOOL_SLOT() { return 0; } double GET_EXTERNAL_FEED_RATE() { return 1; } +double GET_EXTERNAL_AXIS_MAX_VELOCITY(int /*axis*/) { return 0; } +double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int /*spindle*/) { return 0; } double GET_EXTERNAL_TRAVERSE_RATE() { return 0; } int GET_EXTERNAL_FLOOD() { return 0; } int GET_EXTERNAL_MIST() { return 0; } diff --git a/src/emc/rs274ngc/interp_check.cc b/src/emc/rs274ngc/interp_check.cc index 196f2772763..12a4c978623 100644 --- a/src/emc/rs274ngc/interp_check.cc +++ b/src/emc/rs274ngc/interp_check.cc @@ -393,3 +393,127 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block return INTERP_OK; } + +/****************************************************************************/ + +/*! check_spindle_sync_feed + +Returned Value: int + Returns an error if any axis of the move would have to run faster than its + maximum velocity to hold the commanded pitch at the commanded spindle speed. + Otherwise returns INTERP_OK. + +Side effects: none + +Called by: + Interp::convert_straight (G33, G33.1) + Interp::convert_threading_cycle (G76) + +Nothing downstream rejects a feed the machine cannot deliver: the planner +clamps the velocity, the axis falls behind, and the thread is cut wrong. + +The bound is the per-axis maximum, not the traj maximum, because the max +velocity slider is deliberately not applied to position-synchronized moves (see +tpGetMaxTargetVel). The feed is projected onto each axis by its share of the +move length, as the planner distributes it. Rotary axes are ignored: a pitch +is a linear distance per revolution. + +Using the commanded S word means the error names the offending line and does +not depend on the spindle already running. Skipped for any axis whose limit is +unavailable (the standalone interpreter reports zero). + +In constant surface speed mode the S word is a surface speed, so the spindle +speed depends on where the tool is. The worst case over the move is the speed +at its smallest radius, capped the same way motion caps it: by the G96 D word, +and by [SPINDLE_n]MAX_FORWARD_VELOCITY. If the move reaches the centre of +rotation and neither cap is configured the speed is unbounded and the check is +skipped. + +*/ + +/* Fastest the spindle will turn during the move, in RPM, or zero if that + cannot be bounded. MIN_FORWARD_VELOCITY is not applied: motion raises a + speed below it, so ignoring it can only make this too low, and too low + passes a move the runtime overrun check still catches. */ +static double sync_worst_case_rpm(setup_pointer settings, int spindle, + double min_radius) +{ + double speed = settings->speed[spindle]; + double rpm; + + if (speed <= 0.0) + return 0.0; + + if (settings->spindle_mode[spindle] == SPINDLE_MODE::CONSTANT_RPM) { + rpm = speed; + } else if (min_radius > 0.0) { + /* surface speed is metres or feet per minute against a radius in program + units, the css_factor motion works from (see SET_SPINDLE_SPEED) */ + double per_unit = (settings->length_units == CANON_UNITS_INCHES) ? 12.0 : 1000.0; + rpm = per_unit / (2.0 * M_PI) * speed / min_radius; + if (settings->css_maximum[spindle] > 0.0) + rpm = fmin(rpm, settings->css_maximum[spindle]); /* G96 D word */ + } else { + /* the move reaches the centre of rotation, so only the caps bound it */ + rpm = settings->css_maximum[spindle]; + } + + double ini_cap = GET_EXTERNAL_SPINDLE_MAX_VELOCITY(spindle); + if (ini_cap > 0.0 && (rpm <= 0.0 || rpm > ini_cap)) + rpm = ini_cap; + + return rpm; +} + +int Interp::check_spindle_sync_feed(setup_pointer settings, //!< pointer to machine settings + double pitch, //!< program units per revolution + const char *code, //!< G code name, for the message + const double delta[9], //!< move, program units, XYZABCUVW + double min_radius) //!< smallest cutting radius of the move +{ + static const char axis_name[] = "XYZABCUVW"; + int spindle = settings->active_spindle; + bool css = settings->spindle_mode[spindle] == SPINDLE_MODE::CONSTANT_SURFACE; + + double speed = sync_worst_case_rpm(settings, spindle, min_radius); + if (speed <= 0.0 || pitch == 0.0) + return INTERP_OK; + + double length = 0.0; + for (int ax = 0; ax < 9; ax++) { + if (ax >= 3 && ax <= 5) + continue; /* rotary */ + length += delta[ax] * delta[ax]; + } + length = sqrt(length); + if (length <= 0.0) + return INTERP_OK; + + /* program units per minute along the path */ + double required_rate = fabs(pitch) * speed; + + for (int ax = 0; ax < 9; ax++) { + if (ax >= 3 && ax <= 5) + continue; + if (delta[ax] == 0.0) + continue; + double max_rate = GET_EXTERNAL_AXIS_MAX_VELOCITY(ax); + if (max_rate <= 0.0) + continue; + double axis_rate = required_rate * fabs(delta[ax]) / length; + if (css) { + CHKS((axis_rate > max_rate), + _("%s pitch %g reaches spindle speed %g in constant surface speed " + "mode and needs %g per minute on the %c axis, which exceeds its " + "maximum velocity of %g"), + code, fabs(pitch), speed, axis_rate, axis_name[ax], max_rate); + } else { + CHKS((axis_rate > max_rate), + _("%s pitch %g at spindle speed %g needs %g per minute on the %c axis, " + "which exceeds its maximum velocity of %g"), + code, fabs(pitch), speed, axis_rate, axis_name[ax], max_rate); + } + } + + return INTERP_OK; +} diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 638e87eb0c3..c14a282ae88 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -5102,18 +5102,59 @@ int Interp::convert_spindle_mode(int dollar_number, block_pointer block, setup_p if (dollar_number == -1 || s == dollar_number){ if(block->g_modes[GM_SPINDLE_MODE] == G_97) { settings->spindle_mode[s] = SPINDLE_MODE::CONSTANT_RPM; + settings->css_maximum[s] = 0.0; enqueue_SET_SPINDLE_MODE(s, 0); } else { /* G_96 */ settings->spindle_mode[s] = SPINDLE_MODE::CONSTANT_SURFACE; - if(block->d_flag) + if(block->d_flag) { + settings->css_maximum[s] = fabs(block->d_number_float); enqueue_SET_SPINDLE_MODE(s, fabs(block->d_number_float)); - else + } else { + settings->css_maximum[s] = 0.0; enqueue_SET_SPINDLE_MODE(s, 1e30); } + } } } return INTERP_OK; } + +/* Thread cutting re-enters the same helix each pass, so moving the spindle + speed part way through shifts the lead. G33.1 deliberately keeps the + override: a tap is self-guiding and slowing down is useful. */ + +static void suspend_speed_override(setup_pointer settings) +{ + DISABLE_SPEED_OVERRIDE(settings->active_spindle); +} + +static void restore_speed_override(setup_pointer settings) +{ + /* back to what the program asked for, so an M49 or M51 P0 still holds */ + if (settings->speed_override[settings->active_spindle]) { + ENABLE_SPEED_OVERRIDE(settings->active_spindle); + } +} + +/* Displacement of a move, ordered XYZABCUVW. */ + +static void sync_move_delta(setup_pointer settings, + double end_x, double end_y, double end_z, + double AA_end, double BB_end, double CC_end, + double u_end, double v_end, double w_end, + double delta[9]) +{ + delta[0] = end_x - settings->current_x; + delta[1] = end_y - settings->current_y; + delta[2] = end_z - settings->current_z; + delta[3] = AA_end - settings->AA_current; + delta[4] = BB_end - settings->BB_current; + delta[5] = CC_end - settings->CC_current; + delta[6] = u_end - settings->u_current; + delta[7] = v_end - settings->v_current; + delta[8] = w_end - settings->w_current; +} + /****************************************************************************/ /*! convert_stop @@ -5526,9 +5567,16 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Spindle not turning in G33")); + double delta[9]; + sync_move_delta(settings, end_x, end_y, end_z, AA_end, BB_end, CC_end, + u_end, v_end, w_end, delta); + CHP(check_spindle_sync_feed(settings, block->k_number, "G33", delta, + min_abs_over_range(settings->current_x, end_x))); + suspend_speed_override(settings); START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0); STRAIGHT_FEED(block->line_number, end_x, end_y, end_z, AA_end, BB_end, CC_end, u_end, v_end, w_end); STOP_SPEED_FEED_SYNCH(); + restore_speed_override(settings); settings->current_x = end_x; settings->current_y = end_y; settings->current_z = end_z; @@ -5541,7 +5589,6 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 CHKS(((settings->spindle_turning[settings->active_spindle] != CANON_CLOCKWISE) && (settings->spindle_turning[settings->active_spindle] != CANON_COUNTERCLOCKWISE)), _("Spindle not turning in G33.1")); - START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0); double scale = 1; if(block->i_flag){ scale = block->i_number; @@ -5549,6 +5596,13 @@ int Interp::convert_straight(int move, //!< either G_0 or G_1 scale = 1; } } + double delta[9]; + sync_move_delta(settings, end_x, end_y, end_z, AA_end, BB_end, CC_end, + u_end, v_end, w_end, delta); + // I multiplies the spindle speed for the retract + CHP(check_spindle_sync_feed(settings, block->k_number * scale, "G33.1", + delta, fabs(settings->current_x))); + START_SPEED_FEED_SYNCH(settings->active_spindle, block->k_number, 0); RIGID_TAP(block->line_number, end_x, end_y, end_z, scale); STOP_SPEED_FEED_SYNCH(); // after the RIGID_TAP cycle we'll be in the same spot @@ -5771,6 +5825,25 @@ int Interp::convert_threading_cycle(block_pointer block, double target_z = end_z + fabs(k_number) * tan(compound_angle); + // A taper also moves X by the thread height over the taper distance, at + // the correspondingly larger pitch. + double plain_pass[9] = {0.0, 0.0, target_z - start_z, 0, 0, 0, 0, 0, 0}; + /* the passes run between the first and last cut depth, so the tightest + radius is the last cut outside, the first cut boring */ + double thread_min_x = boring + ? min_abs_over_range(safe_x + start_depth, safe_x + end_depth) + : min_abs_over_range(safe_x - end_depth, safe_x - start_depth); + CHP(check_spindle_sync_feed(settings, pitch, "G76", plain_pass, + thread_min_x)); + if (taper_dist != 0.0 && (entry_taper || exit_taper)) { + double taper_pass[9] = {full_threadheight, 0.0, taper_dist, + 0, 0, 0, 0, 0, 0}; + CHP(check_spindle_sync_feed(settings, taper_pitch, "G76", taper_pass, + thread_min_x)); + } + + suspend_speed_override(settings); + depth = start_depth; zoff = (depth - full_dia_depth) * tan(compound_angle); while (depth < end_depth) { @@ -5789,6 +5862,7 @@ int Interp::convert_threading_cycle(block_pointer block, start_z, zoff, taper_dist, entry_taper, exit_taper, taper_pitch, pitch, full_threadheight, target_z); } + restore_speed_override(settings); STRAIGHT_TRAVERSE(block->line_number, end_x, end_y, end_z, AABBCC); settings->current_x = end_x; settings->current_y = end_y; diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 22268854211..8beb7d67312 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -94,6 +94,15 @@ static inline bool equal(double a, double b) return (fabs(a - b) < TOLERANCE_EQUAL); } +/* Smallest distance from zero over the closed interval [a, b], which is zero + if the interval spans it. */ +static inline double min_abs_over_range(double a, double b) +{ + if ((a <= 0.0 && b >= 0.0) || (b <= 0.0 && a >= 0.0)) + return 0.0; + return fmin(fabs(a), fabs(b)); +} + #define TINY 1e-12 /* for arc_data_r */ // max number of m codes on one line @@ -763,6 +772,7 @@ struct setup int active_spindle; // the spindle currently used for CSS, FPR etc. double speed[EMCMOT_MAX_SPINDLES];// array of spindle speeds SPINDLE_MODE spindle_mode[EMCMOT_MAX_SPINDLES];// SPINDLE_MODE::CONSTANT_RPM or SPINDLE_MODE::CONSTANT_SURFACE + double css_maximum[EMCMOT_MAX_SPINDLES];// G96 D word, RPM ceiling in CSS mode, 0 if not given CANON_SPEED_FEED_MODE speed_feed_mode; // independent or synched bool speed_override[EMCMOT_MAX_SPINDLES]; // whether speed override is enabled CANON_DIRECTION spindle_turning[EMCMOT_MAX_SPINDLES]; // direction spindle is turning diff --git a/src/emc/rs274ngc/interp_setup.cc b/src/emc/rs274ngc/interp_setup.cc index 365e4682d6c..ba9b918385b 100644 --- a/src/emc/rs274ngc/interp_setup.cc +++ b/src/emc/rs274ngc/interp_setup.cc @@ -132,6 +132,7 @@ setup::setup() : active_spindle(0), speed {0.0}, spindle_mode{SPINDLE_MODE::CONSTANT_RPM}, + css_maximum{0.0}, speed_feed_mode{CANON_INDEPENDENT}, speed_override{false}, spindle_turning{CANON_STOPPED}, diff --git a/src/emc/rs274ngc/rs274ngc_interp.hh b/src/emc/rs274ngc/rs274ngc_interp.hh index af7d27e58bc..0c8ebfcb20a 100644 --- a/src/emc/rs274ngc/rs274ngc_interp.hh +++ b/src/emc/rs274ngc/rs274ngc_interp.hh @@ -223,6 +223,9 @@ public: int check_items(block_pointer block, setup_pointer settings); int check_m_codes(block_pointer block); int check_other_codes(block_pointer block); + int check_spindle_sync_feed(setup_pointer settings, double pitch, + const char *code, const double delta[9], + double min_radius); int close_and_downcase(char *line); void nurbs_reset_global_variables(void); int convert_nurbs(int move, block_pointer block, setup_pointer settings); diff --git a/src/emc/rs274ngc/rs274ngc_pre.cc b/src/emc/rs274ngc/rs274ngc_pre.cc index 54212aa1850..57de1da0794 100644 --- a/src/emc/rs274ngc/rs274ngc_pre.cc +++ b/src/emc/rs274ngc/rs274ngc_pre.cc @@ -2074,6 +2074,7 @@ int Interp::synch() _setup.spindle_turning[s] = GET_EXTERNAL_SPINDLE(s); _setup.speed_override[s] = GET_EXTERNAL_SPINDLE_OVERRIDE_ENABLE(s); _setup.spindle_mode[s] = SPINDLE_MODE::CONSTANT_RPM; + _setup.css_maximum[s] = 0.0; } GET_EXTERNAL_PARAMETER_FILE_NAME(file_name, (LINELEN - 1)); save_parameters(((file_name[0] == diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index 169e73a8a39..24e85e1e5f5 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -968,6 +968,17 @@ extern CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE(int idx) #endif //} } +/* The standalone interpreter has no machine, so no axis limits */ +double GET_EXTERNAL_AXIS_MAX_VELOCITY(int /*axis*/) +{ + return 0.0; +} + +double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int /*spindle*/) +{ + return 0.0; +} + /* Returns the system traverse rate */ double GET_EXTERNAL_TRAVERSE_RATE() { diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index a5f45837c99..1c8a7875ac7 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -3813,6 +3813,37 @@ double GET_EXTERNAL_FEED_RATE() return feed; } +// maximum velocity of one axis, in program units per minute +double GET_EXTERNAL_AXIS_MAX_VELOCITY(int axis) +{ + if (axis < 0 || axis > 8 || !axis_valid(axis)) { + return 0.0; + } + + double vel = emcAxisGetMaxVelocity(axis); + + if (axis >= 3 && axis <= 5) { + return TO_PROG_ANG(FROM_EXT_ANG(vel)) * 60.0; + } + return TO_PROG_LEN(FROM_EXT_LEN(vel)) * 60.0; +} + +double GET_EXTERNAL_SPINDLE_MAX_VELOCITY(int spindle) +{ + if (spindle < 0 || spindle >= emcStatus->motion.traj.spindles) { + return 0.0; + } + + double rpm = emcSpindleGetMaxVelocity(spindle); + + /* the ini default when MAX_FORWARD_VELOCITY is absent is a stand-in for + "no limit", not a speed anyone can reach */ + if (rpm <= 0.0 || rpm >= 1e30) { + return 0.0; + } + return rpm; +} + // traverse rate wanted is in program units per minute double GET_EXTERNAL_TRAVERSE_RATE() { diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 482d8bf8afe..b8cdfc0dccf 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1957,6 +1957,11 @@ int emcSpindleSetParams(int spindle, double max_pos, double min_pos, double max_ return 0; } + SpindleConfig[spindle].max_pos_speed = max_pos; + SpindleConfig[spindle].max_neg_speed = max_neg; + SpindleConfig[spindle].min_pos_speed = min_pos; + SpindleConfig[spindle].min_neg_speed = min_neg; + emcmotCommand.command = EMCMOT_SET_SPINDLE_PARAMS; emcmotCommand.spindle = spindle; emcmotCommand.maxLimit = max_pos; @@ -1978,6 +1983,15 @@ int emcSpindleSetParams(int spindle, double max_pos, double min_pos, double max_ return retval; } +double emcSpindleGetMaxVelocity(int spindle) +{ + if (spindle < 0 || spindle >= EMCMOT_MAX_SPINDLES) { + return 0; + } + + return SpindleConfig[spindle].max_pos_speed; +} + int emcSpindleAbort(int spindle) { return emcSpindleOff(spindle); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b193b76fb83..9cb2ee7276a 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -498,6 +498,9 @@ int tpInit(TP_STRUCT * const tp) tp->spindle.offset = 0.0; tp->spindle.revs = 0.0; + tp->spindle.overrun_cycles = 0; + tp->spindle.overrun_revs = 0.0; + tp->spindle.overrun_reported = 0; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; @@ -3567,6 +3570,23 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST } +/** + * Record a spindle-synchronized overrun for the motion controller to raise. + * The planner is a separate module and can neither report to the operator nor + * abort re-entrantly from inside its own cycle. + */ +STATIC void tpSyncOverrun(TP_STRUCT * const tp, double amount) +{ + if (tp->spindle.overrun_reported) { + return; /* one per move; it keeps slipping while it stops */ + } + emcmotStatus->syncOverrunSpindle = tp->spindle.spindle_num + 1; + emcmotStatus->syncOverrunError = amount; + tp->spindle.overrun_reported = 1; + tp->spindle.overrun_cycles = 0; +} + + /** * Run position mode synchronization. * Updates requested velocity for a trajectory segment to track the spindle's position. @@ -3611,19 +3631,76 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, tc_debug_print("accelerating in pos_sync\n"); // beginning of move and we are behind: accel as fast as we can tc->target_vel = tc->maxvel; + + /* If the pitch at this speed needs more than the segment can + * deliver, the handoff above never happens and the whole move runs + * here clamped. spindle_vel is revs averaged over the move, so it + * is smooth enough to compare once settled. */ + if (tc->sync_accel * dt > TP_SYNC_OVERRUN_WINDOW && + target_vel > tc->maxvel * TP_SYNC_OVERRUN_MARGIN) { + tpSyncOverrun(tp, target_vel - tc->maxvel); + } } } else { // we have synced the beginning of the move as best we can - // track position (minimize pos_error). tc_debug_print("tracking in pos_sync\n"); - double errorvel; spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime; target_vel = spindle_vel * tc->uu_per_rev; - errorvel = pmSqrt(fabs(pos_error) * tcGetTangentialMaxAccel(tc)); - if(pos_error<0) { - errorvel *= -1.0; + /* Correct the position error without losing the spindle: rise above + * the tracking velocity v_0 and come back to it, so the area of the + * blip is the error. + * + * velocity + * | v_p + * | /\ + * | /..\ v_0 + * |--------....----------- + * | .... + * | .... + * |_________________________ + * |----| t time + * + * That gives v_p = sqrt(v_0^2 + x_err*a_max). The + * old form added sqrt(x_err*a_max) to v_0, which is the same with v_0 + * taken as zero, so it over-corrected and its gain diverged as the + * error went to zero, limit-cycling at the servo rate. + * From robEllenberg, PR #581. */ + double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); + double a_max = tcGetTangentialMaxAccel(tc); + double v_sq = pmSq(target_vel) + pos_error * a_max; + tc->target_vel = pmSqrt(fmax(v_sq, 0.0)); + + /* Rigid tap reversals move the target by design, so watch only the + * tapping pass. */ + bool tap_reversing = (tc->motion_type == TC_RIGIDTAP) && + (tc->coords.rigidtap.state != TAPPING); + + /* Only an axis pinned at its ceiling can be outrun. Below it the + * error grows for reasons a correct G33 has anyway: the spindle turns + * while the axis ramps up, and again while it stops on the endpoint. */ + bool saturated = tc->currentvel >= tc->maxvel * TP_SYNC_OVERRUN_CEILING; + int window_cycles = (int)(TP_SYNC_OVERRUN_WINDOW / dt); + if (window_cycles < 1) { + window_cycles = 1; + } + + if (!tap_reversing && saturated) { + if (tp->spindle.overrun_cycles == 0) { + tp->spindle.overrun_revs = tp->spindle.revs; + } + if (++tp->spindle.overrun_cycles >= window_cycles) { + double window = window_cycles * dt; + double demand = fabs(tp->spindle.revs - tp->spindle.overrun_revs) + * fabs(tc->uu_per_rev) / window; + if (demand > tc->maxvel * TP_SYNC_OVERRUN_MARGIN) { + tpSyncOverrun(tp, demand - tc->maxvel); + } + tp->spindle.overrun_cycles = 0; + } + } else { + tp->spindle.overrun_cycles = 0; } - tc->target_vel = target_vel + errorvel; } //Finally, clip requested velocity at zero @@ -4206,6 +4283,9 @@ int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int mode) { } tp->uu_per_rev = sync; tp->spindle.spindle_num = spindle; + /* each synced move may report again */ + tp->spindle.overrun_reported = 0; + tp->spindle.overrun_cycles = 0; } else tp->synchronized = 0; diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 6687ef3a2d1..0e9ab844322 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -46,6 +46,16 @@ #define TP_MIN_ARC_LENGTH 1e-6 #define TP_BIG_NUM 1e10 +/* The spindle has outrun a position-synchronized move when the feed it asks + * for, averaged over this window, exceeds the segment maximum velocity by this + * margin while the axis is already pinned at that ceiling. Demand is measured + * from the spindle alone, as revolutions turned times the pitch: the tracking + * error carries the v^2/2a lag every G33 picks up while the axis ramps up, and + * a single-cycle spindle velocity is buried in encoder quantization noise. */ +#define TP_SYNC_OVERRUN_WINDOW 0.25 +#define TP_SYNC_OVERRUN_MARGIN 1.02 +#define TP_SYNC_OVERRUN_CEILING 0.99 + /** * TP return codes. * This enum is a catch-all for useful return statuses from TP @@ -83,6 +93,9 @@ typedef struct { double revs; int waiting_for_index; int waiting_for_atspeed; + int overrun_cycles; /* cycles elapsed in the current overrun window */ + double overrun_revs; /* spindle position when that window opened */ + int overrun_reported; /* fault already raised for this synced move */ } tp_spindle_t; /** diff --git a/tests/interp/g76/expected b/tests/interp/g76/expected index c2c4921112c..562e3d39861 100644 --- a/tests/interp/g76/expected +++ b/tests/interp/g76/expected @@ -31,6 +31,7 @@ N..... COMMENT("h = number of spring passes") N..... COMMENT("e = distance along drive line used for tapered start/end") N..... COMMENT("l = which ends get the taper: 0 = neither, 1 = begin, 2 = end, 3 = both") + N..... DISABLE_SPEED_OVERRIDE(0) N..... STRAIGHT_TRAVERSE(0.2370, 0.0000, 0.1955, 0.0000, 0.0000, 0.0000) N..... STRAIGHT_TRAVERSE(0.1170, 0.0000, 0.1955, 0.0000, 0.0000, 0.0000) N..... DISABLE_FEED_OVERRIDE() @@ -381,6 +382,7 @@ N..... STOP_SPEED_FEED_SYNCH() N..... STRAIGHT_TRAVERSE(0.2000, 0.0000, -0.5000, 0.0000, 0.0000, 0.0000) N..... ENABLE_FEED_OVERRIDE() + N..... ENABLE_SPEED_OVERRIDE(0) N..... STRAIGHT_TRAVERSE(0.2000, 0.0000, -0.5000, 0.0000, 0.0000, 0.0000) N..... STRAIGHT_TRAVERSE(0.5000, 0.0000, -0.5000, 0.0000, 0.0000, 0.0000) N..... STRAIGHT_TRAVERSE(0.5000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000)