From e40d93cbf32fdda2150181b459c0cbe64aa3792d Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 3 Sep 2016 13:32:42 -0400 Subject: [PATCH 01/32] tp: Improve position-mode spindle sync performance with new error calculation. The 2.x target velocity calculation in position-sync mode causes jitter around the target position. This commit replaces the error calculation with a simpler version that better tracks spindle velocity. Other changes: * Rename and refactor to be clearer about scope and purpose * Create a separate header for missing math functions from C math (signum and integer min / max). * Rename the signed spindle position helper function to something more meaningful Signed-off-by: Robert W. Ellenberg --- src/emc/tp/math_util.h | 15 +++++++++++++++ src/emc/tp/tp.c | 36 +++++++++++++++++++----------------- 2 files changed, 34 insertions(+), 17 deletions(-) create mode 100644 src/emc/tp/math_util.h diff --git a/src/emc/tp/math_util.h b/src/emc/tp/math_util.h new file mode 100644 index 00000000000..127fd22466f --- /dev/null +++ b/src/emc/tp/math_util.h @@ -0,0 +1,15 @@ +#ifndef MATH_UTIL_H +#define MATH_UTIL_H + +inline long max(long y, long x) { + return y > x ? y : x; +} +inline long min(long y, long x) { + return y < x ? y : x; +} + +inline double signum(double x) { + return (x > 0.0) ? 1.0 : (x < 0.0) ? -1.0 : 0.0; +} + +#endif // MATH_UTIL_H diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b7a018ea35d..3ec7350c606 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -21,6 +21,7 @@ #include "motion_types.h" #include "spherical_arc.h" #include "blendmath.h" +#include "math_util.h" //KLUDGE Don't include all of emc.hh here, just hand-copy the TERM COND //definitions until we can break the emc constants out into a separate file. //#include "emc.hh" @@ -303,9 +304,14 @@ STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp, } /** - * Convert the 2-part spindle position and sign to a signed double. + * Convert a raw spindle position into a "progress" position in the current spindle direction. + * In other words, how far has the spindle moved in the indicated direction? + * + * @param spindle_pos signed raw spindle position (typically from motion) + * @param spindle_dir commanded spindle direction + * @return Spindle progress, POSITIVE if the position and direction have the same sign, NEGATIVE otherwise. */ -STATIC inline double tpGetSignedSpindlePosition(double spindle_pos, int spindle_dir) { +STATIC inline double getSpindleProgress(double spindle_pos, int spindle_dir) { if (spindle_dir < 0.0) { spindle_pos*=-1.0; } @@ -2710,10 +2716,9 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc ) { - double spindle_pos = tpGetSignedSpindlePosition(emcmotStatus->spindleRevs, + double spindle_pos = getSpindleProgress(emcmotStatus->spindleRevs, emcmotStatus->spindle.direction); tp_debug_print("Spindle at %f\n",spindle_pos); - double spindle_vel, target_vel; double oldrevs = tp->spindle.revs; if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || @@ -2731,19 +2736,19 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, pos_error -= nexttc->progress; } + const double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); if(tc->sync_accel) { // detect when velocities match, and move the target accordingly. // acceleration will abruptly stop and we will be on our new target. // FIX: this is driven by TP cycle time, not the segment cycle time - double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); - spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++); - target_vel = spindle_vel * tc->uu_per_rev; - if(tc->currentvel >= target_vel) { + double avg_spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++); + double avg_target_vel = avg_spindle_vel * tc->uu_per_rev; + if(tc->currentvel >= avg_target_vel) { tc_debug_print("Hit accel target in pos sync\n"); // move target so as to drive pos_error to 0 next cycle tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev; tc->sync_accel = 0; - tc->target_vel = target_vel; + tc->target_vel = avg_target_vel; } else { tc_debug_print("accelerating in pos_sync\n"); // beginning of move and we are behind: accel as fast as we can @@ -2752,15 +2757,12 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, } 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) * tpGetScaledAccel(tp,tc)); - if(pos_error<0) { - errorvel *= -1.0; - } + double spindle_vel = (tp->spindle.revs - oldrevs) / dt; + double target_vel = spindle_vel * tc->uu_per_rev; + double errorvel = pos_error / dt; + tc->target_vel = target_vel + errorvel; + tc_debug_print("in position sync, target_vel = %f\n", tc->target_vel); } //Finally, clip requested velocity at zero From 54773de2272e760690cfb187f11494af90ef06cf Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 4 Sep 2016 08:10:00 -0400 Subject: [PATCH 02/32] tp: new formula for spindle position tracking The original "error velocity" formula was correct if the initial and final velocity was zero. However, during spindle position tracking, the target velocity is non-zero (or it wouldn't be tracking!). This lead to a slight over-correction, and steady-state jitter even with a perfect encoder signal. The new formula accounts for the ideal target velocity, and should therefore over-correct less. This fixes the jitter (in simulation with a perfect encoder, at least). Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 37 +++++++++++++++++++++++++++++++++++-- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 3ec7350c606..cb474030990 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2759,9 +2759,42 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, // track position (minimize pos_error). double spindle_vel = (tp->spindle.revs - oldrevs) / dt; double target_vel = spindle_vel * tc->uu_per_rev; - double errorvel = pos_error / dt; - tc->target_vel = target_vel + errorvel; + /* + * Correct for position errors when tracking spindle motion. + * This approach assumes that if position error is 0, the correct + * velocity is just the nominal target velocity. If the position error + * is non-zero, however, then we need to correct it, but then return to + * the nominal velocity. + * + * velocity + * | v_p + * | /\ + * | /..\ v_0 + * |--------....----------- + * | .... + * | .... + * |_________________________ + * |----| t time + * + * To correct a position error x_err (shaded area above), we need to + * momentarily increase the velocity, then decrease back to the nonimal + * velocity. + * + * The area under the "blip" is x_err, given the tracking velocity v_0 + * and maximum axis acceleration a_max. + * + * x_err = (v_0 + v_p) * t / 2 , t = 2 * (v_p - v_0) / a_max + * + * Substitute, rearrange and solve: + * + * v_p = sqrt( v_0^2 + x_err * a_max) + * + */ + double a_max = tpGetScaledAccel(tp, tc); + // Make correction term non-negative + double v_sq = pmSq(target_vel) + pos_error * a_max; + tc->target_vel = pmSqrt(fmax(v_sq, 0.0)); tc_debug_print("in position sync, target_vel = %f\n", tc->target_vel); } From b05f4135cf4fe2da231f061b1bd34870f05d8203 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Sep 2016 14:22:02 -0400 Subject: [PATCH 03/32] sim: Add a simple component for quantizing a signal (useful for simulating encoders) Signed-off-by: Robert W. Ellenberg --- src/hal/components/quant.comp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 src/hal/components/quant.comp diff --git a/src/hal/components/quant.comp b/src/hal/components/quant.comp new file mode 100644 index 00000000000..0030f9cff53 --- /dev/null +++ b/src/hal/components/quant.comp @@ -0,0 +1,29 @@ +// This is a component for EMC2 HAL +// Copyright 2016 Robert W. Ellenberg +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of version 2 of the GNU General +// Public License as published by the Free Software Foundation. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +// +component quant "Quantize an input signal to a specified number of levels"; + +pin in float in "Analog input value" ; +pin in float resolution "Resolution of output signal (levels per unit)" ; +pin out float out "Quantized output value (analog)"; + +function _; +license "GPL"; +;; +#include "rtapi_math.h" +FUNCTION(_) { + out = floor(in * resolution) / resolution; +} From 52f432e2e5d89d272b803bd2e94b58c4902b0d1c Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Sep 2016 14:28:14 -0400 Subject: [PATCH 04/32] sim: First cut of spindle encoder simulation with quantized output Signed-off-by: Robert W. Ellenberg --- lib/hallib/sim_spindle_encoder.hal | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/lib/hallib/sim_spindle_encoder.hal b/lib/hallib/sim_spindle_encoder.hal index 20ae6b95f0b..dfa9a2e4d3b 100644 --- a/lib/hallib/sim_spindle_encoder.hal +++ b/lib/hallib/sim_spindle_encoder.hal @@ -5,16 +5,23 @@ setp sim_spindle.scale 0.01666667 loadrt limit2 names=limit_speed loadrt lowpass names=spindle_mass loadrt near names=near_speed +loadrt quant names=sim_spindle_encoder -# this limit doesnt make any sense to me: +# Bound spindle acceleration to something reasonable setp limit_speed.maxv 5000.0 # rpm/second # encoder reset control # hook up motion controller's sync output net spindle-index-enable motion.spindle-index-enable <=> sim_spindle.index-enable +# Set resolution of spindle encoder in counts / revolution (Note: this value is 4 * LPR or PPR) +setp sim_spindle_encoder.resolution 64.0 + +# Simulated spindle encoder +net spindle-pos-raw sim_spindle.position-fb => sim_spindle_encoder.in + # report our revolution count to the motion controller -net spindle-pos sim_spindle.position-fb => motion.spindle-revs +net spindle-pos sim_spindle_encoder.out => motion.spindle-revs # simulate spindle mass setp spindle_mass.gain .07 @@ -37,3 +44,4 @@ addf limit_speed servo-thread addf spindle_mass servo-thread addf near_speed servo-thread addf sim_spindle servo-thread +addf sim_spindle_encoder servo-thread From 8051a18151340dda32615c0aa4910fc419300802 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 09:53:33 -0400 Subject: [PATCH 05/32] canon: Add a missing forward declaration Signed-off-by: Robert W. Ellenberg --- src/emc/nml_intf/interpl.hh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/emc/nml_intf/interpl.hh b/src/emc/nml_intf/interpl.hh index 185ac1ed279..79cade5989d 100644 --- a/src/emc/nml_intf/interpl.hh +++ b/src/emc/nml_intf/interpl.hh @@ -19,6 +19,8 @@ #include +struct NMLmsg; + #define MAX_NML_COMMAND_SIZE 1000 // these go on the interp list From 8f12589772919a56bb27fa16a2eaf84badb5a991 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 09:54:37 -0400 Subject: [PATCH 06/32] canon: Minor whitespace cleanup Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 48 ++++++++++++++++++++-------------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index f4d76c90454..3e61ac8e8fa 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -567,23 +567,23 @@ void SET_FEED_RATE(double rate) { if(feed_mode) { - START_SPEED_FEED_SYNCH(rate, 1); - currentLinearFeedRate = rate; + START_SPEED_FEED_SYNCH(rate, 1); + currentLinearFeedRate = rate; } else { - /* convert from /min to /sec */ - rate /= 60.0; + /* convert from /min to /sec */ + rate /= 60.0; - /* convert to traj units (mm & deg) if needed */ - double newLinearFeedRate = FROM_PROG_LEN(rate), - newAngularFeedRate = FROM_PROG_ANG(rate); + /* convert to traj units (mm & deg) if needed */ + double newLinearFeedRate = FROM_PROG_LEN(rate), + newAngularFeedRate = FROM_PROG_ANG(rate); - if(newLinearFeedRate != currentLinearFeedRate - || newAngularFeedRate != currentAngularFeedRate) - flush_segments(); + if(newLinearFeedRate != currentLinearFeedRate + || newAngularFeedRate != currentAngularFeedRate) + flush_segments(); - currentLinearFeedRate = newLinearFeedRate; - currentAngularFeedRate = newAngularFeedRate; + currentLinearFeedRate = newLinearFeedRate; + currentAngularFeedRate = newAngularFeedRate; } } @@ -732,8 +732,8 @@ static AccelData getStraightAcceleration(CANON_POSITION pos) } static VelData getStraightVelocity(double x, double y, double z, - double a, double b, double c, - double u, double v, double w) + double a, double b, double c, + double u, double v, double w) { double dx, dy, dz, da, db, dc, du, dv, dw; double tx, ty, tz, ta, tb, tc, tu, tv, tw; @@ -1906,19 +1906,19 @@ void SET_SPINDLE_SPEED(double r) flush_segments(); if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_speed_msg.speed = spindle_dir * css_maximum; - emc_spindle_speed_msg.factor = spindle_dir * css_numerator; - emc_spindle_speed_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); + if(lengthUnits == CANON_UNITS_INCHES) + css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); + else + css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); + emc_spindle_speed_msg.speed = spindle_dir * css_maximum; + emc_spindle_speed_msg.factor = spindle_dir * css_numerator; + emc_spindle_speed_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); } else { - emc_spindle_speed_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; + emc_spindle_speed_msg.speed = spindle_dir * spindleSpeed; + css_numerator = 0; } interp_list.append(emc_spindle_speed_msg); - + } void STOP_SPINDLE_TURNING() From 8c80a85f8e3a685e4b91121fb9adaa2467d86ad0 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 10:26:21 -0400 Subject: [PATCH 07/32] canon: Replace unit conversion macro with a function Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 3e61ac8e8fa..ceafe83203f 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -97,9 +97,9 @@ static int rotary_unlock_for_traverse = -1; #define TO_PROG_ANG(deg) (deg) /* macros for converting program units to internal (mm/deg) units */ -#define FROM_PROG_LEN(prog) ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0)) #define FROM_PROG_ANG(prog) (prog) + /* Certain axes are periodic. Hardcode this for now */ #define IS_PERIODIC(axisnum) \ ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5) @@ -159,6 +159,19 @@ static CANON_POSITION g92Offset(0.0, 0.0, 0.0, static CANON_UNITS lengthUnits = CANON_UNITS_MM; static CANON_PLANE activePlane = CANON_PLANE_XY; +inline static double FROM_PROG_LEN(double prog_len) +{ + switch (lengthUnits) { + case CANON_UNITS_INCHES: + return prog_len * 25.4; + case CANON_UNITS_CM: + return prog_len * 10.0; + case CANON_UNITS_MM: + return prog_len; + } + return prog_len; +} + static int feed_mode = 0; static int synched = 0; From 7f744241977413eddcb5a42ff491073492d62cd5 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 10:28:08 -0400 Subject: [PATCH 08/32] canon: Enable smarter blending in spindle-synchronized motion. Since Canon knows the spindle speed for a given segment, it's trival to calculate the nominal feed for a synchronized motion segment (assuming ideal spindle behavior). This nominal velocity is passed to motion so that blend arcs can be sized correctly. Also, report a canon error if the planner can't meet the required feed for spindle synchronization. Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 181 +++++++++++++++++++++------------------ 1 file changed, 98 insertions(+), 83 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index ceafe83203f..dbe1c37e24d 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -53,7 +53,7 @@ #endif static int debug_velacc = 0; -static double css_maximum, css_numerator; // both always positive +static double css_maximum; // both always positive static int spindle_dir = 0; static const double tiny = 1e-7; @@ -452,7 +452,7 @@ static double canonMotionTolerance = 0.0; static double canonNaivecamTolerance = 0.0; /* Spindle speed is saved here */ -static double spindleSpeed = 0.0; // always positive +static double spindleSpeed_rpm = 0.0; // always positive /* Prepped tool is saved here */ static int preppedTool = 0; @@ -467,6 +467,8 @@ static bool block_delete = ON; //set enabled by default (previous EMC behaviour) Feed rate is saved here; values are in mm/sec or deg/sec. It will be initially set in INIT_CANON() below. */ +static double uuPerRev_vel = 0.0; +static double uuPerRev_pos = 0.0; static double currentLinearFeedRate = 0.0; static double currentAngularFeedRate = 0.0; @@ -477,6 +479,30 @@ static double currentAngularFeedRate = 0.0; static int cartesian_move = 0; static int angular_move = 0; +enum FeedRateType +{ + FEED_LINEAR, + FEED_ANGULAR +}; + +/** + * Get the nominal feed rate currently active. + * If spindle sync is active, this function estimates the equivalent feed rate under ideal conditions. + */ +static double getActiveFeedRate(FeedRateType angular) +{ + double uuPerRev = feed_mode ? uuPerRev_vel : uuPerRev_pos; + double uu_per_sec = uuPerRev * spindleSpeed_rpm / 60.0; + + //KLUDGE relies on external state here + if (!angular) { + return synched ? FROM_PROG_LEN(uu_per_sec) : currentLinearFeedRate; + } else { + return synched ? FROM_PROG_ANG(uu_per_sec) : currentLinearFeedRate; + } +} + + static double toExtVel(double vel) { if (cartesian_move && !angular_move) { return TO_EXT_LEN(vel); @@ -503,7 +529,7 @@ static void send_g5x_msg(int index) { set_g5x_msg.origin = to_ext_pose(g5xOffset); if(css_maximum) { - SET_SPINDLE_SPEED(spindleSpeed); + SET_SPINDLE_SPEED(spindleSpeed_rpm); } interp_list.append(set_g5x_msg); } @@ -518,7 +544,7 @@ static void send_g92_msg(void) { set_g92_msg.origin = to_ext_pose(g92Offset); if(css_maximum) { - SET_SPINDLE_SPEED(spindleSpeed); + SET_SPINDLE_SPEED(spindleSpeed_rpm); } interp_list.append(set_g92_msg); } @@ -574,6 +600,7 @@ void SET_FEED_MODE(int mode) { flush_segments(); feed_mode = mode; if(feed_mode == 0) STOP_SPEED_FEED_SYNCH(); + canon_debug("setting feed mode %d\n", mode); } void SET_FEED_RATE(double rate) @@ -581,7 +608,7 @@ void SET_FEED_RATE(double rate) if(feed_mode) { START_SPEED_FEED_SYNCH(rate, 1); - currentLinearFeedRate = rate; + uuPerRev_vel = rate; } else { /* convert from /min to /sec */ rate /= 60.0; @@ -755,7 +782,7 @@ static VelData getStraightVelocity(double x, double y, double z, /* If we get a move to nowhere (!cartesian_move && !angular_move) we might as well go there at the currentLinearFeedRate... */ - out.vel = currentLinearFeedRate; + out.vel = getActiveFeedRate(FEED_LINEAR); out.tmax = 0; out.dtot = 0; @@ -814,7 +841,7 @@ static VelData getStraightVelocity(double x, double y, double z, out.dtot = sqrt(du * du + dv * dv + dw * dw); if (out.tmax <= 0.0) { - out.vel = currentLinearFeedRate; + out.vel = getActiveFeedRate(FEED_LINEAR); } else { out.vel = out.dtot / out.tmax; } @@ -828,7 +855,7 @@ static VelData getStraightVelocity(double x, double y, double z, out.dtot = sqrt(da * da + db * db + dc * dc); if (out.tmax <= 0.0) { - out.vel = currentAngularFeedRate; + out.vel = getActiveFeedRate(FEED_ANGULAR); } else { out.vel = out.dtot / out.tmax; } @@ -864,7 +891,7 @@ static VelData getStraightVelocity(double x, double y, double z, out.dtot = sqrt(du * du + dv * dv + dw * dw); if (out.tmax <= 0.0) { - out.vel = currentLinearFeedRate; + out.vel = getActiveFeedRate(FEED_LINEAR); } else { out.vel = out.dtot / out.tmax; } @@ -912,20 +939,14 @@ static void flush_segments(void) { VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); double vel = linedata.vel; - if (cartesian_move && !angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } else if (!cartesian_move && angular_move) { - if (vel > currentAngularFeedRate) { - vel = currentAngularFeedRate; - } - } else if (cartesian_move && angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } + double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); + canon_debug("in flush_segments: got vel %f, nominal_vel %f\n", vel, nominal_vel); + vel = std::min(vel, nominal_vel); + if (synched && vel < nominal_vel) { + CANON_ERROR("In spindle-synchronized motion, can't maintain required feed %0.2f (max is %0.2f) with current settings", + TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); + } EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -1078,7 +1099,7 @@ void STRAIGHT_TRAVERSE(int line_number, } if(old_feed_mode) - START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1); + START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } @@ -1158,19 +1179,12 @@ void STRAIGHT_PROBE(int line_number, VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); ini_maxvel = vel = veldata.vel; - if (cartesian_move && !angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; + double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); + vel = std::min(vel, nominal_vel); + if (synched && vel < nominal_vel) { + CANON_ERROR("In spindle-synchronized motion, can't maintain required feed %0.2f (max is %0.2f) with current settings", + TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); } - } else if (!cartesian_move && angular_move) { - if (vel > currentAngularFeedRate) { - vel = currentAngularFeedRate; - } - } else if (cartesian_move && angular_move) { - if (vel > currentLinearFeedRate) { - vel = currentLinearFeedRate; - } - } AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); acc = accdata.acc; @@ -1246,11 +1260,16 @@ void STOP_CUTTER_RADIUS_COMPENSATION() // nothing need be done here } - - void START_SPEED_FEED_SYNCH(double feed_per_revolution, bool velocity_mode) { + canon_debug("Setting sync, feed_per_revolution = %f, velocity_mode = %d\n", feed_per_revolution, velocity_mode); flush_segments(); + // KLUDGE often we pass these values in from internal state, redundant and bug-prone + if (velocity_mode) { + uuPerRev_vel = feed_per_revolution; + } else { + uuPerRev_pos = feed_per_revolution; + } EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg; spindlesyncMsg.feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution)); spindlesyncMsg.velocity_mode = velocity_mode; @@ -1789,8 +1808,13 @@ void ARC_FEED(int line_number, double a_max = total_xyz_length / tt_max; // Limit velocity by maximum - double vel = MIN(currentLinearFeedRate, v_max); - canon_debug("current F = %f\n",currentLinearFeedRate); + double nominal_vel = getActiveFeedRate(FEED_LINEAR); + double vel = MIN(nominal_vel, v_max); + if (synched && v_max < nominal_vel) { + CANON_ERROR("Can't maintain required feed %g (max is %g) with current settings", + TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); + } + canon_debug("current F = %f\n", nominal_vel); canon_debug("vel = %f\n",vel); canon_debug("v_max = %f\n",v_max); @@ -1865,6 +1889,30 @@ void SET_SPINDLE_MODE(double css_max) { css_maximum = fabs(css_max); } + +/** + * Hack to add spindle speed information to an existing spindle msg. + * Using a template gives us static polymorphism, since all the spindle + * messages derive from EMC_SPINDLE_SPEED. + */ +template +void buildSpindleCmd(SpindleMsg & msg) +{ + double css_numerator = 0.0; + if(css_maximum > 0.0) { + if(lengthUnits == CANON_UNITS_INCHES) + css_numerator = 12 / (2.0 * M_PI) * spindleSpeed_rpm * TO_EXT_LEN(25.4); + else + css_numerator = 1000 / (2.0 * M_PI) * spindleSpeed_rpm * TO_EXT_LEN(1); + msg.speed = spindle_dir * css_maximum; + msg.factor = spindle_dir * css_numerator; + msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); + } else { + msg.speed = spindle_dir * spindleSpeed_rpm; + } + +} + void START_SPINDLE_CLOCKWISE() { EMC_SPINDLE_ON emc_spindle_on_msg; @@ -1872,18 +1920,7 @@ void START_SPINDLE_CLOCKWISE() flush_segments(); spindle_dir = 1; - if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_on_msg.speed = spindle_dir * css_maximum; - emc_spindle_on_msg.factor = spindle_dir * css_numerator; - emc_spindle_on_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); - } else { - emc_spindle_on_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; - } + buildSpindleCmd(emc_spindle_on_msg); interp_list.append(emc_spindle_on_msg); } @@ -1893,43 +1930,21 @@ void START_SPINDLE_COUNTERCLOCKWISE() flush_segments(); spindle_dir = -1; + buildSpindleCmd(emc_spindle_on_msg); - if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_on_msg.speed = spindle_dir * css_maximum; - emc_spindle_on_msg.factor = spindle_dir * css_numerator; - emc_spindle_on_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); - } else { - emc_spindle_on_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; - } interp_list.append(emc_spindle_on_msg); } void SET_SPINDLE_SPEED(double r) { // speed is in RPMs everywhere - spindleSpeed = fabs(r); // interp will never send negative anyway ... + spindleSpeed_rpm = fabs(r); // interp will never send negative anyway ... EMC_SPINDLE_SPEED emc_spindle_speed_msg; flush_segments(); - if(css_maximum) { - if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(25.4); - else - css_numerator = 1000 / (2 * M_PI) * spindleSpeed * TO_EXT_LEN(1); - emc_spindle_speed_msg.speed = spindle_dir * css_maximum; - emc_spindle_speed_msg.factor = spindle_dir * css_numerator; - emc_spindle_speed_msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); - } else { - emc_spindle_speed_msg.speed = spindle_dir * spindleSpeed; - css_numerator = 0; - } + buildSpindleCmd(emc_spindle_speed_msg); interp_list.append(emc_spindle_speed_msg); } @@ -2033,7 +2048,7 @@ void USE_TOOL_LENGTH_OFFSET(EmcPose offset) set_offset_msg.offset.w = TO_EXT_LEN(currentToolOffset.w); if(css_maximum) { - SET_SPINDLE_SPEED(spindleSpeed); + SET_SPINDLE_SPEED(spindleSpeed_rpm); } interp_list.append(set_offset_msg); } @@ -2111,7 +2126,7 @@ void CHANGE_TOOL(int slot) interp_list.append(linearMoveMsg); if(old_feed_mode) - START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1); + START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); canonUpdateEndPoint(x, y, z, a, b, c, u, v, w); } @@ -2547,12 +2562,14 @@ void INIT_CANON() activePlane = CANON_PLANE_XY; canonUpdateEndPoint(0, 0, 0, 0, 0, 0, 0, 0, 0); SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS, 0); - spindleSpeed = 0.0; + spindleSpeed_rpm = 0.0; preppedTool = 0; cartesian_move = 0; angular_move = 0; currentLinearFeedRate = 0.0; currentAngularFeedRate = 0.0; + uuPerRev_vel = 0.0; + uuPerRev_pos = 0.0; ZERO_EMC_POSE(currentToolOffset); /* to set the units, note that GET_EXTERNAL_LENGTH_UNITS() returns @@ -2578,8 +2595,6 @@ void CANON_ERROR(const char *fmt, ...) va_list ap; EMC_OPERATOR_ERROR operator_error_msg; - flush_segments(); - operator_error_msg.id = 0; if (fmt != NULL) { va_start(ap, fmt); @@ -2769,7 +2784,7 @@ int GET_EXTERNAL_FLOOD() double GET_EXTERNAL_SPEED() { // speed is in RPMs everywhere - return spindleSpeed; + return spindleSpeed_rpm; } CANON_DIRECTION GET_EXTERNAL_SPINDLE() @@ -3268,7 +3283,7 @@ int UNLOCK_ROTARY(int line_number, int axis) { interp_list.append(m); // no need to update endpoint if(old_feed_mode) - START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1); + START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); // now, the next move is the real indexing move, so be ready rotary_unlock_for_traverse = axis; From 2b15d34ed954f0defadb4edc8ae2c584be01c06c Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 9 Jul 2016 14:45:14 -0400 Subject: [PATCH 09/32] tp: disallow blending when entering position-synch mode Fixes issue #68 by preventing any blending between position-synced motions (G33) and other motion modes (velocity-sync and normal), according to this table. Mode Transitions | blending allowed --------------------+----------------- normal -> position | no position -> normal | yes all others | yes These now cases match 2.6.x behavior, though the blending itself can be done with tangent / arc blends. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 15 +++++++--- .../nc_files/spindle/g33_blend.ngc | 23 ++++++++++++++ .../nc_files/spindle/g95_blend.ngc | 30 +++++++++++++++++++ 3 files changed, 64 insertions(+), 4 deletions(-) create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index cb474030990..1c96887581c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1382,14 +1382,21 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc return TP_ERR_OK; } -STATIC int tpCheckCanonType(TC_STRUCT * const prev_tc, TC_STRUCT const * const tc) +STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT const * const tc) { if (!tc || !prev_tc) { return TP_ERR_FAIL; } if ((prev_tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) ^ (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE)) { - tp_debug_print("Can't blend between rapid and feed move, aborting arc\n"); + tp_debug_print("Blending disabled: can't blend between rapid and feed motions\n"); + tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + } + if (prev_tc->synchronized != TC_SYNC_POSITION && + tc->synchronized == TC_SYNC_POSITION) { + tp_debug_print("Blending disabled: changing spindle sync mode from %d to %d\n", + prev_tc->synchronized, + tc->synchronized); tcSetTermCond(prev_tc, TC_TERM_COND_STOP); } return TP_ERR_OK; @@ -1914,7 +1921,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v //TODO refactor this into its own function TC_STRUCT *prev_tc; prev_tc = tcqLast(&tp->queue); - tpCheckCanonType(prev_tc, &tc); + handleModeChange(prev_tc, &tc); if (emcmotConfig->arcBlendEnable){ tpHandleBlendArc(tp, &tc); } @@ -2006,7 +2013,7 @@ int tpAddCircle(TP_STRUCT * const tp, TC_STRUCT *prev_tc; prev_tc = tcqLast(&tp->queue); - tpCheckCanonType(prev_tc, &tc); + handleModeChange(prev_tc, &tc); if (emcmotConfig->arcBlendEnable){ tpHandleBlendArc(tp, &tc); findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit); diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc new file mode 100644 index 00000000000..e454a777e2d --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc @@ -0,0 +1,23 @@ +(From TP Issue #68) +G20 G64 G8 G18 +M3 S400 +G0 X2.0 Z0 +G0 X1.1 Z0.1 +(Lead-in move in "normal" feed mode) +G1 X1.0 Z0.0 F20 +(No blend here - switch to position sync mode) +G33 K0.1 X1.0 Z-0.5 +(Tangent blend during position sync) +G33 K0.1 X1.0 Z-1.0 +/G64 P0.002 Q0.001 +(Arc blend here during position sync) +G33 K0.1 X0.800 Z-1.5 +(Arc blend here during position sync) +G33 K0.1 X0.800 Z-2.0 +(No blend here - switch to normal mode) +G1 X1.1 Z-2.1 F20 +(Clear work) +G0 X2.0 +G0 Z0 +M5 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc new file mode 100644 index 00000000000..b29e907260f --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g95_blend.ngc @@ -0,0 +1,30 @@ +(Test blend performance during G95 feed mode) +G20 G90 +G94 +#2=1000 +M3 S[#2] +G0 X0.0 Z0.0 +F0 +G95 +#1=0.01 +G1 X0.1 F[#1] +G1 X0.2 F[#1] +G1 X0.3 F[#1] +G1 X0.4 F[#1] +G1 X0.5 F[#1] +G1 X1.0 F[#1] +(Test transition between G94 / G95) +G94 +G1 Z1.0 F[60.0*#2*#1] +G95 +G1 X0.6 F[#1] +G1 X0.5 F[#1] +G1 X0.4 F[#1] +G1 X0.0 F[#1] +G1 Z0.5 F[#1] +G1 Z0.0 F[#1] +G94 +G0 X0.0 Z0.0 +M5 +M2 + From 72304b816129d0dfd6927bd2fb991207a50d6613 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 27 Sep 2016 00:28:26 -0400 Subject: [PATCH 10/32] tp: Limit max feed scale in position-sync blends. We know that feed override can't exceed 1.0 during position-sync moves, so it's a waste to plan blend arcs that allow for higher feed overrides. This makes blend arcs a bit smaller in position-synced moves. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 34 +++++++++++++++++++++++++++------- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 1c96887581c..bdb1a3c02ff 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -224,13 +224,33 @@ STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, return fmin(v_target * tpGetFeedScale(tp,tc), tpGetMaxTargetVel(tp, tc)); } +STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) +{ + //All reasons to disable feed override go here + if (tc && tc->synchronized == TC_SYNC_POSITION ) { + return 1.0; + } else { + return emcmotConfig->maxFeedScale; + } +} + +STATIC inline double getMaxBlendFeedScale(TC_STRUCT const * prev_tc, TC_STRUCT const * tc) +{ + //All reasons to disable feed override go here + if ((tc && tc->synchronized == TC_SYNC_POSITION) || + (prev_tc && prev_tc->synchronized == TC_SYNC_POSITION)) { + return 1.0; + } else { + return emcmotConfig->maxFeedScale; + } +} /** * Get the worst-case target velocity for a segment based on the trajectory planner state. */ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { - double max_scale = emcmotConfig->maxFeedScale; + double max_scale = getMaxFeedScale(tc); if (tc->is_blending) { //KLUDGE: Don't allow feed override to keep blending from overruning max velocity max_scale = fmin(max_scale, 1.0); @@ -817,7 +837,7 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", @@ -980,7 +1000,7 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", res_init); @@ -1130,7 +1150,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", @@ -1293,7 +1313,7 @@ STATIC int tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc tc, &acc_bound, &vel_bound, - emcmotConfig->maxFeedScale); + getMaxBlendFeedScale(prev_tc, tc)); if (res_init != TP_ERR_OK) { tp_debug_print("blend init failed with code %d, aborting blend arc\n", @@ -1738,8 +1758,8 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, // Calculate instantaneous acceleration required for change in direction // from v1 to v2, assuming constant speed - double v_max1 = fmin(prev_tc->maxvel, prev_tc->reqvel * emcmotConfig->maxFeedScale); - double v_max2 = fmin(tc->maxvel, tc->reqvel * emcmotConfig->maxFeedScale); + double v_max1 = fmin(prev_tc->maxvel, prev_tc->reqvel * getMaxFeedScale(prev_tc)); + double v_max2 = fmin(tc->maxvel, tc->reqvel * getMaxFeedScale(tc)); double v_max = fmin(v_max1, v_max2); tp_debug_print("tangent v_max = %f\n",v_max); From 6092454947458943382c8da68dc9353936d015cd Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 29 Sep 2016 00:07:22 -0400 Subject: [PATCH 11/32] canon: Automatically limit spindle speed in spindle-sync motion. Previously, the user could command an arbitrarily large spindle speed with G33 / G95 motion, even if the machine axes couldn't keep up. The user has no way of knowing that this is happening (except in extreme cases). This fix does two things: 1) Pop a warning message to the user telling them the maximum spindle speed possible for the current motion. 2) Attempt to limit the spindle speed by issuing a new speed command in the background before the synced motion starts. Note that (2) will have no effect if the machine does not have active spindle speed control. Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 70 ++++++++++++++++++++++++++++++---------- 1 file changed, 53 insertions(+), 17 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index dbe1c37e24d..08d9de959cc 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -52,6 +52,10 @@ #define canon_debug(...) #endif +static void limitSpindleSpeed(double rpm); +static void resetSpindleLimit(); +static double limit_rpm = 0; + static int debug_velacc = 0; static double css_maximum; // both always positive static int spindle_dir = 0; @@ -944,9 +948,11 @@ static void flush_segments(void) { vel = std::min(vel, nominal_vel); if (synched && vel < nominal_vel) { - CANON_ERROR("In spindle-synchronized motion, can't maintain required feed %0.2f (max is %0.2f) with current settings", - TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); - } + const static double SPINDLE_SYNCH_MARGIN = 0.05; + double maxSpindleRPM = vel / nominal_vel * (1.0 - SPINDLE_SYNCH_MARGIN) * spindleSpeed_rpm; + CANON_ERROR("In spindle-synchronized motion, maximum speed at line %d is %0.0f RPM", line_no, maxSpindleRPM); + limitSpindleSpeed(maxSpindleRPM); + } EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -1091,7 +1097,7 @@ void STRAIGHT_TRAVERSE(int line_number, int old_feed_mode = feed_mode; if(feed_mode) - STOP_SPEED_FEED_SYNCH(); + STOP_SPEED_FEED_SYNCH(); if(vel && acc) { interp_list.set_line_number(line_number); @@ -1184,7 +1190,7 @@ void STRAIGHT_PROBE(int line_number, if (synched && vel < nominal_vel) { CANON_ERROR("In spindle-synchronized motion, can't maintain required feed %0.2f (max is %0.2f) with current settings", TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); - } + } AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); acc = accdata.acc; @@ -1285,6 +1291,13 @@ void STOP_SPEED_FEED_SYNCH() spindlesyncMsg.velocity_mode = false; interp_list.append(spindlesyncMsg); synched = 0; + // KLUDGE force restore the spindle speed after feed synch (in case we limited it + if (limit_rpm < spindleSpeed_rpm) { + // Restore spindle speed if we limited it + canon_debug("Restoring spindle speed to %0.0f after synched motion\n",spindleSpeed_rpm); + // KLUDGE if multiple threading segments are queued in succession, then this command will end up being redundant + resetSpindleLimit(); + } } /* Machining Functions */ @@ -1896,19 +1909,19 @@ void SET_SPINDLE_MODE(double css_max) { * messages derive from EMC_SPINDLE_SPEED. */ template -void buildSpindleCmd(SpindleMsg & msg) +void buildSpindleCmd(SpindleMsg & msg, double spindle_rpm) { double css_numerator = 0.0; if(css_maximum > 0.0) { if(lengthUnits == CANON_UNITS_INCHES) - css_numerator = 12 / (2.0 * M_PI) * spindleSpeed_rpm * TO_EXT_LEN(25.4); + css_numerator = 12 / (2.0 * M_PI) * spindle_rpm * TO_EXT_LEN(25.4); else - css_numerator = 1000 / (2.0 * M_PI) * spindleSpeed_rpm * TO_EXT_LEN(1); + css_numerator = 1000 / (2.0 * M_PI) * spindle_rpm * TO_EXT_LEN(1); msg.speed = spindle_dir * css_maximum; msg.factor = spindle_dir * css_numerator; msg.xoffset = TO_EXT_LEN(g5xOffset.x + g92Offset.x + currentToolOffset.tran.x); } else { - msg.speed = spindle_dir * spindleSpeed_rpm; + msg.speed = spindle_dir * spindle_rpm; } } @@ -1920,7 +1933,7 @@ void START_SPINDLE_CLOCKWISE() flush_segments(); spindle_dir = 1; - buildSpindleCmd(emc_spindle_on_msg); + buildSpindleCmd(emc_spindle_on_msg, spindleSpeed_rpm); interp_list.append(emc_spindle_on_msg); } @@ -1930,7 +1943,7 @@ void START_SPINDLE_COUNTERCLOCKWISE() flush_segments(); spindle_dir = -1; - buildSpindleCmd(emc_spindle_on_msg); + buildSpindleCmd(emc_spindle_on_msg, spindleSpeed_rpm); interp_list.append(emc_spindle_on_msg); } @@ -1939,16 +1952,39 @@ void SET_SPINDLE_SPEED(double r) { // speed is in RPMs everywhere spindleSpeed_rpm = fabs(r); // interp will never send negative anyway ... + // KLUDGE force the limit_rpm to match + limit_rpm = spindleSpeed_rpm; EMC_SPINDLE_SPEED emc_spindle_speed_msg; flush_segments(); - buildSpindleCmd(emc_spindle_speed_msg); + buildSpindleCmd(emc_spindle_speed_msg, spindleSpeed_rpm); interp_list.append(emc_spindle_speed_msg); } +static void resetSpindleLimit() +{ + SET_SPINDLE_SPEED(spindleSpeed_rpm); +} + +/** + * Hack way to immediately update the spindle speed without flushing segments first (typically called within flush segments as a way to limit spindle speed during threading). + * @warning does NOT update the interpreter state. + */ +static void limitSpindleSpeed(double rpm) +{ + limit_rpm = fabs(rpm); + if (spindleSpeed_rpm > limit_rpm) { + EMC_SPINDLE_SPEED emc_spindle_speed_msg; + canon_debug("Reducing spindle speed to %0.0f\n",limit_rpm); + + buildSpindleCmd(emc_spindle_speed_msg, limit_rpm); + interp_list.append(emc_spindle_speed_msg); + } +} + void STOP_SPINDLE_TURNING() { EMC_SPINDLE_OFF emc_spindle_off_msg; @@ -2597,11 +2633,11 @@ void CANON_ERROR(const char *fmt, ...) operator_error_msg.id = 0; if (fmt != NULL) { - va_start(ap, fmt); - vsnprintf(operator_error_msg.error,sizeof(operator_error_msg.error), fmt, ap); - va_end(ap); + va_start(ap, fmt); + vsnprintf(operator_error_msg.error,sizeof(operator_error_msg.error), fmt, ap); + va_end(ap); } else { - operator_error_msg.error[0] = 0; + operator_error_msg.error[0] = 0; } interp_list.append(operator_error_msg); @@ -3278,7 +3314,7 @@ int UNLOCK_ROTARY(int line_number, int axis) { // issue it int old_feed_mode = feed_mode; if(feed_mode) - STOP_SPEED_FEED_SYNCH(); + STOP_SPEED_FEED_SYNCH(); interp_list.set_line_number(line_number); interp_list.append(m); // no need to update endpoint From 9ee8d38b51d4536d4242d01911f902baf91acb58 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 26 Nov 2016 16:10:01 -0500 Subject: [PATCH 12/32] tp: Fix debug level for some debug print statements Reduce spam in TP debug logs. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index bdb1a3c02ff..a16ccf775e4 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2692,13 +2692,14 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_WAITING; } - // Temporary debug message - tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f\n", + tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f max_acc = %f term_cond = %d\n", tc->id, tc->target_vel, tc->reqvel, tc->finalvel, - tc->target); + tc->target, + tc->maxaccel, + tc->term_cond); tc->active = 1; //Do not change initial velocity here, since tangent blending already sets this up @@ -2707,7 +2708,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { tc->on_final_decel = 0; if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { - tp_debug_print("Setting up position sync\n"); + tp_debug_print(" Setting up position sync\n"); // if we aren't already synced, wait tp->spindle.waiting_for_index = tc->id; // ask for an index reset @@ -2745,7 +2746,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, double spindle_pos = getSpindleProgress(emcmotStatus->spindleRevs, emcmotStatus->spindle.direction); - tp_debug_print("Spindle at %f\n",spindle_pos); + tc_debug_print("Spindle at %f\n",spindle_pos); double oldrevs = tp->spindle.revs; if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || @@ -3244,15 +3245,15 @@ int tpRunCycle(TP_STRUCT * const tp, long period) emcmotStatus->spindleSync = 0; break; case TC_SYNC_VELOCITY: - tp_debug_print("sync velocity\n"); + tc_debug_print("sync velocity\n"); tpSyncVelocityMode(tp, tc, nexttc); break; case TC_SYNC_POSITION: - tp_debug_print("sync position\n"); + tc_debug_print("sync position\n"); tpSyncPositionMode(tp, tc, nexttc); break; default: - tp_debug_print("unrecognized spindle sync state!\n"); + tc_debug_print("unrecognized spindle sync state!\n"); break; } From bf4505933eb1039a8bf705ab0c9b3d4d6c91e3bd Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 26 Nov 2016 16:08:31 -0500 Subject: [PATCH 13/32] tp: Re-order checks for parabolic blends Parabolic blends require reduced max acceleration, so that the worst-case blend does not exceed the limits. If a parabolic blend can be "upgraded" to a tangent / arc blend, then we don't need this restriction. Previously, this check occured too early in the process of creating a new segment, which meant some segments ended up with reduced acceleration unnecessarily. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 102 ++++++++++++++++++++++++------------------------ 1 file changed, 52 insertions(+), 50 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index a16ccf775e4..1c9b5f5f757 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -68,6 +68,8 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const, TC_STRUCT * const tc, double normal_acc_scale); + +STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); /** * @section tpcheck Internal state check functions. * These functions compartmentalize some of the messy state checks. @@ -82,9 +84,14 @@ STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const, */ STATIC int tcCheckLastParabolic(TC_STRUCT * const tc, TC_STRUCT const * const prev_tc) { + if (!tc) {return TP_ERR_FAIL;} + if (prev_tc && prev_tc->term_cond == TC_TERM_COND_PARABOLIC) { - tp_debug_print("prev segment parabolic, flagging blend_prev\n"); + tp_debug_print("Found parabolic blend between %d and %d, flagging blend_prev\n", + prev_tc->id, tc->id); tc->blend_prev = 1; + } else { + tc->blend_prev = 0; } return TP_ERR_OK; } @@ -956,9 +963,6 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tp_debug_print("Passed all tests, updating segments\n"); - //Cleanup any mess from parabolic - tc->blend_prev = 0; - //TODO refactor to pass consume to connect function if (param.consume) { //Since we're consuming the previous segment, pop the last line off of the queue @@ -1115,7 +1119,6 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tcSetLineXYZ(tc, &line2_temp); //Cleanup any mess from parabolic - tc->blend_prev = 0; tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } @@ -1175,8 +1178,6 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, return TP_ERR_FAIL; } - - int res_param = blendComputeParameters(¶m); int res_points = blendFindPoints3(&points_approx, &geom, ¶m); @@ -1284,10 +1285,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, tcSetCircleXYZ(prev_tc, &circ1_temp); tcSetCircleXYZ(tc, &circ2_temp); - //Cleanup any mess from parabolic - tc->blend_prev = 0; tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); - return TP_ERR_OK; } @@ -1433,6 +1431,46 @@ STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) { } } +STATIC int tcUpdateArcLengthFit(TC_STRUCT * const tc) +{ + if (!tc) {return -1;} + + switch (tc->motion_type) { + case TC_CIRCULAR: + findSpiralArcLengthFit(&tc->coords.circle.xyz, &tc->coords.circle.fit); + break; + case TC_LINEAR: + case TC_RIGIDTAP: + case TC_SPHERICAL: + default: + break; + } + return 0; +} + +STATIC int tpFinalizeAndEnqueue(TP_STRUCT * const tp, TC_STRUCT * const tc) +{ + //TODO refactor this into its own function + TC_STRUCT *prev_tc; + prev_tc = tcqLast(&tp->queue); + handleModeChange(prev_tc, tc); + if (emcmotConfig->arcBlendEnable){ + tpHandleBlendArc(tp, tc); + tcUpdateArcLengthFit(tc); + } + tcFlagEarlyStop(prev_tc, tc); + // KLUDGE order is important here, the parabolic blend check has to + // happen after all other steps that affect the terminal condition + tcCheckLastParabolic(tc, prev_tc); + tcFinalizeLength(prev_tc); + + int retval = tpAddSegmentToQueue(tp, tc, true); + //Run speed optimization (will abort safely if there are no tangent segments) + tpRunOptimization(tp); + + return retval; +} + /** * Adds a rigid tap cycle to the motion queue. @@ -1484,14 +1522,7 @@ int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxv // Force exact stop mode after rigid tapping regardless of TP setting tcSetTermCond(&tc, TC_TERM_COND_STOP); - TC_STRUCT *prev_tc; - //Assume non-zero error code is failure - prev_tc = tcqLast(&tp->queue); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - int retval = tpAddSegmentToQueue(tp, &tc, true); - tpRunOptimization(tp); - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp, @@ -1880,6 +1911,7 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { if (res_create == TP_ERR_OK) { //Need to do this here since the length changed + tcCheckLastParabolic(&blend_tc, prev_tc); tpAddSegmentToQueue(tp, &blend_tc, false); } else { return res_create; @@ -1938,22 +1970,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v // For linear move, set rotary axis settings tc.indexrotary = indexrotary; - //TODO refactor this into its own function - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - handleModeChange(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); - } - tcCheckLastParabolic(&tc, prev_tc); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - //Run speed optimization (will abort safely if there are no tangent segments) - tpRunOptimization(tp); - - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } @@ -2030,22 +2047,7 @@ int tpAddCircle(TP_STRUCT * const tp, v_max_actual, acc); - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - - handleModeChange(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); - findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit); - } - tcCheckLastParabolic(&tc, prev_tc); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - - tpRunOptimization(tp); - return retval; + return tpFinalizeAndEnqueue(tp, &tc); } From 75d2ca1f480a9dc33e394c8c14b04597a3c943ee Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 27 Nov 2016 11:17:40 -0500 Subject: [PATCH 14/32] tp: Add debug information for spindle sync Mostly for troubleshooting, doesn't affect end-user performance Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 1c9b5f5f757..ae638e45297 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2761,10 +2761,13 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev; double pos_error = pos_desired - tc->progress; + tc_debug_print(" pos_desired %f, progress %f, pos_error %f, expected error %f", pos_desired, tc->progress, pos_error, tp->spindle.revs - oldrevs); if(nexttc) { + tc_debug_print(" nexttc_progress %f", nexttc->progress); pos_error -= nexttc->progress; } + tc_debug_print("\n"); const double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); if(tc->sync_accel) { From 30f38310c03473306c3aa4e459b5d8d7cbc16807 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 27 Nov 2016 11:14:03 -0500 Subject: [PATCH 15/32] sim: Improve simulation of inertia in spindle sim Instead of just low-pass filtering the measured speed, filter the spindle speed command sent to the simulated spindle. Now, both the position and velocity signals will simulate a spindle with inertia. Signed-off-by: Robert W. Ellenberg --- lib/hallib/sim_spindle_encoder.hal | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/lib/hallib/sim_spindle_encoder.hal b/lib/hallib/sim_spindle_encoder.hal index dfa9a2e4d3b..005c4c623b4 100644 --- a/lib/hallib/sim_spindle_encoder.hal +++ b/lib/hallib/sim_spindle_encoder.hal @@ -26,12 +26,14 @@ net spindle-pos sim_spindle_encoder.out => motion.spindle-revs # simulate spindle mass setp spindle_mass.gain .07 -# spindle speed control +# Limit spindle speed by our maximum value net spindle-speed-cmd motion.spindle-speed-out => limit_speed.in -net spindle-speed-limited limit_speed.out => sim_spindle.velocity-cmd spindle_mass.in -# for spindle velocity estimate -net spindle-rpm-filtered spindle_mass.out motion.spindle-speed-in near_speed.in2 +# Simulate spindle mass with a low-pass filter on the velocity command +net spindle-speed-limited limit_speed.out => spindle_mass.in + +# Feed the simulated spindle speed into the sim spindle to generate a position signal +net spindle-rpm-filtered spindle_mass.out => sim_spindle.velocity-cmd motion.spindle-speed-in near_speed.in2 # at-speed detection setp near_speed.scale 1.1 From 80b0ad7c418699d5e37bffb4752b93f1e07594bb Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 26 Nov 2016 18:07:12 -0500 Subject: [PATCH 16/32] tp: Use motion's spindle-speed-in input for spindle velocity The spindle-speed-in pin will in many cases be a better estimate of velocity than the internal estimate we currently use in the TP. Encoder counters can have much finer time resolution than the servo thread, leading to smoother velocity estimates. One benefit is that the "sync_accel" phase actually works now. With a low-count encoder, the estimated velocity could sometimes be zero if there were no counts within one servo timestep (sam's test case shows this). Then, sync_accel ends early, and the position tracker is left with a large error to correct. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tc_types.h | 1 + src/emc/tp/tp.c | 15 ++++++++------- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 58a1bf038fa..164ce0f9609 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -116,6 +116,7 @@ typedef struct { double target; // actual segment length double progress; // where are we in the segment? 0..target double nominal_length; + double sync_offset; // When did we sync up with the spindle? //Velocity double reqvel; // vel requested by F word, calc'd by task diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index ae638e45297..80b0d916084 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2528,7 +2528,7 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, // spindle position so the next synced move can be in // the right place. if(tc->synchronized != TC_SYNC_NONE) { - tp->spindle.offset += tc->target / tc->uu_per_rev; + tp->spindle.offset += (tc->target - tc->sync_offset) / tc->uu_per_rev; } else { tp->spindle.offset = 0.0; } @@ -2749,7 +2749,6 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, double spindle_pos = getSpindleProgress(emcmotStatus->spindleRevs, emcmotStatus->spindle.direction); tc_debug_print("Spindle at %f\n",spindle_pos); - double oldrevs = tp->spindle.revs; if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || tc->coords.rigidtap.state == FINAL_REVERSAL)) { @@ -2760,8 +2759,9 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, } double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev; + double pos_error = pos_desired - tc->progress; - tc_debug_print(" pos_desired %f, progress %f, pos_error %f, expected error %f", pos_desired, tc->progress, pos_error, tp->spindle.revs - oldrevs); + tc_debug_print(" pos_desired %f, progress %f, pos_error %f", pos_desired, tc->progress, pos_error); if(nexttc) { tc_debug_print(" nexttc_progress %f", nexttc->progress); @@ -2769,17 +2769,19 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, } tc_debug_print("\n"); - const double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); + const double avg_spindle_vel = emcmotStatus->spindleSpeedIn / 60.0; //KLUDGE convert to rps if(tc->sync_accel) { // detect when velocities match, and move the target accordingly. // acceleration will abruptly stop and we will be on our new target. // FIX: this is driven by TP cycle time, not the segment cycle time - double avg_spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++); + // Experiment: try syncing with averaged spindle speed double avg_target_vel = avg_spindle_vel * tc->uu_per_rev; if(tc->currentvel >= avg_target_vel) { tc_debug_print("Hit accel target in pos sync\n"); // move target so as to drive pos_error to 0 next cycle tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev; + tc->sync_offset = tc->progress; + tc_debug_print("Spindle offset %f\n", tp->spindle.offset); tc->sync_accel = 0; tc->target_vel = avg_target_vel; } else { @@ -2790,8 +2792,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, } else { // we have synced the beginning of the move as best we can - // track position (minimize pos_error). - double spindle_vel = (tp->spindle.revs - oldrevs) / dt; - double target_vel = spindle_vel * tc->uu_per_rev; + double target_vel = avg_spindle_vel * tc->uu_per_rev; /* * Correct for position errors when tracking spindle motion. From e3ce24623b27d161f795ab4c4c48e4e3a8092bec Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 15:12:14 -0500 Subject: [PATCH 17/32] motion: Add spindle velocity estimator Like the PID component, motion can now estimate spindle velocity if the spindle-speed-in pin is left unconnected. motion computes a simple 2-point backward difference, which is usable at high speeds and with high PPR encoders. Thanks to Peter Wallace for this suggestion (and Jeff Epler for the original idea in pid.c). Signed-off-by: Robert W. Ellenberg --- src/emc/motion/control.c | 12 ++++++++++++ src/emc/motion/mot_priv.h | 3 ++- src/emc/motion/motion.c | 4 +++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 8e7416549b5..1d9ff7d574f 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -375,6 +375,13 @@ static void process_probe_inputs(void) { old_probeVal = emcmotStatus->probeVal; } +// TODO generalize to higher order diff with an array +static double backward_difference(double curr, double prev, double dt) +{ + return (curr - prev) / dt; +} + + static void process_inputs(void) { int joint_num; @@ -382,8 +389,13 @@ static void process_inputs(void) joint_hal_t *joint_data; emcmot_joint_t *joint; unsigned char enables; + /* read spindle angle (for threading, etc) */ + double prev_revs = emcmotStatus->spindleRevs; emcmotStatus->spindleRevs = *emcmot_hal_data->spindle_revs; + *emcmot_hal_data->spindle_speed_in_estimate = backward_difference(emcmotStatus->spindleRevs, + prev_revs, + emcmotConfig->trajCycleTime / 60.0); emcmotStatus->spindleSpeedIn = *emcmot_hal_data->spindle_speed_in; emcmotStatus->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed; /* compute net feed and spindle scale factors */ diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index f82e36403c3..f374db5a8ec 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -147,7 +147,8 @@ typedef struct { hal_float_t *spindle_speed_out_rps_abs; /* spindle speed output absolute*/ hal_float_t *spindle_speed_cmd_rps; /* spindle speed command without SO applied */ hal_float_t *spindle_speed_in; /* spindle speed measured */ - + hal_float_t *spindle_speed_in_estimate; /* spindle speed measured */ + // spindle orient hal_float_t *spindle_orient_angle; /* out: desired spindle angle, degrees */ hal_s32_t *spindle_orient_mode; /* out: 0: least travel; 1: cw; 2: ccw */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index a47ce7b5842..e1cefcdce7e 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -317,9 +317,11 @@ static int init_hal_io(void) *(emcmot_hal_data->spindle_orient) = 0; -// if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error; + // Inspired by pid.c "dummysig" pins that estimate velocity internally + emcmot_hal_data->spindle_speed_in_estimate = emcmot_hal_data->spindle_speed_in; + if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error; *emcmot_hal_data->spindle_is_atspeed = 1; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; From d83c600fc0fcc546cf97009a5a9ab6e20c306e99 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 16:24:06 -0500 Subject: [PATCH 18/32] motion: Refactor spindle status for clarity and organization. Now, all spindle command settings are collected into one status struct. Similarly, spindle feedback statuses (position, velocity, etc.) are collected into a single struct. Note that this change is internal to motion. HAL pin names and externally-facing status fields should be unaffected. Signed-off-by: Robert W. Ellenberg --- src/emc/motion-logger/motion-logger.c | 4 +- src/emc/motion/command.c | 58 ++++++++++++--------------- src/emc/motion/control.c | 50 +++++++++++------------ src/emc/motion/motion.c | 2 +- src/emc/motion/motion.h | 44 +++++++++++--------- src/emc/task/taskintf.cc | 23 +++++++---- src/emc/tp/tp.c | 32 +++++++-------- 7 files changed, 110 insertions(+), 103 deletions(-) diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index 81a3e8db7fb..fead99ebc45 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -572,12 +572,12 @@ int main(int argc, char* argv[]) { case EMCMOT_SPINDLE_ON: log_print("SPINDLE_ON speed=%f, css_factor=%f, xoffset=%f\n", c->vel, c->ini_maxvel, c->acc); - emcmotStatus->spindle.speed = c->vel; + emcmotStatus->spindle_cmd.velocity_rpm_out = c->vel; break; case EMCMOT_SPINDLE_OFF: log_print("SPINDLE_OFF\n"); - emcmotStatus->spindle.speed = 0; + emcmotStatus->spindle_cmd.velocity_rpm_out = 0; break; case EMCMOT_SPINDLE_INCREASE: diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 7754aa16158..ffdb3f0715c 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -891,7 +891,7 @@ check_stuff ( "before command_handler()" ); issue_atspeed = 1; emcmotStatus->atspeed_next_feed = 0; } - if(!is_feed_type(emcmotCommand->motion_type) && emcmotStatus->spindle.css_factor) { + if(!is_feed_type(emcmotCommand->motion_type) && emcmotStatus->spindle_cmd.css_factor) { emcmotStatus->atspeed_next_feed = 1; } /* append it to the emcmotDebug->tp */ @@ -1522,7 +1522,7 @@ check_stuff ( "before command_handler()" ); rtapi_print_msg(RTAPI_MSG_DBG, "spindle-locked cleared by SPINDLE_ON"); *(emcmot_hal_data->spindle_locked) = 0; *(emcmot_hal_data->spindle_orient) = 0; - emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_NONE; + emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_NONE; /* if (emcmotStatus->spindle.orient) { */ /* reportError(_("cant turn on spindle during orient in progress")); */ @@ -1530,30 +1530,24 @@ check_stuff ( "before command_handler()" ); /* tpAbort(&emcmotDebug->tp); */ /* SET_MOTION_ERROR_FLAG(1); */ /* } else { */ - emcmotStatus->spindle.speed = emcmotCommand->vel; - emcmotStatus->spindle.css_factor = emcmotCommand->ini_maxvel; - emcmotStatus->spindle.xoffset = emcmotCommand->acc; - if (emcmotCommand->vel >= 0) { - emcmotStatus->spindle.direction = 1; - } else { - emcmotStatus->spindle.direction = -1; - } - emcmotStatus->spindle.brake = 0; //disengage brake + emcmotStatus->spindle_cmd.velocity_rpm_out = emcmotCommand->vel; + emcmotStatus->spindle_cmd.css_factor = emcmotCommand->ini_maxvel; + emcmotStatus->spindle_cmd.xoffset = emcmotCommand->acc; + emcmotStatus->spindle_cmd.brake = 0; //disengage brake emcmotStatus->atspeed_next_feed = 1; break; case EMCMOT_SPINDLE_OFF: rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_OFF"); - emcmotStatus->spindle.speed = 0; - emcmotStatus->spindle.direction = 0; - emcmotStatus->spindle.brake = 1; // engage brake + emcmotStatus->spindle_cmd.velocity_rpm_out = 0; + emcmotStatus->spindle_cmd.brake = 1; // engage brake if (*(emcmot_hal_data->spindle_orient)) rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT cancelled by SPINDLE_OFF"); if (*(emcmot_hal_data->spindle_locked)) rtapi_print_msg(RTAPI_MSG_DBG, "spindle-locked cleared by SPINDLE_OFF"); *(emcmot_hal_data->spindle_locked) = 0; *(emcmot_hal_data->spindle_orient) = 0; - emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_NONE; + emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_NONE; break; case EMCMOT_SPINDLE_ORIENT: @@ -1567,11 +1561,10 @@ check_stuff ( "before command_handler()" ); /* tpAbort(&emcmotDebug->tp); */ /* SET_MOTION_ERROR_FLAG(1); */ } - emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_IN_PROGRESS; - emcmotStatus->spindle.speed = 0; - emcmotStatus->spindle.direction = 0; + emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_IN_PROGRESS; + emcmotStatus->spindle_cmd.velocity_rpm_out = 0; // so far like spindle stop, except opening brake - emcmotStatus->spindle.brake = 0; // open brake + emcmotStatus->spindle_cmd.brake = 0; // open brake *(emcmot_hal_data->spindle_orient_angle) = emcmotCommand->orientation; *(emcmot_hal_data->spindle_orient_mode) = emcmotCommand->mode; @@ -1579,38 +1572,37 @@ check_stuff ( "before command_handler()" ); *(emcmot_hal_data->spindle_orient) = 1; // mirror in spindle status - emcmotStatus->spindle.orient_fault = 0; // this pin read during spindle-orient == 1 - emcmotStatus->spindle.locked = 0; + emcmotStatus->spindle_cmd.orient_fault = 0; // this pin read during spindle-orient == 1 + emcmotStatus->spindle_cmd.locked = 0; break; case EMCMOT_SPINDLE_INCREASE: rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_INCREASE"); - if (emcmotStatus->spindle.speed > 0) { - emcmotStatus->spindle.speed += 100; //FIXME - make the step a HAL parameter - } else if (emcmotStatus->spindle.speed < 0) { - emcmotStatus->spindle.speed -= 100; + if (emcmotStatus->spindle_cmd.velocity_rpm_out > 0) { + emcmotStatus->spindle_cmd.velocity_rpm_out += 100; //FIXME - make the step a HAL parameter + } else if (emcmotStatus->spindle_cmd.velocity_rpm_out < 0) { + emcmotStatus->spindle_cmd.velocity_rpm_out -= 100; } break; case EMCMOT_SPINDLE_DECREASE: rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_DECREASE"); - if (emcmotStatus->spindle.speed > 100) { - emcmotStatus->spindle.speed -= 100; //FIXME - make the step a HAL parameter - } else if (emcmotStatus->spindle.speed < -100) { - emcmotStatus->spindle.speed += 100; + if (emcmotStatus->spindle_cmd.velocity_rpm_out > 100) { + emcmotStatus->spindle_cmd.velocity_rpm_out -= 100; //FIXME - make the step a HAL parameter + } else if (emcmotStatus->spindle_cmd.velocity_rpm_out < -100) { + emcmotStatus->spindle_cmd.velocity_rpm_out += 100; } break; case EMCMOT_SPINDLE_BRAKE_ENGAGE: rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_BRAKE_ENGAGE"); - emcmotStatus->spindle.speed = 0; - emcmotStatus->spindle.direction = 0; - emcmotStatus->spindle.brake = 1; + emcmotStatus->spindle_cmd.velocity_rpm_out = 0; + emcmotStatus->spindle_cmd.brake = 1; break; case EMCMOT_SPINDLE_BRAKE_RELEASE: rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_BRAKE_RELEASE"); - emcmotStatus->spindle.brake = 0; + emcmotStatus->spindle_cmd.brake = 0; break; case EMCMOT_SET_JOINT_COMP: diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 1d9ff7d574f..bf2da9416e9 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -391,12 +391,12 @@ static void process_inputs(void) unsigned char enables; /* read spindle angle (for threading, etc) */ - double prev_revs = emcmotStatus->spindleRevs; - emcmotStatus->spindleRevs = *emcmot_hal_data->spindle_revs; - *emcmot_hal_data->spindle_speed_in_estimate = backward_difference(emcmotStatus->spindleRevs, + double prev_revs = emcmotStatus->spindle_fb.position_rev; + emcmotStatus->spindle_fb.position_rev = *emcmot_hal_data->spindle_revs; + *emcmot_hal_data->spindle_speed_in_estimate = backward_difference(emcmotStatus->spindle_fb.position_rev, prev_revs, emcmotConfig->trajCycleTime / 60.0); - emcmotStatus->spindleSpeedIn = *emcmot_hal_data->spindle_speed_in; + emcmotStatus->spindle_fb.velocity_rpm = *emcmot_hal_data->spindle_speed_in; emcmotStatus->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed; /* compute net feed and spindle scale factors */ if ( emcmotStatus->motion_state == EMCMOT_MOTION_COORD ) { @@ -536,19 +536,19 @@ static void process_inputs(void) // signal error, and cancel the orient if (*(emcmot_hal_data->spindle_orient)) { if (*(emcmot_hal_data->spindle_orient_fault)) { - emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_FAULTED; + emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_FAULTED; *(emcmot_hal_data->spindle_orient) = 0; - emcmotStatus->spindle.orient_fault = *(emcmot_hal_data->spindle_orient_fault); - reportError(_("fault %d during orient in progress"), emcmotStatus->spindle.orient_fault); + emcmotStatus->spindle_cmd.orient_fault = *(emcmot_hal_data->spindle_orient_fault); + reportError(_("fault %d during orient in progress"), emcmotStatus->spindle_cmd.orient_fault); emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; tpAbort(&emcmotDebug->tp); SET_MOTION_ERROR_FLAG(1); } else if (*(emcmot_hal_data->spindle_is_oriented)) { *(emcmot_hal_data->spindle_orient) = 0; *(emcmot_hal_data->spindle_locked) = 1; - emcmotStatus->spindle.locked = 1; - emcmotStatus->spindle.brake = 1; - emcmotStatus->spindle.orient_state = EMCMOT_ORIENT_COMPLETE; + emcmotStatus->spindle_cmd.locked = 1; + emcmotStatus->spindle_cmd.brake = 1; + emcmotStatus->spindle_cmd.orient_state = EMCMOT_ORIENT_COMPLETE; rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT complete, spindle locked"); } } @@ -1725,16 +1725,16 @@ static void output_to_hal(void) *(emcmot_hal_data->teleop_mode) = GET_MOTION_TELEOP_FLAG(); *(emcmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG(); *(emcmot_hal_data->on_soft_limit) = emcmotStatus->on_soft_limit; - if(emcmotStatus->spindle.css_factor) { - double denom = fabs(emcmotStatus->spindle.xoffset - emcmotStatus->carte_pos_cmd.tran.x); + if(emcmotStatus->spindle_cmd.css_factor) { + double denom = fabs(emcmotStatus->spindle_cmd.xoffset - emcmotStatus->carte_pos_cmd.tran.x); double speed; double maxpositive; - if(denom > 0) speed = emcmotStatus->spindle.css_factor / denom; - else speed = emcmotStatus->spindle.speed; + if(denom > 0) speed = emcmotStatus->spindle_cmd.css_factor / denom; + else speed = emcmotStatus->spindle_cmd.velocity_rpm_out; speed = speed * emcmotStatus->net_spindle_scale; - maxpositive = fabs(emcmotStatus->spindle.speed); + maxpositive = fabs(emcmotStatus->spindle_cmd.velocity_rpm_out); // cap speed to G96 D... if(speed < -maxpositive) speed = -maxpositive; @@ -1744,16 +1744,16 @@ static void output_to_hal(void) *(emcmot_hal_data->spindle_speed_out) = speed; *(emcmot_hal_data->spindle_speed_out_rps) = speed/60.; } else { - *(emcmot_hal_data->spindle_speed_out) = emcmotStatus->spindle.speed * emcmotStatus->net_spindle_scale; - *(emcmot_hal_data->spindle_speed_out_rps) = emcmotStatus->spindle.speed * emcmotStatus->net_spindle_scale / 60.; + *(emcmot_hal_data->spindle_speed_out) = emcmotStatus->spindle_cmd.velocity_rpm_out * emcmotStatus->net_spindle_scale; + *(emcmot_hal_data->spindle_speed_out_rps) = emcmotStatus->spindle_cmd.velocity_rpm_out * emcmotStatus->net_spindle_scale / 60.; } *(emcmot_hal_data->spindle_speed_out_abs) = fabs(*(emcmot_hal_data->spindle_speed_out)); *(emcmot_hal_data->spindle_speed_out_rps_abs) = fabs(*(emcmot_hal_data->spindle_speed_out_rps)); - *(emcmot_hal_data->spindle_speed_cmd_rps) = emcmotStatus->spindle.speed / 60.; - *(emcmot_hal_data->spindle_on) = ((emcmotStatus->spindle.speed * emcmotStatus->net_spindle_scale) != 0) ? 1 : 0; + *(emcmot_hal_data->spindle_speed_cmd_rps) = emcmotStatus->spindle_cmd.velocity_rpm_out / 60.; + *(emcmot_hal_data->spindle_on) = ((emcmotStatus->spindle_cmd.velocity_rpm_out * emcmotStatus->net_spindle_scale) != 0) ? 1 : 0; *(emcmot_hal_data->spindle_forward) = (*emcmot_hal_data->spindle_speed_out > 0) ? 1 : 0; *(emcmot_hal_data->spindle_reverse) = (*emcmot_hal_data->spindle_speed_out < 0) ? 1 : 0; - *(emcmot_hal_data->spindle_brake) = (emcmotStatus->spindle.brake != 0) ? 1 : 0; + *(emcmot_hal_data->spindle_brake) = (emcmotStatus->spindle_cmd.brake != 0) ? 1 : 0; *(emcmot_hal_data->program_line) = emcmotStatus->id; *(emcmot_hal_data->motion_type) = emcmotStatus->motionType; @@ -1786,22 +1786,22 @@ static void output_to_hal(void) emcmot_hal_data->debug_bit_0 = joints[1].free_tp_active; emcmot_hal_data->debug_bit_1 = emcmotStatus->enables_new & AF_ENABLED; emcmot_hal_data->debug_float_0 = emcmotStatus->net_feed_scale; - emcmot_hal_data->debug_float_1 = emcmotStatus->spindleRevs; - emcmot_hal_data->debug_float_2 = emcmotStatus->spindleSpeedIn; + emcmot_hal_data->debug_float_1 = emcmotStatus->spindle_fb.position_rev; + emcmot_hal_data->debug_float_2 = emcmotStatus->spindle_fb.velocity_rpm; emcmot_hal_data->debug_float_3 = emcmotStatus->net_spindle_scale; emcmot_hal_data->debug_s32_0 = emcmotStatus->overrideLimitMask; emcmot_hal_data->debug_s32_1 = emcmotStatus->tcqlen; /* two way handshaking for the spindle encoder */ - if(emcmotStatus->spindle_index_enable && !old_motion_index) { + if(emcmotStatus->spindle_fb.index_enable && !old_motion_index) { *emcmot_hal_data->spindle_index_enable = 1; } if(!*emcmot_hal_data->spindle_index_enable && old_hal_index) { - emcmotStatus->spindle_index_enable = 0; + emcmotStatus->spindle_fb.index_enable = 0; } - old_motion_index = emcmotStatus->spindle_index_enable; + old_motion_index = emcmotStatus->spindle_fb.index_enable; old_hal_index = *emcmot_hal_data->spindle_index_enable; *(emcmot_hal_data->tooloffset_x) = emcmotStatus->tool_offset.tran.x; diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index e1cefcdce7e..dfa0a29d0d3 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -909,7 +909,7 @@ static int init_comm_buffers(void) emcmotStatus->activeDepth = 0; emcmotStatus->paused = 0; emcmotStatus->overrideLimitMask = 0; - emcmotStatus->spindle.speed = 0.0; + emcmotStatus->spindle_cmd.velocity_rpm_out = 0.0; SET_MOTION_INPOS_FLAG(1); SET_MOTION_ENABLE_FLAG(0); emcmotConfig->kinematics_type = kinType; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 5c5d7c93b5b..1dd2d01572b 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -544,17 +544,21 @@ Suggestion: Split this in to an Error and a Status flag register.. double big_vel; /* used for "debouncing" velocity */ } emcmot_joint_t; -/* This structure contains only the "status" data associated with - a joint. "Status" data is that data that should be reported to - user space on a continuous basis. An array of these structs is - part of the main status structure, and is filled in with data - copied from the emcmot_joint_t structs every servo period. - - For now this struct contains more data than it really needs, but - paring it down will take time (and probably needs to be done one - or two items at a time, with much testing). My main goal right - now is to get get the large joint struct out of status. - +typedef enum { + SPINDLE_REVERSE=-1, + SPINDLE_STOPPED=0, + SPINDLE_FORWARD=1 +} spindle_direction_code_t; + +/** + * This structure contains only the "status" data associated with a joint. + * "Status" data is that data that should be reported to user space on a + * continuous basis. An array of these structs is part of the main status + * structure, and is filled in with data copied from the emcmot_joint_t structs + * every servo period. For now this struct contains more data than it really + * needs, but paring it down will take time (and probably needs to be done one + * or two items at a time, with much testing). My main goal right now is to + * get get the large joint struct out of status. */ typedef struct { @@ -580,16 +584,21 @@ Suggestion: Split this in to an Error and a Status flag register.. typedef struct { - double speed; // spindle speed in RPMs + double velocity_rpm_out; double css_factor; double xoffset; - int direction; // 0 stopped, 1 forward, -1 reverse int brake; // 0 released, 1 engaged int locked; // spindle lock engaged after orient int orient_fault; // fault code from motion.spindle-orient-fault int orient_state; // orient_state_t - } spindle_status; + } spindle_cmd_status; + typedef struct { + double position_rev; + double velocity_rpm; + int index_enable; /* hooked to a canon encoder index-enable */ + int synced; /* we are doing spindle-synced motion */ + } spindle_fb_status; /********************************* STATUS STRUCTURE @@ -647,12 +656,9 @@ Suggestion: Split this in to an Error and a Status flag register.. unsigned char probe_type; EmcPose probedPos; /* Axis positions stored as soon as possible after last probeTripped */ - int spindle_index_enable; /* hooked to a canon encoder index-enable */ - int spindleSync; /* we are doing spindle-synced motion */ - double spindleRevs; /* position of spindle in revolutions */ - double spindleSpeedIn; /* velocity of spindle in revolutions per minute */ - spindle_status spindle; /* data types for spindle status */ + spindle_cmd_status spindle_cmd; /* Spindle command output from motion */ + spindle_fb_status spindle_fb; /* Spindle feedback input to motion */ int synch_di[EMCMOT_MAX_DIO]; /* inputs to the motion controller, queried by g-code */ int synch_do[EMCMOT_MAX_DIO]; /* outputs to the motion controller, queried by g-code */ diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 0e9a8cf46d5..f7cd1d4685d 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1395,7 +1395,7 @@ int emcSpindleAbort(void) int emcSpindleSpeed(double speed, double css_factor, double offset) { - if (emcmotStatus.spindle.speed == 0) + if (emcmotStatus.spindle_cmd.velocity_rpm_out == 0) return 0; //spindle stopped, not updating speed return emcSpindleOn(speed, css_factor, offset); @@ -1505,12 +1505,21 @@ int emcMotionUpdate(EMC_MOTION_STAT * stat) stat->echo_serial_number = localMotionEchoSerialNumber; stat->debug = emcmotConfig.debug; - stat->spindle.enabled = emcmotStatus.spindle.speed != 0; - stat->spindle.speed = emcmotStatus.spindle.speed; - stat->spindle.brake = emcmotStatus.spindle.brake; - stat->spindle.direction = emcmotStatus.spindle.direction; - stat->spindle.orient_state = emcmotStatus.spindle.orient_state; - stat->spindle.orient_fault = emcmotStatus.spindle.orient_fault; + stat->spindle.enabled = emcmotStatus.spindle_cmd.velocity_rpm_out != 0; + stat->spindle.speed = emcmotStatus.spindle_cmd.velocity_rpm_out; + stat->spindle.brake = emcmotStatus.spindle_cmd.brake; + + // Report spindle direction based on commanded velocity + if (stat->spindle.speed > 0.0) { + stat->spindle.direction = SPINDLE_FORWARD; + } else if (stat->spindle.speed < 0.0){ + stat->spindle.direction = SPINDLE_REVERSE; + } else { + stat->spindle.direction = SPINDLE_STOPPED; + } + + stat->spindle.orient_state = emcmotStatus.spindle_cmd.orient_state; + stat->spindle.orient_fault = emcmotStatus.spindle_cmd.orient_fault; for (dio = 0; dio < EMC_MAX_DIO; dio++) { stat->synch_di[dio] = emcmotStatus.synch_di[dio]; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 80b0d916084..43b3d715c1f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -419,7 +419,7 @@ int tpClear(TP_STRUCT * const tp) tp->pausing = 0; tp->synchronized = 0; tp->uu_per_rev = 0.0; - emcmotStatus->spindleSync = 0; + emcmotStatus->spindle_fb.synced = 0; emcmotStatus->current_vel = 0.0; emcmotStatus->requested_vel = 0.0; emcmotStatus->distance_to_go = 0.0; @@ -2337,15 +2337,15 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, TC_STRUCT * const tc) { static double old_spindlepos; - double new_spindlepos = emcmotStatus->spindleRevs; - if (emcmotStatus->spindle.direction < 0) new_spindlepos = -new_spindlepos; + double new_spindlepos = emcmotStatus->spindle_fb.position_rev; + if (emcmotStatus->spindle_cmd.velocity_rpm < 0) new_spindlepos = -new_spindlepos; switch (tc->coords.rigidtap.state) { case TAPPING: tc_debug_print("TAPPING\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { // command reversal - emcmotStatus->spindle.speed *= -1.0; + emcmotStatus->spindle.velocity_rpm *= -1.0; tc->coords.rigidtap.state = REVERSING; } break; @@ -2374,7 +2374,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, case RETRACTION: tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { - emcmotStatus->spindle.speed *= -1; + emcmotStatus->spindle.velocity_rpm *= -1; tc->coords.rigidtap.state = FINAL_REVERSAL; } break; @@ -2577,7 +2577,7 @@ STATIC int tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, tp->synchronized = 0; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; - emcmotStatus->spindleSync = 0; + emcmotStatus->spindle_fb.synced = 0; tpResume(tp); return TP_ERR_STOPPED; } //FIXME consistent error codes @@ -2622,12 +2622,12 @@ STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) } if (MOTION_ID_VALID(tp->spindle.waiting_for_index)) { - if (emcmotStatus->spindle_index_enable) { + if (emcmotStatus->spindle_fb.index_enable) { /* haven't passed index yet */ return TP_ERR_WAITING; } else { /* passed index, start the move */ - emcmotStatus->spindleSync = 1; + emcmotStatus->spindle_fb.synced = 1; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tc->sync_accel = 1; tp->spindle.revs = 0; @@ -2678,7 +2678,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { // Do at speed checks that only happen once int needs_atspeed = tc->atspeed || - (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindleSync)); + (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindle_fb.synced)); if ( needs_atspeed && !(emcmotStatus->spindle_is_atspeed)) { tp->spindle.waiting_for_atspeed = tc->id; @@ -2709,12 +2709,12 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { tc->blending_next = 0; tc->on_final_decel = 0; - if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { + if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindle_fb.synced)) { tp_debug_print(" Setting up position sync\n"); // if we aren't already synced, wait tp->spindle.waiting_for_index = tc->id; // ask for an index reset - emcmotStatus->spindle_index_enable = 1; + emcmotStatus->spindle_fb.index_enable = 1; tp->spindle.offset = 0.0; rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync...\n"); return TP_ERR_WAITING; @@ -2729,7 +2729,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { * Update requested velocity to follow the spindle's velocity (scaled by feed rate). */ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT const * nexttc) { - double speed = emcmotStatus->spindleSpeedIn; + double speed = emcmotStatus->spindle_fb.velocity_rpm; double pos_error = fabs(speed) * tc->uu_per_rev; // Account for movement due to parabolic blending with next segment if(nexttc) { @@ -2746,8 +2746,8 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc ) { - double spindle_pos = getSpindleProgress(emcmotStatus->spindleRevs, - emcmotStatus->spindle.direction); + double spindle_pos = getSpindleProgress(emcmotStatus->spindle_fb.position_rev, + emcmotStatus->spindle_cmd.direction); tc_debug_print("Spindle at %f\n",spindle_pos); if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || @@ -2769,7 +2769,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, } tc_debug_print("\n"); - const double avg_spindle_vel = emcmotStatus->spindleSpeedIn / 60.0; //KLUDGE convert to rps + const double avg_spindle_vel = emcmotStatus->spindle_fb.velocity_rpm / 60.0; //KLUDGE convert to rps if(tc->sync_accel) { // detect when velocities match, and move the target accordingly. // acceleration will abruptly stop and we will be on our new target. @@ -3248,7 +3248,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period) * spindle motion.*/ switch (tc->synchronized) { case TC_SYNC_NONE: - emcmotStatus->spindleSync = 0; + emcmotStatus->spindle_fb.synced = 0; break; case TC_SYNC_VELOCITY: tc_debug_print("sync velocity\n"); From 2da063ea9e785059e04494c4778b6729bbbff93f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 16:28:18 -0500 Subject: [PATCH 19/32] tp: Remove discontinuous spindle positions in rigid tapping internals. Rigid tapping used to estimate spindle position both from the measured spindle revs, and the commanded direction. This approach is numerically dangerous, however, because the spindle position itself could be large. Therefore, flipping the sign on the position would cause an apparent jump. The previous approach to rigid tapping accounted for this. However, this new approach should be cleaner and simpler. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tp.c | 38 +++++++++++++++++++++++++++----------- 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 43b3d715c1f..72bffdcdf07 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2327,6 +2327,25 @@ void tpToggleDIOs(TC_STRUCT * const tc) { } +/** + * Helper function to compare commanded and actual spindle velocity. + * If the signs of velocity don't match, then the spindle is reversing direction. + */ +static inline bool spindleReversing() +{ + return (signum(emcmotStatus->spindle_fb.velocity_rpm) != signum(emcmotStatus->spindle_cmd.velocity_rpm_out)); +} + +static inline bool cmdReverseSpindle() +{ + static bool reversed = false; + // Flip sign on commanded velocity + emcmotStatus->spindle_cmd.velocity_rpm_out *= -1; + // (Optional) set an internal flag so we know if the spindle is reversed from the user command + reversed = !reversed; + return reversed; +} + /** * Handle special cases for rigid tapping. * This function deals with updating the goal position and spindle position @@ -2336,26 +2355,24 @@ void tpToggleDIOs(TC_STRUCT * const tc) { STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, TC_STRUCT * const tc) { - static double old_spindlepos; - double new_spindlepos = emcmotStatus->spindle_fb.position_rev; - if (emcmotStatus->spindle_cmd.velocity_rpm < 0) new_spindlepos = -new_spindlepos; + double spindle_pos = emcmotStatus->spindle_fb.position_rev; switch (tc->coords.rigidtap.state) { case TAPPING: tc_debug_print("TAPPING\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { // command reversal - emcmotStatus->spindle.velocity_rpm *= -1.0; + cmdReverseSpindle(); tc->coords.rigidtap.state = REVERSING; } break; case REVERSING: tc_debug_print("REVERSING\n"); - if (new_spindlepos < old_spindlepos) { + if (spindleReversing()) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; // we've stopped, so set a new target at the original position - tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + tp->spindle.offset; + tc->coords.rigidtap.spindlerevs_at_reversal = spindle_pos + tp->spindle.offset; pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; @@ -2368,19 +2385,19 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, tc->coords.rigidtap.state = RETRACTION; } - old_spindlepos = new_spindlepos; - tc_debug_print("Spindlepos = %f\n", new_spindlepos); + tc_debug_print("Spindlepos = %f\n", spindle_pos); break; case RETRACTION: tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { - emcmotStatus->spindle.velocity_rpm *= -1; + // Flip spindle direction againt to start final reversal + cmdReverseSpindle(); tc->coords.rigidtap.state = FINAL_REVERSAL; } break; case FINAL_REVERSAL: tc_debug_print("FINAL_REVERSAL\n"); - if (new_spindlepos > old_spindlepos) { + if (spindleReversing()) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; pmCartLinePoint(aux, tc->progress, &start); @@ -2394,7 +2411,6 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, tc->coords.rigidtap.state = FINAL_PLACEMENT; } - old_spindlepos = new_spindlepos; break; case FINAL_PLACEMENT: tc_debug_print("FINAL_PLACEMENT\n"); From 87e480a97ce3a1caf9fdbdfe7a18d0a2287ef3e8 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 19:39:40 -0500 Subject: [PATCH 20/32] tp: Fix spindle position tracking to work with motion's new spindle data. Signed-off-by: Robert W. Ellenberg --- src/emc/tp/tc_types.h | 4 +- src/emc/tp/tp.c | 95 ++++++++++++++++++++++++++++++------------- src/emc/tp/tp_types.h | 1 - 3 files changed, 68 insertions(+), 32 deletions(-) diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 164ce0f9609..3d3fbac770c 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -106,7 +106,7 @@ typedef struct { PmCartesian abc; PmCartesian uvw; double reversal_target; - double spindlerevs_at_reversal; + double spindlepos_at_reversal; RIGIDTAP_STATE state; } PmRigidTap; @@ -116,7 +116,7 @@ typedef struct { double target; // actual segment length double progress; // where are we in the segment? 0..target double nominal_length; - double sync_offset; // When did we sync up with the spindle? + double progress_at_sync; // When did we sync up with the spindle? //Velocity double reqvel; // vel requested by F word, calc'd by task diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 72bffdcdf07..4fc8e302075 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -451,7 +451,6 @@ int tpInit(TP_STRUCT * const tp) tp->wDotMax = 0.0; tp->spindle.offset = 0.0; - tp->spindle.revs = 0.0; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; @@ -2326,6 +2325,35 @@ void tpToggleDIOs(TC_STRUCT * const tc) { } } +//KLUDGE compine this with the version in taskintf +static inline spindle_direction_code_t getSpindleCmdDir() +{ + if (emcmotStatus->spindle_cmd.velocity_rpm_out > 0.0) { + return SPINDLE_FORWARD; + } else if (emcmotStatus->spindle_cmd.velocity_rpm_out < 0.0) { + return SPINDLE_REVERSE; + } else { + return SPINDLE_STOPPED; + } +} + +static inline double findSpindleDisplacement(double new_pos, + double old_pos, + spindle_direction_code_t spindle_cmd_dir) +{ + // Difference assuming spindle "forward" direction + double forward_diff = new_pos - old_pos; + + switch(spindle_cmd_dir) { + case SPINDLE_STOPPED: + case SPINDLE_FORWARD: + return forward_diff; + case SPINDLE_REVERSE: + return -forward_diff; + } + //no default on purpose, compiler should warn on missing states + return 0; +} /** * Helper function to compare commanded and actual spindle velocity. @@ -2368,11 +2396,11 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, break; case REVERSING: tc_debug_print("REVERSING\n"); - if (spindleReversing()) { + if (!spindleReversing()) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; // we've stopped, so set a new target at the original position - tc->coords.rigidtap.spindlerevs_at_reversal = spindle_pos + tp->spindle.offset; + tc->coords.rigidtap.spindlepos_at_reversal = spindle_pos; pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; @@ -2397,7 +2425,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, break; case FINAL_REVERSAL: tc_debug_print("FINAL_REVERSAL\n"); - if (spindleReversing()) { + if (!spindleReversing()) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; pmCartLinePoint(aux, tc->progress, &start); @@ -2544,7 +2572,7 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, // spindle position so the next synced move can be in // the right place. if(tc->synchronized != TC_SYNC_NONE) { - tp->spindle.offset += (tc->target - tc->sync_offset) / tc->uu_per_rev; + tp->spindle.offset += (tc->target - tc->progress_at_sync) / tc->uu_per_rev; } else { tp->spindle.offset = 0.0; } @@ -2646,7 +2674,6 @@ STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) emcmotStatus->spindle_fb.synced = 1; tp->spindle.waiting_for_index = MOTION_INVALID_ID; tc->sync_accel = 1; - tp->spindle.revs = 0; } } @@ -2762,53 +2789,63 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc ) { - double spindle_pos = getSpindleProgress(emcmotStatus->spindle_fb.position_rev, - emcmotStatus->spindle_cmd.direction); - tc_debug_print("Spindle at %f\n",spindle_pos); + // Start with raw spindle position and our saved offset + double spindle_pos = emcmotStatus->spindle_fb.position_rev; + // If we're backing out of a hole during rigid tapping, our spindle "displacement" is + // measured relative to spindle position at the bottom of the hole. + // Otherwise, we use the stored spindle offset. + double local_spindle_offset = tp->spindle.offset; if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || tc->coords.rigidtap.state == FINAL_REVERSAL)) { - tp->spindle.revs = tc->coords.rigidtap.spindlerevs_at_reversal - - spindle_pos; - } else { - tp->spindle.revs = spindle_pos; + local_spindle_offset = tc->coords.rigidtap.spindlepos_at_reversal; } - double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev; + // Note that this quantity should be non-negative under normal conditions. + double spindle_displacement = findSpindleDisplacement(spindle_pos, + local_spindle_offset, + getSpindleCmdDir()); - double pos_error = pos_desired - tc->progress; - tc_debug_print(" pos_desired %f, progress %f, pos_error %f", pos_desired, tc->progress, pos_error); + tc_debug_print("spindle_displacement %f raw_pos %f", spindle_displacement, spindle_pos); - if(nexttc) { - tc_debug_print(" nexttc_progress %f", nexttc->progress); - pos_error -= nexttc->progress; - } - tc_debug_print("\n"); - - const double avg_spindle_vel = emcmotStatus->spindle_fb.velocity_rpm / 60.0; //KLUDGE convert to rps + const double spindle_vel = emcmotStatus->spindle_fb.velocity_rpm / 60.0; if(tc->sync_accel) { // detect when velocities match, and move the target accordingly. // acceleration will abruptly stop and we will be on our new target. // FIX: this is driven by TP cycle time, not the segment cycle time // Experiment: try syncing with averaged spindle speed - double avg_target_vel = avg_spindle_vel * tc->uu_per_rev; - if(tc->currentvel >= avg_target_vel) { + double target_vel = spindle_vel * tc->uu_per_rev; + if(tc->currentvel >= target_vel) { tc_debug_print("Hit accel target in pos sync\n"); // move target so as to drive pos_error to 0 next cycle - tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev; - tc->sync_offset = tc->progress; + tp->spindle.offset = spindle_pos; + tc->progress_at_sync = tc->progress; tc_debug_print("Spindle offset %f\n", tp->spindle.offset); tc->sync_accel = 0; - tc->target_vel = avg_target_vel; + tc->target_vel = target_vel; } else { 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; } } else { + // Multiply by user feed rate to get equivalent desired position + double pos_desired = spindle_displacement * tc->uu_per_rev; + double pos_error = pos_desired - (tc->progress - tc->progress_at_sync); + tc_debug_print(" pos_desired %f, progress %f, pos_error %f", pos_desired, tc->progress, pos_error); + + if(nexttc) { + // If we're in a parabolic blend, the next segment will be active too, + // so make sure to account for its progress + tc_debug_print(" nexttc_progress %f", nexttc->progress); + pos_error -= nexttc->progress; + } + tc_debug_print("\n"); + + // we have synced the beginning of the move as best we can - // track position (minimize pos_error). - double target_vel = avg_spindle_vel * tc->uu_per_rev; + double target_vel = spindle_vel * tc->uu_per_rev; /* * Correct for position errors when tracking spindle motion. diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index cb272e251e3..56607dc0730 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -77,7 +77,6 @@ typedef enum { */ typedef struct { double offset; - double revs; int waiting_for_index; int waiting_for_atspeed; } tp_spindle_t; From 52fad91df74030c9663159b6a729fb7a86dd0e24 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 14 Dec 2016 23:58:47 -0500 Subject: [PATCH 21/32] canon: actually return angular feed rate when asked Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 08d9de959cc..bf533f122e4 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -502,7 +502,7 @@ static double getActiveFeedRate(FeedRateType angular) if (!angular) { return synched ? FROM_PROG_LEN(uu_per_sec) : currentLinearFeedRate; } else { - return synched ? FROM_PROG_ANG(uu_per_sec) : currentLinearFeedRate; + return synched ? FROM_PROG_ANG(uu_per_sec) : currentAngularFeedRate; } } From 70ad411eb41cb28b7716d0329d259d1c90cc9e4a Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 13:50:54 -0500 Subject: [PATCH 22/32] test: Test case for steady state error Signed-off-by: Robert W. Ellenberg --- .../circular-arcs/nc_files/spindle/g33_blend.ngc | 5 +++-- .../circular-arcs/nc_files/spindle/g33_simple.ngc | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc index e454a777e2d..40847f81382 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_blend.ngc @@ -1,5 +1,6 @@ (From TP Issue #68) -G20 G64 G8 G18 +G20 G8 G18 +G64 P0.01 Q0.0 M3 S400 G0 X2.0 Z0 G0 X1.1 Z0.1 @@ -8,8 +9,8 @@ G1 X1.0 Z0.0 F20 (No blend here - switch to position sync mode) G33 K0.1 X1.0 Z-0.5 (Tangent blend during position sync) +G33 K0.1 X1.0 Z-0.75 G33 K0.1 X1.0 Z-1.0 -/G64 P0.002 Q0.001 (Arc blend here during position sync) G33 K0.1 X0.800 Z-1.5 (Arc blend here during position sync) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc index fb9336ab30f..a7aa197e668 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/g33_simple.ngc @@ -1,7 +1,7 @@ G90 G20 G64 (absolute distance mode) G0 X1 Z0.1 (rapid to position) -S100 M3 (start spindle turning) -G33 Z-2 K0.025 +S321 M3 (start spindle turning) +G33 Z-2 K0.05 G0 X1.25 (rapid move tool away from work) Z0.1 (rapid move to starting Z position) M9 From c0103121f820b272e6274ed7490e4b616b787188 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 23 Jan 2017 13:23:58 -0500 Subject: [PATCH 23/32] test: Match sam's lathe config settings (axis vel / acc). Signed-off-by: Robert W. Ellenberg --- tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini index 1185e66118a..877eb86e91c 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini @@ -183,8 +183,8 @@ HOME_SEQUENCE = 0 TYPE = LINEAR HOME = 0.0 # Make Z accel and max vel really slow to highlight issues with helices -MAX_VELOCITY = .01 -MAX_ACCELERATION = .1 +MAX_VELOCITY = .5 +MAX_ACCELERATION = 10 BACKLASH = 0.0 INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 From b606adc15026b0ed8d70bec57fa317bfe07d9e9e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 14:19:44 -0500 Subject: [PATCH 24/32] Re-arrange a calculation to remove a division Signed-off-by: Robert W. Ellenberg --- src/emc/motion/control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index bf2da9416e9..b53a5fd29f4 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -395,7 +395,7 @@ static void process_inputs(void) emcmotStatus->spindle_fb.position_rev = *emcmot_hal_data->spindle_revs; *emcmot_hal_data->spindle_speed_in_estimate = backward_difference(emcmotStatus->spindle_fb.position_rev, prev_revs, - emcmotConfig->trajCycleTime / 60.0); + emcmotConfig->trajCycleTime) * 60.0; emcmotStatus->spindle_fb.velocity_rpm = *emcmot_hal_data->spindle_speed_in; emcmotStatus->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed; /* compute net feed and spindle scale factors */ From f8b0f0948bdd3c0c6baf19f228b96ad070699783 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 00:17:34 -0500 Subject: [PATCH 25/32] motion: Add a HAL pin to motion to let users control how aggressive position tracking should be. Currently, we correct position errors in spindle-sync as aggressively as possible, given machine acceleration limits. This new HAL pin lets the user control how aggressive it should be. The valid range is [0.0,1.0], where 0.0 means no position tracking (pure velocity). 1.0 is equivalent to 2.x position tracking. In some cases, reducing the gain will greatly shrink the acceleration jitter, without appreciably affecting position tracking performance. Signed-off-by: Robert W. Ellenberg --- src/emc/motion/control.c | 3 +++ src/emc/motion/mot_priv.h | 1 + src/emc/motion/motion.c | 4 ++++ src/emc/motion/motion.h | 1 + src/emc/tp/tp.c | 2 +- 5 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b53a5fd29f4..15b491187b2 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -398,6 +398,9 @@ static void process_inputs(void) emcmotConfig->trajCycleTime) * 60.0; emcmotStatus->spindle_fb.velocity_rpm = *emcmot_hal_data->spindle_speed_in; emcmotStatus->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed; + // Minimum gain is zero (no position error correction), maximum gain is 1 (correct at maximum acceleration) + emcmotStatus->spindle_tracking_gain = fmax(fmin(*emcmot_hal_data->spindle_tracking_gain, 1.0), 0.0); + /* compute net feed and spindle scale factors */ if ( emcmotStatus->motion_state == EMCMOT_MOTION_COORD ) { /* use the enables that were queued with the current move */ diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index f374db5a8ec..f450ab9a68e 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -90,6 +90,7 @@ typedef struct { hal_bit_t *spindle_is_atspeed; hal_float_t *spindle_revs; hal_bit_t *spindle_inhibit; /* RPI: set TRUE to stop spindle (non maskable)*/ + hal_float_t *spindle_tracking_gain; //!< Controls position tracking accuracy from least to most aggressive [0.0-1.0] hal_float_t *adaptive_feed; /* RPI: adaptive feedrate, 0.0 to 1.0 */ hal_bit_t *feed_hold; /* RPI: set TRUE to stop motion maskable with g53 P1*/ hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index dfa0a29d0d3..382ad810cd8 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -324,6 +324,10 @@ static int init_hal_io(void) if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error; *emcmot_hal_data->spindle_is_atspeed = 1; + + if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_tracking_gain), mot_comp_id, "motion.spindle-tracking-gain")) < 0) goto error; + *emcmot_hal_data->spindle_tracking_gain = 1.0; + if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; *(emcmot_hal_data->adaptive_feed) = 1.0; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 1dd2d01572b..66e404a551e 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -659,6 +659,7 @@ typedef enum { spindle_cmd_status spindle_cmd; /* Spindle command output from motion */ spindle_fb_status spindle_fb; /* Spindle feedback input to motion */ + double spindle_tracking_gain; // external control of position trakcing aggressiveness int synch_di[EMCMOT_MAX_DIO]; /* inputs to the motion controller, queried by g-code */ int synch_do[EMCMOT_MAX_DIO]; /* outputs to the motion controller, queried by g-code */ diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 4fc8e302075..3307b1ea982 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2880,7 +2880,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, */ double a_max = tpGetScaledAccel(tp, tc); // Make correction term non-negative - double v_sq = pmSq(target_vel) + pos_error * a_max; + double v_sq = pmSq(target_vel) + pos_error * a_max * emcmotStatus->spindle_tracking_gain; tc->target_vel = pmSqrt(fmax(v_sq, 0.0)); tc_debug_print("in position sync, target_vel = %f\n", tc->target_vel); } From 1100e0bb31c30f5d2adb786a5ebad028d08274c1 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 14:49:25 -0500 Subject: [PATCH 26/32] sim: model velocity quantization in encoder near 360RPM Signed-off-by: Robert W. Ellenberg --- lib/hallib/sim_spindle_encoder.hal | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/lib/hallib/sim_spindle_encoder.hal b/lib/hallib/sim_spindle_encoder.hal index 005c4c623b4..de8b128f6ac 100644 --- a/lib/hallib/sim_spindle_encoder.hal +++ b/lib/hallib/sim_spindle_encoder.hal @@ -5,7 +5,7 @@ setp sim_spindle.scale 0.01666667 loadrt limit2 names=limit_speed loadrt lowpass names=spindle_mass loadrt near names=near_speed -loadrt quant names=sim_spindle_encoder +loadrt quant names=sim_spindle_encoder,spindle_vel_est # Bound spindle acceleration to something reasonable setp limit_speed.maxv 5000.0 # rpm/second @@ -33,7 +33,11 @@ net spindle-speed-cmd motion.spindle-speed-out => limit_speed.in net spindle-speed-limited limit_speed.out => spindle_mass.in # Feed the simulated spindle speed into the sim spindle to generate a position signal -net spindle-rpm-filtered spindle_mass.out => sim_spindle.velocity-cmd motion.spindle-speed-in near_speed.in2 +net spindle-rpm-filtered spindle_mass.out => sim_spindle.velocity-cmd near_speed.in2 spindle_vel_est.in + +# Approximate velocity quantization assuming 360 RPM, 30kHz base thread, 100 PPR encoder +setp spindle_vel_est.resolution 8.0 +net spindle-rpm-est spindle_vel_est.out => motion.spindle-speed-in # at-speed detection setp near_speed.scale 1.1 @@ -47,3 +51,4 @@ addf spindle_mass servo-thread addf near_speed servo-thread addf sim_spindle servo-thread addf sim_spindle_encoder servo-thread +addf spindle_vel_est servo-thread From b5494722c4bea368992a57bb2b55aa4552517c71 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 14:20:21 -0500 Subject: [PATCH 27/32] tp: Allow switching between position-sync algorithms based on a HAL pin. This commit adds a new spindle tracking algorithm based on the trapezoidal motion profile. Particularly for high resolution encoders, this algorithm should have significantly reduced acceleration jitter. This also adds a (probably temporary) pin to HAL: motion.pos-tracking-mode: 0 (default) = new spindle tracking algorithm based on trapezoidal velocity calcs 1 = 2.7.x stock algorithm 2 = 2.7.x algorithm with a correction (here for completeness, but doesn't offer much over the trapezoidal method) Signed-off-by: Robert W. Ellenberg --- src/emc/motion/control.c | 1 + src/emc/motion/mot_priv.h | 1 + src/emc/motion/motion.c | 1 + src/emc/motion/motion.h | 1 + src/emc/tp/blendmath.c | 26 ++++++++++++ src/emc/tp/blendmath.h | 6 +++ src/emc/tp/tp.c | 87 ++++++++++++++++++--------------------- 7 files changed, 77 insertions(+), 46 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 15b491187b2..3745db091ce 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -400,6 +400,7 @@ static void process_inputs(void) emcmotStatus->spindle_is_atspeed = *emcmot_hal_data->spindle_is_atspeed; // Minimum gain is zero (no position error correction), maximum gain is 1 (correct at maximum acceleration) emcmotStatus->spindle_tracking_gain = fmax(fmin(*emcmot_hal_data->spindle_tracking_gain, 1.0), 0.0); + emcmotStatus->pos_tracking_mode = *emcmot_hal_data->pos_tracking_mode; /* compute net feed and spindle scale factors */ if ( emcmotStatus->motion_state == EMCMOT_MOTION_COORD ) { diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index f450ab9a68e..265a93c1c58 100644 --- a/src/emc/motion/mot_priv.h +++ b/src/emc/motion/mot_priv.h @@ -91,6 +91,7 @@ typedef struct { hal_float_t *spindle_revs; hal_bit_t *spindle_inhibit; /* RPI: set TRUE to stop spindle (non maskable)*/ hal_float_t *spindle_tracking_gain; //!< Controls position tracking accuracy from least to most aggressive [0.0-1.0] + hal_s32_t *pos_tracking_mode; hal_float_t *adaptive_feed; /* RPI: adaptive feedrate, 0.0 to 1.0 */ hal_bit_t *feed_hold; /* RPI: set TRUE to stop motion maskable with g53 P1*/ hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 382ad810cd8..86d5dfdc4d0 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -327,6 +327,7 @@ static int init_hal_io(void) if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_tracking_gain), mot_comp_id, "motion.spindle-tracking-gain")) < 0) goto error; *emcmot_hal_data->spindle_tracking_gain = 1.0; + if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->pos_tracking_mode), mot_comp_id, "motion.pos-tracking-mode")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; *(emcmot_hal_data->adaptive_feed) = 1.0; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 66e404a551e..f3f84aa256a 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -660,6 +660,7 @@ typedef enum { spindle_cmd_status spindle_cmd; /* Spindle command output from motion */ spindle_fb_status spindle_fb; /* Spindle feedback input to motion */ double spindle_tracking_gain; // external control of position trakcing aggressiveness + int pos_tracking_mode; int synch_di[EMCMOT_MAX_DIO]; /* inputs to the motion controller, queried by g-code */ int synch_do[EMCMOT_MAX_DIO]; /* outputs to the motion controller, queried by g-code */ diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 52ae4561c32..b64eb21ef5b 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1824,3 +1824,29 @@ double pmCircleEffectiveMinRadius(PmCircle const * const circle) effective_radius); return effective_radius; } + +double findTrapezoidalDesiredVel(double a_max, + double dx, + double v_final, + double v_current, + double cycle_time) +{ + double dt = fmax(cycle_time, TP_TIME_EPSILON); + // Discriminant is 3 terms (when final velocity is non-zero) + double discr_term1 = pmSq(v_final); + double discr_term2 = a_max * (2.0 * dx - v_current * dt); + double tmp_adt = a_max * dt * 0.5; + double discr_term3 = pmSq(tmp_adt); + double discr = discr_term1 + discr_term2 + discr_term3; + + //Start with -B/2 portion of quadratic formula + double maxnewvel = -tmp_adt; + + //If the discriminant term brings our velocity above zero, add it to the total + //We can ignore the calculation otherwise because negative velocities are clipped to zero + if (discr > discr_term3) { + maxnewvel += pmSqrt(discr); + } + + return maxnewvel; +} diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 1ce9e4934fa..742a2cc7b4b 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -148,6 +148,12 @@ int findAccelScale(PmCartesian const * const acc, PmCartesian const * const bounds, PmCartesian * const scale); +double findTrapezoidalDesiredVel(double a_max, + double dx, + double v_final, + double currentvel, + double cycle_time); + int pmCartCartParallel(PmCartesian const * const v1, PmCartesian const * const v2, double tol); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 3307b1ea982..ea90cf0472b 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2179,13 +2179,13 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); /* Debug Output */ - tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f\n", + tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f, ", tc_target_vel, tc_finalvel, tc->maxvel); - tc_debug_print(" currentvel = %f, fs = %f, tc = %f, term = %d\n", + tc_debug_print("currentvel = %f, fs = %f, dt = %f, term = %d, ", tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond); - tc_debug_print(" acc = %f,T = %f, P = %f\n", acc, + tc_debug_print("acc = %f, T = %f, P = %f, ", acc, tc->target, tc->progress); - tc_debug_print(" motion type %d\n", tc->motion_type); + tc_debug_print("motion_type = %d\n", tc->motion_type); if (tc->on_final_decel) { rtapi_print(" on final decel\n"); @@ -2223,30 +2223,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t double dx = tc->target - tc->progress; double maxaccel = tpGetScaledAccel(tp, tc); - double discr_term1 = pmSq(tc_finalvel); - double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time); - double tmp_adt = maxaccel * tc->cycle_time * 0.5; - double discr_term3 = pmSq(tmp_adt); - - double discr = discr_term1 + discr_term2 + discr_term3; - - // Descriminant is a little more complicated with final velocity term. If - // descriminant < 0, we've overshot (or are about to). Do the best we can - // in this situation -#ifdef TP_PEDANTIC - if (discr < 0.0) { - rtapi_print_msg(RTAPI_MSG_ERR, - "discriminant %f < 0 in velocity calculation!\n", discr); - } -#endif - //Start with -B/2 portion of quadratic formula - double maxnewvel = -tmp_adt; - - //If the discriminant term brings our velocity above zero, add it to the total - //We can ignore the calculation otherwise because negative velocities are clipped to zero - if (discr > discr_term3) { - maxnewvel += pmSqrt(discr); - } + double maxnewvel = findTrapezoidalDesiredVel(maxaccel, dx, tc_finalvel, tc->currentvel, tc->cycle_time); // Find bounded new velocity based on target velocity // Note that we use a separate variable later to check if we're on final decel @@ -2832,7 +2809,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, // Multiply by user feed rate to get equivalent desired position double pos_desired = spindle_displacement * tc->uu_per_rev; double pos_error = pos_desired - (tc->progress - tc->progress_at_sync); - tc_debug_print(" pos_desired %f, progress %f, pos_error %f", pos_desired, tc->progress, pos_error); + tc_debug_print(" pos_desired %f, progress %f", pos_desired, tc->progress); if(nexttc) { // If we're in a parabolic blend, the next segment will be active too, @@ -2840,12 +2817,12 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, tc_debug_print(" nexttc_progress %f", nexttc->progress); pos_error -= nexttc->progress; } - tc_debug_print("\n"); - + tc_debug_print(", pos_error %f\n", pos_error); // we have synced the beginning of the move as best we can - // track position (minimize pos_error). - double target_vel = spindle_vel * tc->uu_per_rev; + // This is the velocity we should be at when the position error is c0 + double v_final = spindle_vel * tc->uu_per_rev; /* * Correct for position errors when tracking spindle motion. @@ -2868,21 +2845,39 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, * momentarily increase the velocity, then decrease back to the nonimal * velocity. * - * The area under the "blip" is x_err, given the tracking velocity v_0 - * and maximum axis acceleration a_max. - * - * x_err = (v_0 + v_p) * t / 2 , t = 2 * (v_p - v_0) / a_max - * - * Substitute, rearrange and solve: - * - * v_p = sqrt( v_0^2 + x_err * a_max) - * + * In effect, this is the trapezoidal velocity planning problem, if: + * 1) remaining distance dx = x_err + * 2) "final" velocity = v_0 + * 3) max velocity / acceleration from motion segment */ - double a_max = tpGetScaledAccel(tp, tc); - // Make correction term non-negative - double v_sq = pmSq(target_vel) + pos_error * a_max * emcmotStatus->spindle_tracking_gain; - tc->target_vel = pmSqrt(fmax(v_sq, 0.0)); - tc_debug_print("in position sync, target_vel = %f\n", tc->target_vel); + double a_max = tpGetScaledAccel(tp, tc) * emcmotStatus->spindle_tracking_gain; + double v_max = tc->maxvel; + + switch(emcmotStatus->pos_tracking_mode) { + case 2: + { + double v_sq_alt = pmSq(v_final) + pos_error * a_max; + double v_target_alt = pmSqrt(fmax(v_sq_alt, 0.0)); + tc->target_vel = v_target_alt; + break; + } + case 1: + { + double v_sq = a_max * pos_error; + double v_target_stock = signum(v_sq) * pmSqrt(fabs(v_sq)) + v_final; + tc->target_vel = v_target_stock; + break; + } + case 0: + default: + { + double v_target_trapz = fmin(findTrapezoidalDesiredVel(a_max, pos_error, v_final, tc->currentvel, tc->cycle_time), v_max); + tc->target_vel = v_target_trapz; + break; + } + } + + tc_debug_print("in position sync, target_vel = %f, ideal_vel = %f, vel_err = %f\n", tc->target_vel, v_final, v_final - tc->target_vel); } //Finally, clip requested velocity at zero From a637aa9ff959fdd59a9793eee52fbefb6101a02f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 26 Jan 2017 21:42:59 -0500 Subject: [PATCH 28/32] canon: Simplify spindle speed limits for synced motion Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 50 +++++++++++++++++++++++----------------- 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index bf533f122e4..e1f3c454ceb 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -52,7 +52,8 @@ #define canon_debug(...) #endif -static void limitSpindleSpeed(double rpm); +static bool limitSpindleSpeed(double rpm); +static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel); static void resetSpindleLimit(); static double limit_rpm = 0; @@ -947,12 +948,7 @@ static void flush_segments(void) { canon_debug("in flush_segments: got vel %f, nominal_vel %f\n", vel, nominal_vel); vel = std::min(vel, nominal_vel); - if (synched && vel < nominal_vel) { - const static double SPINDLE_SYNCH_MARGIN = 0.05; - double maxSpindleRPM = vel / nominal_vel * (1.0 - SPINDLE_SYNCH_MARGIN) * spindleSpeed_rpm; - CANON_ERROR("In spindle-synchronized motion, maximum speed at line %d is %0.0f RPM", line_no, maxSpindleRPM); - limitSpindleSpeed(maxSpindleRPM); - } + limitSpindleSpeedFromVel(nominal_vel, vel); EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -1187,10 +1183,7 @@ void STRAIGHT_PROBE(int line_number, double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); vel = std::min(vel, nominal_vel); - if (synched && vel < nominal_vel) { - CANON_ERROR("In spindle-synchronized motion, can't maintain required feed %0.2f (max is %0.2f) with current settings", - TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); - } + limitSpindleSpeedFromVel(nominal_vel, vel); AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); acc = accdata.acc; @@ -1823,10 +1816,10 @@ void ARC_FEED(int line_number, // Limit velocity by maximum double nominal_vel = getActiveFeedRate(FEED_LINEAR); double vel = MIN(nominal_vel, v_max); - if (synched && v_max < nominal_vel) { - CANON_ERROR("Can't maintain required feed %g (max is %g) with current settings", - TO_EXT_LEN(nominal_vel) * 60.0, TO_EXT_LEN(vel) * 60.0); - } + + // Make sure spindle speed is within range (for spindle_sync motion only) + limitSpindleSpeedFromVel(nominal_vel, v_max); + canon_debug("current F = %f\n", nominal_vel); canon_debug("vel = %f\n",vel); @@ -1973,16 +1966,31 @@ static void resetSpindleLimit() * Hack way to immediately update the spindle speed without flushing segments first (typically called within flush segments as a way to limit spindle speed during threading). * @warning does NOT update the interpreter state. */ -static void limitSpindleSpeed(double rpm) +static bool limitSpindleSpeed(double rpm) { limit_rpm = fabs(rpm); - if (spindleSpeed_rpm > limit_rpm) { - EMC_SPINDLE_SPEED emc_spindle_speed_msg; - canon_debug("Reducing spindle speed to %0.0f\n",limit_rpm); + if (spindleSpeed_rpm <= limit_rpm) { + // spindle speed within range, do nothing + return false; + } + EMC_SPINDLE_SPEED emc_spindle_speed_msg; + canon_debug("Reducing spindle speed to %0.0f\n", limit_rpm); - buildSpindleCmd(emc_spindle_speed_msg, limit_rpm); - interp_list.append(emc_spindle_speed_msg); + buildSpindleCmd(emc_spindle_speed_msg, limit_rpm); + interp_list.append(emc_spindle_speed_msg); + return true; +} + +static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel) +{ + if (!synched || nominal_vel <= 0.0) { + return false; } + const static double SPINDLE_SYNCH_MARGIN = 0.05; + + // Scale down spindle RPM in proportion to the maximum allowed spindle velocity, with a safety factor + double maxSpindleRPM = spindleSpeed_rpm * max_vel / nominal_vel * (1.0 - SPINDLE_SYNCH_MARGIN); + return limitSpindleSpeed(maxSpindleRPM); } void STOP_SPINDLE_TURNING() From 3d02b641d9e5cddff107eac5815c34f7ebd08bf8 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 26 Jan 2017 22:02:04 -0500 Subject: [PATCH 29/32] canon: don't restore spindle speed after limiting it for spindle-synched motion. Feed / speed synch is always disabled after a synced segment (even if it's then re-enabled for the next one). Unfortunately, this means that limiting spindle RPM, and then restoring it, has the effect of inserting two spindle speed commands between each segment of synched motion. This completely disrupts blending, and affects the acceleration used during the motion (potentially affecting threading performance). A safer and easier approach is to pop up a nuisance message to the user informing them that the spindle speed will be limited, and then just leave it at the limited speed. Generally, the user will set a new spindle speed for the next operation anyway. Signed-off-by: Robert W. Ellenberg --- src/emc/task/emccanon.cc | 60 ++++++++++++++++++++++------------------ 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index e1f3c454ceb..d6b03d99eb9 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -54,8 +54,6 @@ static bool limitSpindleSpeed(double rpm); static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel); -static void resetSpindleLimit(); -static double limit_rpm = 0; static int debug_velacc = 0; static double css_maximum; // both always positive @@ -142,6 +140,7 @@ static void flush_segments(void); in the 6-axis canon.hh. So, we declare them here now. */ extern void CANON_ERROR(const char *fmt, ...) __attribute__((format(printf,1,2))); +extern void CANON_MSG(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Origin offsets, length units, and active plane are all maintained @@ -942,13 +941,13 @@ static void flush_segments(void) { #endif VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); - double vel = linedata.vel; + double ini_maxvel = linedata.vel; double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); canon_debug("in flush_segments: got vel %f, nominal_vel %f\n", vel, nominal_vel); - vel = std::min(vel, nominal_vel); + double vel = std::min(ini_maxvel, nominal_vel); - limitSpindleSpeedFromVel(nominal_vel, vel); + limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -968,7 +967,7 @@ static void flush_segments(void) { linearMoveMsg.end.c = TO_EXT_ANG(c); linearMoveMsg.vel = toExtVel(vel); - linearMoveMsg.ini_maxvel = toExtVel(linedata.vel); + linearMoveMsg.ini_maxvel = toExtVel(ini_maxvel); AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); double acc = lineaccdata.acc; linearMoveMsg.acc = toExtAcc(acc); @@ -1170,7 +1169,6 @@ void STRAIGHT_PROBE(int line_number, double u, double v, double w, unsigned char probe_type) { - double ini_maxvel, vel, acc; EMC_TRAJ_PROBE probeMsg; from_prog(x,y,z,a,b,c,u,v,w); @@ -1179,14 +1177,14 @@ void STRAIGHT_PROBE(int line_number, flush_segments(); VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); - ini_maxvel = vel = veldata.vel; + double ini_maxvel = veldata.vel; double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); - vel = std::min(vel, nominal_vel); - limitSpindleSpeedFromVel(nominal_vel, vel); + double vel = std::min(ini_maxvel, nominal_vel); + limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); - acc = accdata.acc; + double acc = accdata.acc; probeMsg.vel = toExtVel(vel); probeMsg.ini_maxvel = toExtVel(ini_maxvel); @@ -1284,13 +1282,6 @@ void STOP_SPEED_FEED_SYNCH() spindlesyncMsg.velocity_mode = false; interp_list.append(spindlesyncMsg); synched = 0; - // KLUDGE force restore the spindle speed after feed synch (in case we limited it - if (limit_rpm < spindleSpeed_rpm) { - // Restore spindle speed if we limited it - canon_debug("Restoring spindle speed to %0.0f after synched motion\n",spindleSpeed_rpm); - // KLUDGE if multiple threading segments are queued in succession, then this command will end up being redundant - resetSpindleLimit(); - } } /* Machining Functions */ @@ -1945,8 +1936,6 @@ void SET_SPINDLE_SPEED(double r) { // speed is in RPMs everywhere spindleSpeed_rpm = fabs(r); // interp will never send negative anyway ... - // KLUDGE force the limit_rpm to match - limit_rpm = spindleSpeed_rpm; EMC_SPINDLE_SPEED emc_spindle_speed_msg; @@ -1957,24 +1946,20 @@ void SET_SPINDLE_SPEED(double r) } -static void resetSpindleLimit() -{ - SET_SPINDLE_SPEED(spindleSpeed_rpm); -} - /** * Hack way to immediately update the spindle speed without flushing segments first (typically called within flush segments as a way to limit spindle speed during threading). * @warning does NOT update the interpreter state. */ static bool limitSpindleSpeed(double rpm) { - limit_rpm = fabs(rpm); + // Don't care about fractional RPM here, the limit speed has a safety factor anyway + double limit_rpm = ceil(fabs(rpm)); if (spindleSpeed_rpm <= limit_rpm) { // spindle speed within range, do nothing return false; } EMC_SPINDLE_SPEED emc_spindle_speed_msg; - canon_debug("Reducing spindle speed to %0.0f\n", limit_rpm); + CANON_MSG("Reducing spindle speed from %0.0f to %0.0f for synched motion\n", spindleSpeed_rpm, limit_rpm); buildSpindleCmd(emc_spindle_speed_msg, limit_rpm); interp_list.append(emc_spindle_speed_msg); @@ -2651,6 +2636,27 @@ void CANON_ERROR(const char *fmt, ...) interp_list.append(operator_error_msg); } +/** + * KLUDGE like CANON_ERROR but for sending an info message to the user. + */ +void CANON_MSG(const char *fmt, ...) +{ + va_list ap; + EMC_OPERATOR_TEXT operator_text_msg; + + operator_text_msg.id = 0; + if (fmt != NULL) { + va_start(ap, fmt); + vsnprintf(operator_text_msg.text,sizeof(operator_text_msg.text), fmt, ap); + va_end(ap); + } else { + operator_text_msg.text[0] = 0; + } + + interp_list.append(operator_text_msg); +} + + /* GET_EXTERNAL_TOOL_TABLE(int pocket) From 7424e86b69e50fb83add51ebff469c6d57d2655d Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 20 Jan 2017 11:56:21 +0000 Subject: [PATCH 30/32] Unit test for maxvel violations in spindle-synched moves In a spindle-synched G33 move, it is possible to specify a feed per revolution and spindle speed that requires linear motion exceeding axis velocity limits. This test sets up this kind of condition and measures actual pitch during the motion, failing if pitch is not within an epsilon value. See LCNC #167 for more discussion. https://github.com/LinuxCNC/linuxcnc/issues/167 Signed-off-by: John Morris --- .../spindlesync-exceeds-maxvel/.gitignore | 1 + .../motion/spindlesync-exceeds-maxvel/README | 7 ++ .../spindlesync-exceeds-maxvel/checkresult | 4 + .../spindlesync-exceeds-maxvel/postgui.hal | 3 + .../spindlesync-exceeds-maxvel/simpockets.tbl | 3 + .../spindlesync-exceeds-maxvel/test-ui.py | 74 ++++++++++++++++ .../spindlesync-exceeds-maxvel/test.ini | 87 +++++++++++++++++++ .../motion/spindlesync-exceeds-maxvel/test.sh | 10 +++ 8 files changed, 189 insertions(+) create mode 100644 tests/motion/spindlesync-exceeds-maxvel/.gitignore create mode 100644 tests/motion/spindlesync-exceeds-maxvel/README create mode 100755 tests/motion/spindlesync-exceeds-maxvel/checkresult create mode 100644 tests/motion/spindlesync-exceeds-maxvel/postgui.hal create mode 100644 tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl create mode 100755 tests/motion/spindlesync-exceeds-maxvel/test-ui.py create mode 100644 tests/motion/spindlesync-exceeds-maxvel/test.ini create mode 100755 tests/motion/spindlesync-exceeds-maxvel/test.sh diff --git a/tests/motion/spindlesync-exceeds-maxvel/.gitignore b/tests/motion/spindlesync-exceeds-maxvel/.gitignore new file mode 100644 index 00000000000..b568a3e9e69 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/.gitignore @@ -0,0 +1 @@ +/sim.var* diff --git a/tests/motion/spindlesync-exceeds-maxvel/README b/tests/motion/spindlesync-exceeds-maxvel/README new file mode 100644 index 00000000000..1920ef6394d --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/README @@ -0,0 +1,7 @@ +In a spindle-synched G33 move, it is possible to specify a feed per +revolution and spindle speed that requires linear motion exceeding +axis velocity limits. This test sets up this kind of condition and +measures actual pitch during the motion, failing if pitch is not +within an epsilon value. + +[1]: https://github.com/LinuxCNC/linuxcnc/issues/167 \ No newline at end of file diff --git a/tests/motion/spindlesync-exceeds-maxvel/checkresult b/tests/motion/spindlesync-exceeds-maxvel/checkresult new file mode 100755 index 00000000000..bffcc8f9bda --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +# If test-ui.py succeeds, then test succeeds +exit 0 + diff --git a/tests/motion/spindlesync-exceeds-maxvel/postgui.hal b/tests/motion/spindlesync-exceeds-maxvel/postgui.hal new file mode 100644 index 00000000000..d15f9fda470 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/postgui.hal @@ -0,0 +1,3 @@ +net Zvel => python-ui.Zvel +net spindle-rpm-est => python-ui.spindle-speed +net running halui.program.is-running => python-ui.running diff --git a/tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl b/tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl new file mode 100644 index 00000000000..4b427daaf99 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/simpockets.tbl @@ -0,0 +1,3 @@ +T1 P1 D0.125000 Z+1.000000 ; +T10 P3 D0.500000 Z+3.000000 ; +T99999 P50 Z+2.000000 ; diff --git a/tests/motion/spindlesync-exceeds-maxvel/test-ui.py b/tests/motion/spindlesync-exceeds-maxvel/test-ui.py new file mode 100755 index 00000000000..001f400aba2 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/test-ui.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python + +import linuxcnc, hal +import sys, os, time + + +# Params +spindle_speed = 2000 # RPM +z_axis_maxvel = 4 * 60 # IPM +# Pitch at which Z axis must travel at double maxvel if spindle speed +# is constant +max_pitch = float(z_axis_maxvel) / float(spindle_speed) * 2 + + +# Set up component +h = hal.component("python-ui") +# - Monitor spindle and Z axis speed for debugging +h.newpin("spindle-speed", hal.HAL_FLOAT, hal.HAL_IN) +h.newpin("Zvel", hal.HAL_FLOAT, hal.HAL_IN) +# - Estimated pitch +h.newpin("pitch", hal.HAL_FLOAT, hal.HAL_IN) +# - Signal program start/stop +h.newpin("running", hal.HAL_BIT, hal.HAL_IN) +h.ready() # mark the component as 'ready' +os.system("halcmd source ./postgui.hal") + + +# Initialization +c = linuxcnc.command() +s = linuxcnc.stat() +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MDI) + +# Spindle: set speed and start +c.mdi('S%d M3' % spindle_speed) + +# Spindle synchronized motion at 2x Z axis maxvel +c.mdi('G33 Z-5 K%.3f' % max_pitch) + +# While above MDI command runs, take an average of pitch while the Z +# axis is in motion +running = True +timeout = 5 +avg_sum = 0; avg_count = 0 +while timeout > 0: + if abs(h['pitch']) > 0.001: + rps = h['spindle-speed']/60 + ips = - h['Zvel'] + pitch = ips/rps + print "spindle velocity, revs/second: %.3f" % rps + print "Z velocity: %.3f" % ips + print "pitch: %.3f" % pitch + print + avg_sum += pitch + avg_count += 1 + timeout = 5 if h['running'] else timeout - 1 + time.sleep(0.1) +avg_pitch = avg_sum/avg_count +print "Average pitch = %.3f" % avg_pitch + +# Pitch should either be as specified (by slowing down spindle) or +# else program should have aborted; if it didn't abort and pitch is +# incorrect, then return the special failure result 166 +if (abs(avg_pitch - max_pitch) < 0.01): + res = 0 + print "OK: average pitch = %.3f" % avg_pitch +else: + res = 166 + print "Error: expected pitch = %.3f; got %.3f" % (max_pitch, avg_pitch) + +# Shutdown +c.wait_complete() +sys.exit(res) diff --git a/tests/motion/spindlesync-exceeds-maxvel/test.ini b/tests/motion/spindlesync-exceeds-maxvel/test.ini new file mode 100644 index 00000000000..43817adadc0 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/test.ini @@ -0,0 +1,87 @@ +[EMC] +DEBUG = 0x0 +#DEBUG = 0x7FFFFF +LOG_LEVEL = 5 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[RS274NGC] +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = . +LOG_LEVEL = 5 + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +COMM_WAIT = 0.010 +BASE_PERIOD = 40000 +SERVO_PERIOD = 1000000 + +[HAL] +HALUI = halui +HALFILE = ../../../lib/hallib/core_sim.hal +HALFILE = ../../../lib/hallib/sim_spindle_encoder.hal +POSTGUI_HALFILE = postgui.hal + +[TRAJ] +NO_FORCE_HOMING=1 +AXES = 3 +COORDINATES = X Y Z +HOME = 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +MAX_LINEAR_VELOCITY = 4 + +[AXIS_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 + +[AXIS_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 + +[AXIS_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 = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 + +[EMCIO] +EMCIO = io +CYCLE_TIME = 0.100 +TOOL_TABLE = simpockets.tbl +TOOL_CHANGE_QUILL_UP = 1 +RANDOM_TOOLCHANGER = 0 + diff --git a/tests/motion/spindlesync-exceeds-maxvel/test.sh b/tests/motion/spindlesync-exceeds-maxvel/test.sh new file mode 100755 index 00000000000..925a64afab5 --- /dev/null +++ b/tests/motion/spindlesync-exceeds-maxvel/test.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +linuxcnc -r test.ini +res=$? +echo "Exit status $res" >&2 +if test $res = 166; then + echo "Test failed: unexpected pitch" >&2 + exit 1 +fi +exit 0 From 24810a7b84971c4e740543a528cd0b66bcc74112 Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 27 Jan 2017 04:27:47 -0600 Subject: [PATCH 31/32] Before run, warn if spindle-synched move violates axis limits During a spindle-synched move, too high a spindle speed may require an axis to move faster than it is able. This patch looks for that situation using programmed spindle speed and INI per-axis max velocity settings. See LinuxCNC issue #167 for more information. Signed-off-by: John Morris --- lib/python/rs274/glcanon.py | 41 +++++++++++++++++++++++++++ src/emc/rs274ngc/gcodemodule.cc | 34 +++++++++++++++++----- src/emc/usr_intf/axis/scripts/axis.py | 13 +++++++++ 3 files changed, 81 insertions(+), 7 deletions(-) diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 6552e4b6a2f..9e352f82d0b 100644 --- a/lib/python/rs274/glcanon.py +++ b/lib/python/rs274/glcanon.py @@ -49,6 +49,8 @@ def __init__(self, colors, geometry, is_foam=0): self.arcfeed = []; self.arcfeed_append = self.arcfeed.append # dwell list - [line number, color, pos x, pos y, pos z, plane] self.dwells = []; self.dwells_append = self.dwells.append + # spindle-synched feed list - (line number, (position deltas), S, fpr) + self.feed_synched = [] self.choice = None self.feedrate = 1 self.lo = (0,) * 9 @@ -162,6 +164,33 @@ def calc_extents(self): self.min_extents_notool[0], self.min_extents_notool[1], min_z self.max_extents_notool = \ self.max_extents_notool[0], self.max_extents_notool[1], max_z + + def calc_velocity(self, delta, axis_max_vel): + """Using techniques from getStraightVelocity() in emccanon.cc, given 9 + axis deltas and velocity limits, calculate max velocity of a + straight move; deltas should be absolute; invalid axes should be 0 + """ + # Clean up tiny values + delta = tuple([(0.0 if i<1e-7 else i) for i in delta]) + # Fastest time of coordinated move is the maximum time of any + # one axis to perform move at axis max velocity + tmax = max([(i[0]/i[1] if i[1] else 0.0) + for i in zip(delta, axis_max_vel)]) + # Total distance is the hypotenuse of a set of three axes; + # which set depends on the type of move + if sum(delta[0:3]) > 0: + # Linear XYZ with or without ABC or UVW + dtot = math.sqrt(sum(i*i for i in delta[0:3])) + elif sum(delta[6:9]) > 0: + # Linear UVW without XYZ and with or without ABC + dtot = math.sqrt(sum(i*i for i in delta[6:9])) + else: + # Angular-only + dtot = math.sqrt(sum(i*i for i in delta[3:6])) + # Max velocity = total distance / fastest time + max_vel = dtot/tmax + return max_vel + def tool_offset(self, xo, yo, zo, ao, bo, co, uo, vo, wo): self.first_move = True x, y, z, a, b, c, u, v, w = self.lo @@ -230,6 +259,18 @@ def straight_feed(self, x,y,z, a,b,c, u, v, w): self.lo = l straight_probe = straight_feed + def straight_feed_synched(self, x,y,z, a,b,c, u,v,w, s, fpr): + """For spindle-synched straight feeds, also collect data needed to + check if the commanded spindle rate and feed per revolution + will violate any axis MAX_VELOCITY constraints""" + if self.suppress > 0: return + # save segment start and record straight feed segment + lo = self.lo + self.straight_feed(x,y,z, a,b,c, u, v, w) + # record axis distances, spindle speed and feed per revolution + delta = tuple([abs(i[0]-i[1]) for i in zip(lo, self.lo)]) + self.feed_synched.append((self.lineno, delta, s, fpr)) + def user_defined_function(self, i, p, q): if self.suppress > 0: return color = self.colors['m1xx'] diff --git a/src/emc/rs274ngc/gcodemodule.cc b/src/emc/rs274ngc/gcodemodule.cc index 1d56ae5c797..e11d1e1094c 100644 --- a/src/emc/rs274ngc/gcodemodule.cc +++ b/src/emc/rs274ngc/gcodemodule.cc @@ -144,6 +144,11 @@ static bool metric; static double _pos_x, _pos_y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w; EmcPose tool_offset; +// Track spindle synched motion +static int spindle_synched_motion = 0; +static double spindle_synched_feed_per_revolution; +static double spindle_speed_programmed = 0.0; + static InterpBase *pinterp; #define interp_new (*pinterp) @@ -225,9 +230,13 @@ void STRAIGHT_FEED(int line_number, if(metric) { x /= 25.4; y /= 25.4; z /= 25.4; u /= 25.4; v /= 25.4; w /= 25.4; } maybe_new_line(line_number); if(interp_error) return; - PyObject *result = - callmethod(callback, "straight_feed", "fffffffff", - x, y, z, a, b, c, u, v, w); + PyObject *result = spindle_synched_motion ? + callmethod(callback, "straight_feed_synched", "fffffffffff", + x, y, z, a, b, c, u, v, w, + spindle_speed_programmed, + spindle_synched_feed_per_revolution / (metric?25.4:1.0)) : + callmethod(callback, "straight_feed", "fffffffff", + x, y, z, a, b, c, u, v, w); if(result == NULL) interp_error ++; Py_XDECREF(result); } @@ -398,14 +407,23 @@ void SET_FEED_REFERENCE(double reference) { } void SET_CUTTER_RADIUS_COMPENSATION(double radius) {} void START_CUTTER_RADIUS_COMPENSATION(int direction) {} void STOP_CUTTER_RADIUS_COMPENSATION(int direction) {} -void START_SPEED_FEED_SYNCH() {} -void START_SPEED_FEED_SYNCH(double sync, bool vel) {} -void STOP_SPEED_FEED_SYNCH() {} +void START_SPEED_FEED_SYNCH() { + spindle_synched_motion = 1; +} +void START_SPEED_FEED_SYNCH(double sync, bool vel) { + spindle_synched_motion = 1; + spindle_synched_feed_per_revolution = sync; +} +void STOP_SPEED_FEED_SYNCH() { + spindle_synched_motion = 0; +} void START_SPINDLE_COUNTERCLOCKWISE() {} void START_SPINDLE_CLOCKWISE() {} void SET_SPINDLE_MODE(double) {} void STOP_SPINDLE_TURNING() {} -void SET_SPINDLE_SPEED(double rpm) {} +void SET_SPINDLE_SPEED(double rpm) { + spindle_speed_programmed = rpm; +} void ORIENT_SPINDLE(double d, int i) {} void WAIT_SPINDLE_ORIENT_COMPLETE(double timeout) {} void PROGRAM_STOP() {} @@ -707,6 +725,8 @@ static PyObject *parse_file(PyObject *self, PyObject *args) { metric=false; interp_error = 0; last_sequence_number = -1; + spindle_synched_motion = 0; + spindle_speed_programmed = 0.0; _pos_x = _pos_y = _pos_z = _pos_a = _pos_b = _pos_c = 0; _pos_u = _pos_v = _pos_w = 0; diff --git a/src/emc/usr_intf/axis/scripts/axis.py b/src/emc/usr_intf/axis/scripts/axis.py index bca064350d5..22fe89aacfe 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1684,6 +1684,19 @@ def run_warn(): if o.canon.max_extents_notool[i] > machine_limit_max[i]: warnings.append(_("Program exceeds machine maximum on axis %s") % "XYZABCUVW"[i]) + # Warn user if spindle-synched feeds violate axis limits + axis_max_vel = tuple([ + float(inifile.find("AXIS_%d" % i,"MAX_VELOCITY") or 0.0) * 60 + for i in range(9)]) + for line_no, delta, rpm, fpr in o.canon.feed_synched: + fpm = rpm * fpr # feed per minute + max_fpm = o.canon.calc_velocity(delta, axis_max_vel) * 0.95 + if fpm > max_fpm: + warnings.append(_( + "Spindle speed %(rpm_set).1f RPM exceeds maximum " + "%(rpm_max).1f RPM for spindle-synched motion " + "on line %(line_no)d" % + dict(rpm_set=rpm, rpm_max=max_fpm/fpr, line_no=line_no))) if warnings: text = "\n".join(warnings) return int(root_window.tk.call("nf_dialog", ".error", From 8b7c5b8503273ca9bda831c0c0fcfef0024b6e5d Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 2 Mar 2017 16:17:29 -0600 Subject: [PATCH 32/32] Fix feed rate handling problem The test in linuxcnc/linuxcnc#180 fails because that issue is also dealing with feed per rev. This patch adapts Seb Kuzminsky's fix to Rob Ellenberg's "Enable smarter blending in spindle-synchronized motion" patch. --- src/emc/task/emccanon.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index d6b03d99eb9..90e7646b1f2 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -2765,7 +2765,7 @@ double GET_EXTERNAL_FEED_RATE() // We're in G95 "Units per Revolution" mode, so // currentLinearFeedRate is the FPR and we should just return // it, unchanged. - feed = currentLinearFeedRate; + feed = uuPerRev_vel; } else { // We're in G94 "Units per Minute" mode so unhork // currentLinearFeedRate before returning it, by converting