From 89d05c4bd3c9997170077eb2645647d64bc59edd Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 3 Sep 2016 13:32:42 -0400 Subject: [PATCH 001/129] 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 46c7fa41146..0ac07fc1876 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" @@ -350,9 +351,14 @@ STATIC inline double tpGetScaledAccel( } /** - * 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; } @@ -2877,10 +2883,9 @@ STATIC void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc) { 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 || @@ -2898,19 +2903,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 @@ -2919,15 +2924,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(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 300045b22409a42484577001509125857efc4a91 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 4 Sep 2016 08:10:00 -0400 Subject: [PATCH 002/129] 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 0ac07fc1876..9cb42e57f0c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2926,9 +2926,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 e76bb409b2019ee1458685bea224908bad23d6c1 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Sep 2016 14:22:02 -0400 Subject: [PATCH 003/129] 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 a95fee6dee65e779aca78cc0df8e16490204e2e2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Sep 2016 14:28:14 -0400 Subject: [PATCH 004/129] 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 fb6b108f798406f34e525e69dc8f9d4252069c33 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 09:53:33 -0400 Subject: [PATCH 005/129] 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 ddd71e46630c3101fcd2047d488ef8d386d489b7 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 09:54:37 -0400 Subject: [PATCH 006/129] 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 1f4bd07f74e..5cee3ba4ec2 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 050adc621b707e23c515a1bc6dec5058cb2c14c2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 10:26:21 -0400 Subject: [PATCH 007/129] 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 5cee3ba4ec2..bf1f043731c 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 150751ee07824667c61c75dc64a28518c33816f4 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 25 Sep 2016 10:28:08 -0400 Subject: [PATCH 008/129] 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 bf1f043731c..5cca1cedf77 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 132d8cc5a4c0f1f5c64d488578347af37ccb417d Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 9 Jul 2016 14:45:14 -0400 Subject: [PATCH 009/129] 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 9cb42e57f0c..df6256b7633 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1487,14 +1487,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; @@ -2047,7 +2054,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); } @@ -2139,7 +2146,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 09c594987e8939e5d880564fa8e365d417356fc4 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 27 Sep 2016 00:28:26 -0400 Subject: [PATCH 010/129] 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 | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index df6256b7633..cdb5acc9c13 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -241,7 +241,6 @@ 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 @@ -252,6 +251,17 @@ STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) } } +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. @@ -259,7 +269,7 @@ STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) */ 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); @@ -928,7 +938,7 @@ STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const pre 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", @@ -1086,7 +1096,7 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre 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); @@ -1235,7 +1245,7 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev 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", @@ -1397,7 +1407,7 @@ STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pr 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", @@ -1850,10 +1860,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 = tcGetMaxTargetVel(prev_tc, getMaxFeedScale(prev_tc)); - double v_max2 = tcGetMaxTargetVel(tc, getMaxFeedScale(tc)); - // Note that this is a minimum since the velocity at the intersection must - // be the slower of the two segments not to violate constraints. + 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 68b5e73ef3333ebaf069d059f2dd7b9983d10c34 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 29 Sep 2016 00:07:22 -0400 Subject: [PATCH 011/129] 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 5cca1cedf77..bf002d23d86 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 608289279b438ef939d4e7ebbd41cdd0e42c562b Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 26 Nov 2016 16:10:01 -0500 Subject: [PATCH 012/129] 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 cdb5acc9c13..a5bac81d679 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2841,13 +2841,14 @@ STATIC tp_err_t 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 @@ -2856,7 +2857,7 @@ STATIC tp_err_t 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 @@ -2900,7 +2901,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 || @@ -3414,15 +3415,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(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 f714b3d6f48f909c7763a635274a32db5fb0173f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 26 Nov 2016 16:08:31 -0500 Subject: [PATCH 013/129] 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 | 98 +++++++++++++++++++++++++------------------------ 1 file changed, 51 insertions(+), 47 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index a5bac81d679..8a4934fcd7a 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -79,6 +79,8 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con STATIC int tpAdjustAccelForTangent( 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. @@ -93,9 +95,14 @@ STATIC int tpAdjustAccelForTangent( */ 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; } @@ -1210,7 +1217,6 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre tcSetLineXYZ(tc, &line2_temp); //Cleanup any mess from parabolic - tc->blend_prev = 0; tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } @@ -1270,8 +1276,6 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev return TP_ERR_FAIL; } - - int res_param = blendComputeParameters(¶m); int res_points = blendFindPoints3(&points_approx, &geom, ¶m); @@ -1378,10 +1382,7 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev 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; } @@ -1530,6 +1531,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. @@ -1581,14 +1622,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( @@ -2059,22 +2093,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); } @@ -2151,22 +2170,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 a10bd0b8a9e7e1b9e2e674fcc15c992b4830555e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 27 Nov 2016 11:17:40 -0500 Subject: [PATCH 014/129] 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 8a4934fcd7a..0b3ef2b8a26 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2918,10 +2918,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 3bcda2ac7294cf16ccbeb1b60032390a8994310b Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 27 Nov 2016 11:14:03 -0500 Subject: [PATCH 015/129] 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 8c4c0a096570d3e39fa1326e61d683bf7218258d Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 26 Nov 2016 18:07:12 -0500 Subject: [PATCH 016/129] 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 0b3ef2b8a26..adf339222dc 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2681,7 +2681,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; } @@ -2906,7 +2906,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)) { @@ -2917,8 +2916,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); @@ -2926,17 +2926,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 { @@ -2947,8 +2949,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 ae402de1d65f074ba12808fac42667dda2f6747a Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 15:12:14 -0500 Subject: [PATCH 017/129] 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 b0ef750d301..e99712875ce 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 9000f75d676..35534a0f8a7 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 4583308064e..a360e015cb6 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -321,9 +321,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 873aac44cf3e8f47ee8cfabe503e7449c09a7e12 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 16:24:06 -0500 Subject: [PATCH 018/129] 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 305469c5bf0..d4fb45def6c 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 e99712875ce..5c995acf5c0 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 a360e015cb6..3b1d1844464 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -914,7 +914,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 adf339222dc..fda32608c6f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -456,7 +456,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; @@ -2488,15 +2488,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; @@ -2525,7 +2525,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; @@ -2730,7 +2730,7 @@ STATIC tp_err_t 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 @@ -2775,12 +2775,12 @@ STATIC tp_err_t 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; @@ -2829,7 +2829,7 @@ STATIC tp_err_t 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; @@ -2860,12 +2860,12 @@ STATIC tp_err_t 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; @@ -2880,7 +2880,7 @@ STATIC tp_err_t 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(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) { @@ -2903,8 +2903,8 @@ STATIC void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc) { 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 || @@ -2926,7 +2926,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. @@ -3420,7 +3420,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 f8af6dced1eae59961713faa30634e545e1c9326 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 16:28:18 -0500 Subject: [PATCH 019/129] 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 fda32608c6f..441b4ce809f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2478,6 +2478,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 @@ -2487,26 +2506,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; @@ -2519,19 +2536,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); @@ -2545,7 +2562,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 5390ee45b7084ee3d11c968799c5b5707c75b355 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 10 Dec 2016 19:39:40 -0500 Subject: [PATCH 020/129] 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 441b4ce809f..057888ffe39 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -488,7 +488,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; @@ -2477,6 +2476,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. @@ -2519,11 +2547,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; @@ -2548,7 +2576,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); @@ -2697,7 +2725,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; } @@ -2799,7 +2827,6 @@ STATIC tp_err_t 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; } } return TP_ERR_OK; @@ -2919,53 +2946,63 @@ STATIC void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc) { 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 fbeb45806fd..1b2d80276ba 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 c4dfe6f68df379a546313d466b1325310fc8e7d0 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 14 Dec 2016 23:58:47 -0500 Subject: [PATCH 021/129] 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 bf002d23d86..34c496de531 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 aa53d9f013c19375c6a5b32e64892cc91f04dca2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 13:50:54 -0500 Subject: [PATCH 022/129] 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 7b78fbc47cdd1b707343f95cf8b6d5b8819fc9b1 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 23 Jan 2017 13:23:58 -0500 Subject: [PATCH 023/129] 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 a7e35f5d63ecb2cda6e4470ff1f93927e7c7740e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 14:19:44 -0500 Subject: [PATCH 024/129] 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 5c995acf5c0..2ff7e19a6ed 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 39f7434d998d431c4cd1d8c266c6503175a3c1fe Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 00:17:34 -0500 Subject: [PATCH 025/129] 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 2ff7e19a6ed..b18e4a061ec 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 35534a0f8a7..d52e6811c09 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 3b1d1844464..ce13112ea1a 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -328,6 +328,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 057888ffe39..30b92bee4f6 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3037,7 +3037,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 4864c45cbde07eb0b82d6871b38948c88d2b95b7 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 14:49:25 -0500 Subject: [PATCH 026/129] 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 f7a654ccd9d38e7ae92885d0fbaf595ca4146277 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 11 Dec 2016 14:20:21 -0500 Subject: [PATCH 027/129] 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 b18e4a061ec..58038afd5d1 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 d52e6811c09..d37df0eb61c 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 ce13112ea1a..f5c9f22ebb7 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -331,6 +331,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 e8e402ed580..cddd1738b22 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1835,3 +1835,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 30b92bee4f6..023de2cee1c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2330,13 +2330,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"); @@ -2374,30 +2374,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t double dx = tc->target - tc->progress; double maxaccel = tpGetScaledAccel(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 @@ -2989,7 +2966,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, @@ -2997,12 +2974,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. @@ -3025,21 +3002,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(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 dbc4be1b846f8a2ae9c4adaf8d4977b27832ea4e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 26 Jan 2017 21:42:59 -0500 Subject: [PATCH 028/129] 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 34c496de531..66ff75a9b89 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 6ef4c5eb73fb7f435d5f638e37b28622c66512f7 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 26 Jan 2017 22:02:04 -0500 Subject: [PATCH 029/129] 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 66ff75a9b89..832dfec855e 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 24460976433ce3cd4f115a851c2eea8cecd5d0ec Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 4 Feb 2017 19:41:57 -0500 Subject: [PATCH 030/129] Added tracking error pin to motion HAL for spindle sync testing 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/tp.c | 3 +++ 5 files changed, 7 insertions(+) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 58038afd5d1..b0fcc90aa45 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_error = *emcmot_hal_data->pos_tracking_error; emcmotStatus->pos_tracking_mode = *emcmot_hal_data->pos_tracking_mode; /* compute net feed and spindle scale factors */ diff --git a/src/emc/motion/mot_priv.h b/src/emc/motion/mot_priv.h index d37df0eb61c..f62857533b4 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_float_t *pos_tracking_error; 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*/ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index f5c9f22ebb7..df942a93f18 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -332,6 +332,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->pos_tracking_error), mot_comp_id, "motion.pos-tracking-error")) < 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 f3f84aa256a..c854cbf006f 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -661,6 +661,7 @@ typedef enum { spindle_fb_status spindle_fb; /* Spindle feedback input to motion */ double spindle_tracking_gain; // external control of position trakcing aggressiveness int pos_tracking_mode; + double pos_tracking_error; 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 023de2cee1c..058abb74aff 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -460,6 +460,7 @@ int tpClear(TP_STRUCT * const tp) emcmotStatus->current_vel = 0.0; emcmotStatus->requested_vel = 0.0; emcmotStatus->distance_to_go = 0.0; + emcmotStatus->pos_tracking_error = 0; ZERO_EMC_POSE(emcmotStatus->dtg); SET_MOTION_INPOS_FLAG(1); @@ -2588,6 +2589,7 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const return TP_ERR_FAIL; } + emcmotStatus->pos_tracking_error = 0.0; if (!tc) { // Assume that we have no active segment, so we should clear out the status fields emcmotStatus->distance_to_go = 0; @@ -3033,6 +3035,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, break; } } + emcmotStatus->pos_tracking_error = pos_error; 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); } From 47c60c6064bdb82632810e677fd73254e44ad038 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 5 Nov 2017 21:57:07 -0500 Subject: [PATCH 031/129] Make sim spindle more sluggish to make direction reversal behavior easier to analyze in simulation. --- lib/hallib/sim_spindle_encoder.hal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/hallib/sim_spindle_encoder.hal b/lib/hallib/sim_spindle_encoder.hal index de8b128f6ac..5000cbf7a1c 100644 --- a/lib/hallib/sim_spindle_encoder.hal +++ b/lib/hallib/sim_spindle_encoder.hal @@ -24,7 +24,7 @@ net spindle-pos-raw sim_spindle.position-fb => sim_spindle_encoder.in net spindle-pos sim_spindle_encoder.out => motion.spindle-revs # simulate spindle mass -setp spindle_mass.gain .07 +setp spindle_mass.gain .005 # Limit spindle speed by our maximum value net spindle-speed-cmd motion.spindle-speed-out => limit_speed.in From c260e2f91afb1f9c30cd588c7a63169ded7fad43 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 5 Nov 2017 21:57:59 -0500 Subject: [PATCH 032/129] Fix motion's position tracking error output pin. --- src/emc/motion/control.c | 5 +++-- src/emc/motion/motion.c | 2 +- src/emc/tp/tp.c | 4 +++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b0fcc90aa45..8d491dbd740 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -400,7 +400,6 @@ 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_error = *emcmot_hal_data->pos_tracking_error; emcmotStatus->pos_tracking_mode = *emcmot_hal_data->pos_tracking_mode; /* compute net feed and spindle scale factors */ @@ -1759,7 +1758,9 @@ static void output_to_hal(void) *(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_cmd.brake != 0) ? 1 : 0; - + + *(emcmot_hal_data->pos_tracking_error) = emcmotStatus->pos_tracking_error; + *(emcmot_hal_data->program_line) = emcmotStatus->id; *(emcmot_hal_data->motion_type) = emcmotStatus->motionType; *(emcmot_hal_data->distance_to_go) = emcmotStatus->distance_to_go; diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index df942a93f18..dc449778800 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -332,7 +332,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->pos_tracking_error), mot_comp_id, "motion.pos-tracking-error")) < 0) goto error; + if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->pos_tracking_error), mot_comp_id, "motion.pos-tracking-error")) < 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/tp/tp.c b/src/emc/tp/tp.c index 058abb74aff..c609e46e96d 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2589,7 +2589,6 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const return TP_ERR_FAIL; } - emcmotStatus->pos_tracking_error = 0.0; if (!tc) { // Assume that we have no active segment, so we should clear out the status fields emcmotStatus->distance_to_go = 0; @@ -3467,6 +3466,9 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpUpdateRigidTapState(tp, tc); } + // Assume zero tracking error unless position sync is active + emcmotStatus->pos_tracking_error = 0.0; + /** If synchronized with spindle, calculate requested velocity to track * spindle motion.*/ switch (tc->synchronized) { From a319ff9eda9dec71def98d03a20572ea7d56d937 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 5 Nov 2017 23:07:55 -0500 Subject: [PATCH 033/129] Fix sign reversal in spindle displacement calculation causing hard-stop at end of rigid tap --- src/emc/tp/tp.c | 46 +++++++++++++++++----------------------------- 1 file changed, 17 insertions(+), 29 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index c609e46e96d..80297cc6b3c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2454,34 +2454,21 @@ 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; - } -} - +/** + * Compute the spindle displacement used for spindle position tracking. + */ static inline double findSpindleDisplacement(double new_pos, - double old_pos, - spindle_direction_code_t spindle_cmd_dir) + double old_pos) { // Difference assuming spindle "forward" direction double forward_diff = new_pos - old_pos; - switch(spindle_cmd_dir) { - case SPINDLE_STOPPED: - case SPINDLE_FORWARD: + const double velocity = emcmotStatus->spindle_fb.velocity_rpm; + if (velocity >= 0.0) { return forward_diff; - case SPINDLE_REVERSE: + } else { return -forward_diff; } - //no default on purpose, compiler should warn on missing states - return 0; } /** @@ -2938,10 +2925,9 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, // Note that this quantity should be non-negative under normal conditions. double spindle_displacement = findSpindleDisplacement(spindle_pos, - local_spindle_offset, - getSpindleCmdDir()); + local_spindle_offset); - tc_debug_print("spindle_displacement %f raw_pos %f", spindle_displacement, spindle_pos); + tc_debug_print("spindle_displacement %f, raw_pos %f, ", spindle_displacement, spindle_pos); const double spindle_vel = emcmotStatus->spindle_fb.velocity_rpm / 60.0; if(tc->sync_accel) { @@ -2965,17 +2951,19 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, } } 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_desired, tc->progress); + const double pos_desired = spindle_displacement * tc->uu_per_rev; + double net_progress = tc->progress - tc->progress_at_sync; 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; + net_progress += nexttc->progress; } - tc_debug_print(", pos_error %f\n", pos_error); + const double pos_error = pos_desired - net_progress; + tc_debug_print(", pos_desired %f, net_progress %f, pos_error %f\n", + pos_desired, + net_progress, + pos_error); // we have synced the beginning of the move as best we can - // track position (minimize pos_error). From 4caa8ee9181ce1f16028921595e24e8463c2f913 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 5 Nov 2017 23:08:46 -0500 Subject: [PATCH 034/129] Add debug scripts to extract spindle data from a tp log file --- src/emc/tp/tp.c | 2 +- .../circular-arcs/extract_spindle_data.awk | 1 + .../circular-arcs/octave/plot_spindledata.m | 15 +++++++++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) create mode 100644 tests/trajectory-planner/circular-arcs/extract_spindle_data.awk create mode 100644 tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 80297cc6b3c..62f36f9827a 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2960,7 +2960,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, net_progress += nexttc->progress; } const double pos_error = pos_desired - net_progress; - tc_debug_print(", pos_desired %f, net_progress %f, pos_error %f\n", + tc_debug_print("pos_desired %f, net_progress %f, pos_error %f\n", pos_desired, net_progress, pos_error); diff --git a/tests/trajectory-planner/circular-arcs/extract_spindle_data.awk b/tests/trajectory-planner/circular-arcs/extract_spindle_data.awk new file mode 100644 index 00000000000..3d8f7a4f7a2 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_spindle_data.awk @@ -0,0 +1 @@ +/spindle_displacement/{print $2$4$6$8$10} diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m b/tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m new file mode 100644 index 00000000000..7c89377f6dd --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/plot_spindledata.m @@ -0,0 +1,15 @@ +d=csvread('spindle_data.txt'); + +d(:,5)=d(:,5)*1000; +% Spindle data +figure(1) +plot(d(:,1:5),'Linewidth',2); + +grid on +legend('spindle displacement','raw pos', 'pos desired', 'pos progress', 'pos_error') + +% Position error +figure(2) +plot(d(:,5),'Linewidth',2) +grid on +legend('pos error') \ No newline at end of file From f2290738baf306bb0915eb2977e6436bf00a521a Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 5 Nov 2017 23:18:22 -0500 Subject: [PATCH 035/129] Fix false position tracking error at spindle reversal (needed to reset sync position) --- src/emc/tp/tp.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 62f36f9827a..1ee026a8f21 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2525,6 +2525,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, tc->coords.rigidtap.reversal_target = aux->tmag; tc->target = aux->tmag + 10. * tc->uu_per_rev; tc->progress = 0.0; + tc->progress_at_sync = 0.0; rtapi_print_msg(RTAPI_MSG_DBG, "new target = %f", tc->target); tc->coords.rigidtap.state = RETRACTION; From c6caad2c39c6a334cce9b5251b84e699442b8bbf Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 5 Nov 2017 23:48:05 -0500 Subject: [PATCH 036/129] Increase halscope's shared ram size to collect more samples --- src/hal/utils/scope_rt.c | 2 +- src/hal/utils/scope_shm.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hal/utils/scope_rt.c b/src/hal/utils/scope_rt.c index 738e2007b3d..8530b8f9702 100644 --- a/src/hal/utils/scope_rt.c +++ b/src/hal/utils/scope_rt.c @@ -44,7 +44,7 @@ MODULE_AUTHOR("John Kasunich"); MODULE_DESCRIPTION("Oscilloscope for EMC HAL"); MODULE_LICENSE("GPL"); -long num_samples = 16000; +long num_samples = SCOPE_NUM_SAMPLES_DEFAULT; long shm_size; RTAPI_MP_LONG(num_samples, "Number of samples in the shared memory block") diff --git a/src/hal/utils/scope_shm.h b/src/hal/utils/scope_shm.h index 1e7cef19530..1c807ae0ce0 100644 --- a/src/hal/utils/scope_shm.h +++ b/src/hal/utils/scope_shm.h @@ -42,7 +42,7 @@ ************************************************************************/ #define SCOPE_SHM_KEY 0x130CF406 -#define SCOPE_NUM_SAMPLES_DEFAULT 16000 +#define SCOPE_NUM_SAMPLES_DEFAULT 128000 typedef enum { IDLE = 0, /* waiting for run command */ From a86485c8ec58ef4e297c5bf594b41edfdce3921e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 7 Nov 2017 00:37:50 -0500 Subject: [PATCH 037/129] Add some missing notes --- src/emc/tp/tp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 1ee026a8f21..fd0e0a7c665 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2524,6 +2524,8 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, rtapi_print_msg(RTAPI_MSG_DBG, "old target = %f", tc->target); tc->coords.rigidtap.reversal_target = aux->tmag; tc->target = aux->tmag + 10. * tc->uu_per_rev; + // NOTE: reset both progress and sync location: + // At the point of reversal, the spindle is already synchronized tc->progress = 0.0; tc->progress_at_sync = 0.0; rtapi_print_msg(RTAPI_MSG_DBG, "new target = %f", tc->target); From 43a7e6dee6b890d1650b1dde10210c853478bfac Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 7 Nov 2017 22:49:20 -0500 Subject: [PATCH 038/129] Add max spindle speed check to rigid tapping and factor out some common code. Now, any position-synced motion will have the spindle velocity limited automatically to a safe maximum to prevent accumulating tracking error. --- src/emc/task/emccanon.cc | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 832dfec855e..eff35ce0dc0 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -54,6 +54,7 @@ static bool limitSpindleSpeed(double rpm); static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel); +static double limitSpindleSpeedByActiveFeedRate(double ini_maxvel); static int debug_velacc = 0; static double css_maximum; // both always positive @@ -943,11 +944,7 @@ static void flush_segments(void) { VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w); 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); - double vel = std::min(ini_maxvel, nominal_vel); - - limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); + double vel = limitSpindleSpeedByActiveFeedRate(ini_maxvel); EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -1133,7 +1130,9 @@ void RIGID_TAP(int line_number, double x, double y, double z) canonEndPoint.u, canonEndPoint.v, canonEndPoint.w); ini_maxvel = veldata.vel; - + + limitSpindleSpeedByActiveFeedRate(ini_maxvel); + AccelData accdata = getStraightAcceleration(x, y, z, canonEndPoint.a, canonEndPoint.b, canonEndPoint.c, canonEndPoint.u, canonEndPoint.v, canonEndPoint.w); @@ -1179,9 +1178,7 @@ void STRAIGHT_PROBE(int line_number, VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w); double ini_maxvel = veldata.vel; - double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); - double vel = std::min(ini_maxvel, nominal_vel); - limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); + double vel = limitSpindleSpeedByActiveFeedRate(ini_maxvel); AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); double acc = accdata.acc; @@ -1806,10 +1803,9 @@ void ARC_FEED(int line_number, // Limit velocity by maximum double nominal_vel = getActiveFeedRate(FEED_LINEAR); - double vel = MIN(nominal_vel, v_max); // Make sure spindle speed is within range (for spindle_sync motion only) - limitSpindleSpeedFromVel(nominal_vel, v_max); + double vel = limitSpindleSpeedByActiveFeedRate(v_max); canon_debug("current F = %f\n", nominal_vel); canon_debug("vel = %f\n",vel); @@ -1956,6 +1952,7 @@ static bool limitSpindleSpeed(double rpm) double limit_rpm = ceil(fabs(rpm)); if (spindleSpeed_rpm <= limit_rpm) { // spindle speed within range, do nothing + canon_debug("Spindle speed %f is within max of %f", spindleSpeed_rpm, limit_rpm); return false; } EMC_SPINDLE_SPEED emc_spindle_speed_msg; @@ -1978,6 +1975,19 @@ static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel) return limitSpindleSpeed(maxSpindleRPM); } +static double limitSpindleSpeedByActiveFeedRate(double ini_maxvel) +{ + const double nominal_vel = getActiveFeedRate(static_cast(!cartesian_move && angular_move)); + // Limit spindle speed during rigid tapping + limitSpindleSpeedFromVel(nominal_vel, ini_maxvel); + + double real_max_vel = std::min(ini_maxvel, nominal_vel); + canon_debug("%s: nominal_vel %f, real_max_vel %f\n", __PRETTY_FUNCTION__, nominal_vel, real_max_vel); + return real_max_vel; +} + + + void STOP_SPINDLE_TURNING() { EMC_SPINDLE_OFF emc_spindle_off_msg; From 52204ad1dd70103404bc1e081cf1d182f58cedbd Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 8 Nov 2017 00:39:51 -0500 Subject: [PATCH 039/129] Fix spindle reversal calculations to avoid jitter and accelerations spikes. Previously, the reversal was noisy because the TP would detect that the spindle had reversed, and immediately set up a new motion segment for the reverse motion. Unfortunately, if the current velocity is not zero, the actual acceleration will be higher by v_final / dt. Now, the reversal is smarter in several ways: 1) Store the direction and "origin" position of the spindle together in one location in the TP. This way, the sign of the displacement depends only on the commanded direction when the origin was chosen. It does not depend on the current command / actual velocity. 2) Don't store a separate offset in the rigid tap struct for the spindle. Whenever rigid tapping reverses spindle direction, update the spindle origin in the TP. 3) Wait for the spindle to reverse direction by position (rather than velocity). Also, wait for the current velocity to actually reach zero before starting a reversal / final retract motion. This avoids the acceleration spike. --- src/emc/tp/tc_types.h | 7 +++- src/emc/tp/tp.c | 91 +++++++++++++++++++++++-------------------- src/emc/tp/tp_types.h | 3 +- 3 files changed, 57 insertions(+), 44 deletions(-) diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 3d3fbac770c..636c56e7cb7 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -99,6 +99,12 @@ typedef struct { double aios[EMCMOT_MAX_AIO]; } syncdio_t; + +typedef struct { + double position; //!< Reference position for displacement calculations + double direction; //!< Direction of "positive" spindle motion (as -1, 1, or 0) +} spindle_origin_t; + typedef struct { PmCartLine xyz; // original, but elongated, move down PmCartLine aux_xyz; // this will be generated on the fly, for the other @@ -106,7 +112,6 @@ typedef struct { PmCartesian abc; PmCartesian uvw; double reversal_target; - double spindlepos_at_reversal; RIGIDTAP_STATE state; } PmRigidTap; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index fd0e0a7c665..8551de5725c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -367,19 +367,31 @@ STATIC inline double tpGetScaledAccel( return planMaxSegmentAccel(tc, (tc_term_cond_t)tc->term_cond); } + +STATIC inline void resetSpindleOrigin(spindle_origin_t *origin) +{ + if (!origin) { + return; + } + origin->direction = 0.0; + origin->position = 0.0; +} + + /** - * 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? + * Set up a spindle origin based on the current spindle COMMANDED direction and the given position. * - * @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. + * The origin is used to calculate displacements used in spindle position tracking. + * The direction is stored as part of the origin to prevent discontinuous + * changes in displacement due to sign flips */ -STATIC inline double getSpindleProgress(double spindle_pos, int spindle_dir) { - if (spindle_dir < 0.0) { - spindle_pos*=-1.0; +STATIC inline void setSpindleOrigin(spindle_origin_t *origin, double position) +{ + if (!origin) { + return; } - return spindle_pos; + origin->position = position; + origin->direction = signum(emcmotStatus->spindle_cmd.velocity_rpm_out); } /** @@ -488,7 +500,7 @@ int tpInit(TP_STRUCT * const tp) tp->wMax = 0.0; tp->wDotMax = 0.0; - tp->spindle.offset = 0.0; + resetSpindleOrigin(&tp->spindle.origin); tp->spindle.waiting_for_index = MOTION_INVALID_ID; tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; @@ -2456,28 +2468,24 @@ void tpToggleDIOs(TC_STRUCT * const tc) { /** * Compute the spindle displacement used for spindle position tracking. + * + * The displacement is always computed with respect to a specie */ -static inline double findSpindleDisplacement(double new_pos, - double old_pos) +static inline double findSpindleDisplacement( + double new_pos, + spindle_origin_t origin + ) { - // Difference assuming spindle "forward" direction - double forward_diff = new_pos - old_pos; - - const double velocity = emcmotStatus->spindle_fb.velocity_rpm; - if (velocity >= 0.0) { - return forward_diff; - } else { - return -forward_diff; - } + return origin.direction * (new_pos - origin.position); } /** * 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() +static inline bool spindleReversed(spindle_origin_t origin, double prev_pos, double current_pos) { - return (signum(emcmotStatus->spindle_fb.velocity_rpm) != signum(emcmotStatus->spindle_cmd.velocity_rpm_out)); + return origin.direction * (current_pos - prev_pos) < 0; } static inline bool cmdReverseSpindle() @@ -2496,9 +2504,10 @@ static inline bool cmdReverseSpindle() * during a rigid tap cycle. In particular, the target and spindle goal need to * be carefully handled since we're reversing direction. */ -STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, +STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, TC_STRUCT * const tc) { + static double old_spindle_pos = 0.0; double spindle_pos = emcmotStatus->spindle_fb.position_rev; switch (tc->coords.rigidtap.state) { @@ -2512,11 +2521,11 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, break; case REVERSING: tc_debug_print("REVERSING\n"); - if (!spindleReversing()) { + if (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { 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.spindlepos_at_reversal = spindle_pos; + setSpindleOrigin(&tp->spindle.origin, spindle_pos); pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; @@ -2537,16 +2546,18 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, case RETRACTION: tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { - // Flip spindle direction againt to start final reversal + // Flip spindle direction again to start final reversal cmdReverseSpindle(); tc->coords.rigidtap.state = FINAL_REVERSAL; } break; case FINAL_REVERSAL: tc_debug_print("FINAL_REVERSAL\n"); - if (!spindleReversing()) { + if (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; + + setSpindleOrigin(&tp->spindle.origin, spindle_pos); pmCartLinePoint(aux, tc->progress, &start); end = tc->coords.rigidtap.xyz.start; pmCartLineInit(aux, &start, &end); @@ -2564,6 +2575,7 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, // this is a regular move now, it'll stop at target above. break; } + old_spindle_pos = spindle_pos; } @@ -2693,9 +2705,9 @@ 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->progress_at_sync) / tc->uu_per_rev; + tp->spindle.origin.position += (tc->target - tc->progress_at_sync) / tc->uu_per_rev; } else { - tp->spindle.offset = 0.0; + tp->spindle.origin.position = 0.0; } if(tc->indexrotary != -1) { @@ -2877,7 +2889,9 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp->spindle.waiting_for_index = tc->id; // ask for an index reset emcmotStatus->spindle_fb.index_enable = 1; - tp->spindle.offset = 0.0; + + tp->spindle.origin.position = 0.0; + tp->spindle.origin.direction = signum(emcmotStatus->spindle_cmd.velocity_rpm_out); rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync...\n"); return TP_ERR_WAITING; } @@ -2917,18 +2931,11 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, // 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)) { - local_spindle_offset = tc->coords.rigidtap.spindlepos_at_reversal; - } + spindle_origin_t spindle_origin = tp->spindle.origin; // Note that this quantity should be non-negative under normal conditions. double spindle_displacement = findSpindleDisplacement(spindle_pos, - local_spindle_offset); + spindle_origin); tc_debug_print("spindle_displacement %f, raw_pos %f, ", spindle_displacement, spindle_pos); @@ -2942,9 +2949,9 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, 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 = spindle_pos; + tp->spindle.origin.position = spindle_pos; tc->progress_at_sync = tc->progress; - tc_debug_print("Spindle offset %f\n", tp->spindle.offset); + tc_debug_print("Spindle offset %f\n", tp->spindle.origin.position); tc->sync_accel = 0; tc->target_vel = target_vel; } else { diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 1b2d80276ba..72eefc0df84 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -76,7 +76,8 @@ typedef enum { * synchronized motion code. */ typedef struct { - double offset; + spindle_origin_t origin; //!< initial position of spindle during synchronization (direction-aware) + int waiting_for_index; int waiting_for_atspeed; } tp_spindle_t; From 3df716540f5b54c30f7d3d4eb1ed511266528147 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 8 Nov 2017 23:08:50 -0500 Subject: [PATCH 040/129] Add some extraction scripts for TP test logs --- tests/trajectory-planner/circular-arcs/extract_all.sh | 7 +++++++ .../circular-arcs/extract_displacement.awk | 2 ++ .../{extract_spindle_data.awk => extract_spindle.awk} | 0 .../circular-arcs/extract_tc_state.awk | 1 + .../circular-arcs/octave/plot_displacement.m | 10 ++++++++++ .../circular-arcs/octave/plot_tcdata.m | 7 +++++++ 6 files changed, 27 insertions(+) create mode 100755 tests/trajectory-planner/circular-arcs/extract_all.sh create mode 100644 tests/trajectory-planner/circular-arcs/extract_displacement.awk rename tests/trajectory-planner/circular-arcs/{extract_spindle_data.awk => extract_spindle.awk} (100%) create mode 100644 tests/trajectory-planner/circular-arcs/extract_tc_state.awk create mode 100644 tests/trajectory-planner/circular-arcs/octave/plot_displacement.m create mode 100644 tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m diff --git a/tests/trajectory-planner/circular-arcs/extract_all.sh b/tests/trajectory-planner/circular-arcs/extract_all.sh new file mode 100755 index 00000000000..83d16d19d47 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_all.sh @@ -0,0 +1,7 @@ +#!/usr/bin/env bash +LOGFILE=${1-test.log} +echo "Extracting data from $LOGFILE" +for f in spindle tc_state displacement +do + awk -f extract_"$f".awk $LOGFILE > ./octave/"$f"_data.txt +done diff --git a/tests/trajectory-planner/circular-arcs/extract_displacement.awk b/tests/trajectory-planner/circular-arcs/extract_displacement.awk new file mode 100644 index 00000000000..cbeae5cecb5 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_displacement.awk @@ -0,0 +1,2 @@ +#EXAMPLE: tp_displacement = -9.497722863738e-06 0.000000000000e+00 -9.506120496661e-06 time = 2.108000000000e+00 +/tp_displacement/{print $8","$3","$4","$5} diff --git a/tests/trajectory-planner/circular-arcs/extract_spindle_data.awk b/tests/trajectory-planner/circular-arcs/extract_spindle.awk similarity index 100% rename from tests/trajectory-planner/circular-arcs/extract_spindle_data.awk rename to tests/trajectory-planner/circular-arcs/extract_spindle.awk diff --git a/tests/trajectory-planner/circular-arcs/extract_tc_state.awk b/tests/trajectory-planner/circular-arcs/extract_tc_state.awk new file mode 100644 index 00000000000..647196dcf9b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/extract_tc_state.awk @@ -0,0 +1 @@ +/tc state:/{print $5 $8 $11 $14 $17 $20 $23 $26 $29 $32 $35} diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_displacement.m b/tests/trajectory-planner/circular-arcs/octave/plot_displacement.m new file mode 100644 index 00000000000..b0aa558e7ba --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/plot_displacement.m @@ -0,0 +1,10 @@ +d=csvread('displacement_data.txt'); + +t = d(:,1); +xyz=d(:,2:4); +figure(1) +plot(t, xyz,'Linewidth',2); + +grid on +legend('X','Y','Z') + diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m b/tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m new file mode 100644 index 00000000000..447423f5d53 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/plot_tcdata.m @@ -0,0 +1,7 @@ +d=csvread('tc_state_data.txt'); + +figure(1) +plot(d(:,[1:5,8]),'Linewidth',2); + +grid on +legend('vr', 'vf','vmax','v_current','acc') From 6af640e4a9790eb58569825c130b14cbbf9c51de Mon Sep 17 00:00:00 2001 From: John Morris Date: Fri, 27 Jan 2017 04:27:47 -0600 Subject: [PATCH 041/129] 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 NOTE: Slight adjustments were made to rebase onto LinuxCNC 2.7 (mostly due to incompatibilities w/ state tags) Signed-off-by: Robert W. Ellenberg --- lib/python/rs274/glcanon.py | 40 +++++++++++++++++++++++++++ src/emc/rs274ngc/gcodemodule.cc | 34 ++++++++++++++++++----- src/emc/usr_intf/axis/scripts/axis.py | 13 +++++++++ src/emc/usr_intf/gremlin/gremlin.py | 13 +++++++++ 4 files changed, 93 insertions(+), 7 deletions(-) diff --git a/lib/python/rs274/glcanon.py b/lib/python/rs274/glcanon.py index 8b2e4a075f7..9fb54bcb26a 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,32 @@ 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 +258,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, lineno, 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((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 d30b91e2271..bf75cfac69b 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,11 +230,15 @@ 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 = + PyObject *result = spindle_synched_motion ? + callmethod(callback, "straight_feed_synched", "ifffffffffff", + line_number, 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); + if(result == NULL) interp_error ++; + Py_XDECREF(result); } void STRAIGHT_TRAVERSE(int line_number, @@ -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 b105b7bab1e..42d424969b5 100755 --- a/src/emc/usr_intf/axis/scripts/axis.py +++ b/src/emc/usr_intf/axis/scripts/axis.py @@ -1689,6 +1689,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", diff --git a/src/emc/usr_intf/gremlin/gremlin.py b/src/emc/usr_intf/gremlin/gremlin.py index 99b661c7724..a43ef3f003f 100755 --- a/src/emc/usr_intf/gremlin/gremlin.py +++ b/src/emc/usr_intf/gremlin/gremlin.py @@ -267,6 +267,19 @@ def load(self,filename = None): unitcode = "G%d" % (20 + (s.linear_units == 1)) initcode = self.inifile.find("RS274NGC", "RS274NGC_STARTUP_CODE") or "" result, seq = self.load_preview(filename, canon, unitcode, initcode) + # Warn user if spindle-synched feeds violate axis limits + axis_max_vel = tuple([ + float(self.inifile.find("AXIS_%d" % i,"MAX_VELOCITY") or 0.0)*60 + for i in range(9)]) + for line_no, delta, rpm, fpr in canon.feed_synched: + fpm = rpm * fpr # feed per minute + max_fpm = 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 result > gcode.MIN_ERROR: self.report_gcode_error(result, seq, filename) From b0e51c647efa81b9dfbf0cbeae3d167d9492fa24 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 26 Nov 2017 12:58:39 -0500 Subject: [PATCH 042/129] During G96, plan blends for largest possible spindle speed --- src/emc/task/emccanon.cc | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index eff35ce0dc0..f7695a88c35 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -497,7 +497,13 @@ enum FeedRateType static double getActiveFeedRate(FeedRateType angular) { double uuPerRev = feed_mode ? uuPerRev_vel : uuPerRev_pos; - double uu_per_sec = uuPerRev * spindleSpeed_rpm / 60.0; + // KLUDGE effective spindle speed depends on current state + // If CSS mode is active, then we need to assume the worst-case (maximum) spindle speed specified by the D word + // Otherwise, the nominal spindle speed is used + + double planned_spindle_rpm = css_maximum > 0.0 ? css_maximum : spindleSpeed_rpm; + + double uu_per_sec = uuPerRev * planned_spindle_rpm / 60.0; //KLUDGE relies on external state here if (!angular) { @@ -945,6 +951,7 @@ static void flush_segments(void) { double ini_maxvel = linedata.vel; double vel = limitSpindleSpeedByActiveFeedRate(ini_maxvel); + canon_debug("Line segment maximum velocity %f, requested vel %f\n", ini_maxvel, vel); EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; @@ -1952,7 +1959,7 @@ static bool limitSpindleSpeed(double rpm) double limit_rpm = ceil(fabs(rpm)); if (spindleSpeed_rpm <= limit_rpm) { // spindle speed within range, do nothing - canon_debug("Spindle speed %f is within max of %f", spindleSpeed_rpm, limit_rpm); + canon_debug("Spindle speed %f is within max of %f\n", spindleSpeed_rpm, limit_rpm); return false; } EMC_SPINDLE_SPEED emc_spindle_speed_msg; From 5cd9fd866c258a00afa72481a6a245e021c125cd Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 27 Nov 2017 00:20:07 -0500 Subject: [PATCH 043/129] Add spindle-specific fields to emcmotCommand rather than reusing other fields (with confusing names). This avoids recycling fields where the names don't really make sense for spindle use. --- src/emc/motion/command.c | 6 +++--- src/emc/motion/motion.h | 3 +++ src/emc/task/taskintf.cc | 6 +++--- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index d4fb45def6c..aaa44c14a5e 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1530,9 +1530,9 @@ check_stuff ( "before command_handler()" ); /* tpAbort(&emcmotDebug->tp); */ /* SET_MOTION_ERROR_FLAG(1); */ /* } else { */ - 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.velocity_rpm_out = emcmotCommand->spindle_speed; + emcmotStatus->spindle_cmd.css_factor = emcmotCommand->css_factor; + emcmotStatus->spindle_cmd.xoffset = emcmotCommand->css_xoffset; emcmotStatus->spindle_cmd.brake = 0; //disengage brake emcmotStatus->atspeed_next_feed = 1; break; diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index c854cbf006f..8204ac5c346 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -217,6 +217,9 @@ extern "C" { constraints (the ini file) */ int motion_type; /* this move is because of traverse, feed, arc, or toolchange */ double spindlesync; /* user units per spindle revolution, 0 = no sync */ + double spindle_speed; /* commanded spindle speed */ + double css_factor; // Only used during CSS mode + double css_xoffset; // Only used during CSS mode double acc; /* max acceleration */ double backlash; /* amount of backlash */ int id; /* id for motion */ diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index f7cd1d4685d..c6781fb9b4b 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1414,9 +1414,9 @@ int emcSpindleOn(double speed, double css_factor, double offset) { emcmotCommand.command = EMCMOT_SPINDLE_ON; - emcmotCommand.vel = speed; - emcmotCommand.ini_maxvel = css_factor; - emcmotCommand.acc = offset; + emcmotCommand.spindle_speed = speed; + emcmotCommand.css_factor = css_factor; + emcmotCommand.css_xoffset = offset; return usrmotWriteEmcmotCommand(&emcmotCommand); } From 632639d7ea35afca179f49742735616bbc39fa54 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sun, 26 Nov 2017 14:36:58 -0500 Subject: [PATCH 044/129] Only limit spindle speed during position sync, not velocity sync --- src/emc/task/emccanon.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index f7695a88c35..e8eebab8473 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1973,6 +1973,10 @@ static bool limitSpindleSpeed(double rpm) static bool limitSpindleSpeedFromVel(double nominal_vel, double max_vel) { if (!synched || nominal_vel <= 0.0) { + canon_debug("No spindle speed limit required\n"); + return false; + } else if (feed_mode) { + canon_debug("Skipping spindle speed limit due to velocity sync\n"); return false; } const static double SPINDLE_SYNCH_MARGIN = 0.05; From d804e702f525b3310ed13c202867079e149d4d27 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 24 Oct 2018 14:52:50 -0400 Subject: [PATCH 045/129] Fix linking issue with signum function --- src/Makefile | 1 + src/emc/tp/math_util.c | 25 +++++++++++++++++++++++++ src/emc/tp/math_util.h | 25 +++++++++++++++---------- 3 files changed, 41 insertions(+), 10 deletions(-) create mode 100644 src/emc/tp/math_util.c diff --git a/src/Makefile b/src/Makefile index a99e5268680..724c2799922 100644 --- a/src/Makefile +++ b/src/Makefile @@ -942,6 +942,7 @@ motmod-objs += emc/tp/tcq.o motmod-objs += emc/tp/tp.o motmod-objs += emc/tp/spherical_arc.o motmod-objs += emc/tp/blendmath.o +motmod-objs += emc/tp/math_util.o motmod-objs += emc/motion/motion.o motmod-objs += emc/motion/command.o motmod-objs += emc/motion/control.o diff --git a/src/emc/tp/math_util.c b/src/emc/tp/math_util.c new file mode 100644 index 00000000000..f253ea392eb --- /dev/null +++ b/src/emc/tp/math_util.c @@ -0,0 +1,25 @@ +/******************************************************************** +* Description: math_util.c +* +* Simpler math helper functions that seem to be missing from rtapi_math. +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ + +#include "math_util.h" + +long max(long y, long x) { + return y > x ? y : x; +} + +long min(long y, long x) { + return y < x ? y : x; +} + +double signum(double x) { + return (x > 0.0) ? 1.0 : (x < 0.0) ? -1.0 : 0.0; +} diff --git a/src/emc/tp/math_util.h b/src/emc/tp/math_util.h index 127fd22466f..f4adedbdfb6 100644 --- a/src/emc/tp/math_util.h +++ b/src/emc/tp/math_util.h @@ -1,15 +1,20 @@ +/******************************************************************** +* Description: math_util.h +* +* Simpler math helper functions that seem to be missing from rtapi_math. +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ + #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; -} +long max(long y, long x); +long min(long y, long x); +double signum(double x); #endif // MATH_UTIL_H From 59f8b5b914c253da5f2ad3f2f65a41bbe8de0e4b Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 28 Jan 2019 14:09:27 -0500 Subject: [PATCH 046/129] test: Introduce some optimized configs for manual sim tests. * Streamlined PyVCP table for maximum acceleration / velocity limits (for more axes) * Refactored HAL files for sim axes to make each axis more modular and reusable * Redesign axis VCP for 4-axis and 9 axis configs to show velocity / accel violations more compactly. --- .gitignore | 4 + lib/hallib/core_sim9.hal | 3 - src/hal/components/d2dt2.comp | 37 +++ .../circular-arcs/build-debug.sh | 2 +- .../circular-arcs/build-release.sh | 4 +- .../configs/{axis_mm.ini => 6axis_rotary.ini} | 200 ++++++++----- .../circular-arcs/configs/9axis.ini | 81 +++--- .../circular-arcs/configs/XY.ini | 11 +- .../circular-arcs/configs/XYZ_fast.ini | 2 +- .../circular-arcs/configs/XYZ_medium.ini | 2 +- .../circular-arcs/configs/XYZ_nospindle.ini | 2 +- .../circular-arcs/configs/XY_slowZ.ini | 17 +- .../circular-arcs/configs/add_hal_symlinks.sh | 3 - .../circular-arcs/configs/axis-A.tcl | 34 ++- .../circular-arcs/configs/axis-B.tcl | 35 +++ .../circular-arcs/configs/axis-C.tcl | 35 +++ .../circular-arcs/configs/axis-U.tcl | 35 +++ .../circular-arcs/configs/axis-V.tcl | 35 +++ .../circular-arcs/configs/axis-W.tcl | 35 +++ .../circular-arcs/configs/axis-X.tcl | 34 ++- .../circular-arcs/configs/axis-Y.tcl | 34 ++- .../circular-arcs/configs/axis-Z.tcl | 32 +- .../configs/axis_manualtoolchange.hal | 2 +- .../circular-arcs/configs/core_sim9.hal | 1 - .../circular-arcs/configs/core_sim9.tcl | 64 ++++ .../configs/core_sim_components.hal | 29 -- .../configs/{XZ.ini => lathe_XZ.ini} | 7 +- .../configs/load_constraint_components.tcl | 59 ++++ .../configs/{XYZ.ini => mill_XYZA.ini} | 10 +- .../circular-arcs/configs/postgui-A.tcl | 5 + .../circular-arcs/configs/postgui-B.tcl | 6 + .../circular-arcs/configs/postgui-C.tcl | 5 + .../circular-arcs/configs/postgui-U.tcl | 5 + .../circular-arcs/configs/postgui-V.tcl | 5 + .../circular-arcs/configs/postgui-W.tcl | 5 + .../circular-arcs/configs/postgui-X.tcl | 5 + .../circular-arcs/configs/postgui-XYZA.tcl | 5 + .../circular-arcs/configs/postgui-XYZABC.tcl | 3 + .../configs/postgui-XYZABCUVW.tcl | 4 + .../circular-arcs/configs/postgui-XZ.tcl | 3 + .../circular-arcs/configs/postgui-Y.tcl | 5 + .../circular-arcs/configs/postgui-Z.tcl | 5 + .../circular-arcs/configs/postgui-base.tcl | 14 + .../circular-arcs/configs/postgui.hal | 35 --- .../circular-arcs/configs/vcp-XYZA.xml | 148 ++++++++++ .../circular-arcs/configs/vcp-XYZABC.xml | 198 +++++++++++++ .../circular-arcs/configs/vcp-XYZABCUVW.xml | 273 ++++++++++++++++++ .../circular-arcs/configs/vcp-XZ.xml | 98 +++++++ .../circular-arcs/configs/vcp.xml | 249 +++++----------- 49 files changed, 1502 insertions(+), 423 deletions(-) create mode 100644 src/hal/components/d2dt2.comp rename tests/trajectory-planner/circular-arcs/configs/{axis_mm.ini => 6axis_rotary.ini} (53%) delete mode 100755 tests/trajectory-planner/circular-arcs/configs/add_hal_symlinks.sh create mode 100644 tests/trajectory-planner/circular-arcs/configs/axis-B.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/axis-C.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/axis-U.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/axis-V.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/axis-W.tcl delete mode 120000 tests/trajectory-planner/circular-arcs/configs/core_sim9.hal create mode 100644 tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl rename tests/trajectory-planner/circular-arcs/configs/{XZ.ini => lathe_XZ.ini} (97%) create mode 100644 tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl rename tests/trajectory-planner/circular-arcs/configs/{XYZ.ini => mill_XYZA.ini} (97%) create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl create mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl delete mode 100644 tests/trajectory-planner/circular-arcs/configs/postgui.hal create mode 100644 tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml create mode 100644 tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml create mode 100644 tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml create mode 100644 tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml diff --git a/.gitignore b/.gitignore index b92acacf62f..d3d37422074 100644 --- a/.gitignore +++ b/.gitignore @@ -40,3 +40,7 @@ oprofile* position.txt *.9 *.glade.h + +*.glade.h +# expanded INI files +*.ini.expanded diff --git a/lib/hallib/core_sim9.hal b/lib/hallib/core_sim9.hal index 8d83381ce64..b4bcde1241f 100644 --- a/lib/hallib/core_sim9.hal +++ b/lib/hallib/core_sim9.hal @@ -6,9 +6,6 @@ loadrt trivkins # motion controller, get name and thread periods from ini file loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES # load 6 differentiators (for velocity and accel signals -loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv -# load additional blocks -loadrt hypot names=vel_xy,vel_xyz # add motion controller functions to servo thread addf motion-command-handler servo-thread diff --git a/src/hal/components/d2dt2.comp b/src/hal/components/d2dt2.comp new file mode 100644 index 00000000000..e9a96ea8a6a --- /dev/null +++ b/src/hal/components/d2dt2.comp @@ -0,0 +1,37 @@ +// This is a component for EMC2 HAL +// Copyright 2019 Robert Ellenberg +// Based on Jeff Epler's ddt component +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// 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 d2dt2 "Compute two derivativse of the input function by first-order backward difference"; + +pin in float in; +pin out float out1; +pin out float out2; + +variable double old0; +variable double old1; + +function _; +license "GPL"; +;; +double tmp = in; +// Introduces slight rounding error but is faster than division +const double dt_inv = 1.0e9 / period; +// Cascaded, 1st-order-accuracy backward difference +out1 = (tmp - old0) * dt_inv; +out2 = (out1 - old1) * dt_inv; +old0 = tmp; +old1 = out1; diff --git a/tests/trajectory-planner/circular-arcs/build-debug.sh b/tests/trajectory-planner/circular-arcs/build-debug.sh index aee2aad0e97..0ed90ff67f9 100755 --- a/tests/trajectory-planner/circular-arcs/build-debug.sh +++ b/tests/trajectory-planner/circular-arcs/build-debug.sh @@ -2,7 +2,7 @@ cd ../../../src #Ugly way to force rebuild of kinematics, which assumes that tp_debug isn't #used anywhere else... -touch emc/tp/t[cp]*.[ch] +touch emc/tp/tp_debug.h MAKECMD="make -j4 " # Show debug info for each timestep, as well as segment creation #$MAKECMD EXTRA_DEBUG='-DTC_DEBUG -DTP_DEBUG' diff --git a/tests/trajectory-planner/circular-arcs/build-release.sh b/tests/trajectory-planner/circular-arcs/build-release.sh index 02e83500642..5964cccdf76 100755 --- a/tests/trajectory-planner/circular-arcs/build-release.sh +++ b/tests/trajectory-planner/circular-arcs/build-release.sh @@ -2,5 +2,5 @@ cd ../../../src #Ugly way to force rebuild of kinematics, which assumes that tp_debug isn't #used anywhere else... -touch emc/tp/t[cp]*.[ch] -make EXTRA_DEBUG='' +touch emc/tp/tp_debug.h +make -j12 EXTRA_DEBUG='' diff --git a/tests/trajectory-planner/circular-arcs/configs/axis_mm.ini b/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini similarity index 53% rename from tests/trajectory-planner/circular-arcs/configs/axis_mm.ini rename to tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini index e6e12a71e51..da4b6fe408a 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis_mm.ini +++ b/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini @@ -10,20 +10,22 @@ VERSION = $Revision$ # Name of machine, for use with display, etc. -MACHINE = LinuxCNC-HAL-SIM-AXIS +MACHINE = LinuxCNC-Circular-Blend-Tester-XYZ # Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others -# DEBUG = 0x7FFFFFFF +#DEBUG = 0x7FFFFFFF DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZABC.xml # Name of display program, e.g., xemc -DISPLAY = axis +DISPLAY = axis + +GEOMETRY = XYZABC # Cycle time, in seconds, that display will sleep between polls -CYCLE_TIME = 0.100 +CYCLE_TIME = 0.066666666666 # Path to help file HELP_FILE = doc/help.txt @@ -35,18 +37,19 @@ POSITION_OFFSET = RELATIVE POSITION_FEEDBACK = ACTUAL # Highest value that will be allowed for feed override, 1.0 = 100% -MAX_FEED_OVERRIDE = 1.2 +MAX_FEED_OVERRIDE = 2.0 MAX_SPINDLE_OVERRIDE = 1.0 -# Prefix to be used -PROGRAM_PREFIX = ../../../nc_files/ -# Introductory graphic -INTRO_GRAPHIC = linuxcnc.gif -INTRO_TIME = 5 +MAX_LINEAR_VELOCITY = 12 +DEFAULT_LINEAR_VELOCITY = .25 +# Prefix to be used +PROGRAM_PREFIX = ../nc_files/ EDITOR = gedit +TOOL_EDITOR = tooledit + +INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in -INCREMENTS = 1 mm, .01 in, .1mm, 1 mil, .1 mil, 1/8000 in [FILTER] PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image PROGRAM_EXTENSION = .py Python Script @@ -69,7 +72,7 @@ CYCLE_TIME = 0.001 [RS274NGC] # File containing interpreter variables -PARAMETER_FILE = sim_mm.var +PARAMETER_FILE = sim.var # Motion control section ------------------------------------------------------ [EMCMOT] @@ -93,44 +96,47 @@ SERVO_PERIOD = 1000000 # The run script first uses halcmd to execute any HALFILE # files, and then to execute any individual HALCMD commands. # - # list of hal config files to run through halcmd # files are executed in the order in which they appear -HALFILE = core_sim.hal +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +HALFILE = axis-B.tcl +HALFILE = axis-C.tcl + +# Other HAL files HALFILE = sim_spindle_encoder.hal -HALFILE = axis_manualtoolchange.hal -HALFILE = simulated_home.hal -HALFILE = test_status.tcl -# list of halcmd commands to execute -# commands are executed in the order in which they appear -#HALCMD = save neta +HALUI = halui # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal - -HALUI = halui +POSTGUI_HALFILE = postgui-XYZABC.tcl # Trajectory planner section -------------------------------------------------- [TRAJ] -AXES = 3 -COORDINATES = X Y Z -HOME = 0 0 0 -LINEAR_UNITS = mm +AXES = 6 +COORDINATES = X Y Z A B C +HOME = 0 0 0 0 0 0 +LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 -DEFAULT_VELOCITY = 30.48 -MAX_VELOCITY = 53.34 -DEFAULT_ACCELERATION = 508 -MAX_ACCELERATION = 508 -POSITION_FILE = position_mm.txt +DEFAULT_VELOCITY = 1.0 +DEFAULT_ANGULAR_VELOCITY = 45.0 +POSITION_FILE = position9.txt +MAX_LINEAR_VELOCITY = 1.2 +MAX_ANGULAR_VELOCITY = 90.0 + ARC_BLEND_ENABLE = 1 ARC_BLEND_FALLBACK_ENABLE = 0 ARC_BLEND_OPTIMIZATION_DEPTH = 100 -ARC_BLEND_GAP_CYCLES = 2 -ARC_BLEND_RAMP_FREQ = 20 +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + # Axes sections --------------------------------------------------------------- @@ -139,65 +145,114 @@ ARC_BLEND_RAMP_FREQ = 20 TYPE = LINEAR HOME = 0.000 -MAX_VELOCITY = 30.48 -MAX_ACCELERATION = 508 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 BACKLASH = 0.000 -INPUT_SCALE = 157.48 +INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 -MIN_LIMIT = -254 -MAX_LIMIT = 254 -FERROR = 1.27 -MIN_FERROR = .254 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 127 -HOME_LATCH_VEL = 25.4 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO -HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 -HOME_IS_SHARED = 1 +HOME_SEQUENCE = 0 # Second axis [AXIS_1] TYPE = LINEAR HOME = 0.000 -MAX_VELOCITY = 30.48 -MAX_ACCELERATION = 508 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 BACKLASH = 0.000 -INPUT_SCALE = 157.48 +INPUT_SCALE = 2000 OUTPUT_SCALE = 1.000 -MIN_LIMIT = -254 -MAX_LIMIT = 254 -FERROR = 1.27 -MIN_FERROR = .254 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 127 -HOME_LATCH_VEL = 25.4 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO -HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 # Third axis [AXIS_2] TYPE = LINEAR HOME = 0.0 -MAX_VELOCITY = 30.48 -MAX_ACCELERATION = 508 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] + +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +BACKLASH = 0.000 +INPUT_SCALE = 40 +OUTPUT_SCALE = 1.000 +FERROR = 5.0 +MIN_FERROR = 1.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_IGNORE_LIMITS = NO +HOME_SEQUENCE = 0 + +[AXIS_4] + +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 BACKLASH = 0.000 -INPUT_SCALE = 157.48 +INPUT_SCALE = 40 OUTPUT_SCALE = 1.000 -MIN_LIMIT = -50.8 -MAX_LIMIT = 101.6 -FERROR = 1.27 -MIN_FERROR = .254 -HOME_OFFSET = 25.4 -HOME_SEARCH_VEL = 127 -HOME_LATCH_VEL = 25.4 +FERROR = 5.0 +MIN_FERROR = 1.0 +HOME_OFFSET = 45.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_IGNORE_LIMITS = NO +HOME_SEQUENCE = 0 + +[AXIS_5] + +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 90.0 +MAX_ACCELERATION = 1200.0 +BACKLASH = 0.000 +INPUT_SCALE = 40 +OUTPUT_SCALE = 1.000 +FERROR = 5.0 +MIN_FERROR = 1.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO HOME_SEQUENCE = 0 -HOME_IS_SHARED = 1 # section for main IO controller parameters ----------------------------------- [EMCIO] @@ -206,8 +261,9 @@ HOME_IS_SHARED = 1 EMCIO = io # cycle time, in seconds -CYCLE_TIME = 0.100 +CYCLE_TIME = 0.066666666666 # tool table file -TOOL_TABLE = sim_mm.tbl -TOOL_CHANGE_POSITION = 0 0 50.8 +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/9axis.ini b/tests/trajectory-planner/circular-arcs/configs/9axis.ini index 4f8a973edd6..fd660aeee5f 100644 --- a/tests/trajectory-planner/circular-arcs/configs/9axis.ini +++ b/tests/trajectory-planner/circular-arcs/configs/9axis.ini @@ -18,12 +18,12 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp_9axis.xml +PYVCP = vcp-XYZABCUVW.xml # Name of display program, e.g., xemc DISPLAY = axis # Cycle time, in seconds, that display will sleep between polls -CYCLE_TIME = 0.100 +CYCLE_TIME = 0.066666666666 # Path to help file HELP_FILE = doc/help.txt @@ -35,18 +35,18 @@ POSITION_OFFSET = RELATIVE POSITION_FEEDBACK = ACTUAL # Highest value that will be allowed for feed override, 1.0 = 100% -MAX_FEED_OVERRIDE = 1.2 +MAX_FEED_OVERRIDE = 2.0 MAX_SPINDLE_OVERRIDE = 1.0 + +MAX_LINEAR_VELOCITY = 12 +DEFAULT_LINEAR_VELOCITY = .25 # Prefix to be used PROGRAM_PREFIX = ../nc_files/ -# Introductory graphic -INTRO_GRAPHIC = linuxcnc.gif -INTRO_TIME = 5 - EDITOR = gedit +TOOL_EDITOR = tooledit -INCREMENTS = 1 mm, .01 in, .1mm, 1 mil, .1 mil, 1/8000 in +INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in GEOMETRY = XYZABCUVW @@ -85,6 +85,8 @@ COMM_TIMEOUT = 1.0 # Interval between tries to emcmot, in seconds COMM_WAIT = 0.010 +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 # Servo task period, in nano-seconds SERVO_PERIOD = 1000000 @@ -97,15 +99,23 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear -HALFILE = core_sim9.hal -HALFILE = axis_manualtoolchange.hal -HALFILE = simulated_home.hal -HALFILE = locking_indexer.hal -HALFILE = constraint_check_9axis.tcl +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +HALFILE = axis-B.tcl +HALFILE = axis-C.tcl +HALFILE = axis-U.tcl +HALFILE = axis-V.tcl +HALFILE = axis-W.tcl +# Other HAL files +HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui_9axis.hal +POSTGUI_HALFILE = postgui-XYZABCUVW.tcl HALUI = halui @@ -126,12 +136,14 @@ MAX_ANGULAR_VELOCITY = 90.0 ARC_BLEND_ENABLE = 1 ARC_BLEND_FALLBACK_ENABLE = 0 -ARC_BLEND_OPTIMIZATION_DEPTH = 50 +ARC_BLEND_OPTIMIZATION_DEPTH = 100 ARC_BLEND_GAP_CYCLES = 4 -ARC_BLEND_RAMP_FREQ = 20 +ARC_BLEND_RAMP_FREQ = 100 + # Axes sections --------------------------------------------------------------- +# First axis [AXIS_0] TYPE = LINEAR @@ -146,12 +158,12 @@ MAX_LIMIT = 10.0 FERROR = 0.050 MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 5.0 -HOME_LATCH_VEL = 1.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 -HOME_IS_SHARED = 1 +HOME_SEQUENCE = 0 +HOME_IS_SHARED = 0 [AXIS_1] @@ -167,11 +179,11 @@ MAX_LIMIT = 10.0 FERROR = 0.050 MIN_FERROR = 0.010 HOME_OFFSET = 0.0 -HOME_SEARCH_VEL = 5.0 -HOME_LATCH_VEL = 1.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 [AXIS_2] @@ -186,13 +198,13 @@ MIN_LIMIT = -2.0 MAX_LIMIT = 4.0 FERROR = 0.050 MIN_FERROR = 0.010 -HOME_OFFSET = 1.0 -HOME_SEARCH_VEL = 5.0 -HOME_LATCH_VEL = 1.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO HOME_SEQUENCE = 0 -HOME_IS_SHARED = 1 +HOME_IS_SHARED = 0 [AXIS_3] @@ -210,14 +222,14 @@ HOME_SEARCH_VEL = 0.0 HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 [AXIS_4] TYPE = ANGULAR HOME = 0.0 -MAX_VELOCITY = 90.0 -MAX_ACCELERATION = 1200.0 +MAX_VELOCITY = 100.0 +MAX_ACCELERATION = 1000.0 BACKLASH = 0.000 INPUT_SCALE = 40 OUTPUT_SCALE = 1.000 @@ -228,15 +240,14 @@ HOME_SEARCH_VEL = 0.0 HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 -LOCKING_INDEXER = 1 +HOME_SEQUENCE = 0 [AXIS_5] TYPE = ANGULAR HOME = 0.0 -MAX_VELOCITY = 90.0 -MAX_ACCELERATION = 1200.0 +MAX_VELOCITY = 50.0 +MAX_ACCELERATION = 1000.0 BACKLASH = 0.000 INPUT_SCALE = 40 OUTPUT_SCALE = 1.000 @@ -247,7 +258,7 @@ HOME_SEARCH_VEL = 0.0 HOME_LATCH_VEL = 0.0 HOME_USE_INDEX = NO HOME_IGNORE_LIMITS = NO -HOME_SEQUENCE = 1 +HOME_SEQUENCE = 0 [AXIS_6] diff --git a/tests/trajectory-planner/circular-arcs/configs/XY.ini b/tests/trajectory-planner/circular-arcs/configs/XY.ini index 4d2678ff4f3..80bdd3f4b55 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY.ini @@ -19,7 +19,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] #Includes acceleration / velocity -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -98,13 +98,12 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear -# Core sim stuff for various loaded axes HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl # Other HAL files -HALFILE = axis_manualtoolchange.hal HALFILE = sim_spindle_encoder.hal # list of halcmd commands to execute @@ -113,7 +112,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui-XYZA.ini HALUI = halui @@ -122,7 +121,7 @@ HALUI = halui AXES = 2 COORDINATES = X Y -HOME = 0 0 0 +HOME = 0 0 LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini index a7aae358939..a3efe84420a 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini index d41f40290a9..9c025f08504 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini index b1e63f1b14e..76156db05cb 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis diff --git a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini index 877eb86e91c..18e8a4190cc 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -48,6 +48,8 @@ TOOL_EDITOR = tooledit INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in +GEOMETRY = XYZA + [FILTER] PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image PROGRAM_EXTENSION = .py Python Script @@ -98,12 +100,12 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl # Other HAL files -HALFILE = axis_manualtoolchange.hal HALFILE = sim_spindle_encoder.hal # list of halcmd commands to execute @@ -112,7 +114,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui-XYZA.tcl HALUI = halui @@ -121,7 +123,7 @@ HALUI = halui AXES = 4 COORDINATES = X Y Z A -HOME = 0 0 0 +HOME = 0 0 0 0 LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 @@ -131,9 +133,10 @@ MAX_LINEAR_VELOCITY = 12 ARC_BLEND_ENABLE = 1 ARC_BLEND_FALLBACK_ENABLE = 0 -ARC_BLEND_OPTIMIZATION_DEPTH = 50 +ARC_BLEND_OPTIMIZATION_DEPTH = 100 ARC_BLEND_GAP_CYCLES = 4 -ARC_BLEND_RAMP_FREQ = 20 +ARC_BLEND_RAMP_FREQ = 100 + # Axes sections --------------------------------------------------------------- diff --git a/tests/trajectory-planner/circular-arcs/configs/add_hal_symlinks.sh b/tests/trajectory-planner/circular-arcs/configs/add_hal_symlinks.sh deleted file mode 100755 index 224b40c8e2e..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/add_hal_symlinks.sh +++ /dev/null @@ -1,3 +0,0 @@ -ln -sf ../../../../configs/sim/core_sim.hal -ln -sf ../../../../configs/sim/axis_manualtoolchange.hal -ln -sf ../../../../configs/sim/sim_spindle_encoder.hal diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl index 1719f57583c..9a593cc73b9 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-A.tcl @@ -1,21 +1,35 @@ +set av_id 3 +set aa_id 13 + +# Add axis components to servo thread +addf wcomp.$av_id servo-thread +addf wcomp.$aa_id servo-thread +addf minmax.$av_id servo-thread +addf minmax.$aa_id servo-thread +addf d2dt2.$av_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Apos axis.3.motor-pos-cmd => axis.3.motor-pos-fb ddt_a.in +net a_pos axis.$av_id.motor-pos-cmd => axis.$av_id.motor-pos-fb d2dt2.$av_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Avel ddt_a.out => ddt_av.in -net Aacc <= ddt_av.out +net a_vel <= d2dt2.$av_id.out1 => wcomp.$av_id.in minmax.$av_id.in +net a_acc <= d2dt2.$av_id.out2 => wcomp.$aa_id.in minmax.$aa_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_aacc.max $::AXIS_3(MAX_ACCELERATION)*$acc_limit -setp wcomp_aacc.min $::AXIS_3(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_avel.max $::AXIS_3(MAX_VELOCITY)*$vel_limit -setp wcomp_avel.min $::AXIS_3(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$aa_id.max $::AXIS_3(MAX_ACCELERATION)*$acc_limit +setp wcomp.$aa_id.min $::AXIS_3(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$av_id.max $::AXIS_3(MAX_VELOCITY)*$vel_limit +setp wcomp.$av_id.min $::AXIS_3(MAX_VELOCITY)*-1.0*$vel_limit + +net a_acc-ok <= wcomp.$av_id.out => match_abc.a0 +net a_vel-ok <= wcomp.$aa_id.out => match_abc.a1 + +# Enable match_all pins for axis +setp match_abc.b0 1 +setp match_abc.b1 1 -# Enable match_all pins for A axis -setp match_all.b6 1 -setp match_all.b7 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-B.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-B.tcl new file mode 100644 index 00000000000..72c3e5a358f --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-B.tcl @@ -0,0 +1,35 @@ +set bv_id 4 +set ba_id 14 + +# Add axis components to servo thread +addf wcomp.$bv_id servo-thread +addf wcomp.$ba_id servo-thread +addf minmax.$bv_id servo-thread +addf minmax.$ba_id servo-thread +addf d2dt2.$bv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net b_pos axis.$bv_id.motor-pos-cmd => axis.$bv_id.motor-pos-fb d2dt2.$bv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net b_vel <= d2dt2.$bv_id.out1 => wcomp.$bv_id.in minmax.$bv_id.in +net b_acc <= d2dt2.$bv_id.out2 => wcomp.$ba_id.in minmax.$ba_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ba_id.max $::AXIS_4(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ba_id.min $::AXIS_4(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$bv_id.max $::AXIS_4(MAX_VELOCITY)*$vel_limit +setp wcomp.$bv_id.min $::AXIS_4(MAX_VELOCITY)*-1.0*$vel_limit + +net b_acc-ok <= wcomp.$bv_id.out => match_abc.a2 +net b_vel-ok <= wcomp.$ba_id.out => match_abc.a3 + +# Enable match_all pins for axis +setp match_abc.b2 1 +setp match_abc.b3 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-C.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-C.tcl new file mode 100644 index 00000000000..f08ea4873cb --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-C.tcl @@ -0,0 +1,35 @@ +set cv_id 5 +set ca_id 15 + +# Add axis components to servo thread +addf wcomp.$cv_id servo-thread +addf wcomp.$ca_id servo-thread +addf minmax.$cv_id servo-thread +addf minmax.$ca_id servo-thread +addf d2dt2.$cv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net c_pos axis.$cv_id.motor-pos-cmd => axis.$cv_id.motor-pos-fb d2dt2.$cv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net c_vel <= d2dt2.$cv_id.out1 => wcomp.$cv_id.in minmax.$cv_id.in +net c_acc <= d2dt2.$cv_id.out2 => wcomp.$ca_id.in minmax.$ca_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ca_id.max $::AXIS_5(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ca_id.min $::AXIS_5(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$cv_id.max $::AXIS_5(MAX_VELOCITY)*$vel_limit +setp wcomp.$cv_id.min $::AXIS_5(MAX_VELOCITY)*-1.0*$vel_limit + +net c_acc-ok <= wcomp.$cv_id.out => match_abc.a4 +net c_vel-ok <= wcomp.$ca_id.out => match_abc.a5 + +# Enable match_all pins for axis +setp match_abc.b4 1 +setp match_abc.b5 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-U.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-U.tcl new file mode 100644 index 00000000000..b8ee079f57b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-U.tcl @@ -0,0 +1,35 @@ +set uv_id 6 +set ua_id 16 + +# Add axis components to servo thread +addf wcomp.$uv_id servo-thread +addf wcomp.$ua_id servo-thread +addf minmax.$uv_id servo-thread +addf minmax.$ua_id servo-thread +addf d2dt2.$uv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net u_pos axis.$uv_id.motor-pos-cmd => axis.$uv_id.motor-pos-fb d2dt2.$uv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net u_vel <= d2dt2.$uv_id.out1 => wcomp.$uv_id.in minmax.$uv_id.in +net u_acc <= d2dt2.$uv_id.out2 => wcomp.$ua_id.in minmax.$ua_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ua_id.max $::AXIS_6(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ua_id.min $::AXIS_6(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$uv_id.max $::AXIS_6(MAX_VELOCITY)*$vel_limit +setp wcomp.$uv_id.min $::AXIS_6(MAX_VELOCITY)*-1.0*$vel_limit + +net u_acc-ok <= wcomp.$uv_id.out => match_uvw.a0 +net u_vel-ok <= wcomp.$ua_id.out => match_uvw.a1 + +# Enable match_all pins for axis +setp match_uvw.b0 1 +setp match_uvw.b1 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-V.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-V.tcl new file mode 100644 index 00000000000..8edd456d68c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-V.tcl @@ -0,0 +1,35 @@ +set vv_id 7 +set va_id 17 + +# Add axis components to servo thread +addf wcomp.$vv_id servo-thread +addf wcomp.$va_id servo-thread +addf minmax.$vv_id servo-thread +addf minmax.$va_id servo-thread +addf d2dt2.$vv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net v_pos axis.$vv_id.motor-pos-cmd => axis.$vv_id.motor-pos-fb d2dt2.$vv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net v_vel <= d2dt2.$vv_id.out1 => wcomp.$vv_id.in minmax.$vv_id.in +net v_acc <= d2dt2.$vv_id.out2 => wcomp.$va_id.in minmax.$va_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$va_id.max $::AXIS_7(MAX_ACCELERATION)*$acc_limit +setp wcomp.$va_id.min $::AXIS_7(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$vv_id.max $::AXIS_7(MAX_VELOCITY)*$vel_limit +setp wcomp.$vv_id.min $::AXIS_7(MAX_VELOCITY)*-1.0*$vel_limit + +net v_acc-ok <= wcomp.$vv_id.out => match_uvw.a2 +net v_vel-ok <= wcomp.$va_id.out => match_uvw.a3 + +# Enable match_all pins for axis +setp match_uvw.b2 1 +setp match_uvw.b3 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-W.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-W.tcl new file mode 100644 index 00000000000..037be82939b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/axis-W.tcl @@ -0,0 +1,35 @@ +set wv_id 8 +set wa_id 18 + +# Add axis components to servo thread +addf wcomp.$wv_id servo-thread +addf wcomp.$wa_id servo-thread +addf minmax.$wv_id servo-thread +addf minmax.$wa_id servo-thread +addf d2dt2.$wv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net w_pos axis.$wv_id.motor-pos-cmd => axis.$wv_id.motor-pos-fb d2dt2.$wv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net w_vel <= d2dt2.$wv_id.out1 => wcomp.$wv_id.in minmax.$wv_id.in +net w_acc <= d2dt2.$wv_id.out2 => wcomp.$wa_id.in minmax.$wa_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$wa_id.max $::AXIS_8(MAX_ACCELERATION)*$acc_limit +setp wcomp.$wa_id.min $::AXIS_8(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$wv_id.max $::AXIS_8(MAX_VELOCITY)*$vel_limit +setp wcomp.$wv_id.min $::AXIS_8(MAX_VELOCITY)*-1.0*$vel_limit + +net w_acc-ok <= wcomp.$wv_id.out => match_uvw.a4 +net w_vel-ok <= wcomp.$wa_id.out => match_uvw.a5 + +# Enable match_all pins for axis +setp match_uvw.b4 1 +setp match_uvw.b5 1 + diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl index e39ca202ce2..0c549683595 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-X.tcl @@ -1,21 +1,35 @@ +set xv_id 0 +set xa_id 10 + +# Add axis components to servo thread +addf wcomp.$xv_id servo-thread +addf wcomp.$xa_id servo-thread +addf minmax.$xv_id servo-thread +addf minmax.$xa_id servo-thread +addf d2dt2.$xv_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Xpos axis.0.motor-pos-cmd => axis.0.motor-pos-fb ddt_x.in +net x_pos axis.$xv_id.motor-pos-cmd => axis.$xv_id.motor-pos-fb d2dt2.$xv_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Xvel ddt_x.out => ddt_xv.in vel_xy.in0 -net Xacc <= ddt_xv.out +net x_vel <= d2dt2.$xv_id.out1 => vel_xyz.in$xv_id wcomp.$xv_id.in minmax.$xv_id.in +net x_acc <= d2dt2.$xv_id.out2 => wcomp.$xa_id.in minmax.$xa_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_xacc.max $::AXIS_0(MAX_ACCELERATION)*$acc_limit -setp wcomp_xacc.min $::AXIS_0(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_xvel.max $::AXIS_0(MAX_VELOCITY)*$vel_limit -setp wcomp_xvel.min $::AXIS_0(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$xa_id.max $::AXIS_0(MAX_ACCELERATION)*$acc_limit +setp wcomp.$xa_id.min $::AXIS_0(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$xv_id.max $::AXIS_0(MAX_VELOCITY)*$vel_limit +setp wcomp.$xv_id.min $::AXIS_0(MAX_VELOCITY)*-1.0*$vel_limit + +net x_acc-ok <= wcomp.$xv_id.out => match_xyz.a0 +net x_vel-ok <= wcomp.$xa_id.out => match_xyz.a1 + +# Enable match_all pins for axis +setp match_xyz.b0 1 +setp match_xyz.b1 1 -# Enable match_all pins for X axis -setp match_all.b0 1 -setp match_all.b1 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl index e18ff0207ff..0c393ed57c6 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-Y.tcl @@ -1,21 +1,35 @@ +set yv_id 1 +set ya_id 11 + +# Add axis components to servo thread +addf wcomp.$yv_id servo-thread +addf wcomp.$ya_id servo-thread +addf minmax.$yv_id servo-thread +addf minmax.$ya_id servo-thread +addf d2dt2.$yv_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Ypos axis.1.motor-pos-cmd => axis.1.motor-pos-fb ddt_y.in +net y_pos axis.$yv_id.motor-pos-cmd => axis.$yv_id.motor-pos-fb d2dt2.$yv_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Yvel ddt_y.out => ddt_yv.in vel_xy.in1 -net Yacc <= ddt_yv.out +net y_vel <= d2dt2.$yv_id.out1 => vel_xyz.in$yv_id wcomp.$yv_id.in minmax.$yv_id.in +net y_acc <= d2dt2.$yv_id.out2 => wcomp.$ya_id.in minmax.$ya_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_yacc.max $::AXIS_1(MAX_ACCELERATION)*$acc_limit -setp wcomp_yacc.min $::AXIS_1(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_yvel.max $::AXIS_1(MAX_VELOCITY)*$vel_limit -setp wcomp_yvel.min $::AXIS_1(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$ya_id.max $::AXIS_1(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ya_id.min $::AXIS_1(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$yv_id.max $::AXIS_1(MAX_VELOCITY)*$vel_limit +setp wcomp.$yv_id.min $::AXIS_1(MAX_VELOCITY)*-1.0*$vel_limit + +net y_acc-ok <= wcomp.$yv_id.out => match_xyz.a2 +net y_vel-ok <= wcomp.$ya_id.out => match_xyz.a3 + +# Enable match_all pins for axis +setp match_xyz.b2 1 +setp match_xyz.b3 1 -# Enable match_all pins for Y axis -setp match_all.b2 1 -setp match_all.b3 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl b/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl index 2e166eb624c..87e186ce971 100644 --- a/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/axis-Z.tcl @@ -1,22 +1,34 @@ +set zv_id 2 +set za_id 12 + +# Add axis components to servo thread +addf wcomp.$zv_id servo-thread +addf wcomp.$za_id servo-thread +addf minmax.$zv_id servo-thread +addf minmax.$za_id servo-thread +addf d2dt2.$zv_id servo-thread + # create HAL signals for position commands from motion module # loop position commands back to motion module feedback -net Zpos axis.2.motor-pos-cmd => axis.2.motor-pos-fb ddt_z.in +net z_pos axis.$zv_id.motor-pos-cmd => axis.$zv_id.motor-pos-fb d2dt2.$zv_id.in # send the position commands thru differentiators to # generate velocity and accel signals -net Zvel ddt_z.out => ddt_zv.in vel_xyz.in0 -net Zacc <= ddt_zv.out +net z_vel <= d2dt2.$zv_id.out1 => vel_xyz.in$zv_id wcomp.$zv_id.in minmax.$zv_id.in +net z_acc <= d2dt2.$zv_id.out2 => wcomp.$za_id.in minmax.$za_id.in #Conservative limits set acc_limit 1.0001 set vel_limit 1.01 -setp wcomp_zacc.max $::AXIS_2(MAX_ACCELERATION)*$acc_limit -setp wcomp_zacc.min $::AXIS_2(MAX_ACCELERATION)*-1.0*$acc_limit -setp wcomp_zvel.max $::AXIS_2(MAX_VELOCITY)*$vel_limit -setp wcomp_zvel.min $::AXIS_2(MAX_VELOCITY)*-1.0*$vel_limit +setp wcomp.$za_id.max $::AXIS_2(MAX_ACCELERATION)*$acc_limit +setp wcomp.$za_id.min $::AXIS_2(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$zv_id.max $::AXIS_2(MAX_VELOCITY)*$vel_limit +setp wcomp.$zv_id.min $::AXIS_2(MAX_VELOCITY)*-1.0*$vel_limit -# Enable match_all pins for Z axis -setp match_all.b4 1 -setp match_all.b5 1 +net z_acc-ok <= wcomp.$zv_id.out => match_xyz.a4 +net z_vel-ok <= wcomp.$za_id.out => match_xyz.a5 +# Enable match_all pins for axis +setp match_xyz.b4 1 +setp match_xyz.b5 1 diff --git a/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal b/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal index 356965ee0ad..ba56cdd7fc7 120000 --- a/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal +++ b/tests/trajectory-planner/circular-arcs/configs/axis_manualtoolchange.hal @@ -1 +1 @@ -../../../../configs/sim/axis_manualtoolchange.hal \ No newline at end of file +../../../../lib/hallib/axis_manualtoolchange.hal \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/configs/core_sim9.hal b/tests/trajectory-planner/circular-arcs/configs/core_sim9.hal deleted file mode 120000 index 578484434cc..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/core_sim9.hal +++ /dev/null @@ -1 +0,0 @@ -../../../../configs/sim/core_sim9.hal \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl b/tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl new file mode 100644 index 00000000000..2e191cd6adf --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl @@ -0,0 +1,64 @@ +# core HAL config file for simulation + +# first load all the RT modules that will be needed +# kinematics +loadrt trivkins +# motion controller, get name and thread periods from ini file +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES +# load 6 differentiators (for velocity and accel signals + +# add motion controller functions to servo thread +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +set next_ddt_idx = 0 + +# link the differentiator functions into the code +addf ddt_x servo-thread +addf ddt_xv servo-thread +addf ddt_y servo-thread +addf ddt_yv servo-thread +addf ddt_z servo-thread +addf ddt_zv servo-thread +addf vel_xy servo-thread +addf vel_xyz servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net Xpos axis.0.motor-pos-cmd => axis.0.motor-pos-fb ddt_x.in +net Ypos axis.1.motor-pos-cmd => axis.1.motor-pos-fb ddt_y.in +net Zpos axis.2.motor-pos-cmd => axis.2.motor-pos-fb ddt_z.in +net Apos axis.3.motor-pos-cmd => axis.3.motor-pos-fb +net Bpos axis.4.motor-pos-cmd => axis.4.motor-pos-fb +net Cpos axis.5.motor-pos-cmd => axis.5.motor-pos-fb +net Upos axis.6.motor-pos-cmd => axis.6.motor-pos-fb +net Vpos axis.7.motor-pos-cmd => axis.7.motor-pos-fb +net Wpos axis.8.motor-pos-cmd => axis.8.motor-pos-fb + +# send the position commands thru differentiators to +# generate velocity and accel signals +net Xvel ddt_x.out => ddt_xv.in vel_xy.in0 +net Xacc <= ddt_xv.out +net Yvel ddt_y.out => ddt_yv.in vel_xy.in1 +net Yacc <= ddt_yv.out +net Zvel ddt_z.out => ddt_zv.in vel_xyz.in0 +net Zacc <= ddt_zv.out + +# Cartesian 2- and 3-axis velocities +net XYvel vel_xy.out => vel_xyz.in1 +net XYZvel <= vel_xyz.out + +# estop loopback +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in + +# create signals for tool loading loopback +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed + +net spindle-fwd motion.spindle-forward +net spindle-rev motion.spindle-reverse +#net spindle-speed motion.spindle-speed-out + +net lube iocontrol.0.lube +net flood iocontrol.0.coolant-flood +net mist iocontrol.0.coolant-mist diff --git a/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal b/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal index fad98ec4126..7f32d4a7384 100644 --- a/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal +++ b/tests/trajectory-planner/circular-arcs/configs/core_sim_components.hal @@ -5,40 +5,11 @@ loadrt trivkins # motion controller, get name and thread periods from ini file loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES -# load 6 differentiators (for velocity and accel signals -loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv,ddt_a,ddt_av -# load additional blocks -loadrt hypot names=vel_xy,vel_xyz # add motion controller functions to servo thread addf motion-command-handler servo-thread addf motion-controller servo-thread -# link the differentiator functions into the code -addf ddt_x servo-thread -addf ddt_xv servo-thread -addf ddt_y servo-thread -addf ddt_yv servo-thread -addf ddt_z servo-thread -addf ddt_zv servo-thread -addf ddt_a servo-thread -addf ddt_av servo-thread - -addf vel_xy servo-thread -addf vel_xyz servo-thread - -# create HAL signals for position commands from motion module -# loop position commands back to motion module feedback -# NOTE handled in axis-[XYZ].hal - -# send the position commands thru differentiators to -# generate velocity and accel signals -# NOTE handled in axis-[XYZ].hal - -# Cartesian 2- and 3-axis velocities -net XYvel vel_xy.out => vel_xyz.in1 -net XYZvel <= vel_xyz.out - # estop loopback net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in diff --git a/tests/trajectory-planner/circular-arcs/configs/XZ.ini b/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini similarity index 97% rename from tests/trajectory-planner/circular-arcs/configs/XZ.ini rename to tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini index 974a24d4e2e..6a049c334dc 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini @@ -19,7 +19,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] #Includes acceleration / velocity -PYVCP = vcp.xml +PYVCP = vcp-XZ.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -100,11 +100,10 @@ SERVO_PERIOD = 1000000 # files are executed in the order in which they appear # Core sim stuff for various loaded axes HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Z.tcl # Other HAL files -HALFILE = axis_manualtoolchange.hal HALFILE = sim_spindle_encoder.hal # list of halcmd commands to execute @@ -113,7 +112,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui_XZ.tcl HALUI = halui diff --git a/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl b/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl new file mode 100644 index 00000000000..22f08405d55 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl @@ -0,0 +1,59 @@ +# Load all constraint-checking components used in HAL files for various sim configurations +# NOTE: add only the actively used ones to the RT thread in axis TCL files + +loadrt d2dt2 count=10 +# load additional blocks +loadrt hypot names=vel_xy,vel_xyz +addf vel_xy servo-thread +addf vel_xyz servo-thread + +# Compares computed velocities / accelerations to limits +loadrt wcomp count=20 + +# Track the overall min / max value of each signal reached (for VCP display) +loadrt minmax count=20 + +# Ties all limit checks together (used to force a motion enable / disable) +loadrt match8 names=match_xyz,match_abc,match_uvw,match_all + +net XYZ-limits-ok <= match_xyz.out => match_all.a0 +net ABC-limits-ok <= match_abc.out => match_all.a1 +net UVW-limits-ok <= match_uvw.out => match_all.a2 + +setp match_xyz.b0 0 +setp match_xyz.b1 0 +setp match_xyz.b2 0 +setp match_xyz.b3 0 +setp match_xyz.b4 0 +setp match_xyz.b5 0 +setp match_xyz.b6 0 +setp match_xyz.b7 0 + +setp match_abc.b0 0 +setp match_abc.b1 0 +setp match_abc.b2 0 +setp match_abc.b3 0 +setp match_abc.b4 0 +setp match_abc.b5 0 +setp match_abc.b6 0 +setp match_abc.b7 0 + +setp match_uvw.b0 0 +setp match_uvw.b1 0 +setp match_uvw.b2 0 +setp match_uvw.b3 0 +setp match_uvw.b4 0 +setp match_uvw.b5 0 +setp match_uvw.b6 0 +setp match_uvw.b7 0 + +setp match_all.b0 1 +setp match_all.b1 1 +setp match_all.b2 1 +setp match_all.b3 0 +setp match_all.b4 0 +setp match_all.b5 0 +setp match_all.b6 0 +setp match_all.b7 0 + +net constraints-ok <= match_all.out diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ.ini b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini similarity index 97% rename from tests/trajectory-planner/circular-arcs/configs/XYZ.ini rename to tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini index 9c527b7337d..a8cf7fe9345 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini @@ -18,7 +18,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp.xml +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc DISPLAY = axis @@ -48,6 +48,8 @@ TOOL_EDITOR = tooledit INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in +GEOMETRY = XYZA + [FILTER] PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image PROGRAM_EXTENSION = .py Python Script @@ -98,7 +100,7 @@ SERVO_PERIOD = 1000000 # list of hal config files to run through halcmd # files are executed in the order in which they appear HALFILE = core_sim_components.hal -HALFILE = test_status.tcl +HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl HALFILE = axis-Z.tcl @@ -112,7 +114,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui.hal +POSTGUI_HALFILE = postgui-XYZA.tcl HALUI = halui @@ -121,7 +123,7 @@ HALUI = halui AXES = 4 COORDINATES = X Y Z A -HOME = 0 0 0 +HOME = 0 0 0 0 LINEAR_UNITS = inch ANGULAR_UNITS = degree CYCLE_TIME = 0.010 diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl new file mode 100644 index 00000000000..c7135c8543a --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-A.tcl @@ -0,0 +1,5 @@ +net paacc minmax.13.max pyvcp.paacc +net naacc minmax.13.min pyvcp.naacc + +net pavel minmax.3.max pyvcp.pavel +net navel minmax.3.min pyvcp.navel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl new file mode 100644 index 00000000000..478759c5f82 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-B.tcl @@ -0,0 +1,6 @@ + +net pbacc minmax.14.max pyvcp.pbacc +net nbacc minmax.14.min pyvcp.nbacc + +net pbvel minmax.4.max pyvcp.pbvel +net nbvel minmax.4.min pyvcp.nbvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl new file mode 100644 index 00000000000..b26e7928e57 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-C.tcl @@ -0,0 +1,5 @@ +net pcacc minmax.15.max pyvcp.pcacc +net ncacc minmax.15.min pyvcp.ncacc + +net pcvel minmax.5.max pyvcp.pcvel +net ncvel minmax.5.min pyvcp.ncvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl new file mode 100644 index 00000000000..a109ebac3c7 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-U.tcl @@ -0,0 +1,5 @@ +net puacc minmax.16.max pyvcp.puacc +net nuacc minmax.16.min pyvcp.nuacc + +net puvel minmax.6.max pyvcp.puvel +net nuvel minmax.6.min pyvcp.nuvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl new file mode 100644 index 00000000000..47b20bfb004 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-V.tcl @@ -0,0 +1,5 @@ +net pvacc minmax.17.max pyvcp.pvacc +net nvacc minmax.17.min pyvcp.nvacc + +net pvvel minmax.7.max pyvcp.pvvel +net nvvel minmax.7.min pyvcp.nvvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl new file mode 100644 index 00000000000..7c47ef29bfc --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-W.tcl @@ -0,0 +1,5 @@ +net pwacc minmax.18.max pyvcp.pwacc +net nwacc minmax.18.min pyvcp.nwacc + +net pwvel minmax.8.max pyvcp.pwvel +net nwvel minmax.8.min pyvcp.nwvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl new file mode 100644 index 00000000000..e3d58c6915d --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-X.tcl @@ -0,0 +1,5 @@ +net pxacc minmax.10.max pyvcp.pxacc +net nxacc minmax.10.min pyvcp.nxacc + +net pxvel minmax.0.max pyvcp.pxvel +net nxvel minmax.0.min pyvcp.nxvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl new file mode 100644 index 00000000000..bf18663953e --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZA.tcl @@ -0,0 +1,5 @@ +source postgui-base.tcl +source postgui-X.tcl +source postgui-Y.tcl +source postgui-Z.tcl +source postgui-A.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl new file mode 100644 index 00000000000..a3c91db0c34 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABC.tcl @@ -0,0 +1,3 @@ +source postgui-XYZA.tcl +source postgui-B.tcl +source postgui-C.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl new file mode 100644 index 00000000000..be4e9a1624c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XYZABCUVW.tcl @@ -0,0 +1,4 @@ +source postgui-XYZABC.tcl +source postgui-U.tcl +source postgui-V.tcl +source postgui-W.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl new file mode 100644 index 00000000000..de58c0c86fc --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-XZ.tcl @@ -0,0 +1,3 @@ +source postgui-base.tcl +source postgui-X.tcl +source postgui-Z.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl new file mode 100644 index 00000000000..1b1aa7aed0c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-Y.tcl @@ -0,0 +1,5 @@ +net pyacc minmax.11.max pyvcp.pyacc +net nyacc minmax.11.min pyvcp.nyacc + +net pyvel minmax.1.max pyvcp.pyvel +net nyvel minmax.1.min pyvcp.nyvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl new file mode 100644 index 00000000000..71284baee8b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-Z.tcl @@ -0,0 +1,5 @@ +net pzacc minmax.12.max pyvcp.pzacc +net nzacc minmax.12.min pyvcp.nzacc + +net pzvel minmax.2.max pyvcp.pzvel +net nzvel minmax.2.min pyvcp.nzvel diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl b/tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl new file mode 100644 index 00000000000..d2bd2925de3 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/postgui-base.tcl @@ -0,0 +1,14 @@ +# net constraints-ok <= match_all.out => motion.enable + +net resetall minmax.0.reset minmax.1.reset minmax.2.reset minmax.3.reset minmax.4.reset minmax.5.reset minmax.6.reset minmax.7.reset minmax.8.reset minmax.9.reset minmax.10.reset minmax.11.reset minmax.12.reset minmax.13.reset minmax.14.reset minmax.15.reset minmax.16.reset minmax.17.reset minmax.18.reset minmax.19.reset => pyvcp.resetmm + +loadrt time +loadrt not +addf time.0 servo-thread +addf not.0 servo-thread +net prog-running not.0.in <= halui.program.is-idle +net cycle-timer time.0.start <= not.0.out +net cycle-seconds pyvcp.time-seconds <= time.0.seconds +net cycle-minutes pyvcp.time-minutes <= time.0.minutes +net cycle-hours pyvcp.time-hours <= time.0.hours + diff --git a/tests/trajectory-planner/circular-arcs/configs/postgui.hal b/tests/trajectory-planner/circular-arcs/configs/postgui.hal deleted file mode 100644 index 6c376e7fc06..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/postgui.hal +++ /dev/null @@ -1,35 +0,0 @@ -#NOTE include appropriate postgui-axis-[XYZ].hal here for each axis used - -net constraints-ok <= match_all.out => motion.enable - -net resetall mm_xacc.reset mm_xvel.reset mm_yacc.reset mm_yvel.reset mm_zacc.reset mm_zvel.reset mm_aacc.reset mm_avel.reset pyvcp.resetmm - -loadrt time -loadrt not -addf time.0 servo-thread -addf not.0 servo-thread -net prog-running not.0.in <= halui.program.is-idle -net cycle-timer time.0.start <= not.0.out -net cycle-seconds pyvcp.time-seconds <= time.0.seconds -net cycle-minutes pyvcp.time-minutes <= time.0.minutes -net cycle-hours pyvcp.time-hours <= time.0.hours - -net pxacc mm_xacc.max pyvcp.pxacc -net pxvel mm_xvel.max pyvcp.pxvel -net nxacc mm_xacc.min pyvcp.nxacc -net nxvel mm_xvel.min pyvcp.nxvel - -net pyacc mm_yacc.max pyvcp.pyacc -net pyvel mm_yvel.max pyvcp.pyvel -net nyacc mm_yacc.min pyvcp.nyacc -net nyvel mm_yvel.min pyvcp.nyvel - -net pzacc mm_zacc.max pyvcp.pzacc -net pzvel mm_zvel.max pyvcp.pzvel -net nzacc mm_zacc.min pyvcp.nzacc -net nzvel mm_zvel.min pyvcp.nzvel - -net paacc mm_aacc.max pyvcp.paacc -net pavel mm_avel.max pyvcp.pavel -net naacc mm_aacc.min pyvcp.naacc -net navel mm_avel.min pyvcp.navel diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml new file mode 100644 index 00000000000..8d3e6dcb26b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZA.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + + + + + "paacc" + ("Helvetica",24) + "+4.3f" + + + "naacc" + ("Helvetica",24) + "+4.3f" + + + "pavel" + ("Helvetica",24) + "+4.2f" + + + "navel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml new file mode 100644 index 00000000000..2ffc71e3033 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABC.xml @@ -0,0 +1,198 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + + + + + "paacc" + ("Helvetica",24) + "+4.3f" + + + "naacc" + ("Helvetica",24) + "+4.3f" + + + "pavel" + ("Helvetica",24) + "+4.2f" + + + "navel" + ("Helvetica",24) + "+4.2f" + + + + + "pbacc" + ("Helvetica",24) + "+4.3f" + + + "nbacc" + ("Helvetica",24) + "+4.3f" + + + "pbvel" + ("Helvetica",24) + "+4.2f" + + + "nbvel" + ("Helvetica",24) + "+4.2f" + + + + + "pcacc" + ("Helvetica",24) + "+4.3f" + + + "ncacc" + ("Helvetica",24) + "+4.3f" + + + "pcvel" + ("Helvetica",24) + "+4.2f" + + + "ncvel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml new file mode 100644 index 00000000000..57f3a326566 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XYZABCUVW.xml @@ -0,0 +1,273 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + + + + + "paacc" + ("Helvetica",24) + "+4.3f" + + + "naacc" + ("Helvetica",24) + "+4.3f" + + + "pavel" + ("Helvetica",24) + "+4.2f" + + + "navel" + ("Helvetica",24) + "+4.2f" + + + + + "pbacc" + ("Helvetica",24) + "+4.3f" + + + "nbacc" + ("Helvetica",24) + "+4.3f" + + + "pbvel" + ("Helvetica",24) + "+4.2f" + + + "nbvel" + ("Helvetica",24) + "+4.2f" + + + + + "pcacc" + ("Helvetica",24) + "+4.3f" + + + "ncacc" + ("Helvetica",24) + "+4.3f" + + + "pcvel" + ("Helvetica",24) + "+4.2f" + + + "ncvel" + ("Helvetica",24) + "+4.2f" + + + + + "puacc" + ("Helvetica",24) + "+4.3f" + + + "nuacc" + ("Helvetica",24) + "+4.3f" + + + "puvel" + ("Helvetica",24) + "+4.2f" + + + "nuvel" + ("Helvetica",24) + "+4.2f" + + + + + "pvacc" + ("Helvetica",24) + "+4.3f" + + + "nvacc" + ("Helvetica",24) + "+4.3f" + + + "pvvel" + ("Helvetica",24) + "+4.2f" + + + "nvvel" + ("Helvetica",24) + "+4.2f" + + + + + "pwacc" + ("Helvetica",24) + "+4.3f" + + + "nwacc" + ("Helvetica",24) + "+4.3f" + + + "pwvel" + ("Helvetica",24) + "+4.2f" + + + "nwvel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml b/tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml new file mode 100644 index 00000000000..4c2cd304a00 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/vcp-XZ.xml @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pzacc" + ("Helvetica",24) + "+4.3f" + + + "nzacc" + ("Helvetica",24) + "+4.3f" + + + "pzvel" + ("Helvetica",24) + "+4.2f" + + + "nzvel" + ("Helvetica",24) + "+4.2f" + +
+ + + + + + "time-hours" + ("Helvetica",14) + ".2d :" + + + "time-minutes" + ("Helvetica",14) + ".2d :" + + + "time-seconds" + ("Helvetica",14) + ".2d" + + + diff --git a/tests/trajectory-planner/circular-arcs/configs/vcp.xml b/tests/trajectory-planner/circular-arcs/configs/vcp.xml index ae91989fc32..58f629a01b4 100644 --- a/tests/trajectory-planner/circular-arcs/configs/vcp.xml +++ b/tests/trajectory-planner/circular-arcs/configs/vcp.xml @@ -1,171 +1,74 @@ - - - - - - "pxacc" - ("Helvetica",24) - "+4.4f" - - - - "pxvel" - ("Helvetica",24) - "+4.4f" - - - - - - "nxacc" - ("Helvetica",24) - "+4.4f" - - - - "nxvel" - ("Helvetica",24) - "+4.4f" - - - - - - - "pyacc" - ("Helvetica",24) - "+4.4f" - - - - "pyvel" - ("Helvetica",24) - "+4.4f" - - - - - - "nyacc" - ("Helvetica",24) - "+4.4f" - - - - "nyvel" - ("Helvetica",24) - "+4.4f" - - - - - - - "pzacc" - ("Helvetica",24) - "+4.4f" - - - - "pzvel" - ("Helvetica",24) - "+4.4f" - - - - - - "nzacc" - ("Helvetica",24) - "+4.4f" - - - - "nzvel" - ("Helvetica",24) - "+4.4f" - - - - - - - - - "paacc" - ("Helvetica",24) - "+4.4f" - - - - "pavel" - ("Helvetica",24) - "+4.4f" - - - - - - "naacc" - ("Helvetica",24) - "+4.4f" - - - - "navel" - ("Helvetica",24) - "+4.4f" - - - - + + + + + + + + + + + + + "pxacc" + ("Helvetica",24) + "+4.3f" + + + "nxacc" + ("Helvetica",24) + "+4.3f" + + + "pxvel" + ("Helvetica",24) + "+4.2f" + + + "nxvel" + ("Helvetica",24) + "+4.2f" + + + + + "pyacc" + ("Helvetica",24) + "+4.3f" + + + "nyacc" + ("Helvetica",24) + "+4.3f" + + + "pyvel" + ("Helvetica",24) + "+4.2f" + + + "nyvel" + ("Helvetica",24) + "+4.2f" + +
+ + From 4fd89489e4f071a40272f20a71b226a0902b29ad Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 5 Jan 2019 01:09:20 -0500 Subject: [PATCH 047/129] test: Add test cases for issue 546 and issue 550 --- .../violation_checks/546_simplfied.ngc | 21 +++++++++++++++++++ .../violation_checks/bug550_short_moves.ngc | 13 ++++++++++++ 2 files changed, 34 insertions(+) create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/violation_checks/546_simplfied.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug550_short_moves.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/546_simplfied.ngc b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/546_simplfied.ngc new file mode 100644 index 00000000000..278c84e3006 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/546_simplfied.ngc @@ -0,0 +1,21 @@ +G90 +G21 +G17 G64 P0.0001 M3 S3000 +F800.0 +G0 Z2.000 +G0 X0.724 Y12.368 +G1 Z-0.127 +G1 X0.724 Y12.368 +G1 X0.678 Y12.278 +G1 X0.557 Y12.351 +G1 X0.564 Y12.384 +G1 X0.707 Y12.315 +G1 X0.608 Y12.398 +G1 X0.668 Y12.412 +G1 X0.723 Y12.372 +G1 X0.724 Y12.368 +G0 Z2.000 +G0 X0.858 Y12.449 +M5 +M2 + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug550_short_moves.ngc b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug550_short_moves.ngc new file mode 100644 index 00000000000..265b0803400 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug550_short_moves.ngc @@ -0,0 +1,13 @@ +G0 X0 Y0 A0 +G91 G20 G61 + +G1 X0.1 Y0.1 A-50.0 F100000 +G1 X0.000005 Y0.000005 A50.0 F100000 +G1 X0.0000005 Y0.0000005 A50.0 F100000 +G1 X0.00000005 Y0.00000005 A50.0 F100000 +G1 X0.000000005 Y0.000000005 A50.0 F100000 +G1 X0.1 Y0.1 A.1 F100000 +G90 +G1 X0 Y0 A0 F100000 +G64 +M2 From cdaa0869dc0be1264ab069df6d0e40b2625710d9 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 4 Jan 2019 20:19:46 -0500 Subject: [PATCH 048/129] tp: Add some comments to clarify obscure variable names --- src/emc/tp/tp.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 8551de5725c..3fc2d07737d 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -922,7 +922,7 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, tcCheckLastParabolic(tc, prev_tc); break; case TANGENT_SEGMENTS_BLEND: // tangent - tp_debug_print("using parabolic blending\n"); + tp_debug_print("using approximate tangent blend\n"); forceTangentSegments(prev_tc, tc); break; case ARC_BLEND: // arc blend @@ -1913,6 +1913,9 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, double a_inst = v_max / tp->cycleTime; // Set up worst-case final velocity + // Compute the actual magnitude of acceleration required given the tangent directions + // Do this by assuming that we decelerate to a stop on the previous segment, + // and simultaneously accelerate up to the maximum speed on the next one. PmCartesian acc1, acc2, acc_diff; pmCartScalMult(&prev_tan, a_inst, &acc1); pmCartScalMult(&this_tan, a_inst, &acc2); @@ -1940,6 +1943,8 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, acc_scale_max /= BLEND_ACC_RATIO_TANGENTIAL; } + // Controls the tradeoff between reduction of final velocity, and reduction of allowed segment acceleration + // TODO: this should ideally depend on some function of segment length and acceleration for better optimization const double kink_ratio = tpGetTangentKinkRatio(); if (acc_scale_max < kink_ratio) { tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); From 3379d3ba55b989f060f742e34faa71b10f7e465d Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 4 Jan 2019 21:33:33 -0500 Subject: [PATCH 049/129] tp: When forcing a pair of motion segments to be tangent, store the "kink" velocity in the previous segment. --- src/emc/tp/tp.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 3fc2d07737d..7d10ea965db 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -902,7 +902,7 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, // down the next and previous blends as well. We model this loss by scaling // the blend velocity down to find an "equivalent" velocity. double perf_parabolic = estimateParabolicBlendPerformance(tp, prev_tc, tc) / 2.0; - double perf_tangent = tc->kink_vel; + double perf_tangent = prev_tc->kink_vel; double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0; tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ", @@ -917,7 +917,7 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, switch (best_blend) { case PARABOLIC_BLEND: // parabolic tp_debug_print("using parabolic blend\n"); - tc->kink_vel = -1.0; + prev_tc->kink_vel = -1.0; tcSetTermCond(prev_tc, TC_TERM_COND_PARABOLIC); tcCheckLastParabolic(tc, prev_tc); break; @@ -1949,20 +1949,20 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, if (acc_scale_max < kink_ratio) { tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); - tc->kink_vel = v_max; + prev_tc->kink_vel = v_max; tpAdjustAccelForTangent(tc, acc_scale_max); tpAdjustAccelForTangent(prev_tc, acc_scale_max); return TP_ERR_OK; } else { - tc->kink_vel = v_max * kink_ratio / acc_scale_max; + prev_tc->kink_vel = v_max * kink_ratio / acc_scale_max; tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n", acc_scale_max, kink_ratio, - tc->kink_vel); + prev_tc->kink_vel); + // NOTE: acceleration will be reduced later if tangent blend is used return TP_ERR_NO_ACTION; } - } static bool tpCreateBlendIfPossible( From 250739ccfabfbf6a75f2280010bcc63ea95df970 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Sat, 5 Jan 2019 00:41:56 -0500 Subject: [PATCH 050/129] tp: Fix acceleration violation due to corner case in initialization of optimizer --- src/emc/tp/tp.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 7d10ea965db..6382d6b04d6 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1813,7 +1813,13 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { if (!tc->finalized) { tp_debug_print("Segment %d, type %d not finalized, continuing\n",tc->id,tc->motion_type); // use worst-case final velocity that allows for up to 1/2 of a segment to be consumed. + prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationInitialVel(tp,tc)); + + // Fixes acceleration violations when last segment is not finalized, and previous segment is tangent. + if (prev1_tc->kink_vel >=0 && prev1_tc->term_cond == TC_TERM_COND_TANGENT) { + prev1_tc->finalvel = fmin(prev1_tc->finalvel, prev1_tc->kink_vel); + } tc->finalvel = 0.0; } else { tpComputeOptimalVelocity(tp, tc, prev1_tc); From b22325f0139e06770825cb44a2489bafbebf28ae Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 4 Jan 2019 23:08:59 -0500 Subject: [PATCH 051/129] tp: Fix handling of approximate tangents and required de-rating of max velocity and acceleration. Instead of directly altering the segment's maxaccel field (which is not re-entrant), store an acceleration reduction parameter for approximate tangents. This way, the actual max acceleration can be computed on-demand (knowing the original max acceleration and de-rating due to approximate tangency). A nice side-effect of this is that the reductions are not applied twice, even in the code paths do the tangency check twice. This squeezes slightly more performance out of the blends. --- src/emc/tp/tc.c | 39 ++++++++++++++++++++++++++++++++++---- src/emc/tp/tc.h | 6 ++++++ src/emc/tp/tc_types.h | 2 ++ src/emc/tp/tp.c | 44 ++++++++++++------------------------------- 4 files changed, 55 insertions(+), 36 deletions(-) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 7181fb733bb..914dc2134db 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -52,6 +52,37 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, return fmin(v_max_target, tc->maxvel); } +double tcGetMaxAccel(const TC_STRUCT *tc) +{ + return tc->maxaccel * (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); +} + +int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction) +{ + prev_tc->kink_vel = kink_vel; + // + prev_tc->kink_accel_reduce = fmax(accel_reduction, prev_tc->kink_accel_reduce); + tc->kink_accel_reduce_prev = fmax(accel_reduction, tc->kink_accel_reduce_prev); + + return 0; +} + +int tcInitKinkProperties(TC_STRUCT *tc) +{ + tc->kink_vel = -1.0; + tc->kink_accel_reduce = 0.0; + tc->kink_accel_reduce_prev = 0.0; + return 0; +} + +int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc) +{ + prev_tc->kink_vel = -1.0; + prev_tc->kink_accel_reduce = 0.0; + tc->kink_accel_reduce_prev = 0.0; + return 0; +} + int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { @@ -69,7 +100,7 @@ int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const pmCartCartSub(&tc->coords.circle.xyz.center, &startpoint, &perp); pmCartUnitEq(&perp); - pmCartScalMult(&tan, tc->maxaccel, &tan); + pmCartScalMult(&tan, tcGetMaxAccel(tc), &tan); pmCartScalMultEq(&perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius); pmCartCartAdd(&tan, &perp, out); pmCartUnitEq(out); @@ -574,8 +605,8 @@ int tcSetupMotion(TC_STRUCT * const tc, tc->reqvel = vel; // Initial guess at target velocity is just the requested velocity tc->target_vel = vel; - // TO be filled in by tangent calculation, negative = invalid (KLUDGE) - tc->kink_vel = -1.0; + // To be filled in by tangent calculation, negative = invalid (KLUDGE) + tcInitKinkProperties(tc); return TP_ERR_OK; } @@ -680,7 +711,7 @@ int tcFinalizeLength(TC_STRUCT * const tc) tp_debug_print("blend_prev = %d, term_cond = %d\n",tc->blend_prev, tc->term_cond); if (tc->motion_type == TC_CIRCULAR) { - tc->maxvel = pmCircleActualMaxVel(&tc->coords.circle.xyz, &tc->acc_ratio_tan, tc->maxvel, tc->maxaccel, parabolic); + tc->maxvel = pmCircleActualMaxVel(&tc->coords.circle.xyz, &tc->acc_ratio_tan, tc->maxvel, tcGetMaxAccel(tc), parabolic); } tcClampVelocityByLength(tc); diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index c49012f3d19..30e1744e91c 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -24,6 +24,12 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale); + +double tcGetMaxAccel(TC_STRUCT const * tc); + +int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction); +int tcInitKinkProperties(TC_STRUCT *tc); +int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc); int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 636c56e7cb7..8e417d397bf 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -131,6 +131,8 @@ typedef struct { double finalvel; // velocity to aim for at end of segment double term_vel; // actual velocity at termination of segment double kink_vel; // Temporary way to store our calculation of maximum velocity we can handle if this segment is declared tangent with the next + double kink_accel_reduce_prev; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. + double kink_accel_reduce; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. //Acceleration double maxaccel; // accel calc'd by task diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 6382d6b04d6..b4302bf900c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -76,9 +76,6 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc); -STATIC int tpAdjustAccelForTangent( - TC_STRUCT * const tc, - double normal_acc_scale); STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); /** @@ -87,7 +84,6 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); * Hopefully this makes changes easier to track as much of the churn will be on small functions. */ - /** * Check if the tail of the queue has a parabolic blend condition and update tc appropriately. * This sets flags so that accelerations are correct due to the current segment @@ -337,7 +333,7 @@ STATIC inline double planMaxSegmentAccel( TC_STRUCT const * const tc, tc_term_cond_t term_cond) { - double a_scale = tc->maxaccel; + double a_scale = tcGetMaxAccel(tc); /* Parabolic blending conditions: If the next segment or previous segment * has a parabolic blend with this one, acceleration is scaled down by 1/2 * so that the sum of the two does not exceed the maximum. @@ -849,11 +845,6 @@ static void forceTangentSegments(TC_STRUCT *prev_tc, TC_STRUCT *tc) { // Fall back to tangent, using kink_vel as final velocity tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); - - // Finally, reduce acceleration proportionally to prevent violations during "kink" - const double kink_ratio = tpGetTangentKinkRatio(); - tpAdjustAccelForTangent(tc, kink_ratio); - tpAdjustAccelForTangent(prev_tc, kink_ratio); } static inline int find_max_element(double arr[], int sz) @@ -917,17 +908,19 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, switch (best_blend) { case PARABOLIC_BLEND: // parabolic tp_debug_print("using parabolic blend\n"); - prev_tc->kink_vel = -1.0; + tcRemoveKinkProperties(prev_tc, tc); tcSetTermCond(prev_tc, TC_TERM_COND_PARABOLIC); tcCheckLastParabolic(tc, prev_tc); break; case TANGENT_SEGMENTS_BLEND: // tangent tp_debug_print("using approximate tangent blend\n"); - forceTangentSegments(prev_tc, tc); + // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties + tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); break; case ARC_BLEND: // arc blend tp_debug_print("using blend arc\n"); - tc->kink_vel = -1.0; + tcRemoveKinkProperties(prev_tc, tc); + break; case NO_BLEND: break; @@ -1845,19 +1838,6 @@ STATIC double pmCartAbsMax(PmCartesian const * const v) return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); } -STATIC int tpAdjustAccelForTangent( - TC_STRUCT * const tc, - double normal_acc_scale) -{ - if (normal_acc_scale >= 1.0) { - rtapi_print_msg(RTAPI_MSG_ERR,"Can't have acceleration scale %f > 1.0\n",normal_acc_scale); - return TP_ERR_FAIL; - } - double a_reduction_ratio = 1.0 - normal_acc_scale; - tp_debug_print(" acceleration reduction ratio is %f\n", a_reduction_ratio); - tc->maxaccel *= a_reduction_ratio; - return TP_ERR_OK; -} /** * Check for tangency between the current segment and previous segment. @@ -1917,7 +1897,9 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, double v_max = fmin(v_max1, v_max2); tp_debug_print("tangent v_max = %f\n",v_max); - double a_inst = v_max / tp->cycleTime; + // Account for acceleration past final velocity during a split cycle + // (e.g. next segment starts accelerating again so the average velocity is higher at the end of the split cycle) + double a_inst = v_max / tp->cycleTime + tc->maxaccel; // Set up worst-case final velocity // Compute the actual magnitude of acceleration required given the tangent directions // Do this by assuming that we decelerate to a stop on the previous segment, @@ -1952,16 +1934,14 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, // Controls the tradeoff between reduction of final velocity, and reduction of allowed segment acceleration // TODO: this should ideally depend on some function of segment length and acceleration for better optimization const double kink_ratio = tpGetTangentKinkRatio(); + if (acc_scale_max < kink_ratio) { tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); - prev_tc->kink_vel = v_max; - tpAdjustAccelForTangent(tc, acc_scale_max); - tpAdjustAccelForTangent(prev_tc, acc_scale_max); - + tcSetKinkProperties(prev_tc, tc, v_max, acc_scale_max); return TP_ERR_OK; } else { - prev_tc->kink_vel = v_max * kink_ratio / acc_scale_max; + tcSetKinkProperties(prev_tc, tc, v_max * kink_ratio / acc_scale_max, kink_ratio); tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n", acc_scale_max, kink_ratio, From c99719eeb73154617ecc1241aa9ba6eeb2a1c73b Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 10 Jan 2019 11:31:41 -0500 Subject: [PATCH 052/129] tp: Set flags on next segment when setting terminal condition. The "blend_prev" flag needs to be set on the next segment whenever the current segment uses parabolic blending (i.e. TC_TERM_COND_PARABOLIC). Eventually, we should redesign functions that use blend_prev to look directly at the previous segment's end condition. Using this flag as we do is a potential source of bugs because it has to be changed any time the terminal condition does. --- src/emc/tp/tc.c | 27 ++++++++++------- src/emc/tp/tc.h | 2 +- src/emc/tp/tp.c | 79 ++++++++++++++++++++++++++++--------------------- 3 files changed, 62 insertions(+), 46 deletions(-) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 914dc2134db..244b4dc267b 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -401,12 +401,19 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) /** - * Set the terminal condition of a segment. - * This function will eventually handle state changes associated with altering a terminal condition. + * Set the terminal condition (i.e. blend or stop) for the given motion segment. + * Also sets flags on the next segment relevant to blending (e.g. parabolic blend sets the blend_prev flag). */ -int tcSetTermCond(TC_STRUCT * const tc, int term_cond) { - tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, tc->id, tc->motion_type); - tc->term_cond = term_cond; +int tcSetTermCond(TC_STRUCT *tc, TC_STRUCT *nexttc, int term_cond) +{ + if (tc) { + tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, tc->id, tc->motion_type); + tc->term_cond = term_cond; + } + if (nexttc) { + nexttc->blend_prev = (tc && term_cond == TC_TERM_COND_PARABOLIC); + tp_debug_print("set blend_prev flag %d on tc id %d, type %d\n", nexttc->blend_prev, nexttc->id, nexttc->motion_type); + } return 0; } @@ -433,7 +440,7 @@ int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, prev_tc->target = prev_tc->coords.line.xyz.tmag; tp_debug_print("Target = %f\n",prev_tc->target); //Setup tangent blending constraints - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); tp_debug_print(" L1 end : %f %f %f\n",prev_tc->coords.line.xyz.end.x, prev_tc->coords.line.xyz.end.y, prev_tc->coords.line.xyz.end.z); @@ -451,8 +458,7 @@ int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, tc->coords.line.xyz.start.y, tc->coords.line.xyz.start.z); - //Disable flag for parabolic blending with previous - tc->blend_prev = 0; + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); tp_info_print(" Q1: %f %f %f\n",circ_start->x,circ_start->y,circ_start->z); tp_info_print(" Q2: %f %f %f\n",circ_end->x,circ_end->y,circ_end->z); @@ -524,7 +530,7 @@ int tcFlagEarlyStop(TC_STRUCT * const tc, // we'll have to wait for spindle sync; might as well // stop at the right place (don't blend) tp_debug_print("waiting on spindle sync for tc %d\n", tc->id); - tcSetTermCond(tc, TC_TERM_COND_STOP); + tcSetTermCond(tc, nexttc, TC_TERM_COND_STOP); } if(nexttc->atspeed) { @@ -532,7 +538,7 @@ int tcFlagEarlyStop(TC_STRUCT * const tc, // stop at the right place (don't blend), like above // FIXME change the values so that 0 is exact stop mode tp_debug_print("waiting on spindle atspeed for tc %d\n", tc->id); - tcSetTermCond(tc, TC_TERM_COND_STOP); + tcSetTermCond(tc, nexttc, TC_TERM_COND_STOP); } return TP_ERR_OK; @@ -614,7 +620,6 @@ int tcSetupMotion(TC_STRUCT * const tc, int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp) { - tcSetTermCond(tc, tp->termCond); tc->tolerance = tp->tolerance; tc->synchronized = tp->synchronized; tc->uu_per_rev = tp->uu_per_rev; diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 30e1744e91c..519c8b05e55 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -44,7 +44,7 @@ int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, int tcCanConsume(TC_STRUCT const * const tc); -int tcSetTermCond(TC_STRUCT * const tc, int term_cond); +int tcSetTermCond(TC_STRUCT * tc, TC_STRUCT * nexttc, int term_cond); int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, PmCartesian const * const circ_start, diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b4302bf900c..3789002691d 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -789,7 +789,8 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, // Copy over state data from TP tcSetupState(blend_tc, tp); - + blend_tc->term_cond = TC_TERM_COND_TANGENT; + // Set kinematics parameters from blend calculations tcSetupMotion(blend_tc, vel, @@ -806,9 +807,6 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, blend_tc->target = length; blend_tc->nominal_length = length; - // Set the blend arc to be tangent to the next segment - tcSetTermCond(blend_tc, TC_TERM_COND_TANGENT); - //NOTE: blend arc radius and everything else is finalized, so set this to 1. //In the future, radius may be adjustable. tcFinalizeLength(blend_tc); @@ -833,20 +831,6 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) return TP_ERR_OK; } - -/** - * Force two segments to be treated as tangent based on the computed kink velocity. - * - * @warning for segments that are not close to tangent, the maximum - * acceleration will be drastically reduced to ensure that the transition is - * within velocity / acceleration limits. - */ -static void forceTangentSegments(TC_STRUCT *prev_tc, TC_STRUCT *tc) -{ - // Fall back to tangent, using kink_vel as final velocity - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); -} - static inline int find_max_element(double arr[], int sz) { if (sz < 1) { @@ -909,13 +893,13 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, case PARABOLIC_BLEND: // parabolic tp_debug_print("using parabolic blend\n"); tcRemoveKinkProperties(prev_tc, tc); - tcSetTermCond(prev_tc, TC_TERM_COND_PARABOLIC); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC); tcCheckLastParabolic(tc, prev_tc); break; case TANGENT_SEGMENTS_BLEND: // tangent tp_debug_print("using approximate tangent blend\n"); // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); break; case ARC_BLEND: // arc blend tp_debug_print("using blend arc\n"); @@ -1080,7 +1064,7 @@ STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const pre } tcSetCircleXYZ(tc, &circ2_temp); - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } @@ -1222,7 +1206,7 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre tcSetLineXYZ(tc, &line2_temp); //Cleanup any mess from parabolic - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } @@ -1387,7 +1371,7 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev tcSetCircleXYZ(prev_tc, &circ1_temp); tcSetCircleXYZ(tc, &circ2_temp); - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } @@ -1503,22 +1487,44 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc return TP_ERR_OK; } -STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT const * const tc) +int handlePrevTermCondition(TC_STRUCT *prev_tc, TC_STRUCT *tc) +{ + if (!tc) { + return TP_ERR_FAIL; + } + + switch (tc->motion_type) { + case TC_RIGIDTAP: + tcSetTermCond(tc, NULL, TC_TERM_COND_EXACT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + break; + case TC_LINEAR: + case TC_CIRCULAR: + case TC_SPHERICAL: + { + tc_term_cond_t prev_term = prev_tc ? prev_tc->term_cond : TC_TERM_COND_STOP; + tcSetTermCond(prev_tc, tc, prev_term); + } + break; + } + return TP_ERR_OK; +} + +STATIC int handleModeChange(TC_STRUCT *prev_tc, TC_STRUCT *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("Blending disabled: can't blend between rapid and feed motions\n"); - tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + tcSetTermCond(prev_tc, 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); + tp_debug_print("Blending disabled: entering position-sync mode\n"); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); } return TP_ERR_OK; } @@ -1558,7 +1564,13 @@ 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); + + // Make sure the blending flags are consistent w/ previous segment + handlePrevTermCondition(prev_tc, tc); + + // Prevent blends for specific mode changes (where blending isn't possible anyway) handleModeChange(prev_tc, tc); + if (emcmotConfig->arcBlendEnable){ tpHandleBlendArc(tp, tc); tcUpdateArcLengthFit(tc); @@ -1624,9 +1636,6 @@ int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxv &end); tc.target = pmRigidTapTarget(&tc.coords.rigidtap, tp->uu_per_rev); - // Force exact stop mode after rigid tapping regardless of TP setting - tcSetTermCond(&tc, TC_TERM_COND_STOP); - return tpFinalizeAndEnqueue(tp, &tc); } @@ -1886,7 +1895,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, pmCartCartDot(&prev_tan, &this_tan, &dot); if (dot < SHARP_CORNER_THRESHOLD) { tp_debug_print("Found sharp corner\n"); - tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); return TP_ERR_FAIL; } @@ -1937,7 +1946,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, if (acc_scale_max < kink_ratio) { tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); - tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); tcSetKinkProperties(prev_tc, tc, v_max, acc_scale_max); return TP_ERR_OK; } else { @@ -2074,6 +2083,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + tcSetTermCond(&tc, NULL, tp->termCond); // Copy in motion parameters tcSetupMotion(&tc, @@ -2142,6 +2152,7 @@ int tpAddCircle(TP_STRUCT * const tp, // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + tcSetTermCond(&tc, NULL, tp->termCond); // Setup circle geometry int res_init = pmCircle9Init(&tc.coords.circle, From 1dd276c6c2ccd8d85065dbd89baa3fef226a082f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 8 Jan 2019 00:28:24 -0500 Subject: [PATCH 053/129] tp: Refactor tangential vs overall maximum acceleration lookup Move functions to calculate overall (tangential + normal) and just tangential acceleration (i.e. "along the path") to blendmath. This approach ensures that callers get the properly-scaled acceleration (instead of naively looking at the maxaccel field). --- src/emc/tp/blendmath.c | 6 +--- src/emc/tp/blendmath.h | 3 +- src/emc/tp/tc.c | 37 ++++++++++++++++--- src/emc/tp/tc.h | 3 +- src/emc/tp/tp.c | 82 +++++++----------------------------------- 5 files changed, 49 insertions(+), 82 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index cddd1738b22..c2361fe6b1a 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1630,12 +1630,8 @@ int blendPoints3Print(BlendPoints3 const * const points) double pmCircleActualMaxVel(PmCircle * const circle, double * const acc_ratio_tangential, double v_max, - double a_max, - int parabolic) + double a_max) { - if (parabolic) { - a_max /= 2.0; - } double a_n_max = BLEND_ACC_RATIO_NORMAL * a_max; double eff_radius = pmCircleEffectiveMinRadius(circle); double v_max_acc = pmSqrt(a_n_max * eff_radius); diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 742a2cc7b4b..f1acb78199b 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -238,8 +238,7 @@ int blendPoints3Print(BlendPoints3 const * const points); double pmCircleActualMaxVel(PmCircle * const circle, double * const acc_ratio, double v_max, - double a_max, - int parabolic); + double a_max); int findSpiralArcLengthFit(PmCircle const * const circle, SpiralArcLengthFit * const fit); int pmCircleAngleFromProgress(PmCircle const * const circle, diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 244b4dc267b..f9dc1208253 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -52,11 +52,39 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, return fmin(v_max_target, tc->maxvel); } -double tcGetMaxAccel(const TC_STRUCT *tc) +double tcGetOverallMaxAccel(const TC_STRUCT *tc) { - return tc->maxaccel * (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); + // Handle any acceleration reduction due to an approximate-tangent "blend" with the previous or next segment + double a_scale = (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); + + // Parabolic blending conditions: If the next segment or previous segment + // has a parabolic blend with this one, acceleration is scaled down by 1/2 + // so that the sum of the two does not exceed the maximum. + if (tc->blend_prev || TC_TERM_COND_PARABOLIC == tc->term_cond) { + a_scale *= 0.5; + } + + return tc->maxaccel * a_scale; } +/** + * Get acceleration for a tc based on the trajectory planner state. + */ +double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) +{ + double a_scale = tcGetOverallMaxAccel(tc); + + // Reduce allowed tangential acceleration in circular motions to stay + // within overall limits (accounts for centripetal acceleration while + // moving along the circular path). + if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { + //Limit acceleration for cirular arcs to allow for normal acceleration + a_scale *= tc->acc_ratio_tan; + } + return a_scale; +} + + int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction) { prev_tc->kink_vel = kink_vel; @@ -100,7 +128,7 @@ int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const pmCartCartSub(&tc->coords.circle.xyz.center, &startpoint, &perp); pmCartUnitEq(&perp); - pmCartScalMult(&tan, tcGetMaxAccel(tc), &tan); + pmCartScalMult(&tan, tcGetOverallMaxAccel(tc), &tan); pmCartScalMultEq(&perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius); pmCartCartAdd(&tan, &perp, out); pmCartUnitEq(out); @@ -712,11 +740,10 @@ int tcFinalizeLength(TC_STRUCT * const tc) tp_debug_print("Finalizing tc id %d, type %d\n", tc->id, tc->motion_type); //TODO function to check for parabolic? - int parabolic = (tc->blend_prev || tc->term_cond == TC_TERM_COND_PARABOLIC); tp_debug_print("blend_prev = %d, term_cond = %d\n",tc->blend_prev, tc->term_cond); if (tc->motion_type == TC_CIRCULAR) { - tc->maxvel = pmCircleActualMaxVel(&tc->coords.circle.xyz, &tc->acc_ratio_tan, tc->maxvel, tcGetMaxAccel(tc), parabolic); + tc->maxvel = pmCircleActualMaxVel(&tc->coords.circle.xyz, &tc->acc_ratio_tan, tc->maxvel, tcGetOverallMaxAccel(tc)); } tcClampVelocityByLength(tc); diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 519c8b05e55..804d68c5517 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -25,7 +25,8 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale); -double tcGetMaxAccel(TC_STRUCT const * tc); +double tcGetOverallMaxAccel(TC_STRUCT const * tc); +double tcGetTangentialMaxAccel(TC_STRUCT const * const tc); int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction); int tcInitKinkProperties(TC_STRUCT *tc); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 3789002691d..63e09264e04 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -84,25 +84,6 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); * Hopefully this makes changes easier to track as much of the churn will be on small functions. */ -/** - * Check if the tail of the queue has a parabolic blend condition and update tc appropriately. - * This sets flags so that accelerations are correct due to the current segment - * having to blend with the previous. - */ -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("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; -} - /** * Returns true if there is motion along ABC or UVW axes, false otherwise. */ @@ -325,45 +306,13 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, return finalvel; } -/** - * Compute maximum acceleration a segment can have for a given terminal condition. - * Used (for example) to check if a different terminal condition would improve allowed acceleration. - */ -STATIC inline double planMaxSegmentAccel( - TC_STRUCT const * const tc, - tc_term_cond_t term_cond) -{ - double a_scale = tcGetMaxAccel(tc); - /* Parabolic blending conditions: If the next segment or previous segment - * has a parabolic blend with this one, acceleration is scaled down by 1/2 - * so that the sum of the two does not exceed the maximum. - */ - if (term_cond == TC_TERM_COND_PARABOLIC) { - a_scale *= 0.5; - } - if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { - //Limit acceleration for cirular arcs to allow for normal acceleration - a_scale *= tc->acc_ratio_tan; - } - return a_scale; -} - - -/** +/** * Set up a spindle origin based on the current spindle COMMANDED direction and the given position. * * Convert the 2-part spindle position and sign to a signed double. - * The direction is stored as part of the origin to prevent discontinuous + * Get acceleration for a tc based on the trajectory planner state. * changes in displacement due to sign flips */ -STATIC inline double tpGetScaledAccel( - TC_STRUCT const * const tc) -{ - // Backend to the planning function, but use the actual terminal condition. - return planMaxSegmentAccel(tc, (tc_term_cond_t)tc->term_cond); -} - - STATIC inline void resetSpindleOrigin(spindle_origin_t *origin) { if (!origin) { @@ -723,13 +672,13 @@ int tpErrorCheck(TP_STRUCT const * const tp) { /** - * Find the "peak" velocity a segment can acheive if its velocity profile is triangular. + * Find the "peak" velocity a segment can achieve if its velocity profile is triangular. * This is used to estimate blend velocity, though by itself is not enough * (since requested velocity and max velocity could be lower). */ STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { //Compute peak velocity for blend calculations - double acc_scaled = tpGetScaledAccel(tc); + double acc_scaled = tcGetTangentialMaxAccel(tc); double length = tc->target; if (!tc->finalized) { // blending may remove up to 1/2 of the segment @@ -752,7 +701,7 @@ STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { */ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) { - double acc_scaled = tpGetScaledAccel(tc); + double acc_scaled = tcGetTangentialMaxAccel(tc); //FIXME this is defined in two places! double triangle_vel = pmSqrt( acc_scaled * tc->target * BLEND_DIST_FRACTION); double max_vel = tpGetMaxTargetVel(tp, tc); @@ -894,7 +843,6 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, tp_debug_print("using parabolic blend\n"); tcRemoveKinkProperties(prev_tc, tc); tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC); - tcCheckLastParabolic(tc, prev_tc); break; case TANGENT_SEGMENTS_BLEND: // tangent tp_debug_print("using approximate tangent blend\n"); @@ -1578,7 +1526,6 @@ STATIC int tpFinalizeAndEnqueue(TP_STRUCT * const tp, TC_STRUCT * const 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); @@ -1692,7 +1639,7 @@ STATIC blend_type_t tpCheckBlendArcType( STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT * const prev1_tc) { //Calculate the maximum starting velocity vs_back of segment tc, given the //trajectory parameters - double acc_this = tpGetScaledAccel(tc); + double acc_this = tcGetTangentialMaxAccel(tc); // Find the reachable velocity of tc, moving backwards in time double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); @@ -1989,8 +1936,6 @@ static bool tpCreateBlendIfPossible( break; } - // Can always do this (blend_tc may not be used, in which case it has no effect) - tcCheckLastParabolic(blend_tc, prev_tc); return res_create == TP_ERR_OK; } @@ -2175,7 +2120,7 @@ int tpAddCircle(TP_STRUCT * const tp, //Reduce max velocity to match sample rate tcClampVelocityByLength(&tc); - double v_max_actual = pmCircleActualMaxVel(&tc.coords.circle.xyz, &tc.acc_ratio_tan, ini_maxvel, acc, false); + double v_max_actual = pmCircleActualMaxVel(&tc.coords.circle.xyz, &tc.acc_ratio_tan, ini_maxvel, acc); tp_debug_print("tc.acc_ratio_tan = %f\n",tc.acc_ratio_tan); // Copy in motion parameters @@ -2211,8 +2156,8 @@ STATIC int tpComputeBlendVelocity( return TP_ERR_FAIL; } - double acc_this = tpGetScaledAccel(tc); - double acc_next = tpGetScaledAccel(nexttc); + double acc_this = tcGetTangentialMaxAccel(tc); + double acc_next = tcGetTangentialMaxAccel(nexttc); double v_reachable_this = fmin(tpCalculateTriangleVel(tc), target_vel_this); double v_reachable_next = fmin(tpCalculateTriangleVel(nexttc), target_vel_next); @@ -2387,7 +2332,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t /* Calculations for desired velocity based on trapezoidal profile */ double dx = tc->target - tc->progress; - double maxaccel = tpGetScaledAccel(tc); + double maxaccel = tcGetTangentialMaxAccel(tc); double maxnewvel = findTrapezoidalDesiredVel(maxaccel, dx, tc_finalvel, tc->currentvel, tc->cycle_time); @@ -2442,7 +2387,7 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, double acc_final = dv / dt; // Saturate estimated acceleration against maximum allowed by segment - double acc_max = tpGetScaledAccel(tc); + double acc_max = tcGetTangentialMaxAccel(tc); // Output acceleration and velocity for position update *acc = saturate(acc_final, acc_max); @@ -2981,7 +2926,6 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, // track position (minimize pos_error). // 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. * This approach assumes that if position error is 0, the correct @@ -3008,7 +2952,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, * 2) "final" velocity = v_0 * 3) max velocity / acceleration from motion segment */ - double a_max = tpGetScaledAccel(tc) * emcmotStatus->spindle_tracking_gain; + double a_max = tcGetTangentialMaxAccel(tc) * emcmotStatus->spindle_tracking_gain; double v_max = tc->maxvel; switch(emcmotStatus->pos_tracking_mode) { @@ -3239,7 +3183,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, //If this is a valid acceleration, then we're done. If not, then we solve //for v_f and dt given the max acceleration allowed. - double a_max = tpGetScaledAccel(tc); + double a_max = tcGetTangentialMaxAccel(tc); //If we exceed the maximum acceleration, then the dt estimate is too small. double a = a_f; From e40858a8a07fe50a14a30a44898635a7d7674bb2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 10 Jan 2019 12:46:08 -0500 Subject: [PATCH 054/129] Fix some compiler warnings * return type mismatches * warning about double to bool conversion * Remove unused function * add include guards and missing includes in classic ladder source * Replace float equality comparisons (no effective change since values are forced to be non-negative) * Add include guards and a missing interp include --- src/emc/motion/motion.h | 2 +- src/emc/nml_intf/motion_types.h | 4 ++++ src/emc/rs274ngc/interp_base.hh | 4 ++++ src/emc/rs274ngc/interp_internal.hh | 1 + src/emc/tp/blendmath.c | 12 ++++++------ src/emc/tp/tp.c | 7 ++++--- src/hal/classicladder/drawing_sequential.h | 3 +++ src/hal/classicladder/files_sequential.c | 1 + src/hal/classicladder/sequential.h | 4 +++- 9 files changed, 27 insertions(+), 11 deletions(-) diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 8204ac5c346..7b19895f95c 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -699,7 +699,7 @@ typedef enum { double current_vel; double requested_vel; - unsigned int tcqlen; + int tcqlen; EmcPose tool_offset; int atspeed_next_feed; /* at next feed move, wait for spindle to be at speed */ int spindle_is_atspeed; /* hal input */ diff --git a/src/emc/nml_intf/motion_types.h b/src/emc/nml_intf/motion_types.h index 8d9a430a09e..57eb5b25fad 100644 --- a/src/emc/nml_intf/motion_types.h +++ b/src/emc/nml_intf/motion_types.h @@ -13,9 +13,13 @@ // 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 +#ifndef MOTION_TYPES_H +#define MOTION_TYPES_H #define EMC_MOTION_TYPE_TRAVERSE 1 #define EMC_MOTION_TYPE_FEED 2 #define EMC_MOTION_TYPE_ARC 3 #define EMC_MOTION_TYPE_TOOLCHANGE 4 #define EMC_MOTION_TYPE_PROBING 5 #define EMC_MOTION_TYPE_INDEXROTARY 6 + +#endif diff --git a/src/emc/rs274ngc/interp_base.hh b/src/emc/rs274ngc/interp_base.hh index e8d3e8ce08a..158e7800fb2 100644 --- a/src/emc/rs274ngc/interp_base.hh +++ b/src/emc/rs274ngc/interp_base.hh @@ -15,6 +15,8 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. */ +#ifndef INTERP_BASE_HH +#define INTERP_BASE_HH #include /* Size of certain arrays */ @@ -56,3 +58,5 @@ public: InterpBase *interp_from_shlib(const char *shlib); extern "C" InterpBase *makeInterp(); + +#endif diff --git a/src/emc/rs274ngc/interp_internal.hh b/src/emc/rs274ngc/interp_internal.hh index 356fa1690a1..d91f3f5d1bb 100644 --- a/src/emc/rs274ngc/interp_internal.hh +++ b/src/emc/rs274ngc/interp_internal.hh @@ -26,6 +26,7 @@ #include "emcpos.h" #include "libintl.h" #include "python_plugin.hh" +#include "interp_base.hh" #define _(s) gettext(s) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index c2361fe6b1a..098b5b73e6c 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -476,13 +476,13 @@ int calculateInscribedDiameter(PmCartesian const * const normal, pmCartMag(&planar_z, &z_scale); double x_extent=0, y_extent=0, z_extent=0; - if (bounds->x != 0) { + if (bounds->x > 0) { x_extent = bounds->x / x_scale; } - if (bounds->y != 0) { + if (bounds->y > 0) { y_extent = bounds->y / y_scale; } - if (bounds->z != 0) { + if (bounds->z > 0) { z_extent = bounds->z / z_scale; } @@ -490,13 +490,13 @@ int calculateInscribedDiameter(PmCartesian const * const normal, *diameter = fmax(fmax(x_extent, y_extent),z_extent); // Only for active axes, find the minimum extent - if (bounds->x != 0) { + if (bounds->x > 0) { *diameter = fmin(*diameter, x_extent); } - if (bounds->y != 0) { + if (bounds->y > 0) { *diameter = fmin(*diameter, y_extent); } - if (bounds->z != 0) { + if (bounds->z > 0) { *diameter = fmin(*diameter, z_extent); } diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 63e09264e04..5c1d87048fc 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1959,11 +1959,11 @@ STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const // 1 timestep's worth of distance in prev_tc if ( !prev_tc) { tp_debug_print(" queue empty\n"); - return TP_ERR_FAIL; + return NO_BLEND; } if (prev_tc->progress > prev_tc->target / 2.0) { tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); - return TP_ERR_FAIL; + return NO_BLEND; } // Check for tangency between segments and handle any errors @@ -3471,7 +3471,8 @@ int tpRunCycle(TP_STRUCT * const tp, long period) } int tpSetSpindleSync(TP_STRUCT * const tp, double sync, int mode) { - if(sync) { + // WARNING assumes positive sync + if(sync > 0) { if (mode) { tp->synchronized = TC_SYNC_VELOCITY; } else { diff --git a/src/hal/classicladder/drawing_sequential.h b/src/hal/classicladder/drawing_sequential.h index 6762f77e737..cd67d2a72ca 100644 --- a/src/hal/classicladder/drawing_sequential.h +++ b/src/hal/classicladder/drawing_sequential.h @@ -13,6 +13,9 @@ // 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 +#include "sequential.h" +#include "gdk/gdktypes.h" + void DrawSeqStep(GdkPixmap * DrawPixmap,int x,int y,int Size,StrStep * pStep,char DrawingOption); void DrawSeqTransition(GdkPixmap * DrawPixmap,int x,int y,int Size,StrTransition * pTransi,char DrawingOption); diff --git a/src/hal/classicladder/files_sequential.c b/src/hal/classicladder/files_sequential.c index 95dad5a3901..a1ef0cf409b 100644 --- a/src/hal/classicladder/files_sequential.c +++ b/src/hal/classicladder/files_sequential.c @@ -28,6 +28,7 @@ #include "global.h" #include "files.h" #include "files_sequential.h" +#include "sequential.h" diff --git a/src/hal/classicladder/sequential.h b/src/hal/classicladder/sequential.h index 61623f1e8bd..1decf1a52be 100644 --- a/src/hal/classicladder/sequential.h +++ b/src/hal/classicladder/sequential.h @@ -18,6 +18,8 @@ /* License along with this library; if not, write to the Free Software */ /* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#ifndef SEQUENTIAL_H +#define SEQUENTIAL_H #define NBR_SEQUENTIAL_PAGES 5 @@ -116,4 +118,4 @@ typedef struct StrSequential StrSeqComment SeqComment[ NBR_SEQ_COMMENTS ]; }StrSequential; - +#endif From 87b05bc2cb6777773f30b2b6be537687ac217491 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 10 Jan 2019 13:45:29 -0500 Subject: [PATCH 055/129] tp: Refactor circle maximum velocity / acceleration ratio calculation Slightly redesign the acceleration ratio / maximum velocity calculation for circular motions. For circles with small radii relative to their requested velocity, limit the tangential acceleration to 50% of the peak, and cap maximum velocity so that normal acceleration is 86% of peak. For large radius circles, allow the tangential acceleration to be higher, since less of the acceleration budget will be used for normal acceleration. --- src/emc/tp/blendmath.c | 45 +++++++++++++++++++++++++++--------------- src/emc/tp/blendmath.h | 16 ++++++++++----- src/emc/tp/tc.c | 24 +++++++++++++++------- src/emc/tp/tc.h | 2 ++ src/emc/tp/tp.c | 11 ++++------- src/emc/tp/tp_debug.h | 5 +++++ 6 files changed, 68 insertions(+), 35 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 098b5b73e6c..a5709343281 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1627,28 +1627,41 @@ int blendPoints3Print(BlendPoints3 const * const points) } -double pmCircleActualMaxVel(PmCircle * const circle, - double * const acc_ratio_tangential, +PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, double v_max, double a_max) { - double a_n_max = BLEND_ACC_RATIO_NORMAL * a_max; + double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + double eff_radius = pmCircleEffectiveMinRadius(circle); - double v_max_acc = pmSqrt(a_n_max * eff_radius); - double v_max_eff = fmin(v_max_acc, v_max); - if (acc_ratio_tangential) { - double a_normal = fmin(pmSq(v_max_eff) / eff_radius, a_n_max); - *acc_ratio_tangential = (pmSqrt(pmSq(a_max) - pmSq(a_normal)) / a_max); - tp_debug_print("acc_ratio_tan = %f\n",*acc_ratio_tangential); - } + // Find the acceleration necessary to reach the maximum velocity + double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ); + // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio + double v_max_cutoff = pmSqrt(a_n_max_cutoff * eff_radius); + + double v_max_actual = v_max; + double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; - if (v_max_acc < v_max) { - tp_debug_print("Maxvel limited from %f to %f for tangential acceleration\n", v_max, v_max_acc); - return v_max_acc; + if (a_n_vmax > a_n_max_cutoff) { + v_max_actual = v_max_cutoff; } else { - tp_debug_print("v_max %f is within limit of v_max_acc %f\n",v_max, v_max_acc); - return v_max; + acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); } + + tp_debug_json_start(pmCircleActualMaxVel); + tp_debug_json_double(v_max); + tp_debug_json_double(v_max_cutoff); + tp_debug_json_double(a_n_max_cutoff); + tp_debug_json_double(a_n_vmax); + tp_debug_json_double(acc_ratio_tan); + tp_debug_json_end(); + + PmCircleLimits limits = { + v_max_actual, + acc_ratio_tan + }; + + return limits; } @@ -1816,7 +1829,7 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, * The radius of curvature of a spiral is larger than the circle of the same * radius. */ -double pmCircleEffectiveMinRadius(PmCircle const * const circle) +double pmCircleEffectiveMinRadius(PmCircle const * circle) { double radius0 = circle->radius; double radius1 = circle->radius + circle->spiral; diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index f1acb78199b..f9ec05e5278 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -235,16 +235,22 @@ int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const poi int blendGeom3Print(BlendGeom3 const * const geom); int blendParamPrint(BlendParameters const * const param); int blendPoints3Print(BlendPoints3 const * const points); -double pmCircleActualMaxVel(PmCircle * const circle, - double * const acc_ratio, - double v_max, - double a_max); + +typedef struct { + double v_max; + double acc_ratio; +} PmCircleLimits; + +PmCircleLimits pmCircleActualMaxVel(const PmCircle *circle, + double v_max_nominal, + double a_max_nominal); + int findSpiralArcLengthFit(PmCircle const * const circle, SpiralArcLengthFit * const fit); int pmCircleAngleFromProgress(PmCircle const * const circle, SpiralArcLengthFit const * const fit, double progress, double * const angle); -double pmCircleEffectiveMinRadius(PmCircle const * const circle); +double pmCircleEffectiveMinRadius(const PmCircle *circle); #endif diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index f9dc1208253..b81d94901b0 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -719,6 +719,20 @@ double pmCircle9Target(PmCircle9 const * const circ9) return helical_length; } +int tcUpdateCircleAccRatio(TC_STRUCT * tc) +{ + if (tc->motion_type == TC_CIRCULAR) { + PmCircleLimits limits = pmCircleActualMaxVel(&tc->coords.circle.xyz, + tc->maxvel, + tcGetOverallMaxAccel(tc)); + tc->maxvel = limits.v_max; + tc->acc_ratio_tan = limits.acc_ratio; + return 0; + } + // TODO handle blend arc here too? + return 1; //nothing to do, but not an error +} + /** * "Finalizes" a segment so that its length can't change. * By setting the finalized flag, we tell the optimizer that this segment's @@ -738,16 +752,12 @@ int tcFinalizeLength(TC_STRUCT * const tc) return TP_ERR_NO_ACTION; } - tp_debug_print("Finalizing tc id %d, type %d\n", tc->id, tc->motion_type); - //TODO function to check for parabolic? - tp_debug_print("blend_prev = %d, term_cond = %d\n",tc->blend_prev, tc->term_cond); - - if (tc->motion_type == TC_CIRCULAR) { - tc->maxvel = pmCircleActualMaxVel(&tc->coords.circle.xyz, &tc->acc_ratio_tan, tc->maxvel, tcGetOverallMaxAccel(tc)); - } + tp_debug_print("Finalizing motion id %d, type %d\n", tc->id, tc->motion_type); tcClampVelocityByLength(tc); + tcUpdateCircleAccRatio(tc); + tc->finalized = 1; return TP_ERR_OK; } diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 804d68c5517..d9b0c23e73d 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -100,6 +100,8 @@ int tcSetupMotion(TC_STRUCT * const tc, int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp); +int tcUpdateCircleAccRatio(TC_STRUCT * tc); + int tcFinalizeLength(TC_STRUCT * const tc); int tcClampVelocityByLength(TC_STRUCT * const tc); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 5c1d87048fc..d3a675b9ec1 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2117,18 +2117,15 @@ int tpAddCircle(TP_STRUCT * const tp, tp_debug_print("tc.target = %f\n",tc.target); tc.nominal_length = tc.target; - //Reduce max velocity to match sample rate - tcClampVelocityByLength(&tc); - - double v_max_actual = pmCircleActualMaxVel(&tc.coords.circle.xyz, &tc.acc_ratio_tan, ini_maxvel, acc); - tp_debug_print("tc.acc_ratio_tan = %f\n",tc.acc_ratio_tan); - // Copy in motion parameters tcSetupMotion(&tc, vel, - v_max_actual, + ini_maxvel, acc); + //Reduce max velocity to match sample rate + tcClampVelocityByLength(&tc); + return tpFinalizeAndEnqueue(tp, &tc); } diff --git a/src/emc/tp/tp_debug.h b/src/emc/tp/tp_debug.h index a672ed07888..aea44f6588c 100644 --- a/src/emc/tp/tp_debug.h +++ b/src/emc/tp/tp_debug.h @@ -24,6 +24,11 @@ #define tp_debug_print(...) #endif +// Verbose but effective wrappers for building faux-JSON debug output for a function +#define tp_debug_json_double(varname_) tp_debug_print("%s: %f, ", #varname_, varname_) +#define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_) +#define tp_debug_json_end() tp_debug_print("}\n") + /** Use for profiling to make static function names visible */ #ifdef TP_PROFILE #define STATIC From 1076ca151b1fed2a2fa0dd2226660ad1555577e6 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 17:02:53 -0500 Subject: [PATCH 056/129] tp: Factor out calculation for peak velocity over a distance. --- src/emc/tp/blendmath.c | 9 +-------- src/emc/tp/blendmath.h | 4 ++++ src/emc/tp/tc.c | 4 ---- src/emc/tp/tp.c | 12 +++++------- 4 files changed, 10 insertions(+), 19 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index a5709343281..01dca723cb6 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1649,6 +1649,7 @@ PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, } tp_debug_json_start(pmCircleActualMaxVel); + tp_debug_json_double(eff_radius); tp_debug_json_double(v_max); tp_debug_json_double(v_max_cutoff); tp_debug_json_double(a_n_max_cutoff); @@ -1831,17 +1832,9 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, */ double pmCircleEffectiveMinRadius(PmCircle const * circle) { - double radius0 = circle->radius; - double radius1 = circle->radius + circle->spiral; - - double min_radius = fmin(radius0, radius1); - double dr = circle->spiral / circle->angle; double effective_radius = pmSqrt(pmSq(min_radius)+pmSq(dr)); - tp_debug_print("min_radius = %f, effective_min_radius = %f\n", - min_radius, - effective_radius); return effective_radius; } diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index f9ec05e5278..dfc2b4ad963 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -253,4 +253,8 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, double * const angle); double pmCircleEffectiveMinRadius(const PmCircle *circle); +static inline double findVPeak(double a_t_max, double distance) +{ + return pmSqrt(a_t_max * distance); +} #endif diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index b81d94901b0..99e5612f745 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -252,10 +252,6 @@ int pmCircleTangentVector(PmCircle const * const circle, */ pmCartCartCross(&circle->normal, &radius, &uTan); - // find dz/dtheta and get differential movement along helical axis - double h; - pmCartMag(&circle->rHelix, &h); - /* the binormal component of the tangent vector is (dz / dtheta) * dtheta. */ double dz = 1.0 / circle->angle; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index d3a675b9ec1..2de53243df7 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -684,10 +684,7 @@ STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { // blending may remove up to 1/2 of the segment length /= 2.0; } - double triangle_vel = pmSqrt( acc_scaled * length); - tp_debug_print("triangle vel for segment %d is %f\n", tc->id, triangle_vel); - - return triangle_vel; + return findVPeak(acc_scaled, length); } @@ -702,10 +699,11 @@ STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) { double acc_scaled = tcGetTangentialMaxAccel(tc); - //FIXME this is defined in two places! - double triangle_vel = pmSqrt( acc_scaled * tc->target * BLEND_DIST_FRACTION); + double triangle_vel = findVPeak(acc_scaled, tc->target); double max_vel = tpGetMaxTargetVel(tp, tc); - tp_debug_print("optimization initial vel for segment %d is %f\n", tc->id, triangle_vel); + tp_debug_json_start(tpCalculateOptimizationInitialVel); + tp_debug_json_double(triangle_vel); + tp_debug_json_end(); return fmin(triangle_vel, max_vel); } From e94a7cc881452db58347cd4146524435dbe33f27 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 19:20:48 -0500 Subject: [PATCH 057/129] tp: Compute effective radius of helix too. The effective radius of a helical motion is larger than the equivalent circle / spiral. In effect, the motion is stretched out along the helical axis, so the normal acceleration is lower for the same feed rate. --- src/emc/tp/blendmath.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 01dca723cb6..1604fdda4d2 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1833,7 +1833,17 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, double pmCircleEffectiveMinRadius(PmCircle const * circle) { double dr = circle->spiral / circle->angle; - double effective_radius = pmSqrt(pmSq(min_radius)+pmSq(dr)); + double h2; + pmCartMagSq(&circle->rHelix, &h2); + + // Exact representation of spiral arc length flattened into + double n_inner = pmSq(dr) + pmSq(circle->radius); + double den = n_inner+pmSq(dr); + double num = pmSqrt(n_inner * n_inner * n_inner); + double r_spiral = num / den; + + // Curvature of helix, assuming that helical motion is independent of plane motion + double effective_radius = h2 / r_spiral + r_spiral; return effective_radius; } From ba833b123892cc71cacf38af906ca70e897d521a Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 07:21:08 -0500 Subject: [PATCH 058/129] tp: Refactor and simplify parallel / anti-parallel checks In several places in the TP, we need to check if unit vectors are parallel and / or anti-parallel. Previously, this was implemented with dot product / arccos, which could be expensive to evaluate. This is now implemented as a tolerance on the delta between the sum or difference of the vectors, which is fast, and numerically robust (no divisions / trancendentals). --- src/emc/tp/blendmath.c | 109 ++++++++++++++++++----------------------- src/emc/tp/blendmath.h | 12 ++++- src/emc/tp/tp.c | 24 ++++----- src/emc/tp/tp_types.h | 1 + 4 files changed, 70 insertions(+), 76 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 1604fdda4d2..258d4a378e6 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -301,80 +301,69 @@ int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const ar } - /** - * Check if two cartesian vectors are parallel. - * The input tolerance specifies what the maximum angle between the - * lines containing two vectors is. Note that vectors pointing in - * opposite directions are still considered parallel, since their - * containing lines are parallel. - * @param v1 input vector 1 - * @param v2 input vector 2 - * @param tol angle tolerance for parallelism + * Checks if two UNIT vectors are parallel to the given angle tolerance (in radians). + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! */ -int pmCartCartParallel(PmCartesian const * const v1, - PmCartesian const * const v2, double tol) +int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) { - PmCartesian u1,u2; - pmCartUnit(v1, &u1); - pmCartUnit(v2, &u2); - double dot; - pmCartCartDot(&u1, &u2, &dot); - double theta = acos(fabs(dot)); - if (theta < tol) { - return 1; - } else { - return 0; + double d_diff; + { + PmCartesian u_diff; + pmCartCartSub(u1, u2, &u_diff); + pmCartMagSq(&u_diff, &d_diff); } -} + tp_debug_json_start(pmCartCartParallel); + tp_debug_json_double(d_diff); + tp_debug_json_end(); + + return d_diff < tol; +} /** - * Check if a Circle and line are coplanar. - * - * @param circ PmCircle input - * @param line PmCartLine input - * @param tol deviation tolerance (magnitude of error component) + * Checks if two UNIT vectors are anti-parallel to the given angle tolerance (in radians). + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! */ -int pmCircLineCoplanar(PmCircle const * const circ, - PmCartLine const * const line, double tol) +int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) { - double dot; - int res = pmCartCartDot(&circ->normal, &line->uVec, &dot); - tp_debug_print("normal = %.12g %.12g %.12g, uVec = %.12g %.12g %.12g, dot = %.12g\n", - circ->normal.x, - circ->normal.y, - circ->normal.z, - line->uVec.x, - line->uVec.y, - line->uVec.z, - dot); - if (fabs(dot) < tol && !res) { - return 1; - } else { - return 0; + double d_sum; + { + PmCartesian u_sum; + pmCartCartAdd(u1, u2, &u_sum); + pmCartMagSq(&u_sum, &d_sum); } -} + tp_debug_json_start(pmCartCartAntiParallel); + tp_debug_json_double(d_sum); + tp_debug_json_end(); -int blendCoplanarCheck(PmCartesian const * const normal, - PmCartesian const * const u1_tan, - PmCartesian const * const u2_tan, - double tol) -{ - if (!normal || !u1_tan || !u2_tan) { - return TP_ERR_MISSING_INPUT; - } + return d_sum < tol; +} - double dot1, dot2; - int res1 = pmCartCartDot(normal, u1_tan, &dot1); - int res2 = pmCartCartDot(normal, u2_tan, &dot2); - if (fabs(dot1) < tol && fabs(dot2) < tol && !res1 && !res2) { - return 1; - } else { - return 0; - } +/** + * Check if two cartesian vectors are parallel or anti-parallel + * The input tolerance specifies what the maximum angle between the + * lines containing two vectors is. Note that vectors pointing in + * opposite directions are still considered parallel, since their + * containing lines are parallel. + * @param u1 input unit vector 1 + * @param u2 input unit vector 2 + * @pre BOTH u1 and u2 must be unit vectors or calculation may be skewed. + */ +int pmUnitCartsColinear(PmCartesian const * const u1, + PmCartesian const * const u2) +{ + return pmCartCartParallel(u1, u2, TP_ANGLE_EPSILON_SQ) || pmCartCartAntiParallel(u1, u2, TP_ANGLE_EPSILON_SQ); } diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index dfc2b4ad963..d3ed555ce86 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -154,8 +154,16 @@ double findTrapezoidalDesiredVel(double a_max, double currentvel, double cycle_time); -int pmCartCartParallel(PmCartesian const * const v1, - PmCartesian const * const v2, double tol); +int pmUnitCartsColinear(PmCartesian const * const u1, + PmCartesian const * const u2); + +int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol); + +int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol); int pmCircLineCoplanar(PmCircle const * const circ, PmCartLine const * const line, double tol); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 2de53243df7..7a751f91fcd 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -889,9 +889,8 @@ STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const pre } // Check for coplanarity based on binormal and tangents - int coplanar = pmCartCartParallel(&geom.binormal, - &tc->coords.circle.xyz.normal, - TP_ANGLE_EPSILON); + int coplanar = pmUnitCartsColinear(&geom.binormal, + &tc->coords.circle.xyz.normal); if (!coplanar) { tp_debug_print("aborting arc, not coplanar\n"); @@ -1046,9 +1045,8 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre } // Check for coplanarity based on binormal - int coplanar = pmCartCartParallel(&geom.binormal, - &prev_tc->coords.circle.xyz.normal, - TP_ANGLE_EPSILON); + int coplanar = pmUnitCartsColinear(&geom.binormal, + &prev_tc->coords.circle.xyz.normal); if (!coplanar) { tp_debug_print("aborting arc, not coplanar\n"); @@ -1161,8 +1159,8 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev tp_debug_print("-- Starting ArcArc blend arc --\n"); //TODO type checks - int colinear = pmCartCartParallel(&prev_tc->coords.circle.xyz.normal, - &tc->coords.circle.xyz.normal, TP_ANGLE_EPSILON); + int colinear = pmUnitCartsColinear(&prev_tc->coords.circle.xyz.normal, + &tc->coords.circle.xyz.normal); if (!colinear) { // Fail out if not collinear tp_debug_print("arc abort: not coplanar\n"); @@ -1194,18 +1192,16 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev return res_init; } - int coplanar1 = pmCartCartParallel(&geom.binormal, - &prev_tc->coords.circle.xyz.normal, - TP_ANGLE_EPSILON); + int coplanar1 = pmUnitCartsColinear(&geom.binormal, + &prev_tc->coords.circle.xyz.normal); if (!coplanar1) { tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", prev_tc->id); return TP_ERR_FAIL; } - int coplanar2 = pmCartCartParallel(&geom.binormal, - &tc->coords.circle.xyz.normal, - TP_ANGLE_EPSILON); + int coplanar2 = pmUnitCartsColinear(&geom.binormal, + &tc->coords.circle.xyz.normal); if (!coplanar2) { tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", tc->id); return TP_ERR_FAIL; diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 72eefc0df84..e82b8dcf954 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -41,6 +41,7 @@ #define TP_POS_EPSILON 1e-12 #define TP_TIME_EPSILON 1e-12 #define TP_ANGLE_EPSILON 1e-6 +#define TP_ANGLE_EPSILON_SQ (TP_ANGLE_EPSILON * TP_ANGLE_EPSILON) #define TP_MIN_ARC_ANGLE 1e-3 #define TP_MIN_ARC_LENGTH 1e-6 #define TP_BIG_NUM 1e10 From fc113386c9b362293758438efa2ef990984f83b4 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 17:50:16 -0500 Subject: [PATCH 059/129] unit_test: Add "greatest" unit test framework (C, header-only macro-based unit tests) This header provides many easy-to-use unit testing macros and a simple framework to create / run the tests. It lacks some features (like automatic aggregation of test cases), but it's easy to understand and extend. It's a good starting point for unit tests in LinuxCNC. --- unit_tests/greatest.h | 1226 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1226 insertions(+) create mode 100644 unit_tests/greatest.h diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h new file mode 100644 index 00000000000..3f24502ebba --- /dev/null +++ b/unit_tests/greatest.h @@ -0,0 +1,1226 @@ +/* + * Copyright (c) 2011-2019 Scott Vokes + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#ifndef GREATEST_H +#define GREATEST_H + +#if defined(__cplusplus) && !defined(GREATEST_NO_EXTERN_CPLUSPLUS) +extern "C" { +#endif + +/* 1.4.1 */ +#define GREATEST_VERSION_MAJOR 1 +#define GREATEST_VERSION_MINOR 4 +#define GREATEST_VERSION_PATCH 1 + +/* A unit testing system for C, contained in 1 file. + * It doesn't use dynamic allocation or depend on anything + * beyond ANSI C89. + * + * An up-to-date version can be found at: + * https://github.com/silentbicycle/greatest/ + */ + + +/********************************************************************* + * Minimal test runner template + *********************************************************************/ +#if 0 + +#include "greatest.h" + +TEST foo_should_foo(void) { + PASS(); +} + +static void setup_cb(void *data) { + printf("setup callback for each test case\n"); +} + +static void teardown_cb(void *data) { + printf("teardown callback for each test case\n"); +} + +SUITE(suite) { + /* Optional setup/teardown callbacks which will be run before/after + * every test case. If using a test suite, they will be cleared when + * the suite finishes. */ + SET_SETUP(setup_cb, voidp_to_callback_data); + SET_TEARDOWN(teardown_cb, voidp_to_callback_data); + + RUN_TEST(foo_should_foo); +} + +/* Add definitions that need to be in the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +/* Set up, run suite(s) of tests, report pass/fail/skip stats. */ +int run_tests(void) { + GREATEST_INIT(); /* init. greatest internals */ + /* List of suites to run (if any). */ + RUN_SUITE(suite); + + /* Tests can also be run directly, without using test suites. */ + RUN_TEST(foo_should_foo); + + GREATEST_PRINT_REPORT(); /* display results */ + return greatest_all_passed(); +} + +/* main(), for a standalone command-line test runner. + * This replaces run_tests above, and adds command line option + * handling and exiting with a pass/fail status. */ +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); /* init & parse command-line args */ + RUN_SUITE(suite); + GREATEST_MAIN_END(); /* display results */ +} + +#endif +/*********************************************************************/ + + +#include +#include +#include +#include + +/*********** + * Options * + ***********/ + +/* Default column width for non-verbose output. */ +#ifndef GREATEST_DEFAULT_WIDTH +#define GREATEST_DEFAULT_WIDTH 72 +#endif + +/* FILE *, for test logging. */ +#ifndef GREATEST_STDOUT +#define GREATEST_STDOUT stdout +#endif + +/* Remove GREATEST_ prefix from most commonly used symbols? */ +#ifndef GREATEST_USE_ABBREVS +#define GREATEST_USE_ABBREVS 1 +#endif + +/* Set to 0 to disable all use of setjmp/longjmp. */ +#ifndef GREATEST_USE_LONGJMP +#define GREATEST_USE_LONGJMP 1 +#endif + +/* Make it possible to replace fprintf with another + * function with the same interface. */ +#ifndef GREATEST_FPRINTF +#define GREATEST_FPRINTF fprintf +#endif + +#if GREATEST_USE_LONGJMP +#include +#endif + +/* Set to 0 to disable all use of time.h / clock(). */ +#ifndef GREATEST_USE_TIME +#define GREATEST_USE_TIME 1 +#endif + +#if GREATEST_USE_TIME +#include +#endif + +/* Floating point type, for ASSERT_IN_RANGE. */ +#ifndef GREATEST_FLOAT +#define GREATEST_FLOAT double +#define GREATEST_FLOAT_FMT "%g" +#endif + +/* Size of buffer for test name + optional '_' separator and suffix */ +#ifndef GREATEST_TESTNAME_BUF_SIZE +#define GREATEST_TESTNAME_BUF_SIZE 128 +#endif + + +/********* + * Types * + *********/ + +/* Info for the current running suite. */ +typedef struct greatest_suite_info { + unsigned int tests_run; + unsigned int passed; + unsigned int failed; + unsigned int skipped; + +#if GREATEST_USE_TIME + /* timers, pre/post running suite and individual tests */ + clock_t pre_suite; + clock_t post_suite; + clock_t pre_test; + clock_t post_test; +#endif +} greatest_suite_info; + +/* Type for a suite function. */ +typedef void greatest_suite_cb(void); + +/* Types for setup/teardown callbacks. If non-NULL, these will be run + * and passed the pointer to their additional data. */ +typedef void greatest_setup_cb(void *udata); +typedef void greatest_teardown_cb(void *udata); + +/* Type for an equality comparison between two pointers of the same type. + * Should return non-0 if equal, otherwise 0. + * UDATA is a closure value, passed through from ASSERT_EQUAL_T[m]. */ +typedef int greatest_equal_cb(const void *expd, const void *got, void *udata); + +/* Type for a callback that prints a value pointed to by T. + * Return value has the same meaning as printf's. + * UDATA is a closure value, passed through from ASSERT_EQUAL_T[m]. */ +typedef int greatest_printf_cb(const void *t, void *udata); + +/* Callbacks for an arbitrary type; needed for type-specific + * comparisons via GREATEST_ASSERT_EQUAL_T[m].*/ +typedef struct greatest_type_info { + greatest_equal_cb *equal; + greatest_printf_cb *print; +} greatest_type_info; + +typedef struct greatest_memory_cmp_env { + const unsigned char *exp; + const unsigned char *got; + size_t size; +} greatest_memory_cmp_env; + +/* Callbacks for string and raw memory types. */ +extern greatest_type_info greatest_type_info_string; +extern greatest_type_info greatest_type_info_memory; + +typedef enum { + GREATEST_FLAG_FIRST_FAIL = 0x01, + GREATEST_FLAG_LIST_ONLY = 0x02, + GREATEST_FLAG_ABORT_ON_FAIL = 0x04 +} greatest_flag_t; + +/* Internal state for a PRNG, used to shuffle test order. */ +struct greatest_prng { + unsigned char random_order; /* use random ordering? */ + unsigned char initialized; /* is random ordering initialized? */ + unsigned char pad_0[6]; + unsigned long state; /* PRNG state */ + unsigned long count; /* how many tests, this pass */ + unsigned long count_ceil; /* total number of tests */ + unsigned long count_run; /* total tests run */ + unsigned long mod; /* power-of-2 ceiling of count_ceil */ + unsigned long a; /* LCG multiplier */ + unsigned long c; /* LCG increment */ +}; + +/* Struct containing all test runner state. */ +typedef struct greatest_run_info { + unsigned char flags; + unsigned char verbosity; + unsigned char pad_0[2]; + + unsigned int tests_run; /* total test count */ + + /* currently running test suite */ + greatest_suite_info suite; + + /* overall pass/fail/skip counts */ + unsigned int passed; + unsigned int failed; + unsigned int skipped; + unsigned int assertions; + + /* info to print about the most recent failure */ + unsigned int fail_line; + unsigned int pad_1; + const char *fail_file; + const char *msg; + + /* current setup/teardown hooks and userdata */ + greatest_setup_cb *setup; + void *setup_udata; + greatest_teardown_cb *teardown; + void *teardown_udata; + + /* formatting info for ".....s...F"-style output */ + unsigned int col; + unsigned int width; + + /* only run a specific suite or test */ + const char *suite_filter; + const char *test_filter; + const char *test_exclude; + const char *name_suffix; /* print suffix with test name */ + char name_buf[GREATEST_TESTNAME_BUF_SIZE]; + + struct greatest_prng prng[2]; /* 0: suites, 1: tests */ + +#if GREATEST_USE_TIME + /* overall timers */ + clock_t begin; + clock_t end; +#endif + +#if GREATEST_USE_LONGJMP + int pad_jmp_buf; + unsigned char pad_2[4]; + jmp_buf jump_dest; +#endif +} greatest_run_info; + +struct greatest_report_t { + /* overall pass/fail/skip counts */ + unsigned int passed; + unsigned int failed; + unsigned int skipped; + unsigned int assertions; +}; + +/* Global var for the current testing context. + * Initialized by GREATEST_MAIN_DEFS(). */ +extern greatest_run_info greatest_info; + +/* Type for ASSERT_ENUM_EQ's ENUM_STR argument. */ +typedef const char *greatest_enum_str_fun(int value); + + +/********************** + * Exported functions * + **********************/ + +/* These are used internally by greatest macros. */ +int greatest_test_pre(const char *name); +void greatest_test_post(int res); +int greatest_do_assert_equal_t(const void *expd, const void *got, + greatest_type_info *type_info, void *udata); +void greatest_prng_init_first_pass(int id); +int greatest_prng_init_second_pass(int id, unsigned long seed); +void greatest_prng_step(int id); + +/* These are part of the public greatest API. */ +void GREATEST_SET_SETUP_CB(greatest_setup_cb *cb, void *udata); +void GREATEST_SET_TEARDOWN_CB(greatest_teardown_cb *cb, void *udata); +void GREATEST_INIT(void); +void GREATEST_PRINT_REPORT(void); +int greatest_all_passed(void); +void greatest_set_suite_filter(const char *filter); +void greatest_set_test_filter(const char *filter); +void greatest_set_test_exclude(const char *filter); +void greatest_stop_at_first_fail(void); +void greatest_abort_on_fail(void); +void greatest_list_only(void); +void greatest_get_report(struct greatest_report_t *report); +unsigned int greatest_get_verbosity(void); +void greatest_set_verbosity(unsigned int verbosity); +void greatest_set_flag(greatest_flag_t flag); +void greatest_set_test_suffix(const char *suffix); + + +/******************** +* Language Support * +********************/ + +/* If __VA_ARGS__ (C99) is supported, allow parametric testing +* without needing to manually manage the argument struct. */ +#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 19901L) || \ + (defined(_MSC_VER) && _MSC_VER >= 1800) +#define GREATEST_VA_ARGS +#endif + + +/********** + * Macros * + **********/ + +/* Define a suite. (The duplication is intentional -- it eliminates + * a warning from -Wmissing-declarations.) */ +#define GREATEST_SUITE(NAME) void NAME(void); void NAME(void) + +/* Declare a suite, provided by another compilation unit. */ +#define GREATEST_SUITE_EXTERN(NAME) void NAME(void) + +/* Start defining a test function. + * The arguments are not included, to allow parametric testing. */ +#define GREATEST_TEST static enum greatest_test_res + +/* PASS/FAIL/SKIP result from a test. Used internally. */ +typedef enum greatest_test_res { + GREATEST_TEST_RES_PASS = 0, + GREATEST_TEST_RES_FAIL = -1, + GREATEST_TEST_RES_SKIP = 1 +} greatest_test_res; + +/* Run a suite. */ +#define GREATEST_RUN_SUITE(S_NAME) greatest_run_suite(S_NAME, #S_NAME) + +/* Run a test in the current suite. */ +#define GREATEST_RUN_TEST(TEST) \ + do { \ + if (greatest_test_pre(#TEST) == 1) { \ + enum greatest_test_res res = GREATEST_SAVE_CONTEXT(); \ + if (res == GREATEST_TEST_RES_PASS) { \ + res = TEST(); \ + } \ + greatest_test_post(res); \ + } \ + } while (0) + +/* Ignore a test, don't warn about it being unused. */ +#define GREATEST_IGNORE_TEST(TEST) (void)TEST + +/* Run a test in the current suite with one void * argument, + * which can be a pointer to a struct with multiple arguments. */ +#define GREATEST_RUN_TEST1(TEST, ENV) \ + do { \ + if (greatest_test_pre(#TEST) == 1) { \ + enum greatest_test_res res = GREATEST_SAVE_CONTEXT(); \ + if (res == GREATEST_TEST_RES_PASS) { \ + res = TEST(ENV); \ + } \ + greatest_test_post(res); \ + } \ + } while (0) + +#ifdef GREATEST_VA_ARGS +#define GREATEST_RUN_TESTp(TEST, ...) \ + do { \ + if (greatest_test_pre(#TEST) == 1) { \ + enum greatest_test_res res = GREATEST_SAVE_CONTEXT(); \ + if (res == GREATEST_TEST_RES_PASS) { \ + res = TEST(__VA_ARGS__); \ + } \ + greatest_test_post(res); \ + } \ + } while (0) +#endif + + +/* Check if the test runner is in verbose mode. */ +#define GREATEST_IS_VERBOSE() ((greatest_info.verbosity) > 0) +#define GREATEST_LIST_ONLY() \ + (greatest_info.flags & GREATEST_FLAG_LIST_ONLY) +#define GREATEST_FIRST_FAIL() \ + (greatest_info.flags & GREATEST_FLAG_FIRST_FAIL) +#define GREATEST_ABORT_ON_FAIL() \ + (greatest_info.flags & GREATEST_FLAG_ABORT_ON_FAIL) +#define GREATEST_FAILURE_ABORT() \ + (GREATEST_FIRST_FAIL() && \ + (greatest_info.suite.failed > 0 || greatest_info.failed > 0)) + +/* Message-less forms of tests defined below. */ +#define GREATEST_PASS() GREATEST_PASSm(NULL) +#define GREATEST_FAIL() GREATEST_FAILm(NULL) +#define GREATEST_SKIP() GREATEST_SKIPm(NULL) +#define GREATEST_ASSERT(COND) \ + GREATEST_ASSERTm(#COND, COND) +#define GREATEST_ASSERT_OR_LONGJMP(COND) \ + GREATEST_ASSERT_OR_LONGJMPm(#COND, COND) +#define GREATEST_ASSERT_FALSE(COND) \ + GREATEST_ASSERT_FALSEm(#COND, COND) +#define GREATEST_ASSERT_EQ(EXP, GOT) \ + GREATEST_ASSERT_EQm(#EXP " != " #GOT, EXP, GOT) +#define GREATEST_ASSERT_EQ_FMT(EXP, GOT, FMT) \ + GREATEST_ASSERT_EQ_FMTm(#EXP " != " #GOT, EXP, GOT, FMT) +#define GREATEST_ASSERT_IN_RANGE(EXP, GOT, TOL) \ + GREATEST_ASSERT_IN_RANGEm(#EXP " != " #GOT " +/- " #TOL, EXP, GOT, TOL) +#define GREATEST_ASSERT_EQUAL_T(EXP, GOT, TYPE_INFO, UDATA) \ + GREATEST_ASSERT_EQUAL_Tm(#EXP " != " #GOT, EXP, GOT, TYPE_INFO, UDATA) +#define GREATEST_ASSERT_STR_EQ(EXP, GOT) \ + GREATEST_ASSERT_STR_EQm(#EXP " != " #GOT, EXP, GOT) +#define GREATEST_ASSERT_STRN_EQ(EXP, GOT, SIZE) \ + GREATEST_ASSERT_STRN_EQm(#EXP " != " #GOT, EXP, GOT, SIZE) +#define GREATEST_ASSERT_MEM_EQ(EXP, GOT, SIZE) \ + GREATEST_ASSERT_MEM_EQm(#EXP " != " #GOT, EXP, GOT, SIZE) +#define GREATEST_ASSERT_ENUM_EQ(EXP, GOT, ENUM_STR) \ + GREATEST_ASSERT_ENUM_EQm(#EXP " != " #GOT, EXP, GOT, ENUM_STR) + +/* The following forms take an additional message argument first, + * to be displayed by the test runner. */ + +/* Fail if a condition is not true, with message. */ +#define GREATEST_ASSERTm(MSG, COND) \ + do { \ + greatest_info.assertions++; \ + if (!(COND)) { GREATEST_FAILm(MSG); } \ + } while (0) + +/* Fail if a condition is not true, longjmping out of test. */ +#define GREATEST_ASSERT_OR_LONGJMPm(MSG, COND) \ + do { \ + greatest_info.assertions++; \ + if (!(COND)) { GREATEST_FAIL_WITH_LONGJMPm(MSG); } \ + } while (0) + +/* Fail if a condition is not false, with message. */ +#define GREATEST_ASSERT_FALSEm(MSG, COND) \ + do { \ + greatest_info.assertions++; \ + if ((COND)) { GREATEST_FAILm(MSG); } \ + } while (0) + +/* Fail if EXP != GOT (equality comparison by ==). */ +#define GREATEST_ASSERT_EQm(MSG, EXP, GOT) \ + do { \ + greatest_info.assertions++; \ + if ((EXP) != (GOT)) { GREATEST_FAILm(MSG); } \ + } while (0) + +/* Fail if EXP != GOT (equality comparison by ==). + * Warning: FMT, EXP, and GOT will be evaluated more + * than once on failure. */ +#define GREATEST_ASSERT_EQ_FMTm(MSG, EXP, GOT, FMT) \ + do { \ + greatest_info.assertions++; \ + if ((EXP) != (GOT)) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: "); \ + GREATEST_FPRINTF(GREATEST_STDOUT, FMT, EXP); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: "); \ + GREATEST_FPRINTF(GREATEST_STDOUT, FMT, GOT); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + GREATEST_FAILm(MSG); \ + } \ + } while (0) + +/* Fail if EXP is not equal to GOT, printing enum IDs. */ +#define GREATEST_ASSERT_ENUM_EQm(MSG, EXP, GOT, ENUM_STR) \ + do { \ + int greatest_EXP = (int)(EXP); \ + int greatest_GOT = (int)(GOT); \ + greatest_enum_str_fun *greatest_ENUM_STR = ENUM_STR; \ + if (greatest_EXP != greatest_GOT) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: %s", \ + greatest_ENUM_STR(greatest_EXP)); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: %s\n", \ + greatest_ENUM_STR(greatest_GOT)); \ + GREATEST_FAILm(MSG); \ + } \ + } while (0) \ + +/* Fail if GOT not in range of EXP +|- TOL. */ +#define GREATEST_ASSERT_IN_RANGEm(MSG, EXP, GOT, TOL) \ + do { \ + GREATEST_FLOAT greatest_EXP = (EXP); \ + GREATEST_FLOAT greatest_GOT = (GOT); \ + GREATEST_FLOAT greatest_TOL = (TOL); \ + greatest_info.assertions++; \ + if ((greatest_EXP > greatest_GOT && \ + greatest_EXP - greatest_GOT > greatest_TOL) || \ + (greatest_EXP < greatest_GOT && \ + greatest_GOT - greatest_EXP > greatest_TOL)) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "\nExpected: " GREATEST_FLOAT_FMT \ + " +/- " GREATEST_FLOAT_FMT \ + "\n Got: " GREATEST_FLOAT_FMT \ + "\n", \ + greatest_EXP, greatest_TOL, greatest_GOT); \ + GREATEST_FAILm(MSG); \ + } \ + } while (0) + +/* Fail if EXP is not equal to GOT, according to strcmp. */ +#define GREATEST_ASSERT_STR_EQm(MSG, EXP, GOT) \ + do { \ + GREATEST_ASSERT_EQUAL_Tm(MSG, EXP, GOT, \ + &greatest_type_info_string, NULL); \ + } while (0) \ + +/* Fail if EXP is not equal to GOT, according to strcmp. */ +#define GREATEST_ASSERT_STRN_EQm(MSG, EXP, GOT, SIZE) \ + do { \ + size_t size = SIZE; \ + GREATEST_ASSERT_EQUAL_Tm(MSG, EXP, GOT, \ + &greatest_type_info_string, &size); \ + } while (0) \ + +/* Fail if EXP is not equal to GOT, according to memcmp. */ +#define GREATEST_ASSERT_MEM_EQm(MSG, EXP, GOT, SIZE) \ + do { \ + greatest_memory_cmp_env env; \ + env.exp = (const unsigned char *)EXP; \ + env.got = (const unsigned char *)GOT; \ + env.size = SIZE; \ + GREATEST_ASSERT_EQUAL_Tm(MSG, env.exp, env.got, \ + &greatest_type_info_memory, &env); \ + } while (0) \ + +/* Fail if EXP is not equal to GOT, according to a comparison + * callback in TYPE_INFO. If they are not equal, optionally use a + * print callback in TYPE_INFO to print them. */ +#define GREATEST_ASSERT_EQUAL_Tm(MSG, EXP, GOT, TYPE_INFO, UDATA) \ + do { \ + greatest_type_info *type_info = (TYPE_INFO); \ + greatest_info.assertions++; \ + if (!greatest_do_assert_equal_t(EXP, GOT, \ + type_info, UDATA)) { \ + if (type_info == NULL || type_info->equal == NULL) { \ + GREATEST_FAILm("type_info->equal callback missing!"); \ + } else { \ + GREATEST_FAILm(MSG); \ + } \ + } \ + } while (0) \ + +/* Pass. */ +#define GREATEST_PASSm(MSG) \ + do { \ + greatest_info.msg = MSG; \ + return GREATEST_TEST_RES_PASS; \ + } while (0) + +/* Fail. */ +#define GREATEST_FAILm(MSG) \ + do { \ + greatest_info.fail_file = __FILE__; \ + greatest_info.fail_line = __LINE__; \ + greatest_info.msg = MSG; \ + if (GREATEST_ABORT_ON_FAIL()) { abort(); } \ + return GREATEST_TEST_RES_FAIL; \ + } while (0) + +/* Optional GREATEST_FAILm variant that longjmps. */ +#if GREATEST_USE_LONGJMP +#define GREATEST_FAIL_WITH_LONGJMP() GREATEST_FAIL_WITH_LONGJMPm(NULL) +#define GREATEST_FAIL_WITH_LONGJMPm(MSG) \ + do { \ + greatest_info.fail_file = __FILE__; \ + greatest_info.fail_line = __LINE__; \ + greatest_info.msg = MSG; \ + longjmp(greatest_info.jump_dest, GREATEST_TEST_RES_FAIL); \ + } while (0) +#endif + +/* Skip the current test. */ +#define GREATEST_SKIPm(MSG) \ + do { \ + greatest_info.msg = MSG; \ + return GREATEST_TEST_RES_SKIP; \ + } while (0) + +/* Check the result of a subfunction using ASSERT, etc. */ +#define GREATEST_CHECK_CALL(RES) \ + do { \ + enum greatest_test_res greatest_RES = RES; \ + if (greatest_RES != GREATEST_TEST_RES_PASS) { \ + return greatest_RES; \ + } \ + } while (0) \ + +#if GREATEST_USE_TIME +#define GREATEST_SET_TIME(NAME) \ + NAME = clock(); \ + if (NAME == (clock_t) -1) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "clock error: %s\n", #NAME); \ + exit(EXIT_FAILURE); \ + } + +#define GREATEST_CLOCK_DIFF(C1, C2) \ + GREATEST_FPRINTF(GREATEST_STDOUT, " (%lu ticks, %.3f sec)", \ + (long unsigned int) (C2) - (long unsigned int)(C1), \ + (double)((C2) - (C1)) / (1.0 * (double)CLOCKS_PER_SEC)) +#else +#define GREATEST_SET_TIME(UNUSED) +#define GREATEST_CLOCK_DIFF(UNUSED1, UNUSED2) +#endif + +#if GREATEST_USE_LONGJMP +#define GREATEST_SAVE_CONTEXT() \ + /* setjmp returns 0 (GREATEST_TEST_RES_PASS) on first call * \ + * so the test runs, then RES_FAIL from FAIL_WITH_LONGJMP. */ \ + ((enum greatest_test_res)(setjmp(greatest_info.jump_dest))) +#else +#define GREATEST_SAVE_CONTEXT() \ + /*a no-op, since setjmp/longjmp aren't being used */ \ + GREATEST_TEST_RES_PASS +#endif + +/* Run every suite / test function run within BODY in pseudo-random + * order, seeded by SEED. (The top 3 bits of the seed are ignored.) + * + * This should be called like: + * GREATEST_SHUFFLE_TESTS(seed, { + * GREATEST_RUN_TEST(some_test); + * GREATEST_RUN_TEST(some_other_test); + * GREATEST_RUN_TEST(yet_another_test); + * }); + * + * Note that the body of the second argument will be evaluated + * multiple times. */ +#define GREATEST_SHUFFLE_SUITES(SD, BODY) GREATEST_SHUFFLE(0, SD, BODY) +#define GREATEST_SHUFFLE_TESTS(SD, BODY) GREATEST_SHUFFLE(1, SD, BODY) +#define GREATEST_SHUFFLE(ID, SD, BODY) \ + do { \ + struct greatest_prng *prng = &greatest_info.prng[ID]; \ + greatest_prng_init_first_pass(ID); \ + do { \ + prng->count = 0; \ + if (prng->initialized) { greatest_prng_step(ID); } \ + BODY; \ + if (!prng->initialized) { \ + if (!greatest_prng_init_second_pass(ID, SD)) { break; } \ + } else if (prng->count_run == prng->count_ceil) { \ + break; \ + } \ + } while (!GREATEST_FAILURE_ABORT()); \ + prng->count_run = prng->random_order = prng->initialized = 0; \ + } while(0) + +/* Include several function definitions in the main test file. */ +#define GREATEST_MAIN_DEFS() \ + \ +/* Is FILTER a subset of NAME? */ \ +static int greatest_name_match(const char *name, const char *filter, \ + int res_if_none) { \ + size_t offset = 0; \ + size_t filter_len = filter ? strlen(filter) : 0; \ + if (filter_len == 0) { return res_if_none; } /* no filter */ \ + while (name[offset] != '\0') { \ + if (name[offset] == filter[0]) { \ + if (0 == strncmp(&name[offset], filter, filter_len)) { \ + return 1; \ + } \ + } \ + offset++; \ + } \ + \ + return 0; \ +} \ + \ +static void greatest_buffer_test_name(const char *name) { \ + struct greatest_run_info *g = &greatest_info; \ + size_t len = strlen(name), size = sizeof(g->name_buf); \ + memset(g->name_buf, 0x00, size); \ + (void)strncat(g->name_buf, name, size - 1); \ + if (g->name_suffix && (len + 1 < size)) { \ + g->name_buf[len] = '_'; \ + strncat(&g->name_buf[len+1], g->name_suffix, size-(len+2)); \ + } \ +} \ + \ +/* Before running a test, check the name filtering and \ + * test shuffling state, if applicable, and then call setup hooks. */ \ +int greatest_test_pre(const char *name) { \ + struct greatest_run_info *g = &greatest_info; \ + int match; \ + greatest_buffer_test_name(name); \ + match = greatest_name_match(g->name_buf, g->test_filter, 1) && \ + !greatest_name_match(g->name_buf, g->test_exclude, 0); \ + if (GREATEST_LIST_ONLY()) { /* just listing test names */ \ + if (match) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, " %s\n", g->name_buf); \ + } \ + goto clear; \ + } \ + if (match && (!GREATEST_FIRST_FAIL() || g->suite.failed == 0)) { \ + struct greatest_prng *p = &g->prng[1]; \ + if (p->random_order) { \ + p->count++; \ + if (!p->initialized || ((p->count - 1) != p->state)) { \ + goto clear; /* don't run this test yet */ \ + } \ + } \ + GREATEST_SET_TIME(g->suite.pre_test); \ + if (g->setup) { g->setup(g->setup_udata); } \ + p->count_run++; \ + return 1; /* test should be run */ \ + } else { \ + goto clear; /* skipped */ \ + } \ +clear: \ + g->name_suffix = NULL; \ + return 0; \ +} \ + \ +static void greatest_do_pass(void) { \ + struct greatest_run_info *g = &greatest_info; \ + if (GREATEST_IS_VERBOSE()) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "PASS %s: %s", \ + g->name_buf, g->msg ? g->msg : ""); \ + } else { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "."); \ + } \ + g->suite.passed++; \ +} \ + \ +static void greatest_do_fail(void) { \ + struct greatest_run_info *g = &greatest_info; \ + if (GREATEST_IS_VERBOSE()) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "FAIL %s: %s (%s:%u)", g->name_buf, \ + g->msg ? g->msg : "", g->fail_file, g->fail_line); \ + } else { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "F"); \ + g->col++; /* add linebreak if in line of '.'s */ \ + if (g->col != 0) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + g->col = 0; \ + } \ + GREATEST_FPRINTF(GREATEST_STDOUT, "FAIL %s: %s (%s:%u)\n", \ + g->name_buf, g->msg ? g->msg : "", \ + g->fail_file, g->fail_line); \ + } \ + g->suite.failed++; \ +} \ + \ +static void greatest_do_skip(void) { \ + struct greatest_run_info *g = &greatest_info; \ + if (GREATEST_IS_VERBOSE()) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "SKIP %s: %s", \ + g->name_buf, g->msg ? g->msg : ""); \ + } else { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "s"); \ + } \ + g->suite.skipped++; \ +} \ + \ +void greatest_test_post(int res) { \ + GREATEST_SET_TIME(greatest_info.suite.post_test); \ + if (greatest_info.teardown) { \ + void *udata = greatest_info.teardown_udata; \ + greatest_info.teardown(udata); \ + } \ + \ + if (res <= GREATEST_TEST_RES_FAIL) { \ + greatest_do_fail(); \ + } else if (res >= GREATEST_TEST_RES_SKIP) { \ + greatest_do_skip(); \ + } else if (res == GREATEST_TEST_RES_PASS) { \ + greatest_do_pass(); \ + } \ + greatest_info.name_suffix = NULL; \ + greatest_info.suite.tests_run++; \ + greatest_info.col++; \ + if (GREATEST_IS_VERBOSE()) { \ + GREATEST_CLOCK_DIFF(greatest_info.suite.pre_test, \ + greatest_info.suite.post_test); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + } else if (greatest_info.col % greatest_info.width == 0) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + greatest_info.col = 0; \ + } \ + fflush(GREATEST_STDOUT); \ +} \ + \ +static void report_suite(void) { \ + if (greatest_info.suite.tests_run > 0) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "\n%u test%s - %u passed, %u failed, %u skipped", \ + greatest_info.suite.tests_run, \ + greatest_info.suite.tests_run == 1 ? "" : "s", \ + greatest_info.suite.passed, \ + greatest_info.suite.failed, \ + greatest_info.suite.skipped); \ + GREATEST_CLOCK_DIFF(greatest_info.suite.pre_suite, \ + greatest_info.suite.post_suite); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + } \ +} \ + \ +static void update_counts_and_reset_suite(void) { \ + greatest_info.setup = NULL; \ + greatest_info.setup_udata = NULL; \ + greatest_info.teardown = NULL; \ + greatest_info.teardown_udata = NULL; \ + greatest_info.passed += greatest_info.suite.passed; \ + greatest_info.failed += greatest_info.suite.failed; \ + greatest_info.skipped += greatest_info.suite.skipped; \ + greatest_info.tests_run += greatest_info.suite.tests_run; \ + memset(&greatest_info.suite, 0, sizeof(greatest_info.suite)); \ + greatest_info.col = 0; \ +} \ + \ +static int greatest_suite_pre(const char *suite_name) { \ + struct greatest_prng *p = &greatest_info.prng[0]; \ + if (!greatest_name_match(suite_name, greatest_info.suite_filter, 1) \ + || (GREATEST_FAILURE_ABORT())) { return 0; } \ + if (p->random_order) { \ + p->count++; \ + if (!p->initialized || ((p->count - 1) != p->state)) { \ + return 0; /* don't run this suite yet */ \ + } \ + } \ + p->count_run++; \ + update_counts_and_reset_suite(); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n* Suite %s:\n", suite_name); \ + GREATEST_SET_TIME(greatest_info.suite.pre_suite); \ + return 1; \ +} \ + \ +static void greatest_suite_post(void) { \ + GREATEST_SET_TIME(greatest_info.suite.post_suite); \ + report_suite(); \ +} \ + \ +static void greatest_run_suite(greatest_suite_cb *suite_cb, \ + const char *suite_name) { \ + if (greatest_suite_pre(suite_name)) { \ + suite_cb(); \ + greatest_suite_post(); \ + } \ +} \ + \ +int greatest_do_assert_equal_t(const void *expd, const void *got, \ + greatest_type_info *type_info, void *udata) { \ + int eq = 0; \ + if (type_info == NULL || type_info->equal == NULL) { \ + return 0; \ + } \ + eq = type_info->equal(expd, got, udata); \ + if (!eq) { \ + if (type_info->print != NULL) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: "); \ + (void)type_info->print(expd, udata); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: "); \ + (void)type_info->print(got, udata); \ + GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + } \ + } \ + return eq; \ +} \ + \ +static void greatest_usage(const char *name) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "Usage: %s [-hlfav] [-s SUITE] [-t TEST] [-x EXCLUDE]\n" \ + " -h, --help print this Help\n" \ + " -l List suites and tests, then exit (dry run)\n" \ + " -f Stop runner after first failure\n" \ + " -a Abort on first failure (implies -f)\n" \ + " -v Verbose output\n" \ + " -s SUITE only run suites containing substring SUITE\n" \ + " -t TEST only run tests containing substring TEST\n" \ + " -x EXCLUDE exclude tests containing substring EXCLUDE\n", \ + name); \ +} \ + \ +static void greatest_parse_options(int argc, char **argv) { \ + int i = 0; \ + for (i = 1; i < argc; i++) { \ + if (argv[i][0] == '-') { \ + char f = argv[i][1]; \ + if ((f == 's' || f == 't' || f == 'x') && argc <= i + 1) { \ + greatest_usage(argv[0]); exit(EXIT_FAILURE); \ + } \ + switch (f) { \ + case 's': /* suite name filter */ \ + greatest_set_suite_filter(argv[i + 1]); i++; break; \ + case 't': /* test name filter */ \ + greatest_set_test_filter(argv[i + 1]); i++; break; \ + case 'x': /* test name exclusion */ \ + greatest_set_test_exclude(argv[i + 1]); i++; break; \ + case 'f': /* first fail flag */ \ + greatest_stop_at_first_fail(); break; \ + case 'a': /* abort() on fail flag */ \ + greatest_abort_on_fail(); break; \ + case 'l': /* list only (dry run) */ \ + greatest_list_only(); break; \ + case 'v': /* first fail flag */ \ + greatest_info.verbosity++; break; \ + case 'h': /* help */ \ + greatest_usage(argv[0]); exit(EXIT_SUCCESS); \ + case '-': \ + if (0 == strncmp("--help", argv[i], 6)) { \ + greatest_usage(argv[0]); exit(EXIT_SUCCESS); \ + } else if (0 == strncmp("--", argv[i], 2)) { \ + return; /* ignore following arguments */ \ + } /* fall through */ \ + default: \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "Unknown argument '%s'\n", argv[i]); \ + greatest_usage(argv[0]); \ + exit(EXIT_FAILURE); \ + } \ + } \ + } \ +} \ + \ +int greatest_all_passed(void) { return (greatest_info.failed == 0); } \ + \ +void greatest_set_test_filter(const char *filter) { \ + greatest_info.test_filter = filter; \ +} \ + \ +void greatest_set_test_exclude(const char *filter) { \ + greatest_info.test_exclude = filter; \ +} \ + \ +void greatest_set_suite_filter(const char *filter) { \ + greatest_info.suite_filter = filter; \ +} \ + \ +void greatest_stop_at_first_fail(void) { \ + greatest_set_flag(GREATEST_FLAG_FIRST_FAIL); \ +} \ + \ +void greatest_abort_on_fail(void) { \ + greatest_set_flag(GREATEST_FLAG_ABORT_ON_FAIL); \ +} \ + \ +void greatest_list_only(void) { \ + greatest_set_flag(GREATEST_FLAG_LIST_ONLY); \ +} \ + \ +void greatest_get_report(struct greatest_report_t *report) { \ + if (report) { \ + report->passed = greatest_info.passed; \ + report->failed = greatest_info.failed; \ + report->skipped = greatest_info.skipped; \ + report->assertions = greatest_info.assertions; \ + } \ +} \ + \ +unsigned int greatest_get_verbosity(void) { \ + return greatest_info.verbosity; \ +} \ + \ +void greatest_set_verbosity(unsigned int verbosity) { \ + greatest_info.verbosity = (unsigned char)verbosity; \ +} \ + \ +void greatest_set_flag(greatest_flag_t flag) { \ + greatest_info.flags = (unsigned char)(greatest_info.flags | flag); \ +} \ + \ +void greatest_set_test_suffix(const char *suffix) { \ + greatest_info.name_suffix = suffix; \ +} \ + \ +void GREATEST_SET_SETUP_CB(greatest_setup_cb *cb, void *udata) { \ + greatest_info.setup = cb; \ + greatest_info.setup_udata = udata; \ +} \ + \ +void GREATEST_SET_TEARDOWN_CB(greatest_teardown_cb *cb, \ + void *udata) { \ + greatest_info.teardown = cb; \ + greatest_info.teardown_udata = udata; \ +} \ + \ +static int greatest_string_equal_cb(const void *expd, const void *got, \ + void *udata) { \ + size_t *size = (size_t *)udata; \ + return (size != NULL \ + ? (0 == strncmp((const char *)expd, (const char *)got, *size)) \ + : (0 == strcmp((const char *)expd, (const char *)got))); \ +} \ + \ +static int greatest_string_printf_cb(const void *t, void *udata) { \ + (void)udata; /* note: does not check \0 termination. */ \ + return GREATEST_FPRINTF(GREATEST_STDOUT, "%s", (const char *)t); \ +} \ + \ +greatest_type_info greatest_type_info_string = { \ + greatest_string_equal_cb, \ + greatest_string_printf_cb, \ +}; \ + \ +static int greatest_memory_equal_cb(const void *expd, const void *got, \ + void *udata) { \ + greatest_memory_cmp_env *env = (greatest_memory_cmp_env *)udata; \ + return (0 == memcmp(expd, got, env->size)); \ +} \ + \ +/* Hexdump raw memory, with differences highlighted */ \ +static int greatest_memory_printf_cb(const void *t, void *udata) { \ + greatest_memory_cmp_env *env = (greatest_memory_cmp_env *)udata; \ + const unsigned char *buf = (const unsigned char *)t; \ + unsigned char diff_mark = ' '; \ + FILE *out = GREATEST_STDOUT; \ + size_t i, line_i, line_len = 0; \ + int len = 0; /* format hexdump with differences highlighted */ \ + for (i = 0; i < env->size; i+= line_len) { \ + diff_mark = ' '; \ + line_len = env->size - i; \ + if (line_len > 16) { line_len = 16; } \ + for (line_i = i; line_i < i + line_len; line_i++) { \ + if (env->exp[line_i] != env->got[line_i]) diff_mark = 'X'; \ + } \ + len += GREATEST_FPRINTF(out, "\n%04x %c ", \ + (unsigned int)i, diff_mark); \ + for (line_i = i; line_i < i + line_len; line_i++) { \ + int m = env->exp[line_i] == env->got[line_i]; /* match? */ \ + len += GREATEST_FPRINTF(out, "%02x%c", \ + buf[line_i], m ? ' ' : '<'); \ + } \ + for (line_i = 0; line_i < 16 - line_len; line_i++) { \ + len += GREATEST_FPRINTF(out, " "); \ + } \ + GREATEST_FPRINTF(out, " "); \ + for (line_i = i; line_i < i + line_len; line_i++) { \ + unsigned char c = buf[line_i]; \ + len += GREATEST_FPRINTF(out, "%c", isprint(c) ? c : '.'); \ + } \ + } \ + len += GREATEST_FPRINTF(out, "\n"); \ + return len; \ +} \ + \ +void greatest_prng_init_first_pass(int id) { \ + greatest_info.prng[id].random_order = 1; \ + greatest_info.prng[id].count_run = 0; \ +} \ + \ +int greatest_prng_init_second_pass(int id, unsigned long seed) { \ + static unsigned long primes[] = { 11, 101, 1009, 10007, \ + 100003, 1000003, 10000019, 100000007, 1000000007, \ + 1538461, 1865471, 17471, 2147483647 /* 2**32 - 1 */, }; \ + struct greatest_prng *prng = &greatest_info.prng[id]; \ + if (prng->count == 0) { return 0; } \ + prng->mod = 1; \ + prng->count_ceil = prng->count; \ + while (prng->mod < prng->count) { prng->mod <<= 1; } \ + prng->state = seed & 0x1fffffff; /* only use lower 29 bits... */ \ + prng->a = (4LU * prng->state) + 1; /* to avoid overflow */ \ + prng->c = primes[(seed * 16451) % sizeof(primes)/sizeof(primes[0])];\ + prng->initialized = 1; \ + return 1; \ +} \ + \ +/* Step the pseudorandom number generator until its state reaches \ + * another test ID between 0 and the test count. \ + * This use a linear congruential pseudorandom number generator, \ + * with the power-of-two ceiling of the test count as the modulus, the \ + * masked seed as the multiplier, and a prime as the increment. For \ + * each generated value < the test count, run the corresponding test. \ + * This will visit all IDs 0 <= X < mod once before repeating, \ + * with a starting position chosen based on the initial seed. \ + * For details, see: Knuth, The Art of Computer Programming \ + * Volume. 2, section 3.2.1. */ \ +void greatest_prng_step(int id) { \ + struct greatest_prng *p = &greatest_info.prng[id]; \ + do { \ + p->state = ((p->a * p->state) + p->c) & (p->mod - 1); \ + } while (p->state >= p->count_ceil); \ +} \ + \ +void GREATEST_INIT(void) { \ + /* Suppress unused function warning if features aren't used */ \ + (void)greatest_run_suite; \ + (void)greatest_parse_options; \ + (void)greatest_prng_step; \ + (void)greatest_prng_init_first_pass; \ + (void)greatest_prng_init_second_pass; \ + (void)greatest_set_test_suffix; \ + \ + memset(&greatest_info, 0, sizeof(greatest_info)); \ + greatest_info.width = GREATEST_DEFAULT_WIDTH; \ + GREATEST_SET_TIME(greatest_info.begin); \ +} \ + \ +/* Report passes, failures, skipped tests, the number of \ + * assertions, and the overall run time. */ \ +void GREATEST_PRINT_REPORT(void) { \ + if (!GREATEST_LIST_ONLY()) { \ + update_counts_and_reset_suite(); \ + GREATEST_SET_TIME(greatest_info.end); \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "\nTotal: %u test%s", \ + greatest_info.tests_run, \ + greatest_info.tests_run == 1 ? "" : "s"); \ + GREATEST_CLOCK_DIFF(greatest_info.begin, \ + greatest_info.end); \ + GREATEST_FPRINTF(GREATEST_STDOUT, ", %u assertion%s\n", \ + greatest_info.assertions, \ + greatest_info.assertions == 1 ? "" : "s"); \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "Pass: %u, fail: %u, skip: %u.\n", \ + greatest_info.passed, \ + greatest_info.failed, greatest_info.skipped); \ + } \ +} \ + \ +greatest_type_info greatest_type_info_memory = { \ + greatest_memory_equal_cb, \ + greatest_memory_printf_cb, \ +}; \ + \ +greatest_run_info greatest_info + +/* Handle command-line arguments, etc. */ +#define GREATEST_MAIN_BEGIN() \ + do { \ + GREATEST_INIT(); \ + greatest_parse_options(argc, argv); \ + } while (0) + +/* Report results, exit with exit status based on results. */ +#define GREATEST_MAIN_END() \ + do { \ + GREATEST_PRINT_REPORT(); \ + return (greatest_all_passed() ? EXIT_SUCCESS : EXIT_FAILURE); \ + } while (0) + +/* Make abbreviations without the GREATEST_ prefix for the + * most commonly used symbols. */ +#if GREATEST_USE_ABBREVS +#define TEST GREATEST_TEST +#define SUITE GREATEST_SUITE +#define SUITE_EXTERN GREATEST_SUITE_EXTERN +#define RUN_TEST GREATEST_RUN_TEST +#define RUN_TEST1 GREATEST_RUN_TEST1 +#define RUN_SUITE GREATEST_RUN_SUITE +#define IGNORE_TEST GREATEST_IGNORE_TEST +#define ASSERT GREATEST_ASSERT +#define ASSERTm GREATEST_ASSERTm +#define ASSERT_FALSE GREATEST_ASSERT_FALSE +#define ASSERT_EQ GREATEST_ASSERT_EQ +#define ASSERT_EQ_FMT GREATEST_ASSERT_EQ_FMT +#define ASSERT_IN_RANGE GREATEST_ASSERT_IN_RANGE +#define ASSERT_EQUAL_T GREATEST_ASSERT_EQUAL_T +#define ASSERT_STR_EQ GREATEST_ASSERT_STR_EQ +#define ASSERT_STRN_EQ GREATEST_ASSERT_STRN_EQ +#define ASSERT_MEM_EQ GREATEST_ASSERT_MEM_EQ +#define ASSERT_ENUM_EQ GREATEST_ASSERT_ENUM_EQ +#define ASSERT_FALSEm GREATEST_ASSERT_FALSEm +#define ASSERT_EQm GREATEST_ASSERT_EQm +#define ASSERT_EQ_FMTm GREATEST_ASSERT_EQ_FMTm +#define ASSERT_IN_RANGEm GREATEST_ASSERT_IN_RANGEm +#define ASSERT_EQUAL_Tm GREATEST_ASSERT_EQUAL_Tm +#define ASSERT_STR_EQm GREATEST_ASSERT_STR_EQm +#define ASSERT_STRN_EQm GREATEST_ASSERT_STRN_EQm +#define ASSERT_MEM_EQm GREATEST_ASSERT_MEM_EQm +#define ASSERT_ENUM_EQm GREATEST_ASSERT_ENUM_EQm +#define PASS GREATEST_PASS +#define FAIL GREATEST_FAIL +#define SKIP GREATEST_SKIP +#define PASSm GREATEST_PASSm +#define FAILm GREATEST_FAILm +#define SKIPm GREATEST_SKIPm +#define SET_SETUP GREATEST_SET_SETUP_CB +#define SET_TEARDOWN GREATEST_SET_TEARDOWN_CB +#define CHECK_CALL GREATEST_CHECK_CALL +#define SHUFFLE_TESTS GREATEST_SHUFFLE_TESTS +#define SHUFFLE_SUITES GREATEST_SHUFFLE_SUITES + +#ifdef GREATEST_VA_ARGS +#define RUN_TESTp GREATEST_RUN_TESTp +#endif + +#if GREATEST_USE_LONGJMP +#define ASSERT_OR_LONGJMP GREATEST_ASSERT_OR_LONGJMP +#define ASSERT_OR_LONGJMPm GREATEST_ASSERT_OR_LONGJMPm +#define FAIL_WITH_LONGJMP GREATEST_FAIL_WITH_LONGJMP +#define FAIL_WITH_LONGJMPm GREATEST_FAIL_WITH_LONGJMPm +#endif + +#endif /* USE_ABBREVS */ + +#if defined(__cplusplus) && !defined(GREATEST_NO_EXTERN_CPLUSPLUS) +} +#endif + +#endif From 8f4109d09ded6777c3a48431653837bfffeedece Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 08:13:37 -0500 Subject: [PATCH 060/129] blendmath: implement unit tests for new parallel / anti-parallel check functions Uses a simple meson build file to hackily build and run the unit test executable. Eventually, the shortcuts taken in the build file could be fixed up (i.e. with proper object file dependencies, build flags, etc.) --- meson.build | 40 ++++++++++++++++++ src/emc/tp/blendmath.c | 2 +- src/emc/tp/tp_debug.h | 7 +++- unit_tests/greatest.h | 14 ++++--- unit_tests/tp/test_blendmath.c | 77 ++++++++++++++++++++++++++++++++++ 5 files changed, 132 insertions(+), 8 deletions(-) create mode 100644 meson.build create mode 100644 unit_tests/tp/test_blendmath.c diff --git a/meson.build b/meson.build new file mode 100644 index 00000000000..7da4b87470d --- /dev/null +++ b/meson.build @@ -0,0 +1,40 @@ +project('linuxcnc-unit-test','c') + +src_root = 'src' +tp_src_dir = join_paths(src_root, 'emc/tp') + +inc_dir = include_directories([ + 'unit_tests', + src_root, + tp_src_dir, + 'src/libnml/posemath', + 'src/emc/nml_intf', + 'src/emc/motion', + 'src/rtapi', + ]) + +cc = meson.get_compiler('c') +m_dep = cc.find_library('m', required : true) + +add_global_arguments(['-DULAPI','-DUNIT_TEST'], language : 'c') + +pm = static_library('posemath', + 'src/libnml/posemath/_posemath.c', + include_directories : inc_dir) + +# TODO implement subdir builds for tp, motion, etc + +# KLUDGE just shove all the source files into the test build +test_blendmath=executable('test_blendmath', + ['unit_tests/tp/test_blendmath.c', + join_paths(tp_src_dir, 'blendmath.c'), + join_paths(tp_src_dir, 'spherical_arc.c'), + join_paths(src_root, 'emc/nml_intf/emcpose.c'), + join_paths(tp_src_dir, 'tc.c'), + ], + dependencies : [m_dep], + include_directories : inc_dir, + link_with : pm + ) + +test('blendmath test',test_blendmath) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 258d4a378e6..e8ba6f269b0 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -82,7 +82,7 @@ double fsign(double f) } /** negate a value (or not) based on a bool parameter */ -inline double negate(double f, int neg) +static inline double negate(double f, int neg) { return (neg) ? -f : f; } diff --git a/src/emc/tp/tp_debug.h b/src/emc/tp/tp_debug.h index aea44f6588c..d14c5e5e511 100644 --- a/src/emc/tp/tp_debug.h +++ b/src/emc/tp/tp_debug.h @@ -20,12 +20,15 @@ //Kludge because I didn't know any better at the time //FIXME replace these with better names? #define tp_debug_print(...) rtapi_print(__VA_ARGS__) +#elif defined(UNIT_TEST) +#include +#define tp_debug_print(...) printf(__VA_ARGS__) #else -#define tp_debug_print(...) +#define tp_debug_print(...) #endif // Verbose but effective wrappers for building faux-JSON debug output for a function -#define tp_debug_json_double(varname_) tp_debug_print("%s: %f, ", #varname_, varname_) +#define tp_debug_json_double(varname_) tp_debug_print("%s: %g, ", #varname_, varname_) #define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_) #define tp_debug_json_end() tp_debug_print("}\n") diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index 3f24502ebba..fbab23c9c95 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -761,8 +761,10 @@ static void greatest_do_fail(void) { \ struct greatest_run_info *g = &greatest_info; \ if (GREATEST_IS_VERBOSE()) { \ GREATEST_FPRINTF(GREATEST_STDOUT, \ - "FAIL %s: %s (%s:%u)", g->name_buf, \ - g->msg ? g->msg : "", g->fail_file, g->fail_line); \ + "%s:%u: error: test %s failed: %s", \ + g->fail_file, g->fail_line, \ + g->name_buf, \ + g->msg ? g->msg : ""); \ } else { \ GREATEST_FPRINTF(GREATEST_STDOUT, "F"); \ g->col++; /* add linebreak if in line of '.'s */ \ @@ -770,9 +772,11 @@ static void greatest_do_fail(void) { \ GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ g->col = 0; \ } \ - GREATEST_FPRINTF(GREATEST_STDOUT, "FAIL %s: %s (%s:%u)\n", \ - g->name_buf, g->msg ? g->msg : "", \ - g->fail_file, g->fail_line); \ + GREATEST_FPRINTF(GREATEST_STDOUT, \ + "%s:%u: error: test %s failed: %s", \ + g->fail_file, g->fail_line, \ + g->name_buf, \ + g->msg ? g->msg : ""); \ } \ g->suite.failed++; \ } \ diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c new file mode 100644 index 00000000000..8d9140a3786 --- /dev/null +++ b/unit_tests/tp/test_blendmath.c @@ -0,0 +1,77 @@ +#include "tp_debug.h" +#include "greatest.h" +#include "blendmath.h" +#include "tp_types.h" +#include "math.h" +#include "rtapi.h" + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +// KLUDGE fix link error the ugly way +void rtapi_print_msg(msg_level_t level, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + printf(fmt, args); + va_end(args); +} + +TEST pmCartCartParallel_numerical() { + + PmCartesian u0 = {1,0,0}; + PmCartesian u_close = {cos(TP_ANGLE_EPSILON), sin(TP_ANGLE_EPSILON), 0}; + + ASSERT(pmCartCartParallel(&u0, &u0, TP_ANGLE_EPSILON_SQ)); + ASSERT_FALSE(pmCartCartParallel(&u0, &u_close, 0.0)); + + // Test that the tolerance makes sense + ASSERT_FALSE(pmCartCartParallel(&u0, &u_close, 0.5*TP_ANGLE_EPSILON_SQ)); + ASSERT(pmCartCartParallel(&u0, &u_close, 1.5*TP_ANGLE_EPSILON_SQ)); + + // Try a bunch of other angles including anti-parallel + for (double k=1; k <= 7; ++k) { + PmCartesian u_far = {cos(PM_PI_4 * k), sin(PM_PI_4 * k), 0}; + ASSERT_FALSE(pmCartCartParallel(&u0, &u_far, TP_ANGLE_EPSILON_SQ)); + } + + PASS(); +} + +TEST pmCartCartAntiParallel_numerical() { + + PmCartesian u0 = {1,0,0}; + PmCartesian u_close = {-cos(TP_ANGLE_EPSILON), sin(TP_ANGLE_EPSILON), 0}; + + PmCartesian u_opposite; + pmCartScalMult(&u0, -1.0, &u_opposite); + ASSERT(pmCartCartAntiParallel(&u0, &u_opposite, TP_ANGLE_EPSILON_SQ)); + ASSERT_FALSE(pmCartCartAntiParallel(&u0, &u_close, 0.0)); + + // Test that the tolerance makes sense + ASSERT_FALSE(pmCartCartAntiParallel(&u0, &u_close, 0.5*TP_ANGLE_EPSILON_SQ)); + ASSERT(pmCartCartAntiParallel(&u0, &u_close, 1.5*TP_ANGLE_EPSILON_SQ)); + + // Try a bunch of other angles including anti-parallel + for (double k=1; k <= 7; ++k) { + PmCartesian u_far = {-cos(PM_PI_4 * k), sin(PM_PI_4 * k), 0}; + ASSERT_FALSE(pmCartCartAntiParallel(&u0, &u_far, TP_ANGLE_EPSILON_SQ)); + } + + PASS(); +} + + + SUITE(blendmath) { + RUN_TEST(pmCartCartParallel_numerical); + RUN_TEST(pmCartCartAntiParallel_numerical); + + } + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ + RUN_SUITE(blendmath); /* run a suite */ + GREATEST_MAIN_END(); /* display results */ + } From f65a6a9fa3b657ce6b88f1b4770e40425e182ca2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 15:38:58 -0500 Subject: [PATCH 061/129] tp: Move a pmCartesian function into blendmath for use outside of tp.c --- src/emc/tp/blendmath.c | 6 ++++++ src/emc/tp/blendmath.h | 2 ++ src/emc/tp/tp.c | 5 ----- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index e8ba6f269b0..857e6341d56 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1616,6 +1616,12 @@ int blendPoints3Print(BlendPoints3 const * const points) } +double pmCartAbsMax(PmCartesian const * const v) +{ + return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); +} + + PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, double v_max, double a_max) diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index d3ed555ce86..263132b25d2 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -244,6 +244,8 @@ int blendGeom3Print(BlendGeom3 const * const geom); int blendParamPrint(BlendParameters const * const param); int blendPoints3Print(BlendPoints3 const * const points); +double pmCartAbsMax(PmCartesian const * const v); + typedef struct { double v_max; double acc_ratio; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 7a751f91fcd..66a86ecf3e2 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1783,11 +1783,6 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { return TP_ERR_OK; } -STATIC double pmCartAbsMax(PmCartesian const * const v) -{ - return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); -} - /** * Check for tangency between the current segment and previous segment. From 9314f46a9854c960468b347fa227af4cf3c4e360 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 11 Jan 2019 15:39:41 -0500 Subject: [PATCH 062/129] tp: Reimplement sharp corner detection using new anti-parallel detection This approach is more stable than the old parellel check that depended on arccos. --- src/emc/tp/tp.c | 8 +- .../performance/tight-corner-tolerance.ngc | 247 +----------------- 2 files changed, 5 insertions(+), 250 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 66a86ecf3e2..229668f37fb 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1825,11 +1825,11 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, tp_debug_print("prev tangent vector: %f %f %f\n", prev_tan.x, prev_tan.y, prev_tan.z); tp_debug_print("this tangent vector: %f %f %f\n", this_tan.x, this_tan.y, this_tan.z); - double dot = -1.0; + // Assume small angle approximation here const double SHARP_CORNER_DEG = 2.0; - const double SHARP_CORNER_THRESHOLD = cos(PM_PI * (1.0 - SHARP_CORNER_DEG / 180.0)); - pmCartCartDot(&prev_tan, &this_tan, &dot); - if (dot < SHARP_CORNER_THRESHOLD) { + const double SHARP_CORNER_EPSILON = pmSq(PM_PI * ( SHARP_CORNER_DEG / 180.0)); + if (pmCartCartAntiParallel(&prev_tan, &this_tan, SHARP_CORNER_EPSILON)) + { tp_debug_print("Found sharp corner\n"); tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); return TP_ERR_FAIL; diff --git a/tests/trajectory-planner/circular-arcs/nc_files/performance/tight-corner-tolerance.ngc b/tests/trajectory-planner/circular-arcs/nc_files/performance/tight-corner-tolerance.ngc index e0530bfbf55..45ed8262cf6 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/performance/tight-corner-tolerance.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/performance/tight-corner-tolerance.ngc @@ -222,250 +222,5 @@ X0.984166956339023 Y-0.177243905538055 X0 Y0 X0.99774391607944 Y-0.0671347743458146 X0 Y0 -X0.998995989704814 Y0.0447996936786332 -X0 Y0 -X0.987573085034959 Y0.157160433043863 -X0 Y0 -X0.963285274107048 Y0.268479944667214 -X0 Y0 -X0.926115382553954 Y0.377240371907546 -X0 Y0 -X0.876229651731026 Y0.481893761556762 -X0 Y0 -X0.813986112674972 Y0.580884333040828 -X0 Y0 -X0.739940320468644 Y0.672672522216241 -X0 Y0 -X0.654848115566764 Y0.755760508057055 -X0 Y0 -X0.559665105706009 Y0.828718872389836 -X0 Y0 -X0.45554259862889 Y0.890213985980022 -X0 Y0 -X0.343819762267937 Y0.939035660171657 -X0 Y0 -X0.226011845330215 Y0.974124553519944 -X0 Y0 -X0.103794357219251 Y0.994598779111176 -X0 Y0 -X-0.0210168185064502 Y0.999779122276449 -X0 Y0 -X-0.146489319140334 Y0.989212251934741 -X0 Y0 -X-0.270602057003934 Y0.962691293585456 -X0 Y0 -X-0.391276586057924 Y0.920273129675563 -X0 Y0 -X-0.506411732323618 Y0.862291805228944 -X0 Y0 -X-0.613921035161037 Y0.789367444595228 -X0 Y0 -X-0.711772442006798 Y0.702410130052008 -X0 Y0 -X-0.798029597233241 Y0.602618255564625 -X0 Y0 -X-0.870893969217486 Y0.491470949681274 -X0 Y0 -X-0.928746971579241 Y0.370714260290035 -X0 Y0 -X-0.970191158077358 Y0.242340910265922 -X0 Y0 -X-0.994089509136673 Y0.108563565823937 -X0 Y0 -X-0.999601784682879 Y-0.0282182929817264 -X0 Y0 -X-0.986216896055544 Y-0.165457649972941 -X0 Y0 -X-0.953780252189686 Y-0.300504959248559 -X0 Y0 -X-0.902515064610367 Y-0.430658284665865 -X0 Y0 -X-0.833036654222313 Y-0.553217798630969 -X0 Y0 -X-0.746358891967849 Y-0.665543690812651 -X0 Y0 -X-0.6438920260409 Y-0.76511636945039 -X0 Y0 -X-0.527431300535608 Y-0.849597683150865 -X0 Y0 -X-0.39913595330704 Y-0.916891755213057 -X0 Y0 -X-0.261498392549635 Y-0.96520391145807 -X0 Y0 -X-0.117303589198394 Y-0.993096102077324 -X0 Y0 -X0.0304210183510809 Y-0.999537173717158 -X0 Y0 -X0.178464534736391 Y-0.983946345001252 -X0 Y0 -X0.323499674635867 Y-0.946228281394341 -X0 Y0 -X0.462156738454761 Y-0.886798257272114 -X0 Y0 -X0.59110306510858 Y-0.806596036699439 -X0 Y0 -X0.707126261029996 Y-0.707087300806441 -X0 Y0 -X0.807219259326565 Y-0.590251698322226 -X0 Y0 -X0.888665051871439 Y-0.458556894596879 -X0 Y0 -X0.949118770009714 Y-0.314918339280595 -X0 Y0 -X0.986684676197958 Y-0.162644857761105 -X0 Y0 -X0.999985578285367 Y-0.00537058854132562 -X0 Y0 -X0.988222198257805 Y0.153025771916079 -X0 Y0 -X0.951220124610112 Y0.308513005457991 -X0 Y0 -X0.889462156740413 Y0.457008831125498 -X0 Y0 -X0.804104113285568 Y0.594488498624852 -X0 Y0 -X0.69697252396093 Y0.717097832128595 -X0 Y0 -X0.57054305315998 Y0.821267693563365 -X0 Y0 -X0.427899007117393 Y0.903826553995815 -X0 Y0 -X0.272669845382318 Y0.962107663112182 -X0 Y0 -X0.108950238920721 Y0.994047204834417 -X0 Y0 -X-0.0587991246257123 Y0.998269834735704 -X0 Y0 -X-0.225867112155427 Y0.974158122507105 -X0 Y0 -X-0.387421544614688 Y0.921902677492787 -X0 Y0 -X-0.538649232638999 Y0.842530120635112 -X0 Y0 -X-0.674902865348758 Y0.737906581041283 -X0 Y0 -X-0.791850457681873 Y0.610715033930726 -X0 Y0 -X-0.885622655022248 Y0.464405547890359 -X0 Y0 -X-0.95295291688718 Y0.303118356745702 -X0 Y0 -X-0.991305476373088 Y0.13158059322227 -X0 Y0 -X-0.998986017789419 Y-0.0450215088734121 -X0 Y0 -X-0.975230245724688 Y-0.221192151361127 -X0 Y0 -X-0.920265943324592 Y-0.391293487751712 -X0 Y0 -X-0.835344737831119 Y-0.549726449225303 -X0 Y0 -X-0.722740602000703 Y-0.691119397947751 -X0 Y0 -X-0.585713107408343 Y-0.81051844877835 -X0 Y0 -X-0.428434587990561 Y-0.903572799398786 -X0 Y0 -X-0.255881639218479 Y-0.966708118674331 -X0 Y0 -X-0.0736927316949733 Y-0.997280994151264 -X0 Y0 -X0.11200488792913 Y-0.99370765574186 -X0 Y0 -X0.294794452900125 Y-0.9555606890927 -X0 Y0 -X0.468189218150229 Y-0.883628234048617 -X0 Y0 -X0.625865228749988 Y-0.779931224815191 -X0 Y0 -X0.761899254521394 Y-0.64769555036278 -X0 Y0 -X0.871003073579972 Y-0.491277564940881 -X0 Y0 -X0.94874482839498 Y-0.316043115086818 -X0 Y0 -X0.991748061640057 Y-0.128202114776591 -X0 Y0 -X0.997859300499203 Y0.0653973730912841 -X0 Y0 -X0.966275718913572 Y0.257509679503625 -X0 Y0 -X0.89762547373742 Y0.44075901453932 -X0 Y0 -X0.793994771765035 Y0.607924586120507 -X0 Y0 -X0.658897550258672 Y0.752232688908905 -X0 Y0 -X0.497185794871202 Y0.867644100641669 -X0 Y0 -X0.314900907687934 Y0.949124553647895 -X0 Y0 -X0.119069088771098 Y0.992885971347778 -X0 Y0 -X-0.0825536941398638 Y0.996586618204289 -X0 Y0 -X-0.28177901470213 Y0.959479331134077 -X0 Y0 -X-0.470315058701045 Y0.882498581052136 -X0 Y0 -X-0.640114854170972 Y0.76827922884175 -X0 Y0 -X-0.783729394157074 Y0.621102436586902 -X0 Y0 -X-0.894650254351192 Y0.446767190368034 -X0 Y0 -X-0.967625807614324 Y0.252389176548297 -X0 Y0 -X-0.998935343476041 Y0.0461321965053056 -X0 Y0 -X-0.98660636079333 Y-0.163119247307427 -X0 Y0 -X-0.930562030629931 Y-0.366134274754357 -X0 Y0 -X-0.832688301912741 Y-0.553741990332751 -X0 Y0 -X-0.696813282930446 Y-0.717252569693336 -X0 Y0 -X-0.528595275543214 Y-0.848873980443148 -X0 Y0 -X-0.335320025615998 Y-0.942104283198461 -X0 Y0 -X-0.125612207072656 Y-0.992079418914805 -X0 Y0 -X0.0909293267881452 Y-0.995857347982057 -X0 Y0 -X0.304158638571185 Y-0.952621395194609 -X0 Y0 -X0.503854311005863 Y-0.86378865081732 -X0 Y0 -X0.680214408634247 Y-0.733013204714868 -X0 Y0 -X0.82435119255831 Y-0.566078714780632 -X0 Y0 -X0.928760588047813 Y-0.370680145258795 -X0 Y0 -X0.987741222083085 Y-0.156100218442557 -X0 Y0 -X0.99773898139113 Y0.0672080725254749 -X0 Y0 -X0.957595530885998 Y0.288115947540505 -X0 Y0 -X0.868683045645683 Y0.495368313689662 -X0 Y0 -X0.734912438137292 Y0.67816200739285 -X0 Y0 -X0.562608420422811 Y0.826723511985325 -X0 Y0 -X0.360251568763221 Y0.932855190907805 -X0 Y0 -X0.138094830012609 Y0.990419011289559 -X0 Y0 -X-0.0923307525408791 Y0.995728392753383 -X0 Y0 -X-0.318799324874245 Y0.947822235685429 -X0 Y0 -X-0.529034533147166 Y0.848600296215928 -X0 Y0 -X-0.711381847120382 Y0.702805711123347 -X0 Y0 M30 + From 0bb8dd46b8f649f4077c5d1bd45b72be8f89bba8 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 23 Jan 2019 10:40:59 -0500 Subject: [PATCH 063/129] tp: Simplify kink acceleration calculation to handle negative velocities The absolute value of the velocity change is what matters for this calculation. --- src/emc/tp/blendmath.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 857e6341d56..642b98843c2 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -57,12 +57,7 @@ double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) double findKinkAccel(double kink_angle, double v_plan, double cycle_time) { double dx = v_plan / cycle_time; - if (dx > 0.0) { - return (dx * kink_angle); - } else { - rtapi_print_msg(RTAPI_MSG_ERR, "dx < 0 in KinkAccel\n"); - return 0; - } + return fabs(dx * kink_angle); } From e69556d45bdcb623b7a82952e58eef46dc196a86 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 25 Jan 2019 00:55:13 -0500 Subject: [PATCH 064/129] Apply minimum displacement checks consistently between canon and TP. 1) Change the minimum displacement calculation in PmCartLineInit to use infinity-norm (i.e. max component) rather than the vector magnitude. This is consistent with how Canon checks for minimum displacement 2) Apply the minimum displacement checks in canon with a threshold based on user units, so that the cutoffs are the same for both the TP and canon. To be clear, this is an ugly way to fix the issue. The TP has to repeat the work that canon does, and do so the exact same way to avoid weird edge cases. The long-term fix needs to be more robust, perhaps with an API change to enforce canon's choice of linear / angular or pure-angular move. --- src/emc/task/emccanon.cc | 75 ++++++++++++++++++++++++--------- src/libnml/posemath/_posemath.c | 21 ++++++--- src/libnml/posemath/posemath.h | 1 + 3 files changed, 72 insertions(+), 25 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index e8eebab8473..e3d66928b59 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -60,7 +60,6 @@ static int debug_velacc = 0; static double css_maximum; // both always positive static int spindle_dir = 0; -static const double tiny = 1e-7; static double xy_rotation = 0.; static int rotary_unlock_for_traverse = -1; @@ -643,6 +642,60 @@ void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference) // nothing need be done here } +/** + * Get the shortest linear axis displacement that the TP can handle as a discrete move. + * + * If this looks dirty, it's because it is. Canon runs in its own units, but + * the TP uses user units. Therefore, the minimum displacement has to be + * computed the same way, with the same threshold, or short moves do strange + * things (accel violations or infinite pauses). + * + * @todo revisit this when the TP is overhauled to use a consistent set of internal units. + */ +static double getMinLinearDisplacement() +{ + return FROM_EXT_LEN(CART_FUZZ); +} + +/** + * Equivalent of getMinLinearDisplacement for rotary axes. + */ +static double getMinAngularDisplacement() +{ + return FROM_EXT_ANG(CART_FUZZ); +} + +/** + * Apply the minimum displacement check to each axis delta. + * + * Checks that the axis is valid / active, and looks up the appropriate minimum + * displacement for the axis type and user units. + */ +static void applyMinDisplacement(double &dx, + double &dy, + double &dz, + double &da, + double &db, + double &dc, + double &du, + double &dv, + double &dw + ) +{ + const double tiny_linear = getMinLinearDisplacement(); + const double tiny_angular = getMinAngularDisplacement(); + if(!axis_valid(0) || dx < tiny_linear) dx = 0.0; + if(!axis_valid(1) || dy < tiny_linear) dy = 0.0; + if(!axis_valid(2) || dz < tiny_linear) dz = 0.0; + if(!axis_valid(3) || da < tiny_angular) da = 0.0; + if(!axis_valid(4) || db < tiny_linear) db = 0.0; + if(!axis_valid(5) || dc < tiny_linear) dc = 0.0; + if(!axis_valid(6) || du < tiny_linear) du = 0.0; + if(!axis_valid(7) || dv < tiny_linear) dv = 0.0; + if(!axis_valid(8) || dw < tiny_linear) dw = 0.0; +} + + /** * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. @@ -670,15 +723,7 @@ static AccelData getStraightAcceleration(double x, double y, double z, dv = fabs(v - canonEndPoint.v); dw = fabs(w - canonEndPoint.w); - if(!axis_valid(0) || dx < tiny) dx = 0.0; - if(!axis_valid(1) || dy < tiny) dy = 0.0; - if(!axis_valid(2) || dz < tiny) dz = 0.0; - if(!axis_valid(3) || da < tiny) da = 0.0; - if(!axis_valid(4) || db < tiny) db = 0.0; - if(!axis_valid(5) || dc < tiny) dc = 0.0; - if(!axis_valid(6) || du < tiny) du = 0.0; - if(!axis_valid(7) || dv < tiny) dv = 0.0; - if(!axis_valid(8) || dw < tiny) dw = 0.0; + applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); if(debug_velacc) printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", @@ -808,15 +853,7 @@ static VelData getStraightVelocity(double x, double y, double z, dv = fabs(v - canonEndPoint.v); dw = fabs(w - canonEndPoint.w); - if(!axis_valid(0) || dx < tiny) dx = 0.0; - if(!axis_valid(1) || dy < tiny) dy = 0.0; - if(!axis_valid(2) || dz < tiny) dz = 0.0; - if(!axis_valid(3) || da < tiny) da = 0.0; - if(!axis_valid(4) || db < tiny) db = 0.0; - if(!axis_valid(5) || dc < tiny) dc = 0.0; - if(!axis_valid(6) || du < tiny) du = 0.0; - if(!axis_valid(7) || dv < tiny) dv = 0.0; - if(!axis_valid(8) || dw < tiny) dw = 0.0; + applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); if(debug_velacc) printf("getStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n", diff --git a/src/libnml/posemath/_posemath.c b/src/libnml/posemath/_posemath.c index 9c238dd7548..2d6ba394263 100644 --- a/src/libnml/posemath/_posemath.c +++ b/src/libnml/posemath/_posemath.c @@ -825,6 +825,12 @@ int pmCartCartCross(PmCartesian const * const v1, PmCartesian const * const v2, return pmErrno = 0; } +int pmCartInfNorm(PmCartesian const * v, double * out) +{ + *out = fmax(fabs(v->x),fmax(fabs(v->y),fabs(v->z))); + return pmErrno = 0; +} + int pmCartMag(PmCartesian const * const v, double *d) { *d = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); @@ -1646,7 +1652,6 @@ int pmLinePoint(PmLine const * const line, double len, PmPose * const point) int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end) { int r1 = 0, r2 = 0; - double tmag = 0.0; if (0 == line) { return (pmErrno = PM_ERR); @@ -1659,16 +1664,20 @@ int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmC return r1; } - pmCartMag(&line->uVec, &tmag); - if (IS_FUZZ(tmag, CART_FUZZ)) { + pmCartMag(&line->uVec, &line->tmag); + // NOTE: use the same criteria for "zero" length vectors as used by canon + double max_xyz=0; + pmCartInfNorm(&line->uVec, &max_xyz); + + if (IS_FUZZ(max_xyz, CART_FUZZ)) { line->uVec.x = 1.0; line->uVec.y = 0.0; line->uVec.z = 0.0; + line->tmag_zero = 1; } else { - r2 = pmCartUnit(&line->uVec, &line->uVec); + r2 = pmCartUnitEq(&line->uVec); + line->tmag_zero = 0; } - line->tmag = tmag; - line->tmag_zero = (line->tmag <= CART_FUZZ); /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */ return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index e265585f7f4..f64448f416e 100644 --- a/src/libnml/posemath/posemath.h +++ b/src/libnml/posemath/posemath.h @@ -852,6 +852,7 @@ extern "C" { extern int pmCartCartCross(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); extern int pmCartCartMult(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); extern int pmCartCartDiv(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); + extern int pmCartInfNorm(PmCartesian const * v, double * out); extern int pmCartMag(PmCartesian const * const, double * const); extern int pmCartMagSq(PmCartesian const * const, double * const); extern int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, double *d); From 58dbece7eebc7f467c534b4afa234982f1acd945 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 25 Jan 2019 00:54:24 -0500 Subject: [PATCH 065/129] tp: Overhaul debug output with JSON5-formatted log statements. Provides a simple API to generate JSON5-formatted logs from within TP code. This approach is crude in that the JSON string is built up in place with a sequence of statements, and must verify that the log output is what they expect. A typical TP log statement now looks like this (all on one line): { log_entry: "SomeEntryType", log_level:"MyLogLevel", field1: 123.456, ...} This is an ugly solution that abuses the logging system to dump internal state for debugging. Efficiently packing / exporting internal state would be much more efficient with flatbuffer or protobuf (which both have C implementations). Luckily, the debug code is ignored entirely in release builds due to the prepreocessor flags. There is also an example of a python script that shows how to use a JSON5 library to extract TP state from the debug log and plot it. --- meson.build | 2 + src/Makefile | 1 + src/emc/motion/command.c | 6 +- src/emc/motion/motion.c | 11 +- src/emc/motion/usrmotintf.cc | 1 - src/emc/nml_intf/emcpose.c | 19 ++ src/emc/nml_intf/emcpose.h | 2 + src/emc/tp/blendmath.c | 30 +- src/emc/tp/joint_util.c | 64 ++++ src/emc/tp/joint_util.h | 19 ++ src/emc/tp/rtapi_json5.h | 143 +++++++++ src/emc/tp/spherical_arc.c | 4 +- src/emc/tp/tc.c | 56 +++- src/emc/tp/tc.h | 4 + src/emc/tp/tp.c | 285 +++++++++--------- src/emc/tp/tp.h | 1 - src/emc/tp/tp_debug.h | 31 +- src/emc/tp/tp_types.h | 9 +- src/rtapi/uspace_common.h | 1 + .../circular-arcs/README.md | 21 ++ .../circular-arcs/parse_tp_logs.py | 67 ++++ .../circular-arcs/tp_testing_venv.cfg | 4 + unit_tests/tp/test_blendmath.c | 41 ++- 23 files changed, 635 insertions(+), 187 deletions(-) create mode 100644 src/emc/tp/joint_util.c create mode 100644 src/emc/tp/joint_util.h create mode 100644 src/emc/tp/rtapi_json5.h create mode 100644 tests/trajectory-planner/circular-arcs/README.md create mode 100755 tests/trajectory-planner/circular-arcs/parse_tp_logs.py create mode 100644 tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg diff --git a/meson.build b/meson.build index 7da4b87470d..60de2e574cd 100644 --- a/meson.build +++ b/meson.build @@ -6,6 +6,7 @@ tp_src_dir = join_paths(src_root, 'emc/tp') inc_dir = include_directories([ 'unit_tests', src_root, + join_paths(src_root, 'emc/kinematics'), tp_src_dir, 'src/libnml/posemath', 'src/emc/nml_intf', @@ -29,6 +30,7 @@ test_blendmath=executable('test_blendmath', ['unit_tests/tp/test_blendmath.c', join_paths(tp_src_dir, 'blendmath.c'), join_paths(tp_src_dir, 'spherical_arc.c'), + join_paths(tp_src_dir, 'joint_util.c'), join_paths(src_root, 'emc/nml_intf/emcpose.c'), join_paths(tp_src_dir, 'tc.c'), ], diff --git a/src/Makefile b/src/Makefile index 724c2799922..fc4bb299931 100644 --- a/src/Makefile +++ b/src/Makefile @@ -943,6 +943,7 @@ motmod-objs += emc/tp/tp.o motmod-objs += emc/tp/spherical_arc.o motmod-objs += emc/tp/blendmath.o motmod-objs += emc/tp/math_util.o +motmod-objs += emc/tp/joint_util.o motmod-objs += emc/motion/motion.o motmod-objs += emc/motion/command.o motmod-objs += emc/motion/control.o diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index aaa44c14a5e..b384a23cc93 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1026,9 +1026,9 @@ check_stuff ( "before command_handler()" ); /* set the max acceleration */ /* can do it at any time */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_ACCEL"); - emcmotStatus->acc = emcmotCommand->acc; - tpSetAmax(&emcmotDebug->tp, emcmotStatus->acc); - break; + emcmotStatus->acc = emcmotCommand->acc; + // WARNING has no effect on coordinated motion TP, only single-axis planner + break; case EMCMOT_PAUSE: /* pause the motion */ diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index dc449778800..6de46c3aab1 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -1042,16 +1042,15 @@ static int init_comm_buffers(void) /* init motion emcmotDebug->tp */ if (-1 == tpCreate(&emcmotDebug->tp, DEFAULT_TC_QUEUE_SIZE, - emcmotDebug->queueTcSpace)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "MOTION: failed to create motion emcmotDebug->tp\n"); - return -1; + emcmotDebug->queueTcSpace)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "MOTION: failed to create motion emcmotDebug->tp\n"); + return -1; } -// tpInit(&emcmotDebug->tp); // tpInit called from tpCreate + tpSetCycleTime(&emcmotDebug->tp, emcmotConfig->trajCycleTime); tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd); tpSetVmax(&emcmotDebug->tp, emcmotStatus->vel, emcmotStatus->vel); - tpSetAmax(&emcmotDebug->tp, emcmotStatus->acc); emcmotStatus->tail = 0; diff --git a/src/emc/motion/usrmotintf.cc b/src/emc/motion/usrmotintf.cc index ec4ad552cbc..2362b49b001 100644 --- a/src/emc/motion/usrmotintf.cc +++ b/src/emc/motion/usrmotintf.cc @@ -249,7 +249,6 @@ void printTPstruct(TP_STRUCT * tp) printf("queueSize=%d\n", tp->queueSize); printf("cycleTime=%f\n", tp->cycleTime); printf("vMax=%f\n", tp->vMax); - printf("aMax=%f\n", tp->aMax); printf("vLimit=%f\n", tp->vLimit); printf("wMax=%f\n", tp->wMax); printf("wDotMax=%f\n", tp->wDotMax); diff --git a/src/emc/nml_intf/emcpose.c b/src/emc/nml_intf/emcpose.c index 7e389729640..d0afd3dd81c 100644 --- a/src/emc/nml_intf/emcpose.c +++ b/src/emc/nml_intf/emcpose.c @@ -78,6 +78,25 @@ int emcPoseSub(EmcPose const * const p1, EmcPose const * const p2, EmcPose * con } +int emcPoseMultScalar(EmcPose * const p1, double m) +{ +#ifdef EMCPOSE_PEDANTIC + if (!p1 || !p2) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pmCartScalMultEq(&p1->tran, m); + p1->a *= m; + p1->b *= m; + p1->c *= m; + p1->u *= m; + p1->v *= m; + p1->w *= m; + return EMCPOSE_ERR_OK; +} + + int emcPoseSelfAdd(EmcPose * const self, EmcPose const * const p2) { return emcPoseAdd(self, p2, self); diff --git a/src/emc/nml_intf/emcpose.h b/src/emc/nml_intf/emcpose.h index dc6b7e80e38..5e6ce6581fd 100644 --- a/src/emc/nml_intf/emcpose.h +++ b/src/emc/nml_intf/emcpose.h @@ -29,6 +29,8 @@ void emcPoseZero(EmcPose * const pos); int emcPoseAdd(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out); int emcPoseSub(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out); +int emcPoseMultScalar(EmcPose * const p1, double m); + int emcPoseToPmCartesian(EmcPose const * const pose, PmCartesian * const xyz, PmCartesian * const abc, PmCartesian * const uvw); int pmCartesianToEmcPose(PmCartesian const * const xyz, diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 642b98843c2..63cd058c9aa 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -313,9 +313,11 @@ int pmCartCartParallel(PmCartesian const * const u1, pmCartMagSq(&u_diff, &d_diff); } - tp_debug_json_start(pmCartCartParallel); - tp_debug_json_double(d_diff); - tp_debug_json_end(); +#ifdef TP_DEBUG + print_json5_log_start(pmCartCartParallel, Check); + print_json5_double(d_diff); + print_json5_end_(); +#endif return d_diff < tol; } @@ -337,9 +339,9 @@ int pmCartCartAntiParallel(PmCartesian const * const u1, pmCartMagSq(&u_sum, &d_sum); } - tp_debug_json_start(pmCartCartAntiParallel); + tp_debug_json_log_start(pmCartCartAntiParallel, Check); tp_debug_json_double(d_sum); - tp_debug_json_end(); + tp_debug_json_log_end(); return d_sum < tol; } @@ -1638,14 +1640,14 @@ PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); } - tp_debug_json_start(pmCircleActualMaxVel); + tp_debug_json_log_start(pmCircleActualMaxVel, Check); tp_debug_json_double(eff_radius); tp_debug_json_double(v_max); tp_debug_json_double(v_max_cutoff); tp_debug_json_double(a_n_max_cutoff); tp_debug_json_double(a_n_vmax); tp_debug_json_double(acc_ratio_tan); - tp_debug_json_end(); + tp_debug_json_log_end(); PmCircleLimits limits = { v_max_actual, @@ -1712,14 +1714,16 @@ static int pmCircleAngleFromParam(PmCircle const * const circle, return TP_ERR_OK; } - static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit) { - tp_debug_print("Spiral fit: b0 = %.12f, b1 = %.12f, length = %.12f, spiral_in = %d\n", - fit->b0, - fit->b1, - fit->total_planar_length, - fit->spiral_in); +#ifdef TP_DEBUG + print_json5_log_start(SpiralFit, Command); + print_json5_double_("b0", fit->b0); + print_json5_double_("b1", fit->b1); + print_json5_double_("total_planar_length", fit->total_planar_length); + print_json5_bool_("spiral_in", fit->spiral_in); + print_json5_log_end(); +#endif } /** diff --git a/src/emc/tp/joint_util.c b/src/emc/tp/joint_util.c new file mode 100644 index 00000000000..df306d3747a --- /dev/null +++ b/src/emc/tp/joint_util.c @@ -0,0 +1,64 @@ +#include "joint_util.h" +#include "motion_debug.h" +#include "rtapi_math.h" + +extern emcmot_debug_t *emcmotDebug; + +PmCartesian getXYZAccelBounds() { + PmCartesian acc_bound = { + emcmotDebug->joints[0].acc_limit, + emcmotDebug->joints[1].acc_limit, + emcmotDebug->joints[2].acc_limit, + }; + return acc_bound; +} + +PmCartesian getXYZVelBounds() { + PmCartesian vel_bound = { + emcmotDebug->joints[0].vel_limit, + emcmotDebug->joints[1].vel_limit, + emcmotDebug->joints[2].vel_limit, + }; + return vel_bound; +} + +/** + * Checks for any acceleration violations based on axis limits. + * + * @return 0 if all acceleration bounds are respected, or a bit-set of failed axes (in XYZABCUVW bit order). + */ +unsigned findAccelViolations(EmcPose axis_accel) +{ + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double sf = 1.0001; + // Bit-mask each failure so we can report all failed axes + int fail_bits = (fabs(axis_accel.tran.x) > (emcmotDebug->joints[0].acc_limit * sf + TP_ACCEL_EPSILON)) << 0 + | (fabs(axis_accel.tran.y) > (emcmotDebug->joints[1].acc_limit * sf + TP_ACCEL_EPSILON)) << 1 + | (fabs(axis_accel.tran.z) > (emcmotDebug->joints[2].acc_limit * sf + TP_ACCEL_EPSILON)) << 2 + | (fabs(axis_accel.a) > (emcmotDebug->joints[3].acc_limit * sf + TP_ACCEL_EPSILON)) << 3 + | (fabs(axis_accel.b) > (emcmotDebug->joints[4].acc_limit * sf + TP_ACCEL_EPSILON)) << 4 + | (fabs(axis_accel.c) > (emcmotDebug->joints[5].acc_limit * sf + TP_ACCEL_EPSILON)) << 5 + | (fabs(axis_accel.u) > (emcmotDebug->joints[6].acc_limit * sf + TP_ACCEL_EPSILON)) << 6 + | (fabs(axis_accel.v) > (emcmotDebug->joints[7].acc_limit * sf + TP_ACCEL_EPSILON)) << 7 + | (fabs(axis_accel.w) > (emcmotDebug->joints[8].acc_limit * sf + TP_ACCEL_EPSILON)) << 8; + return fail_bits; +} + +double findMinNonZero(const PmCartesian * const bounds) { + //Start with max accel value + double act_limit = fmax(fmax(bounds->x, bounds->y), bounds->z); + + // Compare only with active axes + if (bounds->x > 0) { + act_limit = fmin(act_limit, bounds->x); + } + if (bounds->y > 0) { + act_limit = fmin(act_limit, bounds->y); + } + if (bounds->z > 0) { + act_limit = fmin(act_limit, bounds->z); + } + return act_limit; +} diff --git a/src/emc/tp/joint_util.h b/src/emc/tp/joint_util.h new file mode 100644 index 00000000000..592bc3c6e2d --- /dev/null +++ b/src/emc/tp/joint_util.h @@ -0,0 +1,19 @@ +#ifndef JOINT_UTIL_H +#define JOINT_UTIL_H + +#include +#include + +PmCartesian getXYZAccelBounds(); + +PmCartesian getXYZVelBounds(); + +unsigned findAccelViolations(EmcPose axis_accel); + +/** + * Finds the smallest non-zero component in a non-negative "bounds" vector. + * Used to identify the "slowest" axis, but ignore axes + */ +double findMinNonZero(PmCartesian const * const bounds); + +#endif // JOINT_UTIL_H diff --git a/src/emc/tp/rtapi_json5.h b/src/emc/tp/rtapi_json5.h new file mode 100644 index 00000000000..74533cb114f --- /dev/null +++ b/src/emc/tp/rtapi_json5.h @@ -0,0 +1,143 @@ +/** + * @file rtapi_json5.h + * + * @author Robert W. Ellenberg + * @copyright Copyright (c) 2019 All rights reserved. This project is released under the GPL v2 license. + */ + +#ifndef RTAPI_JSON5_H +#define RTAPI_JSON5_H + +#include "rtapi.h" /* printing functions */ +#include "posemath.h" +#include "emcpos.h" +#include "blendmath.h" + +// Macro wrappers to stringify the variable name +// NOTE: callers can directly call the underlying print function if a custom field name is needed +#define print_json5_double(varname_) print_json5_double_(#varname_, (double)varname_) +#define print_json5_string(varname_) print_json5_string_(#varname_, varname_) +#define print_json5_long(varname_) print_json5_long_(#varname_, (long)varname_) +#define print_json5_unsigned(varname_) print_json5_unsigned_(#varname_, (unsigned)varname_) +#define print_json5_PmCartesian(varname_) print_json5_PmCartesian_(#varname_, varname_) +#define print_json5_EmcPose(varname_) print_json5_EmcPose_(#varname_, varname_) +#define print_json5_PmCartLine(varname_) print_json5_PmCartLine_( #varname_, varname_) +#define print_json5_PmCircle(varname_) print_json5_PmCircle_(#varname_, varname_) + +// Hacky way to do realtime-logging of +// KLUDGE shove these in the header to avoid linking issues + +static inline void print_json5_start_() +{ + rtapi_print("{"); +} + +static inline void print_json5_end_() +{ + rtapi_print("}\n"); +} + +static inline void print_json5_object_start_(const char *fname) +{ + rtapi_print("%s: {", fname ?: "NULL"); +} + +static inline void print_json5_object_end_() +{ + rtapi_print("}, "); +} + +static inline void print_json5_double_(const char *varname, double value) +{ + rtapi_print("%s: %0.17g, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_bool_(const char *varname, double value) +{ + rtapi_print("%s: %s, ", varname ?: "NO_NAME", value ? "true" : "false"); +} + +static inline void print_json5_int_(const char *varname, int value) +{ + rtapi_print("%s: %d, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_long_(const char *varname, long value) +{ + rtapi_print("%s: %ld, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_ll_(const char *varname, long long value) +{ + rtapi_print("%s: %lld, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_unsigned_(const char *varname, unsigned value) +{ + rtapi_print("%s: %u, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_string_(const char *field_name, const char *value) +{ + // TODO handle nulls gracefully with proper JSON null + rtapi_print("%s: \"%s\", ", field_name ?: "NULL", value ?: "NULL"); +} + +static inline void print_json5_PmCartesian_(const char *varname, PmCartesian value) +{ + + rtapi_print("%s: [%0.17g, %0.17g, %0.17g], ", varname ?: "NO_NAME", value.x, value.y, value.z); +} + +static inline void print_json5_EmcPose_(const char *varname, EmcPose value) +{ + rtapi_print("%s: [%0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g], ", + varname, + value.tran.x, + value.tran.y, + value.tran.z, + value.a, + value.b, + value.c, + value.u, + value.v, + value.w); +} + +static inline void print_json5_PmCartLine_(const char *name, PmCartLine value) +{ + print_json5_object_start_(name); + // Need manual field names here + print_json5_PmCartesian_("start", value.start); + print_json5_PmCartesian_("end", value.start); + print_json5_PmCartesian_("uVec", value.uVec); + print_json5_double_("tmag", value.tmag); + print_json5_double_("tmag_zero", value.tmag_zero); + print_json5_object_end_(); +} + +static inline void print_json5_PmCircle_(const char *name, PmCircle circle) +{ + print_json5_object_start_(name); + print_json5_PmCartesian_("center", circle.center); + print_json5_PmCartesian_("normal", circle.normal); + print_json5_PmCartesian_("rTan", circle.rTan); + print_json5_PmCartesian_("rPerp", circle.rPerp); + print_json5_PmCartesian_("rHelix", circle.rHelix); + print_json5_double_("radius", circle.radius); + print_json5_double_("angle", circle.angle); + print_json5_double_("spiral", circle.spiral); + print_json5_object_end_(); +} + +static inline void print_json5_SpiralArcLengthFit_(const char *name, SpiralArcLengthFit fit) +{ + print_json5_object_start_(name); + print_json5_double_("b0", fit.b0); + print_json5_double_("b1", fit.b1); + print_json5_double_("total_planar_length", fit.total_planar_length); + print_json5_long_("spiral_in", fit.spiral_in); + print_json5_object_end_(); +} + +#endif // RTAPI_JSON5_H diff --git a/src/emc/tp/spherical_arc.c b/src/emc/tp/spherical_arc.c index 18a2c099abe..75012d5ec93 100644 --- a/src/emc/tp/spherical_arc.c +++ b/src/emc/tp/spherical_arc.c @@ -23,9 +23,9 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, PmCartesian const * const center) { #ifdef ARC_PEDANTIC - if (!P0 || !P1 || !center) { + if (!start || !end || !center) { return TP_ERR_MISSING_INPUT; - + } if (!arc) { return TP_ERR_MISSING_OUTPUT; } diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 99e5612f745..9c4fff6642d 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -668,6 +668,14 @@ int pmLine9Init(PmLine9 * const line9, int abc_fail = pmCartLineInit(&line9->abc, &start_abc, &end_abc); int uvw_fail = pmCartLineInit(&line9->uvw, &start_uvw, &end_uvw); +#ifdef TP_DEBUG + print_json5_log_start(pmLine9Init, Command); + print_json5_PmCartLine_("xyz", line9->xyz); + print_json5_PmCartLine_("abc", line9->abc); + print_json5_PmCartLine_("uvw", line9->abc); + print_json5_end_(); +#endif + if (xyz_fail || abc_fail || uvw_fail) { rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Line9, err codes %d, %d, %d\n", xyz_fail,abc_fail,uvw_fail); @@ -747,13 +755,24 @@ int tcFinalizeLength(TC_STRUCT * const tc) tp_debug_print("tc %d already finalized\n", tc->id); return TP_ERR_NO_ACTION; } - - tp_debug_print("Finalizing motion id %d, type %d\n", tc->id, tc->motion_type); +#ifdef TP_DEBUG + double maxvel_old = tc->maxvel; +#endif tcClampVelocityByLength(tc); tcUpdateCircleAccRatio(tc); +#ifdef TP_DEBUG + print_json5_log_start(tpFinalizeLength, Lookahead); + print_json5_int_("id", tc->id); + print_json5_string_("motion_type", tcMotionTypeAsString((tc_motion_type_t)tc->motion_type)); + print_json5_double(maxvel_old); + print_json5_double_("maxvel", tc->maxvel); + print_json5_double_("acc_ratio_tan", tc->acc_ratio_tan); + print_json5_log_end(); +#endif + tc->finalized = 1; return TP_ERR_OK; } @@ -769,7 +788,6 @@ int tcClampVelocityByLength(TC_STRUCT * const tc) //Reduce max velocity to match sample rate //Assume that cycle time is valid here double sample_maxvel = tc->target / tc->cycle_time; - tp_debug_print("sample_maxvel = %f\n",sample_maxvel); tc->maxvel = fmin(tc->maxvel, sample_maxvel); return TP_ERR_OK; } @@ -883,4 +901,36 @@ int tcClearFlags(TC_STRUCT * const tc) return TP_ERR_OK; } +// Helper functions to convert enums to pretty-print for debug output + +const char *tcTermCondAsString(tc_term_cond_t c) +{ + switch (c) + { + case TC_TERM_COND_STOP: + return "EXACT_STOP"; + case TC_TERM_COND_EXACT: + return "EXACT_PATH"; + case TC_TERM_COND_PARABOLIC: + return "PARABOLIC"; + case TC_TERM_COND_TANGENT: + return "TANGENT"; + } + return "NONE"; +} +const char *tcMotionTypeAsString(tc_motion_type_t c) +{ + switch (c) + { + case TC_LINEAR: + return "Linear"; + case TC_CIRCULAR: + return "Circular"; + case TC_RIGIDTAP: + return "RigidTap"; + case TC_SPHERICAL: + return "SphericalArc"; + } + return "NONE"; +} diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index d9b0c23e73d..2dbe9729460 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -111,4 +111,8 @@ int tcPureRotaryCheck(TC_STRUCT const * const tc); int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ); int tcClearFlags(TC_STRUCT * const tc); + +const char *tcTermCondAsString(tc_term_cond_t c); +const char *tcMotionTypeAsString(tc_motion_type_t c); + #endif /* TC_H */ diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 229668f37fb..aa8a823c777 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -22,6 +22,9 @@ #include "spherical_arc.h" #include "blendmath.h" #include "math_util.h" +#include "joint_util.h" +#include "string.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" @@ -133,51 +136,6 @@ STATIC double tpGetTangentKinkRatio(void) { } -STATIC int tpGetMachineAccelBounds(PmCartesian * const acc_bound) { - if (!acc_bound) { - return TP_ERR_FAIL; - } - - acc_bound->x = emcmotDebug->joints[0].acc_limit; - acc_bound->y = emcmotDebug->joints[1].acc_limit; - acc_bound->z = emcmotDebug->joints[2].acc_limit; - return TP_ERR_OK; -} - - -STATIC int tpGetMachineVelBounds(PmCartesian * const vel_bound) { - if (!vel_bound) { - return TP_ERR_FAIL; - } - - vel_bound->x = emcmotDebug->joints[0].vel_limit; - vel_bound->y = emcmotDebug->joints[1].vel_limit; - vel_bound->z = emcmotDebug->joints[2].vel_limit; - return TP_ERR_OK; -} - -STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * const bounds) { - if (!act_limit) { - return TP_ERR_FAIL; - } - //Start with max accel value - *act_limit = fmax(fmax(bounds->x,bounds->y),bounds->z); - - // Compare only with active axes - if (bounds->x > 0) { - *act_limit = fmin(*act_limit, bounds->x); - } - if (bounds->y > 0) { - *act_limit = fmin(*act_limit, bounds->y); - } - if (bounds->z > 0) { - *act_limit = fmin(*act_limit, bounds->z); - } - tp_debug_print(" arc blending a_max=%f\n", *act_limit); - return TP_ERR_OK; -} - - /** * Get a segment's feed scale based on the current planner state and emcmotStatus. * @note depends on emcmotStatus for system information. @@ -299,7 +257,6 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, v_target_next = tpGetRealTargetVel(tp, nexttc); } - tc_debug_print("v_target_next = %f\n",v_target_next); // Limit final velocity to minimum of this and next target velocities double v_target = fmin(v_target_this, v_target_next); double finalvel = fmin(tc->finalvel, v_target); @@ -402,6 +359,7 @@ int tpClear(TP_STRUCT * const tp) tcqInit(&tp->queue); tp->queueSize = 0; tp->goalPos = tp->currentPos; + ZERO_EMC_POSE(tp->currentVel); tp->nextId = 0; tp->execId = 0; tp->motionType = 0; @@ -435,12 +393,6 @@ int tpInit(TP_STRUCT * const tp) //Velocity limits tp->vLimit = 0.0; tp->ini_maxvel = 0.0; - //Accelerations - tp->aLimit = 0.0; - PmCartesian acc_bound; - //FIXME this acceleration bound isn't valid (nor is it used) - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineActiveLimit(&tp->aMax, &acc_bound); //Angular limits tp->wMax = 0.0; tp->wDotMax = 0.0; @@ -450,10 +402,10 @@ int tpInit(TP_STRUCT * const tp) tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; ZERO_EMC_POSE(tp->currentPos); + ZERO_EMC_POSE(tp->currentVel); - PmCartesian vel_bound; - tpGetMachineVelBounds(&vel_bound); - tpGetMachineActiveLimit(&tp->vMax, &vel_bound); + PmCartesian vel_bound = getXYZVelBounds(); + tp->vMax = findMinNonZero(&vel_bound); return tpClear(tp); } @@ -510,18 +462,6 @@ int tpSetVlimit(TP_STRUCT * const tp, double vLimit) return TP_ERR_OK; } -/** Sets the max acceleration for the trajectory planner. */ -int tpSetAmax(TP_STRUCT * const tp, double aMax) -{ - if (0 == tp || aMax <= 0.0) { - return TP_ERR_FAIL; - } - - tp->aMax = aMax; - - return TP_ERR_OK; -} - /** * Sets the id that will be used for the next appended motions. * nextId is incremented so that the next time a motion is appended its id will @@ -701,9 +641,9 @@ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_S double acc_scaled = tcGetTangentialMaxAccel(tc); double triangle_vel = findVPeak(acc_scaled, tc->target); double max_vel = tpGetMaxTargetVel(tp, tc); - tp_debug_json_start(tpCalculateOptimizationInitialVel); + tp_debug_json_log_start(tpCalculateOptimizationInitialVel, Command); tp_debug_json_double(triangle_vel); - tp_debug_json_end(); + tp_debug_json_log_end(); return fmin(triangle_vel, max_vel); } @@ -863,12 +803,9 @@ STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const pre { tp_debug_print("-- Starting LineArc blend arc --\n"); - PmCartesian acc_bound, vel_bound; + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - //Populate blend geometry struct BlendGeom3 geom; BlendParameters param; @@ -1019,11 +956,8 @@ STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pre { tp_debug_print("-- Starting ArcLine blend arc --\n"); - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); //Populate blend geometry struct BlendGeom3 geom; @@ -1167,11 +1101,8 @@ STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev return TP_ERR_FAIL; } - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); //Populate blend geometry struct BlendGeom3 geom; @@ -1323,12 +1254,10 @@ STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const pr { tp_debug_print("-- Starting LineLine blend arc --\n"); - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - + + PmCartesian vel_bound = getXYZVelBounds(); + PmCartesian acc_bound = getXYZAccelBounds(); + // Setup blend data structures BlendGeom3 geom; BlendParameters param; @@ -1424,7 +1353,13 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc tp->done = 0; tp->depth = tcqLen(&tp->queue); //Fixing issue with duplicate id's? - tp_debug_print("Adding TC id %d of type %d, total length %0.08f\n",tc->id,tc->motion_type,tc->target); +#ifdef TP_DEBUG + print_json5_log_start(tpAddSegmentToQueue, Command); + print_json5_long_("id", tc->id); + print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_double_("target", tc->target); + print_json5_end_(); +#endif return TP_ERR_OK; } @@ -1854,9 +1789,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, pmCartScalMult(&this_tan, a_inst, &acc2); pmCartCartSub(&acc2,&acc1,&acc_diff); - //TODO store this in TP struct instead? - PmCartesian acc_bound; - tpGetMachineAccelBounds(&acc_bound); + PmCartesian acc_bound = getXYZAccelBounds(); PmCartesian acc_scale; findAccelScale(&acc_diff,&acc_bound,&acc_scale); @@ -2001,7 +1934,28 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v if (tpErrorCheck(tp) < 0) { return TP_ERR_FAIL; } - tp_info_print("== AddLine ==\n"); +#ifdef TP_DEBUG + { + // macros use the variable name, need a plain name to please the JSON5 parser + long nextId = tp->nextId; + EmcPose goal = tp->goalPos; + tp_debug_json_log_start(tpAddLine, Command); + tp_debug_json_long(nextId); + tp_debug_json_EmcPose(goal); + tp_debug_json_EmcPose(end); + tp_debug_json_double(vel); + tp_debug_json_double(ini_maxvel); + tp_debug_json_double(acc); + tp_debug_json_unsigned(enables); + tp_debug_json_long(indexrotary); + tp_debug_json_long(atspeed); + tp_debug_json_long(canon_motion_type); + EmcPose delta = tp->goalPos; + emcPoseSub(&end, &tp->goalPos, &delta); + tp_debug_json_EmcPose(delta); + tp_debug_json_log_end(); + } +#endif // Initialize new tc struct for the line segment TC_STRUCT tc = {0}; @@ -2070,8 +2024,29 @@ int tpAddCircle(TP_STRUCT * const tp, return TP_ERR_FAIL; } - tp_info_print("== AddCircle ==\n"); - tp_debug_print("ini_maxvel = %f\n",ini_maxvel); +#ifdef TP_DEBUG + { + // macros use the variable name, need a plain name to please the JSON5 parser + long nextId = tp->nextId; + EmcPose goal = tp->goalPos; + tp_debug_json_log_start(tpAddCircle, Command); + tp_debug_json_long(nextId); + tp_debug_json_EmcPose(goal); + tp_debug_json_PmCartesian(center); + tp_debug_json_PmCartesian(normal); + tp_debug_json_long(turn); + tp_debug_json_double(vel); + tp_debug_json_double(ini_maxvel); + tp_debug_json_double(acc); + tp_debug_json_unsigned(enables); + tp_debug_json_long(atspeed); + tp_debug_json_long(canon_motion_type); + EmcPose delta = tp->goalPos; + emcPoseSub(&end, &tp->goalPos, &delta); + tp_debug_json_EmcPose(delta); + tp_debug_json_log_end(); + } +#endif TC_STRUCT tc = {0}; @@ -2271,22 +2246,31 @@ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_de STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc) { #ifdef TC_DEBUG // Find maximum allowed velocity from feed and machine limits - double tc_target_vel = tpGetRealTargetVel(tp, tc); + const double v_target = tpGetRealTargetVel(tp, tc); // Store a copy of final velocity - double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); + const double v_final = tpGetRealFinalVel(tp, tc, nexttc); + const double v_max = tpGetMaxTargetVel(tp, tc); /* Debug Output */ - 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, dt = %f, term = %d, ", - tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond); - tc_debug_print("acc = %f, T = %f, P = %f, ", acc, - tc->target, tc->progress); - tc_debug_print("motion_type = %d\n", tc->motion_type); - - if (tc->on_final_decel) { - rtapi_print(" on final decel\n"); - } + print_json5_log_start(tc_state, Run); + print_json5_double_("id", tc->id); + print_json5_double(v_target); + print_json5_double(v_final); + print_json5_double(v_max); + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + print_json5_double_("v_current", tc->currentvel); + print_json5_double_("a_current", acc); + print_json5_double_("feed_scale", tpGetFeedScale(tp, tc)); + print_json5_double_("dt", tc->cycle_time); + print_json5_string_("term_cond", tcTermCondAsString((tc_term_cond_t)tc->term_cond)); + print_json5_bool_("final_decel", tc->on_final_decel); + print_json5_bool_("splitting", tc->splitting); + print_json5_bool_("is_blending", tc->is_blending); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_long_("canon_type", tc->canon_motion_type); + print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_end_(); #endif } @@ -2540,8 +2524,6 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const EmcPose tc_pos; tcGetEndpoint(tc, &tc_pos); - tc_debug_print("tc id = %u canon_type = %u mot type = %u\n", - tc->id, tc->canon_motion_type, tc->motion_type); tp->motionType = tc->canon_motion_type; tp->activeDepth = tc->active_depth; emcmotStatus->distance_to_go = tc->target - tc->progress; @@ -3240,12 +3222,6 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, // Update tp's position (checking for valid pose) tpAddCurrentPos(tp, &displacement); -#ifdef TC_DEBUG - double mag; - emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n",mag); -#endif - // Trigger removal of current segment at the end of the cycle tc->remove = 1; @@ -3352,12 +3328,10 @@ int tpRunCycle(TP_STRUCT * const tp, long period) //Set GUI status to "zero" state tpUpdateInitialStatus(tp); -#ifdef TC_DEBUG //Hack debug output for timesteps // NOTE: need to track every timestep, even those where the trajectory planner is idle - static double time_elapsed = 0; - time_elapsed+=tp->cycleTime; -#endif + tp->time_elapsed_sec+=tp->cycleTime; + ++tp->time_elapsed_ticks; //If we have a NULL pointer, then the queue must be empty, so we're done. if(!tc) { @@ -3365,8 +3339,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period) return TP_ERR_WAITING; } - tc_debug_print("-------------------\n"); - + tc_debug_print("--- TP Update <%lld> ---\n", tp->time_elapsed_ticks); /* If the queue empties enough, assume that the program is near the end. * This forces the last segment to be "finalized" to let the optimizer run.*/ @@ -3418,10 +3391,9 @@ int tpRunCycle(TP_STRUCT * const tp, long period) break; } -#ifdef TC_DEBUG - EmcPose pos_before = tp->currentPos; -#endif - + EmcPose const axis_pos_old = tp->currentPos; + EmcPose const axis_vel_old = tp->currentVel; + tcClearFlags(tc); tcClearFlags(nexttc); @@ -3432,21 +3404,52 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpHandleRegularCycle(tp, tc, nexttc); } + { + + EmcPose const axis_pos = tp->currentPos; + + EmcPose axis_vel; + emcPoseSub(&axis_pos, &axis_pos_old, &axis_vel); + emcPoseMultScalar(&axis_vel, 1.0 / tp->cycleTime); + tp->currentVel = axis_vel; + + EmcPose axis_accel; + emcPoseSub(&axis_vel, &axis_vel_old, &axis_accel); + emcPoseMultScalar(&axis_accel, 1.0 / tp->cycleTime); + + unsigned failed_axes = findAccelViolations(axis_accel); + if (failed_axes) + { + // KLUDGE mask out good axes from print list with space + char failed_axes_list[9] = "XYZABCUVW"; + for (int i = 0; i < 9;++i) { + if (failed_axes & (1 << i)) { + continue; + } + failed_axes_list[i]=' '; + } + + rtapi_print_msg(RTAPI_MSG_ERR, "Acceleration violation on axes [%s] at %g sec\n", failed_axes_list, tp->time_elapsed_sec); + print_json5_start_(); + print_json5_object_start_("accel_violation"); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_EmcPose(axis_accel); + print_json5_object_end_(); + print_json5_end_(); + } + #ifdef TC_DEBUG - double mag; - EmcPose disp; - emcPoseSub(&tp->currentPos, &pos_before, &disp); - emcPoseMagnitude(&disp, &mag); - tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n", - time_elapsed, - mag, emcmotStatus->current_vel); - - tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n", - disp.tran.x, - disp.tran.y, - disp.tran.z, - time_elapsed); + print_json5_log_start(tpRunCycle, Run); + print_json5_EmcPose(axis_pos); + print_json5_EmcPose(axis_vel); + print_json5_EmcPose(axis_accel); + double current_vel = emcmotStatus->current_vel; + print_json5_double(current_vel); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_end_(); #endif + } // If TC is complete, remove it from the queue. if (tc->remove) { diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index 301e4ac5c18..0281b89546b 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -26,7 +26,6 @@ int tpClearDIOs(TP_STRUCT * const tp); int tpSetCycleTime(TP_STRUCT * const tp, double secs); int tpSetVmax(TP_STRUCT * const tp, double vmax, double ini_maxvel); int tpSetVlimit(TP_STRUCT * const tp, double vLimit); -int tpSetAmax(TP_STRUCT * const tp, double aMax); int tpSetId(TP_STRUCT * const tp, int id); int tpGetExecId(TP_STRUCT * const tp); int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance); diff --git a/src/emc/tp/tp_debug.h b/src/emc/tp/tp_debug.h index d14c5e5e511..ceda010aabc 100644 --- a/src/emc/tp/tp_debug.h +++ b/src/emc/tp/tp_debug.h @@ -6,7 +6,7 @@ * License: GPL Version 2 * System: Linux * -* Copyright (c) 2013 All rights reserved. +* Copyright (c) 2013-2019 All rights reserved. * * Last change: ********************************************************************/ @@ -14,23 +14,44 @@ #define TP_DEBUG_H #include "rtapi.h" /* printing functions */ +#include "posemath.h" +#include "emcpos.h" +#include "rtapi_json5.h" /** TP debug stuff */ #ifdef TP_DEBUG //Kludge because I didn't know any better at the time //FIXME replace these with better names? #define tp_debug_print(...) rtapi_print(__VA_ARGS__) +#define tp_debug_only(funcname,...) funcname(__VA_ARGS__) #elif defined(UNIT_TEST) #include #define tp_debug_print(...) printf(__VA_ARGS__) +#define tp_debug_only(funcname,...) funcname(__VA_ARGS__) #else #define tp_debug_print(...) +#define tp_debug_only(funcname,...) #endif -// Verbose but effective wrappers for building faux-JSON debug output for a function -#define tp_debug_json_double(varname_) tp_debug_print("%s: %g, ", #varname_, varname_) -#define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_) -#define tp_debug_json_end() tp_debug_print("}\n") +#define print_json5_log_start(fname_, level_) do { \ + print_json5_start_(); \ + print_json5_string_("log_entry", #fname_); \ + print_json5_string_("log_level", #level_); \ +} while (0) + +#define print_json5_log_end() print_json5_end_() + +// Another layer of macro wrappers to conditionally compile debug-only statements +#define tp_debug_json_double(varname_) tp_debug_only(print_json5_double, varname_) +#define tp_debug_json_string(varname_) tp_debug_only(print_json5_string, varname_) +#define tp_debug_json_long(varname_) tp_debug_only(print_json5_long, varname_) +#define tp_debug_json_unsigned(varname_) tp_debug_only(print_json5_unsigned, varname_) +#define tp_debug_json_PmCartesian(varname_) tp_debug_only(print_json5_PmCartesian, varname_) +#define tp_debug_json_EmcPose(varname_) tp_debug_only(print_json5_EmcPose, varname_) +#define tp_debug_json_PmCartLine(name_, value_) tp_debug_only(print_json5_PmCartLine_, name_, value_) +#define tp_debug_json_PmCircle(name_, value_) tp_debug_only(print_json5_PmCircle_, name_, value_) +#define tp_debug_json_log_start(fname_, level) tp_debug_only(print_json5_log_start, fname_, level) +#define tp_debug_json_log_end() tp_debug_only(print_json5_end_) /** Use for profiling to make static function names visible */ #ifdef TP_PROFILE diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index e82b8dcf954..7ec85a78765 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -94,6 +94,7 @@ typedef struct { EmcPose currentPos; EmcPose goalPos; + EmcPose currentVel; int queueSize; double cycleTime; @@ -104,11 +105,6 @@ typedef struct { subsequent moves */ double vLimit; /* absolute upper limit on all vels */ - double aMax; /* max accel (unused) */ - //FIXME this shouldn't be a separate limit, - double aMaxCartesian; /* max cartesian acceleration by machine bounds */ - double aLimit; /* max accel (unused) */ - double wMax; /* rotational velocity max */ double wDotMax; /* rotational accelleration max */ int nextId; @@ -131,6 +127,9 @@ typedef struct { syncdio_t syncdio; //record tpSetDout's here + double time_elapsed_sec; // Total elapsed TP run time in seconds + long long time_elapsed_ticks; // Total elapsed TP run time in cycles (ticks) + } TP_STRUCT; diff --git a/src/rtapi/uspace_common.h b/src/rtapi/uspace_common.h index 16805e212b4..2da17b4f9ef 100644 --- a/src/rtapi/uspace_common.h +++ b/src/rtapi/uspace_common.h @@ -20,6 +20,7 @@ #include #include #include +#include static int msg_level = RTAPI_MSG_ERR; /* message printing level */ diff --git a/tests/trajectory-planner/circular-arcs/README.md b/tests/trajectory-planner/circular-arcs/README.md new file mode 100644 index 00000000000..c4bf3f91c5b --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/README.md @@ -0,0 +1,21 @@ +# Decoding JSON5 logsa + +TP debug log output can be compile-time enabled with the following make arguments: + +``` +make ... EXTRA_DEBUG='-DTP_DEBUG -DTC_DEBUG -DTP_INFO_LOGGING' +``` + +These log files use JSON5 format, a superset of JSON. The reason we use this format is that it makes the debug macros easier to write (leave in trailing commas, and don't have to quote field names). + +An example of how to parse the log files is shown in parse\_tp\_logs.py. To run this python file, you need python 3 installed, along with some dependencies. A virtual environment is the easiest way to do this: + +``` +sudo apt install python3 python3-tk python3-pip python3-virtualenv python3-numpy +mkvirtualenv -p `which python3` lcnc_tp_test +``` + +Once the virtual environment is running, you can install requirements from the config file: +``` +pip install -r tp_log_parsing.cfg +``` diff --git a/tests/trajectory-planner/circular-arcs/parse_tp_logs.py b/tests/trajectory-planner/circular-arcs/parse_tp_logs.py new file mode 100755 index 00000000000..904c71f5deb --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/parse_tp_logs.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +import pyjson5 + +import sys +import re +from matplotlib import pyplot as plt +import numpy as np + +axes={ + 'X': 0, + 'Y': 1, + 'Z': 2, + 'A': 3, + 'B': 4, + 'C': 5, + 'U': 6, + 'V': 7, + 'W': 8, +} + +if __name__ == '__main__': + if len(sys.argv) < 2: + print(sys.stderr, 'No logfile specified') + exit(1) + + logs = [] + with open(sys.argv[1], 'r') as log_file: + for l in log_file.readlines(): + # Try to filter out obviously non-JSON lines + if re.search('{', l) is None or re.search('}', l) is None: + continue + try: + logs.append(pyjson5.decode(l)) + except Exception as e: + print('Ignoring exception ', e) + + axis_pos = np.array([e.get('axis_pos', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + axis_vel = np.array([e.get('axis_vel', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + axis_accel = np.array([e.get('axis_accel', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + times = np.array([e.get('time', None) for e in logs if e.get('log_entry', None) == 'tpRunCycle']) + + np.savetxt('axis_pos.csv', axis_pos, delimiter=',') + np.savetxt('axis_vel.csv', axis_vel, delimiter=',') + np.savetxt('axis_accel.csv', axis_accel, delimiter=',') + + plt_axis = 'X' + idx = axes[plt_axis] + + plt.figure(1) + plt.plot(times * 1000.0, axis_pos[:, idx]) + plt.grid(True) + plt.title('{} Axis position vs time, ms'.format(plt_axis)) + + plt.figure(2) + plt.plot(times * 1000.0, axis_vel[:, idx]) + plt.grid(True) + plt.title('{} Axis velocity vs time, ms'.format(plt_axis)) + + plt.figure(3) + plt.plot(times * 1000.0, axis_accel[:, idx]) + plt.grid(True) + plt.title('{} Axis acceleration vs time, ms'.format(plt_axis)) + + plt.show() + + + diff --git a/tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg b/tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg new file mode 100644 index 00000000000..9ac5d225004 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/tp_testing_venv.cfg @@ -0,0 +1,4 @@ +matplotlib==3.0.2 +# Only do this if the system package is old, takes forever +#numpy==1.16.0 +pyjson5==0.4.5 diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 8d9140a3786..35c41ab4adb 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -4,6 +4,9 @@ #include "tp_types.h" #include "math.h" #include "rtapi.h" +#include "joint_util.h" +#include "motion_debug.h" +struct emcmot_debug_t emcmotDebug = {0}; /* Expand to all the definitions that need to be in the test runner's main file. */ @@ -15,7 +18,7 @@ void rtapi_print_msg(msg_level_t level, const char *fmt, ...) va_list args; va_start(args, fmt); - printf(fmt, args); + vprintf(fmt, args); va_end(args); } @@ -63,15 +66,39 @@ TEST pmCartCartAntiParallel_numerical() { PASS(); } +SUITE(blendmath) { + RUN_TEST(pmCartCartParallel_numerical); + RUN_TEST(pmCartCartAntiParallel_numerical); +} + + +TEST findMinNonZeroComplete() { + PmCartesian normal_bounds = {3.0,1.0,2.0}; + + double min_bound = findMinNonZero(&normal_bounds); + ASSERT_EQ(min_bound, 1.0); + PASS(); +} + +TEST findMinNonZeroPartial() { + PmCartesian partial_bounds = {3.0,0.0,2.0}; + + double min_bound = findMinNonZero(&partial_bounds); + ASSERT_EQ(min_bound, 2.0); + PASS(); +} + +SUITE(joint_utils) { + RUN_TEST(findMinNonZeroComplete); + RUN_TEST(findMinNonZeroPartial); +} + - SUITE(blendmath) { - RUN_TEST(pmCartCartParallel_numerical); - RUN_TEST(pmCartCartAntiParallel_numerical); - } int main(int argc, char **argv) { GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ - RUN_SUITE(blendmath); /* run a suite */ + RUN_SUITE(blendmath); + RUN_SUITE(joint_utils); GREATEST_MAIN_END(); /* display results */ - } +} From e6369f9560b416503ca647be89a5412a3717dee7 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Feb 2019 12:09:11 -0500 Subject: [PATCH 066/129] tests: Add a stripped down test case for split-cycle debugging --- .../violation_checks/split-cycle-check.ngc | 515 ++++++++++++++++++ 1 file changed, 515 insertions(+) create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/violation_checks/split-cycle-check.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/split-cycle-check.ngc b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/split-cycle-check.ngc new file mode 100644 index 00000000000..9dc4b93cd2a --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/split-cycle-check.ngc @@ -0,0 +1,515 @@ +G20 +G0 Z0.0120 +G17 G40 +G80 G90 G94 +S1000 M3 +(G04 P3) +G64 +F999.0000 +G18 +F999.0000 +G0 X3.3390 Y0 +G1 Z-0.0319 +G3 X3.214000 Z-0.156892 R0.125000 +F999.0000 +G1 X3.2100 Z-0.1530 + X3.2040 Z-0.1539 + X3.2000 Z-0.1569 + X3.1940 Z-0.1539 + X3.1840 Z-0.1588 + X3.1680 Z-0.1549 + X3.1620 Z-0.1565 + X3.1380 Z-0.1539 + X3.1280 Z-0.1589 + X3.1260 Z-0.1575 + X3.1240 Z-0.1520 + X3.1200 Z-0.1510 + X3.1160 Z-0.1549 + X3.1060 Z-0.1588 +G3 X3.0940 Z-0.1540 I0.0013 K0.0205 +G1 X3.0860 Z-0.1556 + X3.0800 Z-0.1539 + X3.0760 Z-0.1569 + X3.0700 Z-0.1539 + X3.0660 Z-0.1549 + X3.0620 Z-0.1529 + X3.0560 Z-0.1539 + X3.0520 Z-0.1520 + X3.0440 Z-0.1520 + X3.0400 Z-0.1549 + X3.0360 Z-0.1556 + X3.0280 Z-0.1549 + X3.0240 Z-0.1529 + X3.0040 Z-0.1549 + X2.9900 Z-0.1490 + X2.9860 Z-0.1529 + X2.9800 Z-0.1510 + X2.9740 Z-0.1546 + X2.9380 Z-0.1510 + X2.9300 Z-0.1526 + X2.9220 Z-0.1520 + X2.9180 Z-0.1559 + X2.9100 Z-0.1556 + X2.9080 Z-0.1536 + X2.9040 Z-0.1530 + X2.9000 Z-0.1559 + X2.8940 Z-0.1529 + X2.8860 Z-0.1520 + X2.8780 Z-0.1546 + X2.8760 Z-0.1569 + X2.8640 +G2 X2.8540 Z-0.1510 I-0.0110 K-0.0073 +G1 X2.8500 Z-0.1516 + X2.8480 Z-0.1546 + X2.8320 Z-0.1539 + X2.8220 Z-0.1556 + X2.8200 Z-0.1536 + X2.8160 Z-0.1530 + X2.8140 Z-0.1536 + X2.8100 Z-0.1598 + X2.8060 Z-0.1526 + X2.8040 Z-0.1520 + X2.8000 Z-0.1526 + X2.7980 Z-0.1556 + X2.7960 Z-0.1559 + X2.7920 Z-0.1529 + X2.7740 Z-0.1559 + X2.7700 Z-0.1529 + X2.7600 Z-0.1549 + X2.7560 Z-0.1490 + X2.7520 Z-0.1480 + X2.7500 Z-0.1490 + X2.7480 Z-0.1539 + X2.7420 Z-0.1559 + X2.7200 Z-0.1539 + X2.7040 Z-0.1575 + X2.6980 Z-0.1529 + X2.6920 Z-0.1549 + X2.6820 Z-0.1530 + X2.6740 Z-0.1559 + X2.6600 Z-0.1529 + X2.6560 Z-0.1579 + X2.6540 Z-0.1556 + X2.6480 Z-0.1546 + X2.6460 Z-0.1487 + X2.6440 Z-0.1481 + X2.6400 Z-0.1487 + X2.6360 Z-0.1540 + X2.6300 Z-0.1529 + X2.6220 Z-0.1559 + X2.6140 Z-0.1539 + X2.6120 Z-0.1497 + X2.6080 Z-0.1490 + X2.6040 Z-0.1530 + X2.6000 Z-0.1520 + X2.5960 Z-0.1559 + X2.5920 Z-0.1559 + X2.5900 Z-0.1500 + X2.5860 Z-0.1471 + X2.5820 Z-0.1481 + X2.5800 Z-0.1530 + X2.5760 Z-0.1539 + X2.5720 Z-0.1530 + X2.5700 Z-0.1471 + X2.5680 Z-0.1461 + X2.5640 Z-0.1471 + X2.5600 Z-0.1539 + X2.5540 Z-0.1540 + X2.5460 Z-0.1510 + X2.5420 Z-0.1546 + X2.5360 Z-0.1530 + X2.5320 Z-0.1549 + X2.5260 Z-0.1520 + X2.5140 Z-0.1529 + X2.5080 Z-0.1510 + X2.5040 Z-0.1526 + X2.4920 Z-0.1490 + X2.4760 Z-0.1510 + X2.4640 Z-0.1480 + X2.4580 Z-0.1510 + X2.4480 Z-0.1510 + X2.4420 Z-0.1536 + X2.4380 Z-0.1487 + X2.4340 Z-0.1480 + X2.4240 Z-0.1529 + X2.4180 Z-0.1490 +G3 X2.4020 Z-0.1529 I-0.0164 K0.0321 +G1 X2.3860 Z-0.1490 + X2.3760 Z-0.1516 + X2.3700 Z-0.1500 + X2.3660 Z-0.1529 + X2.3620 + X2.3580 Z-0.1510 + X2.3540 Z-0.1520 + X2.3500 Z-0.1510 + X2.3460 Z-0.1480 + X2.3400 Z-0.1500 + X2.3380 Z-0.1526 + X2.3360 Z-0.1497 + X2.3260 Z-0.1500 + X2.3200 Z-0.1461 + X2.3140 Z-0.1481 + X2.3060 Z-0.1461 + X2.3020 Z-0.1500 + X2.2800 Z-0.1497 + X2.2760 Z-0.1490 + X2.2720 Z-0.1461 + X2.2680 Z-0.1467 + X2.2640 Z-0.1441 + X2.2560 Z-0.1438 + X2.2520 Z-0.1392 + X2.2460 Z-0.1412 + X2.2420 Z-0.1402 + X2.2320 Z-0.1412 + X2.2260 Z-0.1373 + X2.2140 + X2.2080 Z-0.1353 + X2.2000 Z-0.1369 + X2.1940 Z-0.1333 + X2.1880 Z-0.1363 + X2.1740 Z-0.1373 + X2.1680 Z-0.1343 + X2.1600 Z-0.1363 + X2.1520 Z-0.1334 + X2.1480 Z-0.1340 + X2.1440 Z-0.1314 + X2.1380 Z-0.1340 + X2.1280 Z-0.1343 + X2.1220 Z-0.1324 + X2.1160 Z-0.1340 + X2.1060 Z-0.1324 + X2.0940 Z-0.1350 + X2.0920 Z-0.1324 + X2.0880 Z-0.1314 + X2.0820 Z-0.1343 + X2.0780 Z-0.1333 + X2.0660 Z-0.1369 + X2.0600 Z-0.1324 + X2.0540 Z-0.1350 + X2.0500 Z-0.1333 + X2.0400 +G2 X2.0340 Z-0.1275 I-0.0061 K-0.0003 +G1 X2.0300 Z-0.1281 + X2.0260 Z-0.1359 + X2.0180 + X2.0040 Z-0.1333 + X1.9980 Z-0.1304 + X1.9900 Z-0.1324 + X1.9860 Z-0.1353 + X1.9740 Z-0.1324 + X1.9680 Z-0.1340 + X1.9560 Z-0.1314 + X1.9520 + X1.9480 Z-0.1343 + X1.9340 Z-0.1353 + X1.9320 Z-0.1304 + X1.9300 Z-0.1294 + X1.9260 Z-0.1304 + X1.9240 Z-0.1333 + X1.9140 Z-0.1334 + X1.9100 Z-0.1350 + X1.9020 Z-0.1314 + X1.8940 Z-0.1324 + X1.8900 Z-0.1285 + X1.8840 + X1.8800 Z-0.1314 + X1.8740 Z-0.1324 + X1.8660 Z-0.1304 + X1.8600 Z-0.1324 + X1.8440 Z-0.1314 + X1.8400 Z-0.1294 + X1.8340 Z-0.1324 + X1.8280 Z-0.1304 + X1.8200 Z-0.1314 + X1.8180 Z-0.1304 + X1.7820 Z-0.1324 + X1.7760 Z-0.1304 +G2 X1.7640 Z-0.1343 I0.0005 K-0.0218 +G1 X1.7460 Z-0.1304 + X1.7420 + X1.7360 Z-0.1330 + X1.7300 Z-0.1294 + X1.7260 Z-0.1301 + X1.7240 Z-0.1334 + X1.7200 Z-0.1333 + X1.7180 Z-0.1304 + X1.7140 Z-0.1294 +G3 X1.7040 Z-0.1343 I-0.0122 K0.0123 +G1 X1.7000 Z-0.1314 + X1.6800 + X1.6760 Z-0.1275 + X1.6700 Z-0.1304 + X1.6560 Z-0.1275 + X1.6520 Z-0.1324 + X1.6340 Z-0.1320 + X1.6280 Z-0.1275 + X1.6120 Z-0.1314 + X1.6000 Z-0.1304 + X1.5960 Z-0.1324 + X1.5800 Z-0.1284 + X1.5680 Z-0.1320 + X1.5580 Z-0.1324 + X1.5540 Z-0.1304 + X1.5460 Z-0.1314 + X1.5420 Z-0.1294 + X1.5340 Z-0.1310 + X1.5280 Z-0.1275 + X1.5240 + X1.5200 Z-0.1314 + X1.5100 Z-0.1284 + X1.5020 Z-0.1314 + X1.4960 Z-0.1310 + X1.4900 Z-0.1284 + X1.4860 Z-0.1294 + X1.4840 Z-0.1271 + X1.4800 Z-0.1265 + X1.4760 Z-0.1304 + X1.4620 + X1.4520 Z-0.1275 + X1.4480 Z-0.1304 + X1.4340 Z-0.1294 + X1.4180 Z-0.1324 + X1.4120 Z-0.1304 + X1.3980 Z-0.1294 + X1.3960 Z-0.1271 + X1.3920 Z-0.1265 + X1.3900 Z-0.1271 + X1.3880 Z-0.1310 + X1.3820 Z-0.1324 + X1.3740 Z-0.1304 + X1.3600 Z-0.1314 + X1.3560 Z-0.1333 + X1.3500 Z-0.1304 + X1.3400 Z-0.1333 + X1.3340 Z-0.1304 + X1.3300 Z-0.1353 + X1.3220 Z-0.1324 + X1.3100 Z-0.1412 + X1.3040 Z-0.1418 + X1.2980 Z-0.1500 + X1.2940 Z-0.1507 + X1.2920 Z-0.1546 + X1.2860 Z-0.1556 + X1.2840 Z-0.1585 + X1.2780 + X1.2760 Z-0.1647 + X1.2720 Z-0.1654 + X1.2700 Z-0.1722 + X1.2640 Z-0.1765 + X1.2580 Z-0.1755 + X1.2520 Z-0.1771 + X1.2500 Z-0.1732 + X1.2460 Z-0.1726 + X1.2340 Z-0.1801 + X1.2280 Z-0.1771 +G3 X1.2220 Z-0.1657 I0.0155 K0.0154 +G1 X1.2200 Z-0.1647 + X1.2140 Z-0.1667 + X1.2100 Z-0.1637 + X1.2020 Z-0.1647 + X1.1980 Z-0.1627 + X1.1940 Z-0.1638 + X1.1840 Z-0.1628 +G2 X1.1760 Z-0.1706 I0.0021 K-0.0101 +G1 X1.1720 Z-0.1676 + X1.1660 Z-0.1696 +G3 X1.1560 Z-0.1624 I0.0019 K0.0132 +G1 X1.1520 Z-0.1618 + X1.1480 Z-0.1588 + X1.1440 Z-0.1598 + X1.1380 Z-0.1578 + X1.1320 Z-0.1627 + X1.1280 Z-0.1638 + X1.1220 Z-0.1706 + X1.1180 Z-0.1722 + X1.1160 Z-0.1703 + X1.1120 Z-0.1696 + X1.1060 Z-0.1736 + X1.1040 Z-0.1687 + X1.0940 Z-0.1667 + X1.0900 Z-0.1638 + X1.0820 Z-0.1657 + X1.0780 Z-0.1686 +G2 X1.0640 Z-0.1736 I0.0011 K-0.0254 +G1 X1.0620 Z-0.1693 + X1.0600 Z-0.1687 + X1.0540 Z-0.1686 + X1.0500 Z-0.1706 + X1.0320 Z-0.1676 + X1.0240 Z-0.1761 + X1.0220 Z-0.1804 + X1.0200 Z-0.1810 + X1.0140 Z-0.1654 + X1.0120 Z-0.1647 + X1.0060 Z-0.1667 + X1.0000 Z-0.1647 + X0.9960 Z-0.1657 + X0.9920 Z-0.1608 + X0.9860 + X0.9820 Z-0.1657 + X0.9800 Z-0.1742 + X0.9740 Z-0.1775 + X0.9660 Z-0.1677 + X0.9620 Z-0.1657 + X0.9560 Z-0.1667 + X0.9520 Z-0.1657 + X0.9500 Z-0.1624 + X0.9460 Z-0.1618 + X0.9400 Z-0.1654 + X0.9360 Z-0.1647 +G3 X0.9260 Z-0.1687 I-0.0103 K0.0116 +G1 X0.9220 Z-0.1676 + X0.9180 Z-0.1687 +G2 X0.9100 Z-0.1863 I0.0719 K-0.0433 +G1 X0.9040 Z-0.1869 + X0.9020 Z-0.1889 + X0.8960 Z-0.1820 + X0.8860 Z-0.1804 + X0.8820 Z-0.1814 + X0.8720 Z-0.1703 + X0.8600 Z-0.1706 + X0.8500 Z-0.1608 + X0.8460 Z-0.1618 + X0.8400 Z-0.1608 + X0.8360 Z-0.1654 + X0.8320 Z-0.1647 + X0.8300 Z-0.1614 + X0.8240 + X0.8140 Z-0.1771 + X0.8100 Z-0.1794 + X0.8040 Z-0.1775 +G2 X0.7920 Z-0.1686 I-0.0196 K-0.0141 +G1 X0.7840 Z-0.1706 + X0.7820 Z-0.1667 + X0.7740 Z-0.1657 + X0.7700 Z-0.1627 + X0.7620 Z-0.1654 + X0.7520 Z-0.1608 + X0.7460 Z-0.1624 + X0.7360 Z-0.1588 + X0.7200 Z-0.1628 + X0.7180 Z-0.1716 + X0.7160 Z-0.1725 + X0.7140 Z-0.1693 + X0.7060 + X0.7000 Z-0.1647 + X0.6900 Z-0.1657 + X0.6880 Z-0.1608 + X0.6820 Z-0.1598 + X0.6800 Z-0.1608 + X0.6780 Z-0.1673 + X0.6720 Z-0.1732 + X0.6640 Z-0.1735 + X0.6560 Z-0.1703 + X0.6540 Z-0.1667 + X0.6480 Z-0.1657 + X0.6440 Z-0.1627 + X0.6280 Z-0.1686 + X0.6240 Z-0.1657 + X0.6180 Z-0.1667 + X0.6140 Z-0.1637 + X0.6060 Z-0.1638 + X0.6020 Z-0.1608 + X0.5980 Z-0.1614 + X0.5960 Z-0.1579 + X0.5900 Z-0.1559 + X0.5860 Z-0.1608 + X0.5780 Z-0.1634 + X0.5740 Z-0.1608 + X0.5620 Z-0.1598 + X0.5480 Z-0.1706 + X0.5360 Z-0.1745 + X0.5340 Z-0.1735 + X0.5300 Z-0.1745 + X0.5260 Z-0.1824 + X0.5240 + X0.5220 Z-0.1801 + X0.5180 Z-0.1791 + X0.5160 Z-0.1752 + X0.5120 Z-0.1745 + X0.5080 Z-0.1765 + X0.5020 Z-0.1712 + X0.4940 Z-0.1696 + X0.4900 Z-0.1667 + X0.4840 Z-0.1676 + X0.4800 Z-0.1634 + X0.4740 + X0.4700 Z-0.1608 + X0.4620 Z-0.1598 + X0.4540 Z-0.1618 + X0.4500 Z-0.1578 + X0.4340 Z-0.1605 + X0.4300 Z-0.1683 + X0.4260 Z-0.1677 + X0.4220 Z-0.1706 + X0.4160 Z-0.1696 + X0.4140 Z-0.1706 + X0.4120 Z-0.1755 + X0.4080 + X0.4040 Z-0.1726 + X0.4000 Z-0.1732 + X0.3980 Z-0.1761 + X0.3860 Z-0.1736 + X0.3780 Z-0.1742 + X0.3740 Z-0.1732 + X0.3720 Z-0.1677 + X0.3680 Z-0.1657 + X0.3600 Z-0.1676 + X0.3560 Z-0.1667 + X0.3520 Z-0.1696 + X0.3480 Z-0.1696 + X0.3380 Z-0.1676 + X0.3260 Z-0.1677 + X0.3200 Z-0.1627 + X0.3140 Z-0.1638 + X0.3080 Z-0.1618 +G3 X0.2900 Z-0.1663 I-0.0180 K0.0331 +G1 X0.2840 Z-0.1618 + X0.2780 Z-0.1628 + X0.2720 Z-0.1608 + X0.2660 Z-0.1647 +G3 X0.2540 Z-0.1589 I0.0049 K0.0252 +G1 X0.2500 Z-0.1595 + X0.2480 Z-0.1618 + X0.2380 Z-0.1588 + X0.2320 Z-0.1598 + X0.2280 Z-0.1578 + X0.2240 Z-0.1589 + X0.2200 Z-0.1732 + X0.2180 +G3 X0.2140 Z-0.1569 I0.0473 K0.0202 +G1 X0.2120 Z-0.1559 + X0.2080 Z-0.1471 + X0.2060 Z-0.1461 + X0.2020 Z-0.1471 + X0.1980 Z-0.1441 + X0.1940 Z-0.1448 + X0.1900 Z-0.1487 + X0.1860 Z-0.1461 + X0.1720 Z-0.1471 + X0.1680 Z-0.1441 + X0.1580 Z-0.1458 + X0.1520 Z-0.1441 + X0.1460 Z-0.1461 + X0.1400 Z-0.1431 + X0.1320 Z-0.1458 + X0.1180 Z-0.1412 + X0.1100 Z-0.1441 + X0.1060 Z-0.1402 + X0.1000 Z-0.1422 + X0.0880 Z-0.1392 + X0.0820 Z-0.1412 + X0.0740 + X0.0700 Z-0.1451 + X0.0660 Z-0.1431 + X0.0580 Z-0.1448 + X0.0540 Z-0.1432 + X0.0420 Z-0.1441 + X0.0380 Z-0.1422 + X0.0240 Z-0.1441 + X0.0200 Z-0.1402 + X0.0120 Z-0.1408 + X0.0060 Z-0.1373 + X0.0020 Z-0.1379 + X0.0000 Z-0.1431 +M2 From 399173028b565518a2639a57d8e97ff3fbcd855b Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Feb 2019 10:22:46 -0500 Subject: [PATCH 067/129] tp: Refactor tpCheckEndCondition to isolate computation from TP global state This is a preparatory step for a larger refactor, done with a minimal diff to ensure that the logic is preserved. --- src/emc/tp/blendmath.h | 7 +++ src/emc/tp/tp.c | 124 +++++++++++++++++++++++++++-------------- 2 files changed, 89 insertions(+), 42 deletions(-) diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 263132b25d2..a65391b1d34 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -267,4 +267,11 @@ static inline double findVPeak(double a_t_max, double distance) { return pmSqrt(a_t_max * distance); } + +// Start by mimicing the current structure for minimal changes +typedef struct { + double v_f; + double dt; + int remove; +} EndCondition; #endif diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index aa8a823c777..ec0877327ea 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3090,39 +3090,41 @@ STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, } -/** - * Check remaining time in a segment and calculate split cycle if necessary. - * This function estimates how much time we need to complete the next segment. - * If it's greater than one timestep, then we do nothing and carry on. If not, - * then we flag the segment as "splitting", so that during the next cycle, - * it handles the transition to the next segment. - */ -STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { - - //Assume no split time unless we find otherwise - tc->cycle_time = tp->cycleTime; - //Initial guess at dt for next round - double dx = tc->target - tc->progress; - tc_debug_print("tpCheckEndCondition: dx = %e\n",dx); +EndCondition checkEndCondition(double cycleTime, + double progress, + double target, + double currentvel, + double v_f, + double a_max, + tc_term_cond_t term_cond) +{ + double dx = target - progress; + // Start with safe defaults (segment will not end next cycle + EndCondition out = { + v_f, + TP_BIG_NUM * cycleTime, + 0 + }; + + // This block essentially ignores split cycles for exact-stop moves if (dx <= TP_POS_EPSILON) { //If the segment is close to the target position, then we assume that it's done. tp_debug_print("close to target, dx = %.12f\n",dx); //Force progress to land exactly on the target to prevent numerical errors. - tc->progress = tc->target; - tcSetSplitCycle(tc, 0.0, tc->currentvel); - if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { - tc->remove = 1; + out.dt = 0.0; + out.v_f = currentvel; + if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { + out.remove = 1; } - return TP_ERR_OK; - } else if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { - return TP_ERR_NO_ACTION; + return out; + } else if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { + return out; } - double v_f = tpGetRealFinalVel(tp, tc, nexttc); - double v_avg = (tc->currentvel + v_f) / 2.0; + double v_avg = (currentvel + v_f) / 2.0; //Check that we have a non-zero "average" velocity between now and the //finish. If not, it means that we have to accelerate from a stop, which @@ -3138,21 +3140,19 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, //Get dt from distance and velocity (avoid div by zero) dt = fmax(dt, dx / v_avg); } else { - if ( dx > (v_avg * tp->cycleTime) && dx > TP_POS_EPSILON) { + if ( dx > (v_avg * cycleTime) && dx > TP_POS_EPSILON) { tc_debug_print(" below velocity threshold, assuming far from end\n"); - return TP_ERR_NO_ACTION; + return out; } } - //Calculate the acceleration this would take: - - double dv = v_f - tc->currentvel; + + // Assuming a full timestep: + double dv = v_f - currentvel; double a_f = dv / dt; //If this is a valid acceleration, then we're done. If not, then we solve //for v_f and dt given the max acceleration allowed. - double a_max = tcGetTangentialMaxAccel(tc); - //If we exceed the maximum acceleration, then the dt estimate is too small. double a = a_f; int recalc = sat_inplace(&a, a_max); @@ -3160,39 +3160,79 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, //Need to recalculate vf and above if (recalc) { tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); - double disc = pmSq(tc->currentvel / a) + 2.0 / a * dx; + double disc = pmSq(currentvel / a) + 2.0 / a * dx; if (disc < 0) { //Should mean that dx is too big, i.e. we're not close enough tc_debug_print(" dx = %f, too large, not at end yet\n",dx); - return TP_ERR_NO_ACTION; + return out; } if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { tc_debug_print("disc too small, skipping sqrt\n"); - dt = -tc->currentvel / a; + dt = -currentvel / a; } else if (a > 0) { tc_debug_print("using positive sqrt\n"); - dt = -tc->currentvel / a + pmSqrt(disc); + dt = -currentvel / a + pmSqrt(disc); } else { tc_debug_print("using negative sqrt\n"); - dt = -tc->currentvel / a - pmSqrt(disc); + dt = -currentvel / a - pmSqrt(disc); } tc_debug_print(" revised dt = %f\n", dt); //Update final velocity with actual result - v_f = tc->currentvel + dt * a; + out.v_f = currentvel + dt * a; + } + + out.dt = dt; + return out; +} + +/** + * Check remaining time in a segment and calculate split cycle if necessary. + * This function estimates how much time we need to complete the next segment. + * If it's greater than one timestep, then we do nothing and carry on. If not, + * then we flag the segment as "splitting", so that during the next cycle, + * it handles the transition to the next segment. + */ +STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) +{ + EndCondition ec = checkEndCondition(tp->cycleTime, + tc->progress, + tc->target, + tc->currentvel, + tpGetRealFinalVel(tp, tc, nexttc), + tcGetTangentialMaxAccel(tc), + (tc_term_cond_t)tc->term_cond + ); + + // Copy results into tc + tc->remove = ec.remove; + + if (ec.remove || ec.dt < TP_TIME_EPSILON) { + tc->progress = tc->target; } + +#ifdef TC_DEBUG + { + print_json5_log_start(checkEndCondition, Run); + print_json5_long_("id", tc->id); + print_json5_double_("v_final", ec.v_f); + print_json5_double_("dt", ec.dt); + print_json5_bool_("remove", ec.remove); + print_json5_end_(); + } +#endif - if (dt < TP_TIME_EPSILON) { + if (ec.dt < TP_TIME_EPSILON) { //Close enough, call it done tc_debug_print("revised dt small, finishing tc\n"); tc->progress = tc->target; - tcSetSplitCycle(tc, 0.0, v_f); - } else if (dt < tp->cycleTime ) { - tc_debug_print(" corrected v_f = %f, a = %f\n", v_f, a); - tcSetSplitCycle(tc, dt, v_f); + tcSetSplitCycle(tc, 0.0, ec.v_f); + } else if (ec.dt < tp->cycleTime ) { + tc_debug_print(" corrected v_f = %f\n", ec.v_f); + tcSetSplitCycle(tc, ec.dt, ec.v_f); } else { - tc_debug_print(" dt = %f, not at end yet\n",dt); + tc_debug_print(" dt = %f, not at end yet\n", ec.dt); return TP_ERR_NO_ACTION; } return TP_ERR_OK; From 0dffa56bba3fb6912c080f4ffe3d49a4109b9128 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Feb 2019 15:28:18 -0500 Subject: [PATCH 068/129] tp: Move split cycle function to blendmath file and simplify logic --- src/emc/tp/blendmath.c | 95 +++++++++++++++++++++++++++++++ src/emc/tp/blendmath.h | 9 +++ src/emc/tp/tp.c | 124 +++++------------------------------------ 3 files changed, 117 insertions(+), 111 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 63cd058c9aa..f9274468ed0 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1867,3 +1867,98 @@ double findTrapezoidalDesiredVel(double a_max, return maxnewvel; } + +EndCondition checkEndCondition(double cycleTime, + double progress, + double target, + double currentvel, + double v_f, + double a_max, + tc_term_cond_t term_cond) +{ + double dx = target - progress; + // Start with safe defaults (segment will not end next cycle + EndCondition out = { + v_f, + TP_BIG_NUM * cycleTime, + 0 + }; + + // This block essentially ignores split cycles for exact-stop moves + if (dx <= TP_POS_EPSILON) { + //If the segment is close to the target position, then we assume that it's done. + tp_debug_print("close to target, dx = %.12f\n",dx); + //Force progress to land exactly on the target to prevent numerical errors. + out.dt = 0.0; + out.v_f = currentvel; + if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { + out.remove = 1; + } + return out; + } else if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { + return out; + } + + + double v_avg = (currentvel + v_f) / 2.0; + + //Check that we have a non-zero "average" velocity between now and the + //finish. If not, it means that we have to accelerate from a stop, which + //will take longer than the minimum 2 timesteps that each segment takes, so + //we're safely far form the end. + + //Get dt assuming that we can magically reach the final velocity at + //the end of the move. + // + //KLUDGE: start with a value below the cutoff + double dt = TP_TIME_EPSILON / 2.0; + if (v_avg > TP_VEL_EPSILON) { + //Get dt from distance and velocity (avoid div by zero) + dt = fmax(dt, dx / v_avg); + } else { + if ( dx > (v_avg * cycleTime) && dx > TP_POS_EPSILON) { + tc_debug_print(" below velocity threshold, assuming far from end\n"); + return out; + } + } + + + // Assuming a full timestep: + double dv = v_f - currentvel; + double a_f = dv / dt; + + //If this is a valid acceleration, then we're done. If not, then we solve + //for v_f and dt given the max acceleration allowed. + //If we exceed the maximum acceleration, then the dt estimate is too small. + double a = a_f; + int recalc = sat_inplace(&a, a_max); + + //Need to recalculate vf and above + if (recalc) { + tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); + double disc = pmSq(currentvel / a) + 2.0 / a * dx; + if (disc < 0) { + //Should mean that dx is too big, i.e. we're not close enough + tc_debug_print(" dx = %f, too large, not at end yet\n",dx); + return out; + } + + if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { + tc_debug_print("disc too small, skipping sqrt\n"); + dt = -currentvel / a; + } else if (a > 0) { + tc_debug_print("using positive sqrt\n"); + dt = -currentvel / a + pmSqrt(disc); + } else { + tc_debug_print("using negative sqrt\n"); + dt = -currentvel / a - pmSqrt(disc); + } + + tc_debug_print(" revised dt = %f\n", dt); + //Update final velocity with actual result + out.v_f = currentvel + dt * a; + } + + out.dt = dt; + return out; +} diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index a65391b1d34..8cc35b279a6 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -274,4 +274,13 @@ typedef struct { double dt; int remove; } EndCondition; + +EndCondition checkEndCondition(double cycleTime, + double progress, + double target, + double currentvel, + double v_f, + double a_max, + tc_term_cond_t term_cond); + #endif diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index ec0877327ea..c0d9ef80d6a 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3090,103 +3090,6 @@ STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, } - - -EndCondition checkEndCondition(double cycleTime, - double progress, - double target, - double currentvel, - double v_f, - double a_max, - tc_term_cond_t term_cond) -{ - double dx = target - progress; - // Start with safe defaults (segment will not end next cycle - EndCondition out = { - v_f, - TP_BIG_NUM * cycleTime, - 0 - }; - - // This block essentially ignores split cycles for exact-stop moves - if (dx <= TP_POS_EPSILON) { - //If the segment is close to the target position, then we assume that it's done. - tp_debug_print("close to target, dx = %.12f\n",dx); - //Force progress to land exactly on the target to prevent numerical errors. - out.dt = 0.0; - out.v_f = currentvel; - if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { - out.remove = 1; - } - return out; - } else if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { - return out; - } - - - double v_avg = (currentvel + v_f) / 2.0; - - //Check that we have a non-zero "average" velocity between now and the - //finish. If not, it means that we have to accelerate from a stop, which - //will take longer than the minimum 2 timesteps that each segment takes, so - //we're safely far form the end. - - //Get dt assuming that we can magically reach the final velocity at - //the end of the move. - // - //KLUDGE: start with a value below the cutoff - double dt = TP_TIME_EPSILON / 2.0; - if (v_avg > TP_VEL_EPSILON) { - //Get dt from distance and velocity (avoid div by zero) - dt = fmax(dt, dx / v_avg); - } else { - if ( dx > (v_avg * cycleTime) && dx > TP_POS_EPSILON) { - tc_debug_print(" below velocity threshold, assuming far from end\n"); - return out; - } - } - - - // Assuming a full timestep: - double dv = v_f - currentvel; - double a_f = dv / dt; - - //If this is a valid acceleration, then we're done. If not, then we solve - //for v_f and dt given the max acceleration allowed. - //If we exceed the maximum acceleration, then the dt estimate is too small. - double a = a_f; - int recalc = sat_inplace(&a, a_max); - - //Need to recalculate vf and above - if (recalc) { - tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); - double disc = pmSq(currentvel / a) + 2.0 / a * dx; - if (disc < 0) { - //Should mean that dx is too big, i.e. we're not close enough - tc_debug_print(" dx = %f, too large, not at end yet\n",dx); - return out; - } - - if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { - tc_debug_print("disc too small, skipping sqrt\n"); - dt = -currentvel / a; - } else if (a > 0) { - tc_debug_print("using positive sqrt\n"); - dt = -currentvel / a + pmSqrt(disc); - } else { - tc_debug_print("using negative sqrt\n"); - dt = -currentvel / a - pmSqrt(disc); - } - - tc_debug_print(" revised dt = %f\n", dt); - //Update final velocity with actual result - out.v_f = currentvel + dt * a; - } - - out.dt = dt; - return out; -} - /** * Check remaining time in a segment and calculate split cycle if necessary. * This function estimates how much time we need to complete the next segment. @@ -3211,30 +3114,29 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, if (ec.remove || ec.dt < TP_TIME_EPSILON) { tc->progress = tc->target; } - + + double dt = ec.dt; + int splitting = dt < tp->cycleTime; + if (splitting) { + if (dt < TP_TIME_EPSILON) { + dt = 0.0; + } + tcSetSplitCycle(tc, dt, ec.v_f); + } #ifdef TC_DEBUG { print_json5_log_start(checkEndCondition, Run); print_json5_long_("id", tc->id); print_json5_double_("v_final", ec.v_f); - print_json5_double_("dt", ec.dt); + print_json5_double_("t_remaining", ec.dt); + print_json5_double_("dt_used", dt); print_json5_bool_("remove", ec.remove); + print_json5_bool_("splitting", splitting); print_json5_end_(); } #endif - if (ec.dt < TP_TIME_EPSILON) { - //Close enough, call it done - tc_debug_print("revised dt small, finishing tc\n"); - tc->progress = tc->target; - tcSetSplitCycle(tc, 0.0, ec.v_f); - } else if (ec.dt < tp->cycleTime ) { - tc_debug_print(" corrected v_f = %f\n", ec.v_f); - tcSetSplitCycle(tc, ec.dt, ec.v_f); - } else { - tc_debug_print(" dt = %f, not at end yet\n", ec.dt); - return TP_ERR_NO_ACTION; - } + return TP_ERR_OK; } From ce83fb64fe1672aaebd4f4ae92077d18e61a1ad4 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Feb 2019 17:06:53 -0500 Subject: [PATCH 069/129] Handle pure-angular moves consistently between canon and TP Adds an explicit parameter for pure-angular moves so that motion and canon don't accidentally come to different conclusions about the units of vel / acc. This prevents weird mismatches that are difficult to debug; each module was internally consistent, but they did not agree on what the threshold for a "pure angular" move was. Of course, the correct way to fix this is to use a more robust definition of progress (e.g. overall path length in all 9 axes). --- src/emc/motion/command.c | 9 +++++++-- src/emc/motion/motion.h | 1 + src/emc/nml_intf/emc.cc | 2 ++ src/emc/nml_intf/emc.hh | 4 ++-- src/emc/nml_intf/emc_nml.hh | 4 +++- src/emc/task/emccanon.cc | 13 +++++++++---- src/emc/task/emctaskmain.cc | 3 ++- src/emc/task/taskintf.cc | 6 ++++-- src/emc/tp/tc.c | 6 ++++-- src/emc/tp/tc.h | 2 +- src/emc/tp/tp.c | 15 ++++++++++++--- src/emc/tp/tp.h | 38 ++++++++++++++++++++++++++++++------- 12 files changed, 78 insertions(+), 25 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index b384a23cc93..ebf91582562 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -899,7 +899,8 @@ check_stuff ( "before command_handler()" ); int res_addline = tpAddLine(&emcmotDebug->tp, emcmotCommand->pos, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new, issue_atspeed, - emcmotCommand->turn); + emcmotCommand->turn, + emcmotCommand->pure_angular); //KLUDGE ignore zero length line if (res_addline < 0) { reportError(_("can't add linear move at line %d, error code %d"), @@ -1394,7 +1395,11 @@ check_stuff ( "before command_handler()" ); /* append it to the emcmotDebug->tp */ tpSetId(&emcmotDebug->tp, emcmotCommand->id); - if (-1 == tpAddLine(&emcmotDebug->tp, emcmotCommand->pos, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new, 0, -1)) { + if (-1 == tpAddLine(&emcmotDebug->tp, emcmotCommand->pos, + emcmotCommand->motion_type, emcmotCommand->vel, + emcmotCommand->ini_maxvel, emcmotCommand->acc, + emcmotStatus->enables_new, 0, -1, + emcmotCommand->pure_angular)) { reportError(_("can't add probe move")); emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; tpAbort(&emcmotDebug->tp); diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 7b19895f95c..df7e52fecc2 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -215,6 +215,7 @@ extern "C" { double vel; /* max velocity */ double ini_maxvel; /* max velocity allowed by machine constraints (the ini file) */ + int pure_angular; // Canon decided this move should be treated as an angular move (may have small XYZUVW motion anyway) int motion_type; /* this move is because of traverse, feed, arc, or toolchange */ double spindlesync; /* user units per spindle revolution, 0 = no sync */ double spindle_speed; /* commanded spindle speed */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index d6b9f22dca5..4c69db155d2 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1515,6 +1515,7 @@ void EMC_TRAJ_LINEAR_MOVE::update(CMS * cms) cms->update(acc); cms->update(feed_mode); cms->update(indexrotary); + cms->update(pure_angular); } /* @@ -2818,6 +2819,7 @@ void EMC_TRAJ_PROBE::update(CMS * cms) cms->update(vel); cms->update(ini_maxvel); cms->update(acc); + cms->update(pure_angular); cms->update(probe_type); } diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index ab18a04e9cd..aa417096d66 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -441,7 +441,7 @@ extern int emcTrajStep(); extern int emcTrajResume(); extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(EmcPose end, int type, double vel, - double ini_maxvel, double acc, int indexrotary); + double ini_maxvel, double acc, int indexrotary, int pure_angular); extern int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc); extern int emcTrajSetTermCond(int cond, double tolerance); @@ -452,7 +452,7 @@ extern int emcTrajSetRotation(double rotation); extern int emcTrajSetHome(EmcPose home); extern int emcTrajClearProbeTrippedFlag(); extern int emcTrajProbe(EmcPose pos, int type, double vel, - double ini_maxvel, double acc, unsigned char probe_type); + double ini_maxvel, double acc, int pure_angular, unsigned char probe_type); extern int emcAuxInputWait(int index, int input_type, int wait_type, int timeout); extern int emcTrajRigidTap(EmcPose pos, double vel, double ini_maxvel, double acc); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 095568b2853..142af73854b 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -489,7 +489,7 @@ class EMC_AXIS_STAT:public EMC_AXIS_STAT_MSG { class EMC_TRAJ_CMD_MSG:public RCS_CMD_MSG { public: EMC_TRAJ_CMD_MSG(NMLTYPE t, size_t s):RCS_CMD_MSG(t, s) { - }; + } // For internal NML/CMS use only. void update(CMS * cms); @@ -788,6 +788,7 @@ class EMC_TRAJ_LINEAR_MOVE:public EMC_TRAJ_CMD_MSG { double vel, ini_maxvel, acc; int feed_mode; int indexrotary; + int pure_angular; }; class EMC_TRAJ_CIRCULAR_MOVE:public EMC_TRAJ_CMD_MSG { @@ -944,6 +945,7 @@ class EMC_TRAJ_PROBE:public EMC_TRAJ_CMD_MSG { EmcPose pos; int type; double vel, ini_maxvel, acc; + int pure_angular; unsigned char probe_type; }; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index e3d66928b59..22951fc7346 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1007,6 +1007,7 @@ static void flush_segments(void) { linearMoveMsg.end.b = TO_EXT_ANG(b); linearMoveMsg.end.c = TO_EXT_ANG(c); + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = toExtVel(vel); linearMoveMsg.ini_maxvel = toExtVel(ini_maxvel); AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w); @@ -1127,6 +1128,7 @@ void STRAIGHT_TRAVERSE(int line_number, acc = accdata.acc; linearMoveMsg.end = to_ext_pose(x,y,z,a,b,c,u,v,w); + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel); linearMoveMsg.acc = toExtAcc(acc); linearMoveMsg.indexrotary = rotary_unlock_for_traverse; @@ -1151,9 +1153,6 @@ void STRAIGHT_FEED(int line_number, double a, double b, double c, double u, double v, double w) { - EMC_TRAJ_LINEAR_MOVE linearMoveMsg; - linearMoveMsg.feed_mode = feed_mode; - from_prog(x,y,z,a,b,c,u,v,w); rotate_and_offset_pos(x,y,z,a,b,c,u,v,w); see_segment(line_number, x, y, z, a, b, c, u, v, w); @@ -1230,6 +1229,7 @@ void STRAIGHT_PROBE(int line_number, probeMsg.vel = toExtVel(vel); probeMsg.ini_maxvel = toExtVel(ini_maxvel); probeMsg.acc = toExtAcc(acc); + probeMsg.pure_angular = angular_move & ! cartesian_move; probeMsg.type = EMC_MOTION_TYPE_PROBING; probeMsg.probe_type = probe_type; @@ -1548,7 +1548,6 @@ void ARC_FEED(int line_number, { EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg; - EMC_TRAJ_LINEAR_MOVE linearMoveMsg; canon_debug("line = %d\n", line_number); canon_debug("first_end = %f, second_end = %f\n", first_end,second_end); @@ -1586,6 +1585,8 @@ void ARC_FEED(int line_number, } } + + EMC_TRAJ_LINEAR_MOVE linearMoveMsg; linearMoveMsg.feed_mode = feed_mode; circularMoveMsg.feed_mode = feed_mode; flush_segments(); @@ -1865,6 +1866,8 @@ void ARC_FEED(int line_number, // or we wouldn't be calling ARC_FEED linearMoveMsg.end = to_ext_pose(endpt); linearMoveMsg.type = EMC_MOTION_TYPE_ARC; + + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = toExtVel(vel); linearMoveMsg.ini_maxvel = toExtVel(v_max); linearMoveMsg.acc = toExtAcc(a_max); @@ -2154,6 +2157,7 @@ void START_CHANGE() void CHANGE_TOOL(int slot) { EMC_TRAJ_LINEAR_MOVE linearMoveMsg; + linearMoveMsg.feed_mode = feed_mode; EMC_TOOL_LOAD load_tool_msg; @@ -2199,6 +2203,7 @@ void CHANGE_TOOL(int slot) linearMoveMsg.end = to_ext_pose(x, y, z, a, b, c, u, v, w); + linearMoveMsg.pure_angular = angular_move && !cartesian_move; linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel); linearMoveMsg.acc = toExtAcc(acc); linearMoveMsg.type = EMC_MOTION_TYPE_TOOLCHANGE; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 0f0e70f6793..51faf796146 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1817,7 +1817,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) retval = emcTrajLinearMove(emcTrajLinearMoveMsg->end, emcTrajLinearMoveMsg->type, emcTrajLinearMoveMsg->vel, emcTrajLinearMoveMsg->ini_maxvel, emcTrajLinearMoveMsg->acc, - emcTrajLinearMoveMsg->indexrotary); + emcTrajLinearMoveMsg->indexrotary, emcTrajLinearMoveMsg->pure_angular); break; case EMC_TRAJ_CIRCULAR_MOVE_TYPE: @@ -1894,6 +1894,7 @@ static int emcTaskIssueCommand(NMLmsg * cmd) ((EMC_TRAJ_PROBE *) cmd)->vel, ((EMC_TRAJ_PROBE *) cmd)->ini_maxvel, ((EMC_TRAJ_PROBE *) cmd)->acc, + ((EMC_TRAJ_PROBE *) cmd)->pure_angular, ((EMC_TRAJ_PROBE *) cmd)->probe_type); break; diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index c6781fb9b4b..9fe181baf4d 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -987,7 +987,7 @@ int emcTrajSetTermCond(int cond, double tolerance) } int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, double acc, - int indexrotary) + int indexrotary, int pure_angular) { #ifdef ISNAN_TRAP if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || @@ -1008,6 +1008,7 @@ int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, doub emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; emcmotCommand.turn = indexrotary; + emcmotCommand.pure_angular = pure_angular; return usrmotWriteEmcmotCommand(&emcmotCommand); } @@ -1056,7 +1057,7 @@ int emcTrajClearProbeTrippedFlag() return usrmotWriteEmcmotCommand(&emcmotCommand); } -int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double acc, unsigned char probe_type) +int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double acc, int pure_angular, unsigned char probe_type) { #ifdef ISNAN_TRAP if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z) || @@ -1074,6 +1075,7 @@ int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double ac emcmotCommand.vel = vel; emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; + emcmotCommand.pure_angular = pure_angular; emcmotCommand.probe_type = probe_type; return usrmotWriteEmcmotCommand(&emcmotCommand); diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 9c4fff6642d..922248ed6fe 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -568,9 +568,11 @@ int tcFlagEarlyStop(TC_STRUCT * const tc, return TP_ERR_OK; } -double pmLine9Target(PmLine9 * const line9) +double pmLine9Target(PmLine9 * const line9, int pure_angular) { - if (!line9->xyz.tmag_zero) { + if (pure_angular && !line9->abc.tmag_zero) { + return line9->abc.tmag; + } else if (!line9->xyz.tmag_zero) { return line9->xyz.tmag; } else if (!line9->uvw.tmag_zero) { return line9->uvw.tmag; diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 2dbe9729460..b08cb99d8f9 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -63,7 +63,7 @@ int pmCircleTangentVector(PmCircle const * const circle, int tcFlagEarlyStop(TC_STRUCT * const tc, TC_STRUCT * const nexttc); -double pmLine9Target(PmLine9 * const line9); +double pmLine9Target(PmLine9 * const line9, int pure_angular); int pmLine9Init(PmLine9 * const line9, EmcPose const * const start, diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index c0d9ef80d6a..93c0ab04e5f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1928,8 +1928,17 @@ STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const * end of the previous move to the new end specified here at the * currently-active accel and vel settings from the tp struct. */ -int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double vel, double - ini_maxvel, double acc, unsigned char enables, char atspeed, int indexrotary) { +int tpAddLine(TP_STRUCT * const tp, + EmcPose end, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + int indexrotary, + int pure_angular) +{ if (tpErrorCheck(tp) < 0) { return TP_ERR_FAIL; @@ -1983,7 +1992,7 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v pmLine9Init(&tc.coords.line, &tp->goalPos, &end); - tc.target = pmLine9Target(&tc.coords.line); + tc.target = pmLine9Target(&tc.coords.line, pure_angular); if (tc.target < TP_POS_EPSILON) { rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId); return TP_ERR_ZERO_LENGTH; diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index 0281b89546b..591134c84be 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -32,13 +32,37 @@ int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance); int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos); int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); -int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double - ini_maxvel, double acc, unsigned char enables); -int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double vel, double - ini_maxvel, double acc, unsigned char enables, char atspeed, int indexrotary); -int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, - PmCartesian normal, int turn, int canon_motion_type, double vel, double ini_maxvel, - double acc, unsigned char enables, char atspeed); + +int tpAddRigidTap(TP_STRUCT * const tp, + EmcPose end, + double vel, + double ini_maxvel, + double acc, + unsigned char enables); + +int tpAddLine(TP_STRUCT * const tp, + EmcPose end, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + int indexrotary, + int pure_angular); + +int tpAddCircle(TP_STRUCT * const tp, + EmcPose end, + PmCartesian center, + PmCartesian normal, + int turn, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed); + int tpRunCycle(TP_STRUCT * const tp, long period); int tpPause(TP_STRUCT * const tp); int tpResume(TP_STRUCT * const tp); From 5662ea954cef41166f35fb433930f1894d3fc828 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Feb 2019 17:09:28 -0500 Subject: [PATCH 070/129] tp: Simplify logic in split-cycle update --- src/emc/tp/blendmath.c | 10 +--------- src/emc/tp/blendmath.h | 4 +--- src/emc/tp/tp.c | 32 ++++++++++++++------------------ 3 files changed, 16 insertions(+), 30 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index f9274468ed0..873e6cb7063 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1873,15 +1873,13 @@ EndCondition checkEndCondition(double cycleTime, double target, double currentvel, double v_f, - double a_max, - tc_term_cond_t term_cond) + double a_max) { double dx = target - progress; // Start with safe defaults (segment will not end next cycle EndCondition out = { v_f, TP_BIG_NUM * cycleTime, - 0 }; // This block essentially ignores split cycles for exact-stop moves @@ -1891,15 +1889,9 @@ EndCondition checkEndCondition(double cycleTime, //Force progress to land exactly on the target to prevent numerical errors. out.dt = 0.0; out.v_f = currentvel; - if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { - out.remove = 1; - } - return out; - } else if (term_cond == TC_TERM_COND_STOP || term_cond == TC_TERM_COND_EXACT) { return out; } - double v_avg = (currentvel + v_f) / 2.0; //Check that we have a non-zero "average" velocity between now and the diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 8cc35b279a6..aab0cf7b13e 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -272,7 +272,6 @@ static inline double findVPeak(double a_t_max, double distance) typedef struct { double v_f; double dt; - int remove; } EndCondition; EndCondition checkEndCondition(double cycleTime, @@ -280,7 +279,6 @@ EndCondition checkEndCondition(double cycleTime, double target, double currentvel, double v_f, - double a_max, - tc_term_cond_t term_cond); + double a_max); #endif diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 93c0ab04e5f..fe86e057cfd 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3113,17 +3113,9 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, tc->target, tc->currentvel, tpGetRealFinalVel(tp, tc, nexttc), - tcGetTangentialMaxAccel(tc), - (tc_term_cond_t)tc->term_cond + tcGetTangentialMaxAccel(tc) ); - // Copy results into tc - tc->remove = ec.remove; - - if (ec.remove || ec.dt < TP_TIME_EPSILON) { - tc->progress = tc->target; - } - double dt = ec.dt; int splitting = dt < tp->cycleTime; if (splitting) { @@ -3139,7 +3131,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, print_json5_double_("v_final", ec.v_f); print_json5_double_("t_remaining", ec.dt); print_json5_double_("dt_used", dt); - print_json5_bool_("remove", ec.remove); + print_json5_bool_("remove", tc->remove); print_json5_bool_("splitting", splitting); print_json5_end_(); } @@ -3181,13 +3173,20 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, return TP_ERR_OK; } + // Handle various cases of updates for split cycles + // Tangent: next segment gets a partial update for the remaining cycle time + // Parabolic: next segment updates with full cycle time (since both current / next are active at the same time) + // Exact: NO motion in next segment here since current segment must stop completely switch (tc->term_cond) { case TC_TERM_COND_TANGENT: nexttc->cycle_time = tp->cycleTime - tc->cycle_time; nexttc->currentvel = tc->term_vel; - tp_debug_print("Doing tangent split\n"); - break; + // Intentional fallthrough case TC_TERM_COND_PARABOLIC: + { + TC_STRUCT *next2tc = tcqItem(&tp->queue, 2); + tpUpdateCycle(tp, nexttc, next2tc); + } break; case TC_TERM_COND_STOP: break; @@ -3199,15 +3198,12 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, tc->id); } - // Run split cycle update with remaining time in nexttc - // KLUDGE: use next cycle after nextc to prevent velocity dip (functions fail gracefully w/ NULL) - TC_STRUCT *next2tc = tcqItem(&tp->queue, 2); - - tpUpdateCycle(tp, nexttc, next2tc); // Update status for the split portion // FIXME redundant tangent check, refactor to switch - if (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT) { + if (tc->term_cond == TC_TERM_COND_STOP + || tc->term_cond == TC_TERM_COND_EXACT + || (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT)) { //Majority of time spent in current segment tpToggleDIOs(tc); tpUpdateMovementStatus(tp, tc); From ed89b6d390b8c07b3e5f8e320904df030274e25e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Feb 2019 18:59:26 -0500 Subject: [PATCH 071/129] tp: Simplify debug output and present more useful info --- src/emc/tp/blendmath.c | 14 ++--- src/emc/tp/tp.c | 57 +++++++++++-------- src/emc/tp/tp_debug.h | 7 +++ .../circular-arcs/run_all_tests.py | 0 4 files changed, 47 insertions(+), 31 deletions(-) mode change 100644 => 100755 tests/trajectory-planner/circular-arcs/run_all_tests.py diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 873e6cb7063..f8bf99decef 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1885,7 +1885,7 @@ EndCondition checkEndCondition(double cycleTime, // This block essentially ignores split cycles for exact-stop moves if (dx <= TP_POS_EPSILON) { //If the segment is close to the target position, then we assume that it's done. - tp_debug_print("close to target, dx = %.12f\n",dx); + tc_pdebug_print("close to target, dx = %.12f\n",dx); //Force progress to land exactly on the target to prevent numerical errors. out.dt = 0.0; out.v_f = currentvel; @@ -1909,7 +1909,7 @@ EndCondition checkEndCondition(double cycleTime, dt = fmax(dt, dx / v_avg); } else { if ( dx > (v_avg * cycleTime) && dx > TP_POS_EPSILON) { - tc_debug_print(" below velocity threshold, assuming far from end\n"); + tc_pdebug_print(" below velocity threshold, assuming far from end\n"); return out; } } @@ -1927,26 +1927,24 @@ EndCondition checkEndCondition(double cycleTime, //Need to recalculate vf and above if (recalc) { - tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); double disc = pmSq(currentvel / a) + 2.0 / a * dx; if (disc < 0) { //Should mean that dx is too big, i.e. we're not close enough - tc_debug_print(" dx = %f, too large, not at end yet\n",dx); + tc_pdebug_print(" dx = %f, too large, not at end yet\n",dx); return out; } if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { - tc_debug_print("disc too small, skipping sqrt\n"); + tc_pdebug_print("disc too small, skipping sqrt\n"); dt = -currentvel / a; } else if (a > 0) { - tc_debug_print("using positive sqrt\n"); + tc_pdebug_print("using positive sqrt\n"); dt = -currentvel / a + pmSqrt(disc); } else { - tc_debug_print("using negative sqrt\n"); dt = -currentvel / a - pmSqrt(disc); } - tc_debug_print(" revised dt = %f\n", dt); + tc_pdebug_print(" revised dt = %f\n", dt); //Update final velocity with actual result out.v_f = currentvel + dt * a; } diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index fe86e057cfd..6d47a0d0d70 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -70,8 +70,16 @@ STATIC double estimateParabolicBlendPerformance( STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc); +typedef enum { + UPDATE_NORMAL, + UPDATE_PARABOLIC_BLEND, + UPDATE_SPLIT +} UpdateCycleMode; + STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc); + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode); STATIC int tpRunOptimization(TP_STRUCT * const tp); @@ -1852,9 +1860,6 @@ static bool tpCreateBlendIfPossible( res_create = tpCreateArcArcBlend(tp, prev_tc, tc, blend_tc); break; case BLEND_NONE: - default: - tp_debug_print("intersection type not recognized, aborting arc\n"); - res_create = TP_ERR_FAIL; break; } @@ -2252,7 +2257,20 @@ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_de return TP_ERR_OK; } -STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc) { +static const char *cycleModeToString(UpdateCycleMode mode) +{ + switch (mode) { + case UPDATE_NORMAL: + return "normal"; + case UPDATE_PARABOLIC_BLEND: + return "parabolic_blend"; + case UPDATE_SPLIT: + return "split_cycle"; + } + return "unknown"; +} + +STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc, int accel_mode, UpdateCycleMode cycle) { #ifdef TC_DEBUG // Find maximum allowed velocity from feed and machine limits const double v_target = tpGetRealTargetVel(tp, tc); @@ -2279,6 +2297,8 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const print_json5_double_("time", tp->time_elapsed_sec); print_json5_long_("canon_type", tc->canon_motion_type); print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_string_("accel_mode",accel_mode ? "ramp" : "trapezoidal"); + print_json5_string_("cycle", cycleModeToString(cycle)); print_json5_end_(); #endif } @@ -2295,8 +2315,6 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, double * const acc, double * const vel_desired) { - tc_debug_print("using trapezoidal acceleration\n"); - // Find maximum allowed velocity from feed and machine limits double tc_target_vel = tpGetRealTargetVel(tp, tc); // Store a copy of final velocity @@ -2335,7 +2353,6 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, double * const acc, double * const vel_desired) { - tc_debug_print("using ramped acceleration\n"); // displacement remaining in this segment double dx = tc->target - tc->progress; @@ -2391,7 +2408,6 @@ void tpToggleDIOs(TC_STRUCT * const tc) { tc->syncdio.anychanged = 0; //we have turned them all on/off, nothing else to do for this TC the next time } } - /** * Compute the spindle displacement used for spindle position tracking. * @@ -2574,7 +2590,7 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, nexttc->target_vel = 0.0; } - tpUpdateCycle(tp, nexttc, NULL); + tpUpdateCycle(tp, nexttc, NULL, UPDATE_PARABOLIC_BLEND); //Restore the original target velocity nexttc->target_vel = save_vel; } @@ -2981,7 +2997,6 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { - tc_debug_print("in DoParabolicBlend\n"); tpUpdateBlend(tp,tc,nexttc); /* Status updates */ @@ -3009,8 +3024,10 @@ STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, * Handles the majority of updates on a single segment for the current cycle. */ STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { - + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode) +{ //placeholders for position for this update EmcPose before; @@ -3037,8 +3054,9 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); } + int accel_mode_ramp = (res_accel == TP_ERR_OK); tcUpdateDistFromAccel(tc, acc, vel_desired); - tpDebugCycleInfo(tp, tc, nexttc, acc); + tpDebugCycleInfo(tp, tc, nexttc, acc, accel_mode_ramp, cycle_mode); //Check if we're near the end of the cycle and set appropriate changes tpCheckEndCondition(tp, tc, nexttc); @@ -3052,12 +3070,6 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, //Store displacement (checking for valid pose) int res_set = tpAddCurrentPos(tp, &displacement); -#ifdef TC_DEBUG - double mag; - emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n", mag); -#endif - return res_set; } @@ -3185,7 +3197,7 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, case TC_TERM_COND_PARABOLIC: { TC_STRUCT *next2tc = tcqItem(&tp->queue, 2); - tpUpdateCycle(tp, nexttc, next2tc); + tpUpdateCycle(tp, nexttc, next2tc, UPDATE_SPLIT); } break; case TC_TERM_COND_STOP: @@ -3224,9 +3236,8 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, return TP_ERR_NO_ACTION; } //Run with full cycle time - tc_debug_print("Normal cycle\n"); tc->cycle_time = tp->cycleTime; - tpUpdateCycle(tp, tc, nexttc); + tpUpdateCycle(tp, tc, nexttc, UPDATE_NORMAL); /* Parabolic blending */ diff --git a/src/emc/tp/tp_debug.h b/src/emc/tp/tp_debug.h index ceda010aabc..9f63a19ffad 100644 --- a/src/emc/tp/tp_debug.h +++ b/src/emc/tp/tp_debug.h @@ -67,6 +67,13 @@ #define tc_debug_print(...) #endif +/** "TC" debug info for inspecting trajectory planner output at each timestep */ +#ifdef TP_PEDANTIC_DEBUG +#define tc_pdebug_print(...) rtapi_print(__VA_ARGS__) +#else +#define tc_pdebug_print(...) +#endif + /** TP position data output to debug acceleration spikes */ #ifdef TP_POSEMATH_DEBUG #define tp_posemath_debug(...) rtapi_print(__VA_ARGS__) diff --git a/tests/trajectory-planner/circular-arcs/run_all_tests.py b/tests/trajectory-planner/circular-arcs/run_all_tests.py old mode 100644 new mode 100755 From 5796a121a98f1bdf4fc21a2d655061a0e6c86786 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 6 Feb 2019 13:08:43 -0500 Subject: [PATCH 072/129] tp: Factor out accel scale calculation Depending on the active state in the TP, the actual acceleration is scaled down from the computed maximum from canon (e.g. for parabolic blending, circle normal acceleration, almost-tangent intersections). This little refactor isolates that logic to make the scale factor reusable. --- src/emc/tp/tc.c | 12 ++++++++---- src/emc/tp/tc.h | 1 + 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 922248ed6fe..bb3c5020b86 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -52,7 +52,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, return fmin(v_max_target, tc->maxvel); } -double tcGetOverallMaxAccel(const TC_STRUCT *tc) +double tcGetAccelScale(const TC_STRUCT *tc) { // Handle any acceleration reduction due to an approximate-tangent "blend" with the previous or next segment double a_scale = (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); @@ -64,7 +64,11 @@ double tcGetOverallMaxAccel(const TC_STRUCT *tc) a_scale *= 0.5; } - return tc->maxaccel * a_scale; + return a_scale; +} +double tcGetOverallMaxAccel(const TC_STRUCT *tc) +{ + return tc->maxaccel * tcGetAccelScale(tc); } /** @@ -72,7 +76,7 @@ double tcGetOverallMaxAccel(const TC_STRUCT *tc) */ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) { - double a_scale = tcGetOverallMaxAccel(tc); + double a_scale = tcGetAccelScale(tc); // Reduce allowed tangential acceleration in circular motions to stay // within overall limits (accounts for centripetal acceleration while @@ -81,7 +85,7 @@ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) //Limit acceleration for cirular arcs to allow for normal acceleration a_scale *= tc->acc_ratio_tan; } - return a_scale; + return tc->maxaccel * a_scale; } diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index b08cb99d8f9..8d6b1bc4b34 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -25,6 +25,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale); +double tcGetAccelScale(TC_STRUCT const * tc); double tcGetOverallMaxAccel(TC_STRUCT const * tc); double tcGetTangentialMaxAccel(TC_STRUCT const * const tc); From 960b5d7d18d35ab4ed1585f99c9d3f212e323495 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 6 Feb 2019 13:12:37 -0500 Subject: [PATCH 073/129] tp: Extend JSON5 debug output to include some spindle-sync information --- src/emc/tp/tc.c | 14 +++++++ src/emc/tp/tc.h | 1 + src/emc/tp/tc_types.h | 8 ++-- src/emc/tp/tp.c | 94 +++++++++++++++++++++++++++---------------- src/emc/tp/tp_types.h | 3 +- 5 files changed, 82 insertions(+), 38 deletions(-) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index bb3c5020b86..b8ac08aabda 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -940,3 +940,17 @@ const char *tcMotionTypeAsString(tc_motion_type_t c) } return "NONE"; } + +const char *tcSyncModeAsString(tc_spindle_sync_t c) +{ + switch (c) + { + case TC_SYNC_NONE: + return "sync_none"; + case TC_SYNC_VELOCITY: + return "sync_velocity"; + case TC_SYNC_POSITION: + return "sync_position"; + } + return "NONE"; +} diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 8d6b1bc4b34..48eb5e9be07 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -115,5 +115,6 @@ int tcClearFlags(TC_STRUCT * const tc); const char *tcTermCondAsString(tc_term_cond_t c); const char *tcMotionTypeAsString(tc_motion_type_t c); +const char *tcSyncModeAsString(tc_spindle_sync_t c); #endif /* TC_H */ diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 8e417d397bf..a49537f7e1d 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -49,8 +49,10 @@ typedef enum { #define TC_OPTIM_UNTOUCHED 0 #define TC_OPTIM_AT_MAX 1 -#define TC_ACCEL_TRAPZ 0 -#define TC_ACCEL_RAMP 1 +typedef enum { + TC_ACCEL_TRAPZ, + TC_ACCEL_RAMP, +} TCAccelMode; /** * Spiral arc length approximation by quadratic fit. @@ -159,7 +161,7 @@ typedef struct { double blend_vel; // velocity below which we should start blending double tolerance; // during the blend at the end of this move, // stay within this distance from the path. - int synchronized; // spindle sync state + tc_spindle_sync_t synchronized; // spindle sync state double uu_per_rev; // for sync, user units per rev (e.g. 0.0625 for 16tpi) double vel_at_blend_start; int sync_accel; // we're accelerating up to sync with the spindle diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 6d47a0d0d70..2798a64b529 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1362,10 +1362,11 @@ STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc tp->depth = tcqLen(&tp->queue); //Fixing issue with duplicate id's? #ifdef TP_DEBUG - print_json5_log_start(tpAddSegmentToQueue, Command); + print_json5_log_start(Enqueue, Command); print_json5_long_("id", tc->id); print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); print_json5_double_("target", tc->target); + print_json5_string_("term_cond", tcTermCondAsString(tc->term_cond)); print_json5_end_(); #endif @@ -2257,7 +2258,7 @@ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_de return TP_ERR_OK; } -static const char *cycleModeToString(UpdateCycleMode mode) +const char *cycleModeToString(UpdateCycleMode mode) { switch (mode) { case UPDATE_NORMAL: @@ -2280,6 +2281,7 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const /* Debug Output */ print_json5_log_start(tc_state, Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); print_json5_double_("id", tc->id); print_json5_double(v_target); print_json5_double(v_final); @@ -2292,9 +2294,8 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const print_json5_double_("dt", tc->cycle_time); print_json5_string_("term_cond", tcTermCondAsString((tc_term_cond_t)tc->term_cond)); print_json5_bool_("final_decel", tc->on_final_decel); - print_json5_bool_("splitting", tc->splitting); + print_json5_bool_("need_split", tc->splitting); print_json5_bool_("is_blending", tc->is_blending); - print_json5_double_("time", tp->time_elapsed_sec); print_json5_long_("canon_type", tc->canon_motion_type); print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); print_json5_string_("accel_mode",accel_mode ? "ramp" : "trapezoidal"); @@ -2452,9 +2453,9 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, static double old_spindle_pos = 0.0; double spindle_pos = emcmotStatus->spindle_fb.position_rev; - switch (tc->coords.rigidtap.state) { + RIGIDTAP_STATE const initial_state = tc->coords.rigidtap.state; + switch (initial_state) { case TAPPING: - tc_debug_print("TAPPING\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { // command reversal cmdReverseSpindle(); @@ -2462,7 +2463,6 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, } break; case REVERSING: - tc_debug_print("REVERSING\n"); if (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; @@ -2483,10 +2483,8 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, tc->coords.rigidtap.state = RETRACTION; } - tc_debug_print("Spindlepos = %f\n", spindle_pos); break; case RETRACTION: - tc_debug_print("RETRACTION\n"); if (tc->progress >= tc->coords.rigidtap.reversal_target) { // Flip spindle direction again to start final reversal cmdReverseSpindle(); @@ -2494,7 +2492,6 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, } break; case FINAL_REVERSAL: - tc_debug_print("FINAL_REVERSAL\n"); if (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { PmCartesian start, end; PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; @@ -2513,11 +2510,16 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT * const tp, } break; case FINAL_PLACEMENT: - tc_debug_print("FINAL_PLACEMENT\n"); // this is a regular move now, it'll stop at target above. break; } old_spindle_pos = spindle_pos; +#ifdef TC_DEBUG + RIGIDTAP_STATE current_state = tc->coords.rigidtap.state; + print_json5_log_start(tpUpdateRigidTapState, Run); + print_json5_unsigned(current_state); + print_json5_log_end(); +#endif } @@ -2764,9 +2766,11 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_OK; } +#ifdef TP_PEDANTIC if (!tp) { return TP_ERR_MISSING_INPUT; } +#endif /* Based on the INI setting for "cutoff frequency", this calculation finds * short segments that can have their acceleration be simple ramps, instead @@ -2779,14 +2783,11 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { // Given what velocities we can actually reach, estimate the total time for the segment under ramp conditions double segment_time = 2.0 * length / (tc->currentvel + fmin(tc->finalvel,tpGetRealTargetVel(tp,tc))); - if (segment_time < cutoff_time && tc->canon_motion_type != EMC_MOTION_TYPE_TRAVERSE && tc->term_cond == TC_TERM_COND_TANGENT && tc->motion_type != TC_RIGIDTAP) { - tp_debug_print("segment_time = %f, cutoff_time = %f, ramping\n", - segment_time, cutoff_time); tc->accel_mode = TC_ACCEL_RAMP; } @@ -2808,23 +2809,16 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { return TP_ERR_WAITING; } - 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->maxaccel, - tc->term_cond); - tc->active = 1; //Do not change initial velocity here, since tangent blending already sets this up tp->motionType = tc->canon_motion_type; tc->blending_next = 0; tc->on_final_decel = 0; + tp_err_t res = TP_ERR_OK; + if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindle_fb.synced)) { - 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 @@ -2833,10 +2827,31 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp->spindle.origin.position = 0.0; tp->spindle.origin.direction = signum(emcmotStatus->spindle_cmd.velocity_rpm_out); rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync...\n"); - return TP_ERR_WAITING; + res = TP_ERR_WAITING; } - return TP_ERR_OK; +#ifdef TP_DEBUG + print_json5_log_start(ActivateSegment,Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_int_("id", tc->id); + // Position settings + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + // Velocity settings + print_json5_double_("reqvel", tc->reqvel); + print_json5_double_("target_vel", tc->target_vel); + print_json5_double_("finalvel", tc->finalvel); + // Acceleration settings + print_json5_double_("accel_scale", tcGetAccelScale(tc)); + print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); + print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); + print_json5_bool_("accel_ramp", tc->accel_mode); + print_json5_bool_("blend_prev", tc->blend_prev); + print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); + print_json5_log_end(); +#endif + + return res; } @@ -3095,7 +3110,7 @@ STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) { STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, double v_f) { - tp_debug_print("split time for id %d is %.16g\n", tc->id, split_time); + tc_pdebug_print("split time for id %d is %.16g\n", tc->id, split_time); if (tc->splitting != 0 && split_time > 0.0) { rtapi_print_msg(RTAPI_MSG_ERR,"already splitting on id %d with cycle time %.16g, dx = %.16g, split time %.12g\n", tc->id, @@ -3144,7 +3159,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, print_json5_double_("t_remaining", ec.dt); print_json5_double_("dt_used", dt); print_json5_bool_("remove", tc->remove); - print_json5_bool_("splitting", splitting); + print_json5_bool_("need_split", splitting); print_json5_end_(); } #endif @@ -3166,7 +3181,18 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, EmcPose before; tcGetPos(tc, &before); - tp_debug_print("tc id %d splitting\n",tc->id); +#ifdef TC_DEBUG + /* Debug Output */ + print_json5_log_start(tc_splitting, Run); + print_json5_double_("id", tc->id); + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + print_json5_double_("v_terminal", tc->term_vel); + print_json5_double_("dt", tc->cycle_time); + print_json5_end_(); +#endif + + //Shortcut tc update by assuming we arrive at end tc->progress = tc->target; //Get displacement from prev. position @@ -3297,8 +3323,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) return TP_ERR_WAITING; } - tc_debug_print("--- TP Update <%lld> ---\n", tp->time_elapsed_ticks); - /* If the queue empties enough, assume that the program is near the end. * This forces the last segment to be "finalized" to let the optimizer run.*/ /*tpHandleLowQueue(tp);*/ @@ -3318,6 +3342,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period) int res = tpActivateSegment(tp, tc); // Need to wait to continue motion, end planning here if (res == TP_ERR_WAITING) { + tp->time_at_wait = tp->time_elapsed_ticks; return TP_ERR_WAITING; } } @@ -3345,7 +3370,7 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpSyncPositionMode(tp, tc, nexttc); break; default: - tc_debug_print("unrecognized spindle sync state!\n"); + rtapi_print_msg(RTAPI_MSG_WARN, "Unrecognized spindle sync state %d, no sync applied\n", tc->synchronized); break; } @@ -3390,7 +3415,8 @@ int tpRunCycle(TP_STRUCT * const tp, long period) rtapi_print_msg(RTAPI_MSG_ERR, "Acceleration violation on axes [%s] at %g sec\n", failed_axes_list, tp->time_elapsed_sec); print_json5_start_(); print_json5_object_start_("accel_violation"); - print_json5_double_("time", tp->time_elapsed_sec); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_int_("id", tc->id); print_json5_EmcPose(axis_accel); print_json5_object_end_(); print_json5_end_(); @@ -3398,13 +3424,13 @@ int tpRunCycle(TP_STRUCT * const tp, long period) #ifdef TC_DEBUG print_json5_log_start(tpRunCycle, Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); print_json5_EmcPose(axis_pos); print_json5_EmcPose(axis_vel); print_json5_EmcPose(axis_accel); double current_vel = emcmotStatus->current_vel; print_json5_double(current_vel); print_json5_double_("time", tp->time_elapsed_sec); - print_json5_ll_("time_ticks", tp->time_elapsed_ticks); print_json5_end_(); #endif } diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 7ec85a78765..5e1aa0e47f6 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -119,7 +119,7 @@ typedef struct { double tolerance; /* for subsequent motions, stay within this distance of the programmed path during blends */ - int synchronized; // spindle sync required for this move + tc_spindle_sync_t synchronized; // spindle sync required for this move int velocity_mode; /* TRUE if spindle sync is in velocity mode, FALSE if in position mode */ double uu_per_rev; /* user units per spindle revolution */ @@ -129,6 +129,7 @@ typedef struct { double time_elapsed_sec; // Total elapsed TP run time in seconds long long time_elapsed_ticks; // Total elapsed TP run time in cycles (ticks) + long long time_at_wait; // Time when TP started to wait for spindle } TP_STRUCT; From b05d66605722cab4c1f8cf29dd76de8486dc4a23 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 4 Feb 2019 21:55:09 -0500 Subject: [PATCH 074/129] tp: Fix effective radius curvature formula. Accidentally used the full helix height instead of dh/dtheta in the curvature formula. --- src/emc/tp/blendmath.c | 48 +++++++++++++++++++++++----------- src/emc/tp/blendmath.h | 1 + src/emc/tp/tc.c | 6 ++--- src/emc/tp/tc_types.h | 1 + src/emc/tp/tp.c | 3 +++ src/libnml/posemath/posemath.h | 1 + 6 files changed, 41 insertions(+), 19 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index f8bf99decef..648739fbdc2 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1625,7 +1625,9 @@ PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, { double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + // For debugging only double eff_radius = pmCircleEffectiveMinRadius(circle); + // Find the acceleration necessary to reach the maximum velocity double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ); // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio @@ -1640,14 +1642,21 @@ PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); } - tp_debug_json_log_start(pmCircleActualMaxVel, Check); - tp_debug_json_double(eff_radius); - tp_debug_json_double(v_max); - tp_debug_json_double(v_max_cutoff); - tp_debug_json_double(a_n_max_cutoff); - tp_debug_json_double(a_n_vmax); - tp_debug_json_double(acc_ratio_tan); - tp_debug_json_log_end(); +#ifdef TP_DEBUG + print_json5_log_start(pmCircleActualMaxVel, Check); + print_json5_double_("angle", circle->angle); + print_json5_double_("nominal_radius", circle->radius); + double r_spiral = spiralEffectiveRadius(circle); + print_json5_double_("spiral_radius", r_spiral); + print_json5_double(eff_radius); + print_json5_double(v_max); + print_json5_double(v_max_cutoff); + print_json5_double(a_max); + print_json5_double(a_n_max_cutoff); + print_json5_double(a_n_vmax); + print_json5_double(acc_ratio_tan); + print_json5_log_end(); +#endif PmCircleLimits limits = { v_max_actual, @@ -1818,6 +1827,19 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, return pmCircleAngleFromParam(circle, fit, t, angle); } +double spiralEffectiveRadius(PmCircle const * circle) +{ + double dr = circle->spiral / circle->angle; + + // Exact representation of spiral arc length flattened into + double n_inner = pmSq(dr) + pmSq(circle->radius); + double den = n_inner+pmSq(dr); + double num = pmSqrt(pmCb(n_inner)); + double r_spiral = num / den; + + return r_spiral; +} + /** * Find the effective minimum radius for acceleration calculations. @@ -1826,18 +1848,14 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, */ double pmCircleEffectiveMinRadius(PmCircle const * circle) { - double dr = circle->spiral / circle->angle; double h2; pmCartMagSq(&circle->rHelix, &h2); + double dh2 = h2 / pmSq(circle->angle); - // Exact representation of spiral arc length flattened into - double n_inner = pmSq(dr) + pmSq(circle->radius); - double den = n_inner+pmSq(dr); - double num = pmSqrt(n_inner * n_inner * n_inner); - double r_spiral = num / den; + double r_spiral = spiralEffectiveRadius(circle); // Curvature of helix, assuming that helical motion is independent of plane motion - double effective_radius = h2 / r_spiral + r_spiral; + double effective_radius = dh2 / r_spiral + r_spiral; return effective_radius; } diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index aab0cf7b13e..7b3d6ca5a60 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -262,6 +262,7 @@ int pmCircleAngleFromProgress(PmCircle const * const circle, double progress, double * const angle); double pmCircleEffectiveMinRadius(const PmCircle *circle); +double spiralEffectiveRadius(PmCircle const * circle); static inline double findVPeak(double a_t_max, double distance) { diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index b8ac08aabda..aeead876f11 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -31,7 +31,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale) { - double v_max_target; + double v_max_target = tc->maxvel; switch (tc->synchronized) { case TC_SYNC_NONE: @@ -43,8 +43,6 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, max_scale = 1.0; case TC_SYNC_POSITION: // Assume no spindle override during blend target - default: - v_max_target = tc->maxvel; break; } @@ -734,7 +732,7 @@ int tcUpdateCircleAccRatio(TC_STRUCT * tc) if (tc->motion_type == TC_CIRCULAR) { PmCircleLimits limits = pmCircleActualMaxVel(&tc->coords.circle.xyz, tc->maxvel, - tcGetOverallMaxAccel(tc)); + tc->acc_normal_max * tcGetAccelScale(tc)); tc->maxvel = limits.v_max; tc->acc_ratio_tan = limits.acc_ratio; return 0; diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index a49537f7e1d..3cf94ea7e67 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -139,6 +139,7 @@ typedef struct { //Acceleration double maxaccel; // accel calc'd by task double acc_ratio_tan;// ratio between normal and tangential accel + double acc_normal_max; // Max acceleration allowed in normal direction (worst-case for the whole curve) int id; // segment's serial number diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 2798a64b529..28c3e51c84c 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2088,6 +2088,9 @@ int tpAddCircle(TP_STRUCT * const tp, if (res_init) return res_init; + PmCartesian acc_bound = getXYZAccelBounds(); + calculateInscribedDiameter(&normal, &acc_bound, &tc.acc_normal_max); + // Update tc target with existing circular segment tc.target = pmCircle9Target(&tc.coords.circle); if (tc.target < TP_POS_EPSILON) { diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index f64448f416e..610a7d761fd 100644 --- a/src/libnml/posemath/posemath.h +++ b/src/libnml/posemath/posemath.h @@ -686,6 +686,7 @@ extern "C" { #define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0) #define pmSq(x) ((x)*(x)) +#define pmCb(x) ((x)*(x)*(x)) #ifdef TO_DEG #undef TO_DEG From 21def12ab32aa90c8ece9cc223dc4705e7e566ed Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 4 Feb 2019 22:03:48 -0500 Subject: [PATCH 075/129] canon: Fix a mistaken unit conversion of normal axis unit vector in circular move command This has no practical effect since the vector is re-normalized in the TP anyway, but it make the debug output less surprising, and may prevent future bugs. --- 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 22951fc7346..25bd1a23bd8 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1881,7 +1881,7 @@ void ARC_FEED(int line_number, // Convert internal center and normal to external units circularMoveMsg.center = to_ext_len(center_cart); - circularMoveMsg.normal = to_ext_len(normal_cart); + circularMoveMsg.normal = normal_cart; if (rotation > 0) circularMoveMsg.turn = rotation - 1; From f0e15980e921a95a2b5661c3e555575fe880237f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 4 Feb 2019 22:13:09 -0500 Subject: [PATCH 076/129] tp: Use already-calculated max normal acceleration from canon for optimal circle max velocity in TP This is a small API change to the TP to pass in some additional calculations that canon has already done. Basically, the TP needs to know more about what the acceleration limits are so it can scale down the maximum value correctly. For high-slope helices, the effective radius is quite high, so the normal acceleration has a much smaller effect. Previously, the TP had no easy way of knowing how much of the acceleration limit was due to the helical axis limit, and how much was from the axes in circular motion. --- src/emc/motion/command.c | 9 +++++---- src/emc/motion/motion.h | 3 ++- src/emc/nml_intf/emc.cc | 1 + src/emc/nml_intf/emc.hh | 2 +- src/emc/nml_intf/emc_nml.hh | 2 +- src/emc/task/emccanon.cc | 5 +++-- src/emc/task/emctaskmain.cc | 3 ++- src/emc/task/taskintf.cc | 8 +++++--- src/emc/tp/tp.c | 5 +++-- src/emc/tp/tp.h | 1 + 10 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index ebf91582562..2cdfceae9bd 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -954,10 +954,11 @@ check_stuff ( "before command_handler()" ); /* append it to the emcmotDebug->tp */ tpSetId(&emcmotDebug->tp, emcmotCommand->id); int res_addcircle = tpAddCircle(&emcmotDebug->tp, emcmotCommand->pos, - emcmotCommand->center, emcmotCommand->normal, - emcmotCommand->turn, emcmotCommand->motion_type, - emcmotCommand->vel, emcmotCommand->ini_maxvel, - emcmotCommand->acc, emcmotStatus->enables_new, issue_atspeed); + emcmotCommand->center, emcmotCommand->normal, + emcmotCommand->turn, emcmotCommand->motion_type, + emcmotCommand->vel, emcmotCommand->ini_maxvel, + emcmotCommand->acc, emcmotCommand->acc_normal, + emcmotStatus->enables_new, issue_atspeed); if (res_addcircle < 0) { reportError(_("can't add circular move at line %d, error code %d"), emcmotCommand->id, res_addcircle); diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index df7e52fecc2..bff0816a831 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -222,7 +222,8 @@ extern "C" { double css_factor; // Only used during CSS mode double css_xoffset; // Only used during CSS mode double acc; /* max acceleration */ - double backlash; /* amount of backlash */ + double acc_normal; /* max normal acceleration (circular moves only)*/ + double backlash; /* amount of backlash */ int id; /* id for motion */ int termCond; /* termination condition */ double tolerance; /* tolerance for path deviation in CONTINUOUS mode */ diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index 4c69db155d2..d0c841507ff 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1537,6 +1537,7 @@ void EMC_TRAJ_CIRCULAR_MOVE::update(CMS * cms) cms->update(vel); cms->update(ini_maxvel); cms->update(acc); + cms->update(acc_normal); cms->update(feed_mode); } diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index aa417096d66..385999799b2 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -443,7 +443,7 @@ extern int emcTrajDelay(double delay); extern int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, double acc, int indexrotary, int pure_angular); extern int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, PM_CARTESIAN - normal, int turn, int type, double vel, double ini_maxvel, double acc); + normal, int turn, int type, double vel, double ini_maxvel, double acc, double acc_normal); extern int emcTrajSetTermCond(int cond, double tolerance); extern int emcTrajSetSpindleSync(double feed_per_revolution, bool wait_for_index); extern int emcTrajSetOffset(EmcPose tool_offset); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index 142af73854b..72c88eca816 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -806,7 +806,7 @@ class EMC_TRAJ_CIRCULAR_MOVE:public EMC_TRAJ_CMD_MSG { PM_CARTESIAN normal; int turn; int type; - double vel, ini_maxvel, acc; + double vel, ini_maxvel, acc, acc_normal; int feed_mode; }; diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 25bd1a23bd8..22b353a0932 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1792,11 +1792,11 @@ void ARC_FEED(int line_number, } //FIXME allow tangential acceleration like in TP - double a_max_normal = a_max_axes * sqrt(3.0)/2.0; + double a_normal_budget = a_max_axes * sqrt(3.0)/2.0; canon_debug("a_max_axes = %f\n", a_max_axes); // Compute the centripetal acceleration - double v_max_radial = sqrt(a_max_normal * effective_radius); + double v_max_radial = sqrt(a_normal_budget * effective_radius); canon_debug("v_max_radial = %f\n", v_max_radial); // Restrict our maximum velocity in-plane if need be @@ -1894,6 +1894,7 @@ void ARC_FEED(int line_number, circularMoveMsg.vel = toExtVel(vel); circularMoveMsg.ini_maxvel = toExtVel(v_max); circularMoveMsg.acc = toExtAcc(a_max); + circularMoveMsg.acc_normal = toExtAcc(a_max_axes); //FIXME what happens if accel or vel is zero? // The end point is still updated, but nothing is added to the interp list diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 51faf796146..9062aada703 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1827,7 +1827,8 @@ static int emcTaskIssueCommand(NMLmsg * cmd) emcTrajCircularMoveMsg->turn, emcTrajCircularMoveMsg->type, emcTrajCircularMoveMsg->vel, emcTrajCircularMoveMsg->ini_maxvel, - emcTrajCircularMoveMsg->acc); + emcTrajCircularMoveMsg->acc, + emcTrajCircularMoveMsg->acc_normal); break; case EMC_TRAJ_PAUSE_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 9fe181baf4d..0f50242ff5a 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1014,15 +1014,16 @@ int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, doub } int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, - PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc) + PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc, double acc_normal) { #ifdef ISNAN_TRAP if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) || std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) || std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w) || std::isnan(center.x) || std::isnan(center.y) || std::isnan(center.z) || - std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z)) { - printf("std::isnan error in emcTrajCircularMove()\n"); + std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z) || + std::isnan(acc_normal)) { + printf("std::isnan error in emcTrajCircularMove()\n"); return 0; // ignore it for now, just don't send it } #endif @@ -1046,6 +1047,7 @@ int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center, emcmotCommand.vel = vel; emcmotCommand.ini_maxvel = ini_maxvel; emcmotCommand.acc = acc; + emcmotCommand.acc_normal = acc_normal; return usrmotWriteEmcmotCommand(&emcmotCommand); } diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 28c3e51c84c..d5f3bba7bd1 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2032,6 +2032,7 @@ int tpAddCircle(TP_STRUCT * const tp, double vel, double ini_maxvel, double acc, + double acc_normal, unsigned char enables, char atspeed) { @@ -2053,6 +2054,7 @@ int tpAddCircle(TP_STRUCT * const tp, tp_debug_json_double(vel); tp_debug_json_double(ini_maxvel); tp_debug_json_double(acc); + tp_debug_json_double(acc_normal); tp_debug_json_unsigned(enables); tp_debug_json_long(atspeed); tp_debug_json_long(canon_motion_type); @@ -2088,8 +2090,7 @@ int tpAddCircle(TP_STRUCT * const tp, if (res_init) return res_init; - PmCartesian acc_bound = getXYZAccelBounds(); - calculateInscribedDiameter(&normal, &acc_bound, &tc.acc_normal_max); + tc.acc_normal_max = acc_normal; // Update tc target with existing circular segment tc.target = pmCircle9Target(&tc.coords.circle); diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index 591134c84be..a17bb5ab544 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -60,6 +60,7 @@ int tpAddCircle(TP_STRUCT * const tp, double vel, double ini_maxvel, double acc, + double acc_normal, unsigned char enables, char atspeed); From fb96f533f52c79e3e73959849ad02d82967bc7d4 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 6 Feb 2019 13:14:37 -0500 Subject: [PATCH 077/129] tp: convert more debug outputs to JSON5 format --- src/emc/tp/tp.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index d5f3bba7bd1..799b055ebd6 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1952,11 +1952,9 @@ int tpAddLine(TP_STRUCT * const tp, #ifdef TP_DEBUG { // macros use the variable name, need a plain name to please the JSON5 parser - long nextId = tp->nextId; - EmcPose goal = tp->goalPos; tp_debug_json_log_start(tpAddLine, Command); - tp_debug_json_long(nextId); - tp_debug_json_EmcPose(goal); + print_json5_long_("id", tp->nextId); + print_json5_EmcPose_("goal", tp->goalPos); tp_debug_json_EmcPose(end); tp_debug_json_double(vel); tp_debug_json_double(ini_maxvel); @@ -2097,7 +2095,6 @@ int tpAddCircle(TP_STRUCT * const tp, if (tc.target < TP_POS_EPSILON) { return TP_ERR_ZERO_LENGTH; } - tp_debug_print("tc.target = %f\n",tc.target); tc.nominal_length = tc.target; // Copy in motion parameters From 7c84c0bbed9c1cde9d8ed168c475b3675a579fe6 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 6 Feb 2019 13:15:50 -0500 Subject: [PATCH 078/129] tp: don't try to compute blend velocity for segments that don't parabolic blend --- src/emc/tp/tp.c | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 799b055ebd6..99a99366aec 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2162,10 +2162,15 @@ STATIC int tpComputeBlendVelocity( double theta; - PmCartesian v1, v2; + PmCartesian v1={0}, v2={0}; - tcGetEndAccelUnitVector(tc, &v1); - tcGetStartAccelUnitVector(nexttc, &v2); + int res1 = tcGetEndAccelUnitVector(tc, &v1); + int res2 = tcGetStartAccelUnitVector(nexttc, &v2); + + if (res1 || res2) { + rtapi_print_msg(RTAPI_MSG_WARN, "Got bad unit vectors in parabolic blend velocity calculation\n"); + return TP_ERR_FAIL; + } findIntersectionAngle(&v1, &v2, &theta); double cos_theta = cos(theta); @@ -3258,6 +3263,9 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + if (!tc) { + return TP_ERR_FAIL; + } if (tc->remove) { //Don't need to update since this segment is flagged for removal return TP_ERR_NO_ACTION; @@ -3274,10 +3282,12 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, double target_vel_this = tpGetRealTargetVel(tp, tc); double target_vel_next = tpGetRealTargetVel(tp, nexttc); - tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); - tc->blend_vel = v_this; - if (nexttc) { - nexttc->blend_vel = v_next; + if (tc->term_cond == TC_TERM_COND_PARABOLIC) { + tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); + tc->blend_vel = v_this; + if (nexttc) { + nexttc->blend_vel = v_next; + } } if (nexttc && tcIsBlending(tc)) { From 25ea5d9e5d7d7415c61592f7d0d0f6a9442ebf0c Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Feb 2019 08:22:15 -0500 Subject: [PATCH 079/129] tests: Add XYZ velocity plot to log parser --- tests/trajectory-planner/circular-arcs/parse_tp_logs.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/trajectory-planner/circular-arcs/parse_tp_logs.py b/tests/trajectory-planner/circular-arcs/parse_tp_logs.py index 904c71f5deb..d49158b7d00 100755 --- a/tests/trajectory-planner/circular-arcs/parse_tp_logs.py +++ b/tests/trajectory-planner/circular-arcs/parse_tp_logs.py @@ -54,13 +54,18 @@ plt.figure(2) plt.plot(times * 1000.0, axis_vel[:, idx]) plt.grid(True) - plt.title('{} Axis velocity vs time, ms'.format(plt_axis)) + plt.title('{} velocity vs time, ms'.format(plt_axis)) plt.figure(3) plt.plot(times * 1000.0, axis_accel[:, idx]) plt.grid(True) plt.title('{} Axis acceleration vs time, ms'.format(plt_axis)) + plt.figure(4) + plt.plot(times * 1000.0, np.sqrt(np.sum(np.square(axis_vel[:, 0:2]), 1))) + plt.grid(True) + plt.title('XYZ velocity vs time, ms') + plt.show() From 075d757dc867cb946b639b009acacd04efd7a616 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Feb 2019 10:29:22 -0500 Subject: [PATCH 080/129] test: Add some TP test cases for optimal circle velocity / acceleration * show blend arcs between lines introduce no geometry errors * show improvements due to optimal circle acc ratio calculation --- .../nc_files/XYtests/51MeanderAve.ngc | 2 +- .../arc-feed/optimal_circle_acc_ratio.ngc | 20 +++++++++++++++++++ .../blend_arc_simple_pos_test.ngc | 12 +++++++++++ 3 files changed, 33 insertions(+), 1 deletion(-) create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/violation_checks/blend_arc_simple_pos_test.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc b/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc index 6d16d6ac067..92fd7d3bf14 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc @@ -8,7 +8,7 @@ G64 P0.01 q00.01 (Part: 51 Meander Ave) (Operation: Outside Offset, LAYER_1, T210: Stainless 2.0mm) G00 -X21.000 Y20.000 +X21.000 Y20.000 Z0 M01 M03 G04 P0.6 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc new file mode 100644 index 00000000000..a11dc9a8ead --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc @@ -0,0 +1,20 @@ +G90 G20 G17 +(Disable NCD to force tangent segments to be sent separately) +G64 P0.005 Q0 +G0 X0 Y0 Z0 +G91 +G2 Y-10 R5 F1000 +G3 Y-2 R1 +G2 Y-1 R.5 +G3 Y-.2 R.1 +G2 Y-.1 R.05 +G3 Y-.02 Z.5 R.01 +G3 Y+.02 Z.5 R.01 +G3 Y-.02 Z.5 R.01 +G1 X2 +G90 +G0 X0 +Y0 + +M5 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/blend_arc_simple_pos_test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/blend_arc_simple_pos_test.ngc new file mode 100644 index 00000000000..ea0fc731703 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/blend_arc_simple_pos_test.ngc @@ -0,0 +1,12 @@ +G20 +G90 +G0 X0 Y0 Z0 +G64 P0.005 Q0.00000 +G91 +G1 X1 F2000 +G1 Y1 +G1 Z1 +G1 Z1 +G90 +M30 + From 3f81bc29b827d77a62139541c80e429aa74b5ec2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Feb 2019 10:51:57 -0500 Subject: [PATCH 081/129] tests: Add an ignore file for generated files from TP debugging --- tests/trajectory-planner/circular-arcs/.gitignore | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 tests/trajectory-planner/circular-arcs/.gitignore diff --git a/tests/trajectory-planner/circular-arcs/.gitignore b/tests/trajectory-planner/circular-arcs/.gitignore new file mode 100644 index 00000000000..f39e83d7c44 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/.gitignore @@ -0,0 +1,4 @@ +.idea +sim.var +axis*csv +test*.log From 67508fecc1c1e96b6eadc5925cf98d55e77b9806 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Feb 2019 10:32:40 -0500 Subject: [PATCH 082/129] test: clean up old octave scripts and fix up broken / missing functions in test code. This includes some octave scratchwork to verify behavior of split cycles and circular segment velocity / acceleration tradeoffs. --- .../circular-arcs/.gitignore | 1 + .../circular-arcs/octave/create_3d_line.m | 62 ----------- .../octave/create_3d_random_walk.m | 105 ++++++++++++++++-- .../circular-arcs/octave/estimate_depth.m | 6 +- .../circular-arcs/octave/ezrand.m | 4 + .../circular-arcs/octave/make_peak_img.m | 29 ----- .../octave/optimal_circular_arc_accel.m | 34 ++++++ .../circular-arcs/octave/parallel_test.m | 10 ++ .../circular-arcs/octave/plot_movement.m | 10 -- .../circular-arcs/octave/process_log.m | 7 -- .../circular-arcs/octave/test_kink_accel.m | 29 +++++ .../circular-arcs/octave/test_split_cycle.m | 63 +++++++++++ 12 files changed, 243 insertions(+), 117 deletions(-) delete mode 100644 tests/trajectory-planner/circular-arcs/octave/create_3d_line.m create mode 100644 tests/trajectory-planner/circular-arcs/octave/ezrand.m delete mode 100644 tests/trajectory-planner/circular-arcs/octave/make_peak_img.m create mode 100644 tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m create mode 100644 tests/trajectory-planner/circular-arcs/octave/parallel_test.m delete mode 100755 tests/trajectory-planner/circular-arcs/octave/plot_movement.m delete mode 100644 tests/trajectory-planner/circular-arcs/octave/process_log.m create mode 100644 tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m create mode 100644 tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m diff --git a/tests/trajectory-planner/circular-arcs/.gitignore b/tests/trajectory-planner/circular-arcs/.gitignore index f39e83d7c44..47e12288fc1 100644 --- a/tests/trajectory-planner/circular-arcs/.gitignore +++ b/tests/trajectory-planner/circular-arcs/.gitignore @@ -2,3 +2,4 @@ sim.var axis*csv test*.log +generated diff --git a/tests/trajectory-planner/circular-arcs/octave/create_3d_line.m b/tests/trajectory-planner/circular-arcs/octave/create_3d_line.m deleted file mode 100644 index 15c4c476d28..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/create_3d_line.m +++ /dev/null @@ -1,62 +0,0 @@ -function move=create_3d_line(v_range,dp,theta,prev_move,limits) - %% Create a random trajectory segment based on the input - - if ~isstruct(prev_move) - %wlog assume +X as initial direction and origin as start - prev_move.P1=[-1,0,0]; - prev_move.P2=[0,0,0]; - end - - move.P1=prev_move.P2; - - %Hard-coded settings for G-code dump, make this more general later if need be - if length(v_range)==2 - move.v_req=ezrand(v_range(1),v_range(2)); - else - move.v_req=v_range; - end - - %dummy value - move.a_max=1; - - move.tolerance=10; - - dp_old=prev_move.P2-prev_move.P1; - u_old=dp_old/norm(dp_old); - - if length(theta)==1 - theta_vec=[-theta,theta,-theta,theta,-theta,theta]; - elseif length(theta)==2 - theta_vec=[theta,theta,theta]; - theta_vec=theta_vec(:); - else - theta_vec=theta; - end - - inlimits = false; - while ~inlimits - %Rotate about Z - theta_z=rand(1)*(theta_vec(2)-theta_vec(1)) + theta_vec(1); - u_raw=u_old*[cos(theta_z),-sin(theta_z),0;sin(theta_z),cos(theta_z),0;0,0,1]; - - %rotate about Y - theta_y=rand(1)*(theta_vec(4)-theta_vec(3)) + theta_vec(3); - u_raw=u_raw*[cos(theta_y),0,-sin(theta_y);0,1,0;sin(theta_y),0,cos(theta_y)]; - - theta_x=rand(1)*(theta_vec(6)-theta_vec(5)) + theta_vec(5); - u_raw=u_raw*[1, 0, 0;0, cos(theta_x),sin(theta_x);0,-sin(theta_x),cos(theta_x)]; - - %Hack to make a minimum length move - if length(dp) == 1 - move.P2=move.P1+u_raw*dp; - else - move.P2=move.P1+u_raw*(rand(1)*(dp(2)-dp(1))+dp(1)); - end - - if move.P2(1)limits(1) && move.P2(2)limits(3) && move.P2(3)limits(5) - inlimits = true; - end - end - -end - diff --git a/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m b/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m index 725699892a6..c3c9328fd9e 100644 --- a/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m +++ b/tests/trajectory-planner/circular-arcs/octave/create_3d_random_walk.m @@ -1,16 +1,40 @@ -function create_3d_random_walk(num_moves, v_range, dp, theta, prefix,limits) - %% Create a random path for G code testing - % This version of the code produces random lengths, and random 3d bends. Desired velocity is held at - if ~exist('limits','var') +%% Create a random path for G code testing. +% Produces random lengths, and random 3d bends, constrained by the provided settings. Writes a unique file name to the nc_files directory +% +% num_moves: number of moves in the generated file +% v_range: interval [min, max] of desired feeds (each move gets a randomly selected feed in this range) +% dp: length of moves (can be a min / max interval instead of a single value) +% limits: specify a bounding box for the program in a 6-element array [-X,+X,-Y,+Y,-Z,+Z] (default +/- 10.0 in XY, 5.0 in Z) +% tol: blend tolerance in user units (default 0.005) +% ncd_tol: naivecam tolerance (default 0) +function create_3d_random_walk(num_moves, v_range, dp, theta, limits, tol, ncd_tol, file_path, file_prefix) + if ~exist('file_path','var') + file_path = '../nc_files/generated' + end + mkdir(file_path); + if ~exist('file_prefix','var') + file_prefix = 'random_walk_linear' + end + + if ~exist('limits','var') || min(size(limits)) < 1 limits=[-10,10,-10,10,-5,5] end - + if ~exist('tol','var') + tol = 0.005 + end + if ~exist('ncd_tol','var') + ncd_tol = 0.0 + end tag = datestr(now(),"YYYY-MM-DD_HH-MM"); fsuf=sprintf('%dsteps_%gin-%gin_%gips_%gdeg_max_angle_%s.ngc',num_moves,dp(1),dp(end),v_range(end),max(theta)*180/pi,tag); - fid=fopen(sprintf('../nc_files/cv_random_walk_%s_%s',prefix,fsuf),'w'); + fid=fopen(sprintf('%s/%s_%s', file_path, file_prefix, fsuf),'w'); + if fid < 1 + error('Got invalid file handle') + end + %Initial state - header= 'G61 G90\n G0 X-.1 Y0 Z0 F20\n G1 X0 Y0 Z0\nG64\n'; + header= sprintf('G61 G90\n G0 X-.1 Y0 Z0\n G1 X0 Y0 Z0 F20\nG64 P%f Q%f\n', tol, ncd_tol); fprintf(fid, header); move=create_3d_line(v_range, dp, theta,[],limits); for k=1:num_moves @@ -22,3 +46,70 @@ function create_3d_random_walk(num_moves, v_range, dp, theta, prefix,limits) fclose(fid); end + +function move=create_3d_line(v_range,dp,theta,prev_move,limits) + %% Create a random trajectory segment based on the input + + if ~isstruct(prev_move) + %wlog assume +X as initial direction and origin as start + prev_move.P1=[-1,0,0]; + prev_move.P2=[0,0,0]; + end + + move.P1=prev_move.P2; + + %Hard-coded settings for G-code dump, make this more general later if need be + if length(v_range)==2 + move.v_req=ezrand(v_range(1),v_range(2)); + else + move.v_req=v_range; + end + + %dummy value + move.a_max=1; + + move.tolerance=10; + + dp_old=prev_move.P2-prev_move.P1; + u_old=dp_old/norm(dp_old); + + if length(theta)==1 + theta_vec=[-theta,theta,-theta,theta,-theta,theta]; + elseif length(theta)==2 + theta_vec=[theta,theta,theta]; + theta_vec=theta_vec(:); + else + theta_vec=theta; + end + + inlimits = false; + iter = 0; + while ~inlimits + %Rotate about Z + theta_z=rand(1)*(theta_vec(2)-theta_vec(1)) + theta_vec(1); + u_raw=u_old*[cos(theta_z),-sin(theta_z),0;sin(theta_z),cos(theta_z),0;0,0,1]; + + %rotate about Y + theta_y=rand(1)*(theta_vec(4)-theta_vec(3)) + theta_vec(3); + u_raw=u_raw*[cos(theta_y),0,-sin(theta_y);0,1,0;sin(theta_y),0,cos(theta_y)]; + + theta_x=rand(1)*(theta_vec(6)-theta_vec(5)) + theta_vec(5); + u_raw=u_raw*[1, 0, 0;0, cos(theta_x),sin(theta_x);0,-sin(theta_x),cos(theta_x)]; + + %Hack to make a minimum length move + if length(dp) == 1 + move.P2=move.P1+u_raw*dp; + else + move.P2=move.P1+u_raw*(rand(1)*(dp(2)-dp(1))+dp(1)); + end + + if move.P2(1)limits(1) && move.P2(2)limits(3) && move.P2(3)limits(5) + inlimits = true; + end + iter = iter + 1; + if iter > 100 + error('Exceeded max iterations trying to fit random move within limits, try a smaller length'); + end + end +end + diff --git a/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m b/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m index 48161a5a04d..7b67d17b032 100644 --- a/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m +++ b/tests/trajectory-planner/circular-arcs/octave/estimate_depth.m @@ -1,5 +1,8 @@ +%% -- estimate_depth(a_limit, v_limit, L_min, dt) +% +% For the given a_limit (max acceleration), v_limit (max velocity), estimate the queue depth +% to reach peak velocity, for a series of worst-case moves of length L_min function depth = estimate_depth(a_limit,v_limit,L_min,dt) - v_sample= L_min/(2.0*dt); v_max = min(v_sample,v_limit); v = 0; @@ -8,5 +11,4 @@ v = sqrt(v^2 + 2*a_limit*L_min); depth+=1; end - end diff --git a/tests/trajectory-planner/circular-arcs/octave/ezrand.m b/tests/trajectory-planner/circular-arcs/octave/ezrand.m new file mode 100644 index 00000000000..6492a78688c --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/ezrand.m @@ -0,0 +1,4 @@ +function out = ezrand(rmin,rmax) + out = rand(size(rmin))*(rmax-rmin)+rmin; +end + diff --git a/tests/trajectory-planner/circular-arcs/octave/make_peak_img.m b/tests/trajectory-planner/circular-arcs/octave/make_peak_img.m deleted file mode 100644 index 58c7d4d3480..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/make_peak_img.m +++ /dev/null @@ -1,29 +0,0 @@ -## Copyright (C) 2014 Robert W. Ellenberg -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## 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 Octave; see the file COPYING. If not, see -## . - -## make_peak_img - -## Author: Robert W. Ellenberg rob@carbidelabs.com -## Created: 2014-02-18 - -peaks(200) -colormap(gray(1024)) -shading interp -view(0,90) -grid off -print 'peaks.png' '-S - - diff --git a/tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m b/tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m new file mode 100644 index 00000000000..284d1d22542 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/optimal_circular_arc_accel.m @@ -0,0 +1,34 @@ +% Find optimal maximum velocity and acceleration ratio for circular motions. +% This shows the transition from velocity-limited to acceleration-limited motions +% Circles with large radii relative to their max velocity can use more of their +% acceleration budget for tangential acceleration, since normal accelerations are lower. +% +% NOTE: this calculation currently does not account for helical motion (which increases the effective radius) +a_max = 10.0 +v_max = 1.0 + +a_t_max_nominal = 0.5 * a_max; +a_n_max_nominal = sqrt(1.0 - a_t_max_nominal^2); + +r_eff = logspace(-2,.1,1000); + +v_max_cutoff = sqrt(a_max*sqrt(3)/2*r_eff); +v_max_req = 0*r_eff + v_max; + +figure(1) +plot(r_eff,v_max_cutoff,r_eff, v_max_req) +grid on +title('Circle maximum velocity vs radius') +xlabel('radius, units') +ylabel('velocity, units / sec') + +a_n_max_cutoff = v_max^2 ./ r_eff; +a_t_max_cutoff = sqrt(a_max^2 - a_n_max_cutoff.^2); + +figure(2) +plot(r_eff, a_t_max_cutoff / a_max,r_eff, r_eff*0+a_t_max_nominal/ a_max) +grid on +title('Circle maximum tangential acceleration vs radius') +xlabel('radius, units') +ylabel('velocity, units / sec') + diff --git a/tests/trajectory-planner/circular-arcs/octave/parallel_test.m b/tests/trajectory-planner/circular-arcs/octave/parallel_test.m new file mode 100644 index 00000000000..05c78ce3943 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/parallel_test.m @@ -0,0 +1,10 @@ +% Demonstrate dot product of unit vectors in various directions as a sanity check +theta=0:pi/36:pi; +u0=[1;0;0]; +u1=[cos(theta);sin(theta);0*theta]; + +udot = u0'*u1; +plot(udot) +title(sprintf('Dot product of unit vectors as angle varies from %0.3f to %0.3f', theta(1), theta(end))) +xlabel('Angle, rad') +grid on diff --git a/tests/trajectory-planner/circular-arcs/octave/plot_movement.m b/tests/trajectory-planner/circular-arcs/octave/plot_movement.m deleted file mode 100755 index 435f5d2ccee..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/plot_movement.m +++ /dev/null @@ -1,10 +0,0 @@ -#! /usr/bin/octave --persist -load movement.log -%Assume 1kHz -t=movement(:,1); -dt=diff(movement(:,1)); -v_nominal=movement(:,3); -dx = [0;movement(2:end,2)./dt]; -dv = [0;diff(dx)./dt]; -da = [0;diff(dv)./dt]; -plot(t,dv) diff --git a/tests/trajectory-planner/circular-arcs/octave/process_log.m b/tests/trajectory-planner/circular-arcs/octave/process_log.m deleted file mode 100644 index cb6fff5e220..00000000000 --- a/tests/trajectory-planner/circular-arcs/octave/process_log.m +++ /dev/null @@ -1,7 +0,0 @@ -load constraints.log -plot(constraints(:,1:6)); -legend('Xacc','Yacc','Zacc','Xvel','Yvel','Zvel','Xpos','Ypos','Zpos'); - -indX=find(abs(constraints(:,1))>30); -indY=find(abs(constraints(:,2))>30); -indZ=find(abs(constraints(:,3))>30); diff --git a/tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m b/tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m new file mode 100644 index 00000000000..f4d5ba1daab --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/test_kink_accel.m @@ -0,0 +1,29 @@ +% Test behavior of acceleration as TP hits a sharp corner. +% this demonstrates the momentary acceleration spike encountered due to a not-quite-tangent intersection +% The sharp corner, or "kink" requires a potentially large reduction in velocity to avoid violating overall limits. + +a_bound=[30;30;30]; +dt = 0.001; +v_max = 1.05; + +u0=[0.900637; -0.434573; 0.000000] +u1=[-0.766314; 0.642466; 0.000000] + +a_kink = v_max / dt * (u1-u0) + +a_kink_violation = a_kink ./ a_bound; + +accel_loss_ratio = max(abs(a_kink_violation(:))) + +% How much segment acceleration to budget for the end point +allowed_acceleration_loss = 0.1; + +if (accel_violation_factor > allowed_acceleration_loss) + disp('Too much acceleration loss') + kink_vel = v_max * allowed_acceleration_loss / accel_loss_ratio +else + kink_vel = v_max * allowed_acceleration_loss / accel_loss_ratio +end + + + diff --git a/tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m b/tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m new file mode 100644 index 00000000000..8f1d86ab89a --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/octave/test_split_cycle.m @@ -0,0 +1,63 @@ +%% Estimate time remaining in a motion segment given some assumptions. +% From time 0 to dt_1, the TP tries to reach the final velocity, accelerating at max acceleration to do so. +% From time dt_1 onwards, the TP accelerates at max acceleration, then +% decelerates back to final velocity, such that it reaches the end position +% just when it also reaches the final velocity. This handles the case where the +% TP reaches vf with some distance to spare in region 1. +% TODO: handle case where final velocity is lower than current velocity by accelerating to peak first +vc = 1.0; +vf = 0.5; +a_sign = sign(vf-vc); +dx = logspace(-3,-1,200); + + +a_max = 10.0; + +t_step = 0.001; + +dt_cutoff = abs(vf-vc) / a_max; + +dx_1 = abs(vc.^2 - vf.^2) ./ (2.0 * a_max); + +dx_2 = dx-dx_1; + +for k = 1:length(dx_2) + if dx_2(k) > 0 + % Case where we can accelerate to meet vf and still have distance left. + % Assumes that the TP accelerates to peak velocity, then back down to final velocity + A=a_max/4; + B=vf; + C=-dx_2(k); + + disc = sqrt(B^2-4*A*C); + dt_2_a=(-B+disc)/(2*A); + dt_2_b=(-B-disc)/(2*A); + dt_2(k) = max(dt_2_a, dt_2_b); + + dt_1(k) = dt_cutoff; + else + dt_2(k) = 0; + A=a_sign*a_max/2; + B=vc; + C=-dx(k); + + disc = sqrt(B^2-4*A*C); + + dt_1_a(k)=(-B+disc)/(2*A); + dt_1_b(k)=(-B-disc)/(2*A); + if dt_1_a(k) < 0 + dt_1(k) = dt_1_b(k); + elseif dt_1_b < 0 + dt_1(k) = dt_1_a(k); + else + dt_1(k) = min(dt_1_a(k), dt_1_b(k)); + end + end +end + +dt_2flat = max(dx_2,0) ./ vf; +plot(dx,dt_1,dx,dt_2,dx,dt_1+dt_2,'--') +grid on +legend('dt velocity match','dt position match','total time','no reaccel') +xlabel('net distance') +ylabel('time, seconds') From 91de43ede3c324207274dd7743263ae92544f9aa Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Feb 2019 12:41:48 -0500 Subject: [PATCH 083/129] motion: Catch and propagate failures to set TP position / velocity in HAL --- src/emc/motion/motion.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/emc/motion/motion.c b/src/emc/motion/motion.c index 6de46c3aab1..586f5d946b8 100644 --- a/src/emc/motion/motion.c +++ b/src/emc/motion/motion.c @@ -1049,13 +1049,13 @@ static int init_comm_buffers(void) } tpSetCycleTime(&emcmotDebug->tp, emcmotConfig->trajCycleTime); - tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd); - tpSetVmax(&emcmotDebug->tp, emcmotStatus->vel, emcmotStatus->vel); + int res_pos = tpSetPos(&emcmotDebug->tp, &emcmotStatus->carte_pos_cmd); + int res_vel = tpSetVmax(&emcmotDebug->tp, emcmotStatus->vel, emcmotStatus->vel); emcmotStatus->tail = 0; rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n"); - return 0; + return res_pos | res_vel; } /* init_threads() creates realtime threads, exports functions to From bdeae75073b6f1f94368a4ae2a95b722576d9d28 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Feb 2019 12:44:17 -0500 Subject: [PATCH 084/129] tp: Add position mismatch check when starting a motion segment This adds a check to prevent "drift" in TP's position output. When a new segment is activated, check the TP's current position against the initial position of the new motion segment. If they don't match, then the TP has failed to execute the previous path, and arrived at the wrong position. This should only occur if there is a serious regression in the TP, but it will be useful during testing to catch dumb mistakes. One simple way to cause such a bug is to fail to abort creation of an arc blend after the previous / next segments have been shortened (to make space). The machine will very quickly and very drastically deviate from the expected position as a program executes. --- src/emc/nml_intf/emcpose.c | 22 ++++++++++++++++++++++ src/emc/nml_intf/emcpose.h | 2 ++ src/emc/tp/tp.c | 27 ++++++++++++++++++++++++++- 3 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/emc/nml_intf/emcpose.c b/src/emc/nml_intf/emcpose.c index d0afd3dd81c..6ddf333a7f9 100644 --- a/src/emc/nml_intf/emcpose.c +++ b/src/emc/nml_intf/emcpose.c @@ -325,3 +325,25 @@ int emcPoseValid(EmcPose const * const pose) return 1; } } + +/** + * Checks all axis values in an EmcPose to see if they exceed a magnitude threshold. + * @return a bitmask that is 0 if all axes are within the threshold. Any + * out-of-limit axes set their corresponding bit to 1 in the returned value (X + * is 0th bit, Y is 1st, etc.). + */ +int findAbsThresholdViolations(EmcPose vec, double threshold) +{ + threshold = fabs(threshold); + // Bit-mask each failure so we can report all failed axes + int fail_bits = (fabs(vec.tran.x) > threshold) << 0 + | (fabs(vec.tran.y) > threshold) << 1 + | (fabs(vec.tran.z) > threshold) << 2 + | (fabs(vec.a) > threshold) << 3 + | (fabs(vec.b) > threshold) << 4 + | (fabs(vec.c) > threshold) << 5 + | (fabs(vec.u) > threshold) << 6 + | (fabs(vec.v) > threshold) << 7 + | (fabs(vec.w) > threshold) << 8; + return fail_bits; +} diff --git a/src/emc/nml_intf/emcpose.h b/src/emc/nml_intf/emcpose.h index 5e6ce6581fd..f0f5750c740 100644 --- a/src/emc/nml_intf/emcpose.h +++ b/src/emc/nml_intf/emcpose.h @@ -49,6 +49,8 @@ int emcPoseGetUVW(EmcPose const * const pose, PmCartesian * const uvw); int emcPoseMagnitude(EmcPose const * const pose, double * const out); +int findAbsThresholdViolations(EmcPose vec, double threshold); + int emcPoseValid(EmcPose const * const pose); #endif diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 99a99366aec..d9aaab2e372 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2836,6 +2836,32 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { res = TP_ERR_WAITING; } + + EmcPose tp_position_error; + ZERO_EMC_POSE(tp_position_error); + tcGetPos(tc, &tp_position_error); + emcPoseSelfSub(&tp_position_error, &tp->currentPos); + int failed_axes = findAbsThresholdViolations(tp_position_error, TP_POS_EPSILON); + if (failed_axes) { + // KLUDGE mask out good axes from print list with space + char failed_axes_list[9] = "XYZABCUVW"; + for (int i = 0; i < 9;++i) { + if (failed_axes & (1 << i)) { + continue; + } + failed_axes_list[i]=' '; + } + + rtapi_print_msg(RTAPI_MSG_ERR, "TP Position violation on axes [%s] at segment %d\n", failed_axes_list, tc->id); + print_json5_start_(); + print_json5_object_start_("accel_violation"); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_int_("id", tc->id); + print_json5_EmcPose(tp_position_error); + print_json5_object_end_(); + print_json5_end_(); + } + #ifdef TP_DEBUG print_json5_log_start(ActivateSegment,Run); print_json5_ll_("time_ticks", tp->time_elapsed_ticks); @@ -3399,7 +3425,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) } { - EmcPose const axis_pos = tp->currentPos; EmcPose axis_vel; From 0b65e70555e5d9dc0e4cdb3f81535c6e685166eb Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 6 Feb 2019 14:10:18 -0500 Subject: [PATCH 085/129] tp: Convert debug output for spiral arc length fit to JSON5 --- src/emc/tp/blendmath.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 648739fbdc2..5909efa7f79 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1723,10 +1723,16 @@ static int pmCircleAngleFromParam(PmCircle const * const circle, return TP_ERR_OK; } -static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit) +static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit, + double min_radius, + double total_angle, + double spiral_coef) { #ifdef TP_DEBUG print_json5_log_start(SpiralFit, Command); + print_json5_double(min_radius); + print_json5_double(total_angle); + print_json5_double(spiral_coef); print_json5_double_("b0", fit->b0); print_json5_double_("b1", fit->b1); print_json5_double_("total_planar_length", fit->total_planar_length); @@ -1772,9 +1778,6 @@ int findSpiralArcLengthFit(PmCircle const * const circle, } else { fit->spiral_in = false; } - tp_debug_print("radius = %.12f, angle = %.12f\n", min_radius, circle->angle); - tp_debug_print("spiral_coef = %.12f\n", spiral_coef); - //Compute the slope of the arc length vs. angle curve at the start and end of the segment double slope_start = pmSqrt(pmSq(min_radius) + pmSq(spiral_coef)); @@ -1784,7 +1787,7 @@ int findSpiralArcLengthFit(PmCircle const * const circle, fit->b1 = slope_start; fit->total_planar_length = fit->b0 * pmSq(circle->angle) + fit->b1 * circle->angle; - printSpiralArcLengthFit(fit); + printSpiralArcLengthFit(fit, min_radius, circle->angle, spiral_coef); // Check against start and end angle double angle_end_chk = 0.0; From 2b16c209827b3d680be60617963412a6c813d1ee Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 6 Feb 2019 16:24:53 -0500 Subject: [PATCH 086/129] test: Provide common setup subroutines and clean up old TP test programs * Factor out common initial state into a subroutine to ensure a new program can be loaded with a specific initial state. * Remove some redundant test programs (accidentally copied / merged at some point in the past) Having a consistent setup routine lets a batch test string multiple programs together without modal state bleeding over between programs. This may catch subtle bugs that running a single program (and restarting LinuxCNC) won't. --- .../circular-arcs/configs/6axis_rotary.ini | 1 + .../circular-arcs/configs/9axis.ini | 1 + .../circular-arcs/configs/XY.ini | 1 + .../circular-arcs/configs/XYZ_fast.ini | 1 + .../circular-arcs/configs/XYZ_medium.ini | 1 + .../circular-arcs/configs/XYZ_nospindle.ini | 2 + .../circular-arcs/configs/XYZ_uneven.ini | 1 + .../circular-arcs/configs/XY_slowZ.ini | 1 + .../circular-arcs/configs/lathe_XZ.ini | 1 + .../circular-arcs/configs/mill_XYZA.ini | 1 + .../nc_files/XYtests/51MeanderAve.ngc | 4 +- .../nc_files/arc-feed/arc_maxvel_xy.ngc | 3 +- .../nc_files/arc-feed/arc_time.ngc | 2 +- .../nc_files/arc-feed/basic_xy.ngc | 5 +- .../nc_files/arc-feed/full_xy.ngc | 4 +- .../nc_files/arc-feed/full_xz.ngc | 2 +- .../nc_files/arc-feed/full_yz.ngc | 2 +- .../arc-feed/optimal_circle_acc_ratio.ngc | 3 +- .../nc_files/arc-feed/simple_arc2.ngc | 2 +- .../nc_files/arc-feed/simple_arc3.ngc | 2 +- .../arc-arc-concave-convex.ngc | 3 +- .../arc-arc-convex-concave.ngc | 3 +- .../arc-arc-convex-convex.ngc | 2 +- .../arc-intersections/arc-arc-convex.ngc | 2 +- .../arc-intersections/arc-arc-neartangent.ngc | 2 +- .../arc-intersections/arc-arc-planeswitch.ngc | 2 +- .../arc-intersections/arc-arc-sawtooth.ngc | 3 +- .../arc-large-radii.ngc} | 3 +- .../arc-intersections/arc-terminal.ngc | 3 +- .../nc_files/arc-intersections/blur_torus.ngc | 1905 ----------------- .../arc-intersections/line-arc-burnin.ngc | 3 +- .../arc-intersections/linearc-convex.ngc | 4 +- .../arc-intersections/linearc-end-overrun.ngc | 5 +- .../arc-intersections/linearc-stellabee.ngc | 5 +- .../nc_files/arc-intersections/linearc.ngc | 2 +- .../arc-intersections/tort-sam-mach3.ngc | 5 +- .../nc_files/diagnostic/abc_tangent.ngc | 2 +- .../nc_files/diagnostic/adjustment.ngc | 4 +- .../nc_files/diagnostic/arc-with-spiral.ngc | 2 +- .../nc_files/diagnostic/bad_spirals_1.ngc | 6 +- .../nc_files/diagnostic/bad_spirals_comp.ngc | 5 +- .../nc_files/diagnostic/bad_spirals_lathe.ngc | 4 +- .../diagnostic/default_tolerance_check.ngc | 2 +- .../diagnostic/feed-tracking-test.ngc | 3 +- .../nc_files/diagnostic/helix_slowdown.ngc | 2 +- .../nc_files/diagnostic/modal_state_test.ngc | 3 +- .../nc_files/diagnostic/parabolic_split1.ngc | 3 +- .../nc_files/diagnostic/ripple1.ngc | 4 +- .../nc_files/diagnostic/simple_blend.ngc | 2 +- .../nc_files/diagnostic/spiral_aggressive.ngc | 5 +- .../nc_files/diagnostic/tangent_check.ngc | 3 +- .../diagnostic/test-active-states.ngc | 3 +- .../circular-arcs/nc_files/find_dupes.sh | 9 + .../sam-tail-slowdown.ngc | 0 .../nc_files/performance/tort-sam-mach3.ngc | 9 - .../quick-tests/arc-arc-concave-convex.ngc | 12 - .../quick-tests/arc-arc-convex-concave.ngc | 12 - .../quick-tests/arc-arc-convex-convex.ngc | 10 - .../nc_files/quick-tests/arc-arc-convex.ngc | 12 - .../quick-tests/arc-arc-neartangent.ngc | 10 - .../quick-tests/arc-arc-planeswitch.ngc | 13 - .../nc_files/quick-tests/arc-arc-sawtooth.ngc | 56 - .../nc_files/quick-tests/arc-terminal.ngc | 14 - .../nc_files/quick-tests/linearc-convex.ngc | 11 - .../quick-tests/linearc-end-overrun.ngc | 55 - .../quick-tests/linearc-stellabee.ngc | 132 -- .../nc_files/quick-tests/linearc.ngc | 12 - .../nc_files/quick-tests/spindle-wait.ngc | 33 - .../nc_files/spindle/g33_simple.ngc | 3 +- .../nc_files/spindle/rigidtap_test.ngc | 2 +- .../nc_files/spindle/spindle-wait.ngc | 2 +- .../nc_files/spindle/spiral_spindleon.ngc | 3 +- .../nc_files/spindle/straight_spindleon.ngc | 4 +- .../nc_files/spindle/tapping.ngc | 2 +- .../nc_files/test_subs/default_inch.ngc | 4 + .../nc_files/test_subs/default_mm.ngc | 4 + .../test_subs/default_modal_state.ngc | 19 + .../nc_files/wz-tests/130207L.ngc | 8 +- .../nc_files/wz-tests/130207LZW.ngc | 7 +- .../circular-arcs/nc_files/wz-tests/xw.ngc | 12 +- .../circular-arcs/nc_files/wz-tests/xz.ngc | 12 +- 81 files changed, 122 insertions(+), 2405 deletions(-) rename tests/trajectory-planner/circular-arcs/nc_files/{quick-tests/arc-arc-test.ngc => arc-intersections/arc-large-radii.ngc} (67%) delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh rename tests/trajectory-planner/circular-arcs/nc_files/{speed-tests => performance}/sam-tail-slowdown.ngc (100%) delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc create mode 100644 tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc diff --git a/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini b/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini index da4b6fe408a..9c004c1965e 100644 --- a/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini +++ b/tests/trajectory-planner/circular-arcs/configs/6axis_rotary.ini @@ -73,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/9axis.ini b/tests/trajectory-planner/circular-arcs/configs/9axis.ini index fd660aeee5f..19062374aca 100644 --- a/tests/trajectory-planner/circular-arcs/configs/9axis.ini +++ b/tests/trajectory-planner/circular-arcs/configs/9axis.ini @@ -73,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim-9axis.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XY.ini b/tests/trajectory-planner/circular-arcs/configs/XY.ini index 80bdd3f4b55..5d6594b811d 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY.ini @@ -72,6 +72,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini index a3efe84420a..9409f960cf0 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_fast.ini @@ -71,6 +71,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini index 9c025f08504..17cf1b90bb1 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_medium.ini @@ -71,6 +71,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini index 76156db05cb..5400a102d8f 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_nospindle.ini @@ -71,6 +71,8 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs + # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini b/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini index dea3b9d7015..f85248d7e0b 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XYZ_uneven.ini @@ -71,6 +71,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini index 18e8a4190cc..d61e62449db 100644 --- a/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/XY_slowZ.ini @@ -73,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini b/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini index 6a049c334dc..5fecf1a51d5 100644 --- a/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini +++ b/tests/trajectory-planner/circular-arcs/configs/lathe_XZ.ini @@ -72,6 +72,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini index a8cf7fe9345..e8be7d32172 100644 --- a/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini +++ b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini @@ -73,6 +73,7 @@ CYCLE_TIME = 0.001 # File containing interpreter variables PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs # Motion control section ------------------------------------------------------ [EMCMOT] diff --git a/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc b/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc index 92fd7d3bf14..11a2655812d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/XYtests/51MeanderAve.ngc @@ -4,11 +4,11 @@ G21 (Units: Metric) G40 G90 F1 S1 -G64 P0.01 q00.01 +G64 P0.01 q0.01 (Part: 51 Meander Ave) (Operation: Outside Offset, LAYER_1, T210: Stainless 2.0mm) G00 -X21.000 Y20.000 Z0 +X21.000 Y20.000 M01 M03 G04 P0.6 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc index 1f292bd1fd6..22569c681fe 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc @@ -1,9 +1,8 @@ -G20 G90 +o call F100000 G61 G0 X0 Y0 Z0 G1 X0 Y0.1 -G17 G2 Y-.1 J-0.1 Z5 I.5 P4 G0 X0 Y0 Z0 M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc index 15910d7c6c6..fc249c5e082 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc @@ -1,4 +1,4 @@ -G21 G90 +o call F100000 G0 X0 Y0 Z0 G1 X0 Y2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc index a020e73e968..7a7e89f194e 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc @@ -1,10 +1,9 @@ +o call (Basic ARC_FEED test using canon default units) -G21 G90 -G10 L2 P0 R0 +G61 F1000 G0 X0 Y0 Z0 G1 X10 -G17 G3 Y-10 R10 (270 deg. arc CCW) G0 X0 Y0 Z0 M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc index 5b6b8a5fa51..57cb5a7dc13 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc @@ -1,12 +1,12 @@ (Basic ARC_FEED test using english to test unit conv) -G20 G90 +o call +G61 (Arbitrary rotation of current csys) G10 L2 P0 X-2 Y1 R60 F1000 G0 X0 Y0 Z0 G1 X1.5 Y1.5 G1 X1.5 Y.5 -G17 G3 X.5 Y1.5 J1 (270 deg. arc CCW) G1 X1.5 Y1.5 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc index ceda118f58c..11466e88416 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc @@ -1,5 +1,5 @@ +o call (Basic ARC_FEED test using english to test unit conv) -G20 G90 (Force exact path to ignore blend effects) G61 (Arbitrary rotation of current csys) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc index 41141e78a43..5bf62916e1e 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc @@ -1,5 +1,5 @@ +o call (Basic ARC_FEED test using english to test unit conv) -G20 G90 (Force exact path to ignore blend effects) G61 (Arbitrary rotation of current csys) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc index a11dc9a8ead..ae94957f276 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc @@ -1,6 +1,5 @@ -G90 G20 G17 +o call (Disable NCD to force tangent segments to be sent separately) -G64 P0.005 Q0 G0 X0 Y0 Z0 G91 G2 Y-10 R5 F1000 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc index f906f5a6ecf..eaec8cd70bc 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc @@ -1,4 +1,4 @@ -G21 G90 +o call F1000 G0 X0 Y0 Z0 G1 X10 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc index 3f96ae2b924..019c57903bc 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc @@ -1,4 +1,4 @@ -G21 G90 +o call F1000 G0 X0 Y0 Z0 G1 X0 Y10 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc index eca82217ad6..1ca8428f31a 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-concave-convex.ngc @@ -1,4 +1,5 @@ -G90 G20 G64 +(Intersection between two arcs where 1st is concave, 2nd is convex wrt intersection) +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc index 5c6e86b0bb8..5dce8b71164 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-concave.ngc @@ -1,4 +1,5 @@ -G90 G20 G64 +(Intersection between two arcs, concave concave, tests arc blend creation) +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc index 5c0067bacfa..006375c8834 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex-convex.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc index eca82217ad6..022c4b24d83 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-convex.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc index d31b544c5ff..b5f68a270cd 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-neartangent.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc index 53f9f1706e7..a25e7d1ba89 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-planeswitch.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc index 127e2ddadde..608ac952304 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-arc-sawtooth.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-large-radii.ngc similarity index 67% rename from tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-test.ngc rename to tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-large-radii.ngc index 412eb027305..cdf4fb777ce 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-large-radii.ngc @@ -1,4 +1,5 @@ -G90 G20 G64 +o call +G64 P0 Q0 F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc index adb38ff00cf..ccb6e51eb32 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/arc-terminal.ngc @@ -1,5 +1,4 @@ -G20 G90 -G64 +o call F1000 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc deleted file mode 100644 index 378acbf84d8..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/blur_torus.ngc +++ /dev/null @@ -1,1905 +0,0 @@ -G20 -G0 Z0.0120 -G17 G40 -G80 G90 G94 -G64 P0.0010 -F1000 -G18 -G0 X1.5550 Y1.4350 -G1 Z-0.3750 -G3 X1.430000 Z-0.500000 R0.125000 -G1 X0.7450 - X0.7300 Z-0.4888 - X0.7250 Z-0.4870 - X0.7050 - X0.7000 Z-0.4888 - X0.6850 Z-0.5000 - X0.0000 - Y1.3950 - X0.4700 - X0.4750 Z-0.4976 -G2 X0.4900 Y1.3950 Z-0.4564 I-0.2697 K0.1216 -G3 X0.5250 Y1.3950 Z-0.3270 I1.7704 K-0.4094 -G3 X0.5500 Y1.3950 Z-0.2760 I0.2712 K-0.1014 -G3 X0.5700 Y1.3950 Z-0.2535 I0.1210 K-0.0876 -G1 X0.5900 Z-0.2398 - X0.6350 Z-0.2223 - X0.6400 Z-0.2222 - X0.6600 Z-0.2164 - X0.6700 Z-0.2163 - X0.6900 Z-0.2125 - X0.7400 - X0.7450 Z-0.2143 - X0.7700 Z-0.2164 - X0.7900 Z-0.2222 - X0.7950 Z-0.2223 - X0.8400 Z-0.2398 -G2 X0.8550 Y1.3950 Z-0.2496 I0.0238 K0.0201 -G1 X0.8650 Z-0.2583 -G3 X0.8850 Y1.3950 Z-0.2838 I-0.0825 K-0.0853 -G3 X0.9050 Y1.3950 Z-0.3270 I-0.2987 K-0.1647 -G1 X0.9400 Z-0.4564 -G2 X0.9550 Y1.3950 Z-0.4976 I0.3224 K0.0941 -G1 X0.9600 Z-0.5000 - X1.4300 - Y1.3550 - X1.0500 -G3 X1.0400 Y1.3550 Z-0.4740 I0.0429 K0.0314 -G1 X1.0150 Z-0.3387 -G2 X1.0050 Y1.3550 Z-0.2956 I-0.5400 K-0.1025 -G2 X0.9750 Y1.3550 Z-0.2387 I-0.1176 K-0.0257 -G1 X0.9600 Z-0.2241 - X0.9350 Z-0.2065 - X0.9050 Z-0.1911 - X0.8750 Z-0.1793 -G3 X0.8500 Y1.3550 Z-0.1723 I0.0197 K0.1174 -G1 X0.8000 Z-0.1624 - X0.7600 Z-0.1585 - X0.7350 Z-0.1582 - X0.6700 Z-0.1585 - X0.6450 Z-0.1605 - X0.5750 Z-0.1734 -G3 X0.5550 Y1.3550 Z-0.1793 I-0.0295 K0.0633 -G2 X0.5050 Y1.3550 Z-0.2006 I0.1272 K-0.3685 -G1 X0.4800 Z-0.2163 - X0.4650 Z-0.2289 -G2 X0.4250 Y1.3550 Z-0.2956 I0.0772 K-0.0917 -G2 X0.4150 Y1.3550 Z-0.3387 I0.5300 K-0.1456 -G1 X0.3900 Z-0.4740 -G3 X0.3800 Y1.3550 Z-0.5000 I-0.0529 K0.0055 -G1 X0.0000 - Y1.3150 - X0.3150 Z-0.4995 -G2 X0.3250 Y1.3150 Z-0.4544 I-0.3974 K0.1118 -G3 X0.3500 Y1.3150 Z-0.2956 I2.1684 K-0.2599 -G3 X0.3850 Y1.3150 Z-0.2241 I0.1418 K-0.0251 -G3 X0.4250 Y1.3150 Z-0.1898 I0.1743 K-0.1626 -G3 X0.4550 Y1.3150 Z-0.1731 I0.1309 K-0.1996 -G1 X0.5050 Z-0.1533 - X0.5850 Z-0.1367 - X0.6650 Z-0.1300 - X0.7850 Z-0.1308 - X0.8450 Z-0.1367 - X0.9100 Z-0.1494 - X0.9250 Z-0.1533 - X0.9800 Z-0.1754 -G3 X1.0300 Y1.3150 Z-0.2094 I-0.0933 K-0.1909 -G3 X1.0750 Y1.3150 Z-0.2779 I-0.0924 K-0.1097 -G3 X1.0850 Y1.3150 Z-0.3191 I-0.3032 K-0.0954 -G1 X1.1050 Z-0.4544 -G2 X1.1150 Y1.3150 Z-0.4995 I0.4074 K0.0667 -G1 X1.4300 Z-0.5000 - Y1.2750 - X1.1700 -G3 X1.1600 Y1.2750 Z-0.4642 I0.0626 K0.0368 -G3 X1.1450 Y1.2750 Z-0.3427 I2.9199 K0.4220 -G2 X1.1350 Y1.2750 Z-0.2838 I-0.6610 K-0.0821 -G2 X1.1000 Y1.2750 Z-0.2124 I-0.1548 K-0.0315 -G2 X1.0350 Y1.2750 Z-0.1633 I-0.1705 K-0.1582 -G1 X0.9700 Z-0.1376 -G2 X0.8850 Y1.2750 Z-0.1221 I-0.1478 K-0.5692 -G1 X0.8150 Z-0.1165 - X0.7800 Z-0.1157 - X0.6150 Z-0.1165 - X0.5450 Z-0.1221 -G2 X0.4600 Y1.2750 Z-0.1376 I0.0752 K-0.6524 -G1 X0.3950 Z-0.1633 -G2 X0.3300 Y1.2750 Z-0.2124 I0.1055 K-0.2073 -G2 X0.3050 Y1.2750 Z-0.2535 I0.0916 K-0.0838 -G2 X0.2900 Y1.2750 Z-0.3093 I0.1787 K-0.0780 -G1 X0.2700 Z-0.4642 -G3 X0.2600 Y1.2750 Z-0.5000 I-0.0726 K0.0010 -G1 X0.0000 - Y1.2350 - X0.2150 - X0.2200 Z-0.4928 - X0.2250 Z-0.4653 - X0.2400 Z-0.3359 -G3 X0.2500 Y1.2350 Z-0.2731 I0.5248 K-0.0515 -G3 X0.2900 Y1.2350 Z-0.1986 I0.1408 K-0.0276 -G3 X0.3400 Y1.2350 Z-0.1594 I0.1732 K-0.1691 -G1 X0.3500 Z-0.1535 - X0.4000 Z-0.1337 -G3 X0.4700 Y1.2350 Z-0.1185 I0.1316 K-0.4356 -G1 X0.5500 Z-0.1118 - X0.8800 - X0.9350 Z-0.1157 -G3 X1.0800 Y1.2350 Z-0.1535 I-0.0280 K-0.4044 -G3 X1.1350 Y1.2350 Z-0.1937 I-0.1305 K-0.2360 -G3 X1.1700 Y1.2350 Z-0.2418 I-0.1052 K-0.1134 -G3 X1.1850 Y1.2350 Z-0.2986 I-0.2253 K-0.0898 -G1 X1.2050 Z-0.4653 - X1.2100 Z-0.4928 - X1.2150 Z-0.5000 - X1.4300 - Y1.1950 - X1.2550 - X1.2500 Z-0.4928 - X1.2450 Z-0.4673 -G3 X1.2300 Y1.1950 Z-0.3359 I3.4995 K0.4661 -G2 X1.2200 Y1.1950 Z-0.2712 I-0.6965 K-0.0745 -G2 X1.1950 Y1.1950 Z-0.2124 I-0.1428 K-0.0260 -G2 X1.1650 Y1.1950 Z-0.1786 I-0.1849 K-0.1343 -G1 X1.1300 Z-0.1535 - X1.1050 Z-0.1406 - X1.0800 Z-0.1309 - X1.0450 Z-0.1213 -G2 X0.9750 Y1.1950 Z-0.1118 I-0.0919 K-0.4146 -G1 X0.8600 - X0.7550 Z-0.1165 - X0.6750 - X0.5700 Z-0.1118 - X0.4550 -G2 X0.3850 Y1.1950 Z-0.1213 I0.0219 K-0.4241 -G1 X0.3500 Z-0.1309 - X0.3250 Z-0.1406 - X0.3100 Z-0.1476 - X0.2850 Z-0.1633 -G2 X0.2550 Y1.1950 Z-0.1884 I0.0888 K-0.1363 -G2 X0.2150 Y1.1950 Z-0.2516 I0.1181 K-0.1190 -G2 X0.2050 Y1.1950 Z-0.2986 I0.3330 K-0.0953 -G1 X0.1850 Z-0.4673 - X0.1800 Z-0.4928 - X0.1750 Z-0.5000 - X0.0000 - Y1.1550 - X0.1450 - X0.1500 Z-0.4740 -G2 X0.1650 Y1.1550 Z-0.3368 I-3.9987 K0.5064 -G3 X0.1750 Y1.1550 Z-0.2692 I0.7103 K-0.0706 -G3 X0.2000 Y1.1550 Z-0.2104 I0.1642 K-0.0351 -G3 X0.2800 Y1.1550 Z-0.1406 I0.1736 K-0.1181 -G3 X0.3950 Y1.1550 Z-0.1118 I0.1349 K-0.2944 -G1 X0.4800 - X0.5750 Z-0.1204 - X0.6150 Z-0.1260 - X0.6600 Z-0.1280 - X0.6700 Z-0.1300 - X0.7600 - X0.7700 Z-0.1280 - X0.8150 Z-0.1260 - X0.8550 Z-0.1204 - X0.9500 Z-0.1118 - X1.0350 -G3 X1.1200 Y1.1550 Z-0.1288 I-0.0197 K-0.3186 -G3 X1.2450 Y1.1550 Z-0.2378 I-0.0584 K-0.1932 -G3 X1.2600 Y1.1550 Z-0.2976 I-0.2120 K-0.0850 -G1 X1.2800 Z-0.4740 - X1.2850 Z-0.5000 - X1.4300 - Y1.1150 - X1.3150 - X1.3100 Z-0.4799 - X1.2900 Z-0.2936 -G2 X1.2650 Y1.1150 Z-0.2163 I-0.2038 K-0.0232 -G2 X1.2300 Y1.1150 Z-0.1728 I-0.1489 K-0.0839 -G2 X1.1850 Y1.1150 Z-0.1406 I-0.1464 K-0.1571 -G1 X1.1400 Z-0.1224 - X1.0850 Z-0.1118 - X1.0150 - X0.9950 Z-0.1137 - X0.9500 Z-0.1202 - X0.8250 Z-0.1435 - X0.7500 Z-0.1523 - X0.7250 - X0.7150 Z-0.1543 - X0.7050 Z-0.1523 - X0.6800 - X0.6050 Z-0.1435 - X0.4700 Z-0.1185 - X0.4150 Z-0.1118 - X0.3450 - X0.2850 Z-0.1241 - X0.2450 Z-0.1406 -G2 X0.2000 Y1.1150 Z-0.1728 I0.1014 K-0.1893 -G2 X0.1550 Y1.1150 Z-0.2359 I0.1213 K-0.1340 -G2 X0.1400 Y1.1150 Z-0.2936 I0.2205 K-0.0881 -G1 X0.1200 Z-0.4799 - X0.1150 Z-0.5000 - X0.0000 - Y1.0750 - X0.0900 - X0.0950 Z-0.4799 - X0.1150 Z-0.2897 -G3 X0.1400 Y1.0750 Z-0.2143 I0.1916 K-0.0217 -G3 X0.2000 Y1.0750 Z-0.1494 I0.1504 K-0.0789 -G3 X0.3050 Y1.0750 Z-0.1118 I0.1253 K-0.1843 -G1 X0.3700 - X0.3950 Z-0.1154 - X0.4550 Z-0.1271 - X0.5750 Z-0.1631 - X0.6200 Z-0.1749 - X0.6500 Z-0.1813 - X0.7100 Z-0.1876 - X0.7500 Z-0.1852 - X0.7900 Z-0.1793 - X0.8550 Z-0.1631 - X0.9750 Z-0.1271 - X1.0450 Z-0.1137 - X1.0600 Z-0.1118 - X1.1250 -G3 X1.1700 Y1.0750 Z-0.1213 I-0.0434 K-0.3169 -G3 X1.3000 Y1.0750 Z-0.2339 I-0.0515 K-0.1908 -G3 X1.3150 Y1.0750 Z-0.2897 I-0.1787 K-0.0780 -G1 X1.3350 Z-0.4799 - X1.3400 Z-0.5000 - X1.4300 - Y1.0350 - X1.3600 - X1.3550 Z-0.4740 - X1.3400 Z-0.3132 - X1.3350 Z-0.2779 -G2 X1.3000 Y1.0350 Z-0.1937 I-0.1775 K-0.0244 -G2 X1.2250 Y1.0350 Z-0.1309 I-0.1616 K-0.1167 -G2 X1.1550 Y1.0350 Z-0.1118 I-0.0976 K-0.2200 -G1 X1.1000 -G2 X1.0350 Y1.0350 Z-0.1232 I0.0666 K-0.5680 -G3 X1.0000 Y1.0350 Z-0.1348 I-0.0530 K0.1018 -G1 X0.9450 Z-0.1563 - X0.8500 Z-0.2004 -G3 X0.8050 Y1.0350 Z-0.2185 I-0.2314 K0.5086 -G1 X0.7650 Z-0.2301 - X0.7250 Z-0.2360 - X0.7050 - X0.6750 Z-0.2321 - X0.6350 Z-0.2222 -G3 X0.5900 Y1.0350 Z-0.2048 I0.3040 K0.8558 -G1 X0.4850 Z-0.1563 - X0.4300 Z-0.1348 -G3 X0.3950 Y1.0350 Z-0.1232 I0.0171 K0.1106 -G2 X0.3300 Y1.0350 Z-0.1118 I-0.1470 K-0.6436 -G1 X0.2750 -G2 X0.2250 Y1.0350 Z-0.1232 I0.0232 K-0.2163 -G2 X0.1550 Y1.0350 Z-0.1652 I0.0517 K-0.1655 -G2 X0.1050 Y1.0350 Z-0.2387 I0.1443 K-0.1519 -G2 X0.0900 Y1.0350 Z-0.3132 I0.2565 K-0.0904 -G1 X0.0750 Z-0.4740 - X0.0700 Z-0.5000 - X0.0000 - Y0.9950 - X0.0500 - X0.0550 Z-0.4858 - X0.0750 Z-0.2819 -G3 X0.1100 Y0.9950 Z-0.1937 I0.2098 K-0.0323 -G1 X0.1300 Z-0.1695 - X0.1700 Z-0.1376 -G3 X0.2500 Y0.9950 Z-0.1118 I0.0973 K-0.1642 -G1 X0.3000 - X0.3250 Z-0.1154 -G3 X0.3600 Y0.9950 Z-0.1232 I-0.0456 K-0.2854 -G3 X0.4100 Y0.9950 Z-0.1425 I-0.5357 K-1.4614 -G1 X0.4500 Z-0.1621 -G2 X0.4850 Y0.9950 Z-0.1829 I0.1837 K0.2701 -G1 X0.5450 Z-0.2231 - X0.6150 Z-0.2731 - X0.6450 Z-0.2908 - X0.6700 Z-0.3026 - X0.6750 Z-0.3027 - X0.6950 Z-0.3084 - X0.7150 Z-0.3105 - X0.7350 Z-0.3084 - X0.7550 Z-0.3027 - X0.7600 Z-0.3026 - X0.7850 Z-0.2908 - X0.8150 Z-0.2731 - X0.9150 Z-0.2025 - X0.9700 Z-0.1680 - X1.0200 Z-0.1425 - X1.0650 Z-0.1250 - X1.0850 Z-0.1193 -G3 X1.1300 Y0.9950 Z-0.1118 I0.0975 K-0.4441 -G1 X1.1800 -G3 X1.2200 Y0.9950 Z-0.1204 I-0.0174 K-0.1770 -G3 X1.2600 Y0.9950 Z-0.1376 I-0.0593 K-0.1929 -G1 X1.2950 Z-0.1651 -G3 X1.3300 Y0.9950 Z-0.2104 I-0.0875 K-0.1038 -G3 X1.3550 Y0.9950 Z-0.2819 I-0.1732 K-0.1007 -G1 X1.3750 Z-0.4858 - X1.3800 Z-0.5000 - X1.4300 - Y0.9550 - X1.3950 - X1.3900 Z-0.4858 - X1.3700 Z-0.2799 -G2 X1.3500 Y0.9550 Z-0.2163 I-0.2104 K-0.0312 -G2 X1.2850 Y0.9550 Z-0.1416 I-0.1889 K-0.0987 -G2 X1.2000 Y0.9550 Z-0.1118 I-0.1046 K-0.1621 -G1 X1.1500 - X1.1150 Z-0.1185 -G2 X0.9950 Y0.9550 Z-0.1767 I0.0925 K-0.3435 -G2 X0.8900 Y0.9550 Z-0.2712 I0.3613 K-0.5070 -G1 X0.8600 Z-0.3065 - X0.8150 Z-0.3662 - X0.7600 Z-0.4564 -G3 X0.7350 Y0.9550 Z-0.4829 I-0.1639 K0.1292 -G1 X0.7200 Z-0.4850 - X0.7050 Z-0.4849 - X0.6950 Z-0.4829 - X0.6800 Z-0.4681 - X0.6700 Z-0.4564 - X0.6150 Z-0.3662 - X0.5700 Z-0.3065 - X0.5400 Z-0.2712 -G2 X0.4250 Y0.9550 Z-0.1692 I-0.4900 K-0.4365 -G2 X0.3150 Y0.9550 Z-0.1185 I-0.2060 K-0.3023 -G1 X0.2800 Z-0.1118 - X0.2300 -G2 X0.1900 Y0.9550 Z-0.1204 I0.0174 K-0.1770 -G2 X0.1300 Y0.9550 Z-0.1533 I0.0480 K-0.1587 -G2 X0.0800 Y0.9550 Z-0.2163 I0.1386 K-0.1614 -G2 X0.0600 Y0.9550 Z-0.2799 I0.1904 K-0.0948 -G1 X0.0400 Z-0.4858 - X0.0350 Z-0.5000 - X0.0000 - Y0.9150 - X0.0250 Z-0.4995 - X0.0300 Z-0.4603 -G3 X0.0450 Y0.9150 Z-0.2956 I3.9179 K-0.2738 -G3 X0.0700 Y0.9150 Z-0.2104 I0.2183 K-0.0178 -G3 X0.2100 Y0.9150 Z-0.1118 I0.1623 K-0.0817 -G1 X0.2650 Z-0.1126 - X0.2950 Z-0.1185 - X0.3450 Z-0.1378 -G3 X0.4400 Y0.9150 Z-0.2061 I-0.1692 K-0.3357 -G3 X0.5250 Y0.9150 Z-0.3163 I-0.3670 K-0.3710 -G3 X0.5500 Y0.9150 Z-0.3692 I-0.3664 K-0.2054 -G1 X0.5600 Z-0.3967 - X0.5850 Z-0.4838 - X0.5900 Z-0.4976 - X0.5950 Z-0.5000 - X0.8350 - X0.8400 Z-0.4976 - X0.8700 Z-0.3967 -G3 X0.9050 Y0.9150 Z-0.3163 I0.4274 K-0.1382 -G3 X1.0000 Y0.9150 Z-0.1966 I0.4475 K-0.2577 -G3 X1.0800 Y0.9150 Z-0.1406 I0.2460 K-0.2660 -G1 X1.1350 Z-0.1185 - X1.1700 Z-0.1118 - X1.2200 - X1.2450 Z-0.1165 -G3 X1.3650 Y0.9150 Z-0.2202 I-0.0489 K-0.1778 -G3 X1.3850 Y0.9150 Z-0.2956 I-0.2019 K-0.0939 -G3 X1.4000 Y0.9150 Z-0.4603 I-3.9029 K-0.4385 -G1 X1.4050 Z-0.4995 - X1.4300 Z-0.5000 - Y0.8750 - X1.4150 Z-0.4995 - X1.4100 Z-0.4623 -G2 X1.3950 Y0.8750 Z-0.2956 I-3.7204 K-0.2508 -G2 X1.3700 Y0.8750 Z-0.2084 I-0.2156 K-0.0147 -G2 X1.2300 Y0.8750 Z-0.1118 I-0.1591 K-0.0807 -G1 X1.1850 Z-0.1126 - X1.1450 Z-0.1204 - X1.1200 Z-0.1300 -G2 X1.0700 Y0.8750 Z-0.1582 I0.1224 K-0.2749 -G2 X1.0100 Y0.8750 Z-0.2107 I0.2257 K-0.3186 -G2 X0.9350 Y0.8750 Z-0.3241 I0.3300 K-0.2997 -G2 X0.9150 Y0.8750 Z-0.3790 I0.2834 K-0.1343 -G2 X0.9050 Y0.8750 Z-0.4182 I0.4006 K-0.1230 -G3 X0.8900 Y0.8750 Z-0.4888 I-1.3362 K0.2471 -G1 X0.8850 Z-0.5000 - X0.5450 - X0.5400 Z-0.4888 -G2 X0.5150 Y0.8750 Z-0.3790 I-1.3743 K-0.2552 -G2 X0.4950 Y0.8750 Z-0.3241 I-0.3034 K-0.0794 -G2 X0.4150 Y0.8750 Z-0.2055 I-0.4009 K-0.1840 -G2 X0.3500 Y0.8750 Z-0.1514 I-0.2769 K-0.2665 -G2 X0.2900 Y0.8750 Z-0.1221 I-0.1625 K-0.2573 -G1 X0.2400 Z-0.1118 - X0.2000 -G2 X0.1650 Y0.8750 Z-0.1193 I0.0264 K-0.2075 -G2 X0.1050 Y0.8750 Z-0.1526 I0.0413 K-0.1450 -G1 X0.0850 Z-0.1715 -G2 X0.0550 Y0.8750 Z-0.2182 I0.2353 K-0.1840 -G2 X0.0350 Y0.8750 Z-0.2956 I0.1985 K-0.0926 -G2 X0.0200 Y0.8750 Z-0.4623 I3.7054 K-0.4175 -G1 X0.0150 Z-0.4995 - X0.0000 Z-0.5000 - Y0.8350 - X0.0050 - X0.0100 Z-0.4869 -G2 X0.0250 Y0.8350 Z-0.3172 I-3.4555 K0.3909 -G1 X0.0300 Z-0.2771 -G3 X0.0450 Y0.8350 Z-0.2222 I0.2111 K-0.0282 -G3 X0.1100 Y0.8350 Z-0.1416 I0.1793 K-0.0781 -G1 X0.1250 Z-0.1319 - X0.1600 Z-0.1182 - X0.1900 Z-0.1118 - X0.2350 Z-0.1126 -G3 X0.3050 Y0.8350 Z-0.1339 I-0.0254 K-0.2092 -G3 X0.3650 Y0.8350 Z-0.1739 I-0.1149 K-0.2373 -G3 X0.4150 Y0.8350 Z-0.2264 I-0.1956 K-0.2364 -G3 X0.4750 Y0.8350 Z-0.3328 I-0.3089 K-0.2442 -G3 X0.4950 Y0.8350 Z-0.3976 I-0.3605 K-0.1469 -G1 X0.5150 Z-0.4995 - X0.5200 Z-0.5000 - X0.9150 Z-0.4995 - X0.9350 Z-0.3976 -G3 X0.9550 Y0.8350 Z-0.3328 I0.3805 K-0.0822 -G3 X1.0350 Y0.8350 Z-0.2029 I0.3839 K-0.1466 -G3 X1.1600 Y0.8350 Z-0.1202 I0.2122 K-0.1849 -G1 X1.2000 Z-0.1118 - X1.2400 - X1.2650 Z-0.1165 - X1.3050 Z-0.1319 -G3 X1.3300 Y0.8350 Z-0.1494 I-0.0546 K-0.1047 -G3 X1.3850 Y0.8350 Z-0.2222 I-0.1226 K-0.1498 -G3 X1.4000 Y0.8350 Z-0.2771 I-0.1961 K-0.0831 -G1 X1.4050 Z-0.3172 -G2 X1.4200 Y0.8350 Z-0.4869 I3.4705 K0.2212 -G1 X1.4250 Z-0.5000 - X1.4300 - Y0.7950 - X1.4250 Z-0.4849 -G3 X1.4100 Y0.7950 Z-0.3143 I3.3931 K0.3843 -G1 X1.4050 Z-0.2751 -G2 X1.3850 Y0.7950 Z-0.2105 I-0.1949 K-0.0250 -G2 X1.3250 Y0.7950 Z-0.1406 I-0.1682 K-0.0835 -G2 X1.2900 Y0.7950 Z-0.1221 I-0.1056 K-0.1579 -G2 X1.2450 Y0.7950 Z-0.1118 I-0.0674 K-0.1899 -G1 X1.2100 - X1.1800 Z-0.1173 - X1.1300 Z-0.1359 -G2 X1.0800 Y0.7950 Z-0.1703 I0.0919 K-0.1870 -G2 X1.0350 Y0.7950 Z-0.2166 I0.1992 K-0.2387 -G2 X0.9750 Y0.7950 Z-0.3202 I0.3124 K-0.2501 -G2 X0.9550 Y0.7950 Z-0.3838 I0.3932 K-0.1586 -G1 X0.9350 Z-0.4936 - X0.9300 Z-0.5000 - X0.5000 - X0.4950 Z-0.4936 -G2 X0.4750 Y0.7950 Z-0.3838 I-2.1792 K-0.3402 -G2 X0.4550 Y0.7950 Z-0.3202 I-0.4132 K-0.0949 -G2 X0.3800 Y0.7950 Z-0.1989 I-0.3721 K-0.1463 -G2 X0.3250 Y0.7950 Z-0.1504 I-0.2976 K-0.2816 -G1 X0.3000 Z-0.1359 - X0.2550 Z-0.1185 - X0.2200 Z-0.1118 - X0.1850 -G2 X0.1450 Y0.7950 Z-0.1204 I0.0432 K-0.2961 -G2 X0.0800 Y0.7950 Z-0.1617 I0.0467 K-0.1454 -G2 X0.0350 Y0.7950 Z-0.2339 I0.1408 K-0.1379 -G2 X0.0250 Y0.7950 Z-0.2751 I0.1734 K-0.0639 -G1 X0.0200 Z-0.3143 -G3 X0.0050 Y0.7950 Z-0.4849 I-3.4081 K0.2137 -G1 X0.0000 Z-0.5000 - Y0.7550 - X0.0050 Z-0.4740 - X0.0200 Z-0.2995 - X0.0250 Z-0.2615 -G3 X0.0400 Y0.7550 Z-0.2145 I0.1674 K-0.0275 -G3 X0.1800 Y0.7550 Z-0.1118 I0.1600 K-0.0713 -G1 X0.2150 - X0.2500 Z-0.1185 - X0.2900 Z-0.1339 -G3 X0.3850 Y0.7550 Z-0.2127 I-0.1319 K-0.2557 -G3 X0.4500 Y0.7550 Z-0.3300 I-0.2722 K-0.2274 -G3 X0.4650 Y0.7550 Z-0.3829 I-0.3195 K-0.1191 -G1 X0.4850 Z-0.4947 - X0.4900 Z-0.5000 - X0.9400 - X0.9450 Z-0.4947 -G2 X0.9600 Y0.7550 Z-0.4065 I-1.7386 K0.3409 -G1 X0.9700 Z-0.3614 -G3 X0.9850 Y0.7550 Z-0.3164 I0.5175 K-0.1477 -G3 X1.0550 Y0.7550 Z-0.2009 I0.3484 K-0.1322 -G3 X1.1750 Y0.7550 Z-0.1202 I0.2011 K-0.1694 -G1 X1.2150 Z-0.1118 - X1.2500 - X1.2750 Z-0.1165 -G3 X1.3500 Y0.7550 Z-0.1585 I-0.0439 K-0.1662 -G3 X1.3950 Y0.7550 Z-0.2261 I-0.1500 K-0.1487 -G3 X1.4100 Y0.7550 Z-0.2995 I-0.1945 K-0.0780 -G1 X1.4250 Z-0.4740 - X1.4300 Z-0.5000 - Y0.7150 Z-0.4870 - X1.4100 Z-0.2811 -G2 X1.3900 Y0.7150 Z-0.2125 I-0.1947 K-0.0195 -G2 X1.3550 Y0.7150 Z-0.1624 I-0.1813 K-0.0896 -G1 X1.3200 Z-0.1347 - X1.2750 Z-0.1165 - X1.2500 Z-0.1118 - X1.2100 Z-0.1126 -G2 X1.1450 Y0.7150 Z-0.1319 I0.0226 K-0.1949 -G2 X1.0950 Y0.7150 Z-0.1641 I0.1568 K-0.2986 -G1 X1.0650 Z-0.1919 - X1.0450 Z-0.2154 -G2 X0.9850 Y0.7150 Z-0.3223 I0.2909 K-0.2336 -G2 X0.9700 Y0.7150 Z-0.3713 I0.6768 K-0.2339 -G2 X0.9500 Y0.7150 Z-0.4850 I1.2644 K-0.2810 -G1 X0.9450 Z-0.5000 - X0.4850 - X0.4800 Z-0.4850 -G2 X0.4600 Y0.7150 Z-0.3713 I-1.2844 K-0.1673 -G2 X0.4450 Y0.7150 Z-0.3223 I-0.6918 K-0.1849 -G2 X0.3650 Y0.7150 Z-0.1919 I-0.3584 K-0.1301 -G1 X0.3350 Z-0.1641 - X0.3100 Z-0.1465 -G2 X0.2550 Y0.7150 Z-0.1202 I-0.1129 K-0.1656 -G1 X0.2150 Z-0.1118 - X0.1800 - X0.1550 Z-0.1165 - X0.1100 Z-0.1347 - X0.0750 Z-0.1624 -G2 X0.0300 Y0.7150 Z-0.2360 I0.1483 K-0.1412 -G2 X0.0150 Y0.7150 Z-0.3223 I0.3189 K-0.0999 -G3 X0.0000 Y0.7150 Z-0.4870 I-3.0562 K0.1953 -G1 Y0.6750 Z-0.5000 - X0.0050 Z-0.4811 - X0.0200 Z-0.3045 - X0.0250 Z-0.2653 -G3 X0.0350 Y0.6750 Z-0.2262 I0.1963 K-0.0294 -G3 X0.0850 Y0.6750 Z-0.1543 I0.1853 K-0.0755 -G3 X0.1200 Y0.6750 Z-0.1300 I0.1024 K-0.1100 -G3 X0.1800 Y0.6750 Z-0.1118 I0.0800 K-0.1557 -G1 X0.2200 - X0.2450 Z-0.1165 - X0.2950 Z-0.1359 -G3 X0.3500 Y0.6750 Z-0.1742 I-0.1039 K-0.2075 -G3 X0.3900 Y0.6750 Z-0.2174 I-0.1513 K-0.1804 -G3 X0.4550 Y0.6750 Z-0.3398 I-0.3071 K-0.2415 -G3 X0.4700 Y0.6750 Z-0.3976 I-0.3760 K-0.1285 -G1 X0.4850 Z-0.4858 - X0.4900 Z-0.5000 - X0.9400 - X0.9450 Z-0.4858 -G3 X0.9650 Y0.6750 Z-0.3751 I1.6036 K-0.2326 -G3 X0.9850 Y0.6750 Z-0.3124 I0.3783 K-0.0860 -G3 X1.0650 Y0.6750 Z-0.1879 I0.3690 K-0.1493 -G3 X1.1100 Y0.6750 Z-0.1504 I0.2916 K-0.3036 -G1 X1.1350 Z-0.1359 - X1.1850 Z-0.1165 - X1.2100 Z-0.1118 - X1.2500 - X1.2750 Z-0.1165 -G3 X1.3500 Y0.6750 Z-0.1585 I-0.0439 K-0.1662 -G3 X1.3950 Y0.6750 Z-0.2262 I-0.1490 K-0.1478 -G3 X1.4050 Y0.6750 Z-0.2653 I-0.1863 K-0.0685 -G1 X1.4100 Z-0.3045 - X1.4250 Z-0.4811 - X1.4300 Z-0.5000 - Y0.6350 - X1.4250 Z-0.4908 - X1.4050 Z-0.2810 -G2 X1.3850 Y0.6350 Z-0.2125 I-0.1922 K-0.0190 -G2 X1.3300 Y0.6350 Z-0.1455 I-0.1715 K-0.0847 -G2 X1.2450 Y0.6350 Z-0.1118 I-0.1057 K-0.1423 -G1 X1.2050 - X1.1800 Z-0.1165 -G2 X1.1300 Y0.6350 Z-0.1348 I0.0430 K-0.1952 -G2 X1.0700 Y0.6350 Z-0.1769 I0.1240 K-0.2407 -G2 X1.0350 Y0.6350 Z-0.2134 I0.1752 K-0.2027 -G2 X0.9750 Y0.6350 Z-0.3124 I0.3211 K-0.2624 -G2 X0.9550 Y0.6350 Z-0.3701 I0.5256 K-0.2144 -G1 X0.9500 Z-0.3897 - X0.9300 Z-0.4976 - X0.9250 Z-0.5000 - X0.5050 - X0.5000 Z-0.4976 - X0.4800 Z-0.3897 -G2 X0.4550 Y0.6350 Z-0.3124 I-0.4733 K-0.1103 -G2 X0.3850 Y0.6350 Z-0.2017 I-0.3774 K-0.1612 -G2 X0.3250 Y0.6350 Z-0.1494 I-0.2374 K-0.2120 -G2 X0.2700 Y0.6350 Z-0.1221 I-0.1317 K-0.1964 -G2 X0.2250 Y0.6350 Z-0.1118 I-0.0674 K-0.1899 -G1 X0.1850 - X0.1600 Z-0.1165 -G2 X0.0800 Y0.6350 Z-0.1624 I0.0471 K-0.1746 -G2 X0.0350 Y0.6350 Z-0.2359 I0.1553 K-0.1457 -G2 X0.0250 Y0.6350 Z-0.2810 I0.2293 K-0.0745 -G1 X0.0050 Z-0.4908 - X0.0000 Z-0.5000 - Y0.5950 - X0.0050 - X0.0100 Z-0.4947 - X0.0300 Z-0.2858 -G3 X0.0500 Y0.5950 Z-0.2163 I0.2392 K-0.0312 -G3 X0.1900 Y0.5950 Z-0.1118 I0.1629 K-0.0722 -G1 X0.2400 Z-0.1126 -G3 X0.3050 Y0.5950 Z-0.1319 I-0.0334 K-0.2310 -G3 X0.3700 Y0.5950 Z-0.1749 I-0.1239 K-0.2580 -G3 X0.4100 Y0.5950 Z-0.2153 I-0.2535 K-0.2911 -G3 X0.4500 Y0.5950 Z-0.2722 I-0.2209 K-0.1979 -G3 X0.4800 Y0.5950 Z-0.3328 I-0.4453 K-0.2579 -G3 X0.5000 Y0.5950 Z-0.3956 I-0.4175 K-0.1677 -G1 X0.5200 Z-0.4976 - X0.5250 Z-0.5000 - X0.9050 - X0.9100 Z-0.4976 - X0.9300 Z-0.3956 -G3 X0.9500 Y0.5950 Z-0.3328 I0.3783 K-0.0860 -G3 X1.0350 Y0.5950 Z-0.1989 I0.3998 K-0.1598 -G3 X1.1500 Y0.5950 Z-0.1221 I0.2105 K-0.1907 -G3 X1.1950 Y0.5950 Z-0.1118 I0.0674 K-0.1899 -G1 X1.2400 - X1.2650 Z-0.1165 -G3 X1.3800 Y0.5950 Z-0.2163 I-0.0490 K-0.1726 -G3 X1.4000 Y0.5950 Z-0.2858 I-0.2192 K-0.1007 -G1 X1.4200 Z-0.4947 - X1.4250 Z-0.5000 - X1.4300 - Y0.5550 - X1.4150 - X1.4100 Z-0.4838 -G3 X1.3950 Y0.5550 Z-0.3152 I4.6155 K0.4955 -G1 X1.3900 Z-0.2760 -G2 X1.3650 Y0.5550 Z-0.2045 I-0.1874 K-0.0254 -G2 X1.2300 Y0.5550 Z-0.1118 I-0.1569 K-0.0837 -G1 X1.1850 - X1.1750 Z-0.1134 -G2 X1.1100 Y0.5550 Z-0.1328 I0.0224 K-0.1936 -G2 X1.0700 Y0.5550 Z-0.1553 I0.1540 K-0.3210 -G2 X1.0000 Y0.5550 Z-0.2153 I0.2382 K-0.3487 -G2 X0.9250 Y0.5550 Z-0.3300 I0.3378 K-0.3027 -G2 X0.9100 Y0.5550 Z-0.3673 I0.3468 K-0.1613 -G2 X0.8850 Y0.5550 Z-0.4731 I0.7799 K-0.2400 -G3 X0.8750 Y0.5550 Z-0.5000 I-0.0647 K0.0088 -G1 X0.5550 -G3 X0.5450 Y0.5550 Z-0.4731 I0.0547 K0.0357 -G2 X0.5200 Y0.5550 Z-0.3673 I-0.8049 K-0.1342 -G2 X0.5050 Y0.5550 Z-0.3300 I-0.3618 K-0.1240 -G2 X0.4700 Y0.5550 Z-0.2673 I-0.2977 K-0.1249 -G1 X0.4450 Z-0.2329 - X0.4300 Z-0.2153 -G2 X0.3600 Y0.5550 Z-0.1553 I-0.3082 K-0.2887 -G2 X0.2900 Y0.5550 Z-0.1211 I-0.1582 K-0.2347 -G1 X0.2450 Z-0.1118 - X0.2000 - X0.1750 Z-0.1165 -G2 X0.0550 Y0.5550 Z-0.2241 I0.0508 K-0.1774 -G2 X0.0400 Y0.5550 Z-0.2760 I0.1558 K-0.0732 -G1 X0.0350 Z-0.3152 -G3 X0.0200 Y0.5550 Z-0.4838 I-4.6305 K0.3269 -G1 X0.0150 Z-0.5000 - X0.0000 - Y0.5150 - X0.0250 - X0.0300 Z-0.4878 -G2 X0.0450 Y0.5150 Z-0.3211 I-3.7054 K0.4175 -G1 X0.0500 Z-0.2819 -G3 X0.0750 Y0.5150 Z-0.2065 I0.2091 K-0.0275 -G3 X0.2150 Y0.5150 Z-0.1118 I0.1614 K-0.0878 -G1 X0.2600 - X0.2800 Z-0.1145 - X0.3000 Z-0.1185 - X0.3500 Z-0.1378 -G3 X0.4000 Y0.5150 Z-0.1671 I-0.1210 K-0.2642 -G3 X0.4650 Y0.5150 Z-0.2237 I-0.2889 K-0.3970 -G3 X0.5400 Y0.5150 Z-0.3222 I-0.3935 K-0.3776 -G3 X0.5700 Y0.5150 Z-0.3858 I-0.5285 K-0.2881 -G1 X0.6000 Z-0.4779 - X0.6100 Z-0.5000 - X0.8200 - X0.8300 Z-0.4779 - X0.8600 Z-0.3858 -G3 X0.8900 Y0.5150 Z-0.3222 I0.5585 K-0.2245 -G3 X1.0000 Y0.5150 Z-0.1907 I0.4774 K-0.2878 -G3 X1.0700 Y0.5150 Z-0.1426 I0.2348 K-0.2666 -G1 X1.1300 Z-0.1185 -G3 X1.1700 Y0.5150 Z-0.1118 I0.0801 K-0.3555 -G1 X1.2150 -G3 X1.2550 Y0.5150 Z-0.1213 I-0.0194 K-0.1706 -G3 X1.3150 Y0.5150 Z-0.1566 I-0.0526 K-0.1581 -G3 X1.3650 Y0.5150 Z-0.2261 I-0.1272 K-0.1442 -G3 X1.3800 Y0.5150 Z-0.2819 I-0.2142 K-0.0875 -G1 X1.3850 Z-0.3211 -G2 X1.4000 Y0.5150 Z-0.4878 I3.7204 K0.2508 -G1 X1.4050 Z-0.5000 - X1.4300 - Y0.4750 - X1.3900 - X1.3850 Z-0.4740 - X1.3700 Z-0.3074 - X1.3650 Z-0.2721 -G2 X1.3350 Y0.4750 Z-0.1976 I-0.1862 K-0.0318 -G2 X1.3150 Y0.4750 Z-0.1715 I-0.1196 K-0.0707 -G2 X1.2800 Y0.4750 Z-0.1416 I-0.1699 K-0.1632 -G2 X1.1950 Y0.4750 Z-0.1118 I-0.1042 K-0.1611 -G1 X1.1450 -G2 X1.1000 Y0.4750 Z-0.1213 I0.0434 K-0.3169 -G2 X1.0000 Y0.4750 Z-0.1672 I0.1053 K-0.3611 -G1 X0.9750 Z-0.1845 - X0.9300 Z-0.2198 - X0.9050 Z-0.2427 -G2 X0.8500 Y0.4750 Z-0.2986 I0.8446 K-0.8861 -G1 X0.7800 Z-0.3760 - X0.7400 Z-0.4163 - X0.7300 Z-0.4222 - X0.7150 Z-0.4243 - X0.7000 Z-0.4222 - X0.6900 Z-0.4163 - X0.6500 Z-0.3760 - X0.5800 Z-0.2986 -G2 X0.5250 Y0.4750 Z-0.2427 I-0.8996 K-0.8302 -G2 X0.4250 Y0.4750 Z-0.1641 I-0.4292 K-0.4429 -G2 X0.3150 Y0.4750 Z-0.1173 I-0.1969 K-0.3105 -G1 X0.2850 Z-0.1118 - X0.2350 - X0.2000 Z-0.1185 -G2 X0.0850 Y0.4750 Z-0.2143 I0.0531 K-0.1806 -G2 X0.0650 Y0.4750 Z-0.2721 I0.1598 K-0.0877 -G1 X0.0600 Z-0.3074 - X0.0450 Z-0.4740 - X0.0400 Z-0.5000 - X0.0000 - Y0.4350 - X0.0550 - X0.0600 Z-0.4760 - X0.0750 Z-0.3172 - X0.0800 Z-0.2799 -G3 X0.1200 Y0.4350 Z-0.1859 I0.2031 K-0.0309 -G1 X0.1350 Z-0.1692 - X0.1700 Z-0.1416 - X0.1850 Z-0.1327 -G3 X0.2550 Y0.4350 Z-0.1118 I0.0915 K-0.1781 -G1 X0.3100 -G3 X0.3850 Y0.4350 Z-0.1289 I-0.0410 K-0.3522 -G1 X0.4500 Z-0.1563 - X0.4650 Z-0.1641 -G3 X0.6050 Y0.4350 Z-0.2506 I-1.2849 K-2.2366 -G1 X0.6350 Z-0.2653 - X0.6400 Z-0.2692 - X0.6700 Z-0.2810 - X0.6750 Z-0.2811 - X0.6850 Z-0.2849 - X0.7050 Z-0.2870 - X0.7300 Z-0.2869 - X0.7350 Z-0.2850 - X0.7450 Z-0.2849 - X0.7550 Z-0.2811 - X0.7600 Z-0.2810 - X0.7900 Z-0.2692 - X0.7950 Z-0.2653 - X0.8250 Z-0.2506 - X0.9100 Z-0.1966 - X0.9650 Z-0.1641 -G3 X1.0100 Y0.4350 Z-0.1445 I0.1129 K-0.1976 -G1 X1.0200 Z-0.1387 - X1.0500 Z-0.1271 -G3 X1.1200 Y0.4350 Z-0.1118 I0.1129 K-0.3471 -G1 X1.1750 - X1.2050 Z-0.1173 -G3 X1.2600 Y0.4350 Z-0.1416 I-0.0499 K-0.1879 -G1 X1.3000 Z-0.1741 -G3 X1.3250 Y0.4350 Z-0.2094 I-0.1155 K-0.1083 -G3 X1.3500 Y0.4350 Z-0.2799 I-0.1638 K-0.0978 -G1 X1.3550 Z-0.3172 - X1.3700 Z-0.4760 - X1.3750 Z-0.5000 - X1.4300 - Y0.3950 - X1.3550 - X1.3500 Z-0.4740 - X1.3350 Z-0.3132 - X1.3300 Z-0.2779 -G2 X1.2950 Y0.3950 Z-0.1937 I-0.1999 K-0.0337 -G2 X1.2250 Y0.3950 Z-0.1347 I-0.1574 K-0.1157 -G2 X1.1450 Y0.3950 Z-0.1118 I-0.0974 K-0.1890 -G1 X1.0900 -G2 X1.0250 Y0.3950 Z-0.1232 I0.0666 K-0.5680 -G1 X0.9600 Z-0.1445 - X0.8950 Z-0.1711 - X0.8850 Z-0.1769 - X0.8050 Z-0.2068 -G3 X0.7200 Y0.3950 Z-0.2223 I-0.1085 K0.3538 -G1 X0.6800 Z-0.2193 -G3 X0.6250 Y0.3950 Z-0.2068 I0.0615 K0.3967 -G1 X0.5450 Z-0.1769 - X0.5350 Z-0.1711 - X0.4700 Z-0.1445 - X0.4050 Z-0.1232 -G2 X0.3400 Y0.3950 Z-0.1118 I-0.1470 K-0.6436 -G1 X0.2850 -G2 X0.2400 Y0.3950 Z-0.1204 I0.0239 K-0.2452 -G2 X0.1150 Y0.3950 Z-0.2289 I0.0541 K-0.1886 -G2 X0.1000 Y0.3950 Z-0.2779 I0.1669 K-0.0779 -G1 X0.0950 Z-0.3132 - X0.0800 Z-0.4740 - X0.0750 Z-0.5000 - X0.0000 - Y0.3550 - X0.0950 - X0.1000 Z-0.4897 -G2 X0.1150 Y0.3550 Z-0.3387 I-2.5067 K0.3253 -G3 X0.1250 Y0.3550 Z-0.2681 I0.5491 K-0.0418 -G3 X0.1500 Y0.3550 Z-0.2074 I0.1750 K-0.0366 -G3 X0.1800 Y0.3550 Z-0.1711 I0.1329 K-0.0792 -G3 X0.2250 Y0.3550 Z-0.1386 I0.1413 K-0.1482 -G3 X0.2600 Y0.3550 Z-0.1232 I0.0857 K-0.1473 -G3 X0.3150 Y0.3550 Z-0.1118 I0.0965 K-0.3255 -G1 X0.3800 - X0.4200 Z-0.1173 - X0.5000 Z-0.1348 - X0.5650 Z-0.1533 - X0.6200 Z-0.1671 -G2 X0.7000 Y0.3550 Z-0.1778 I0.1137 K0.5417 -G1 X0.7300 - X0.7950 Z-0.1700 - X0.8350 Z-0.1612 - X0.9150 Z-0.1387 - X0.9700 Z-0.1252 -G3 X1.0500 Y0.3550 Z-0.1118 I0.1616 K-0.7183 -G1 X1.1150 -G3 X1.1700 Y0.3550 Z-0.1232 I-0.0548 K-0.4009 -G3 X1.2500 Y0.3550 Z-0.1711 I-0.0640 K-0.1977 -G3 X1.2950 Y0.3550 Z-0.2359 I-0.1246 K-0.1346 -G3 X1.3100 Y0.3550 Z-0.2956 I-0.2391 K-0.0918 -G1 X1.3300 Z-0.4897 - X1.3350 Z-0.5000 - X1.4300 - Y0.3150 - X1.3100 - X1.3050 Z-0.4917 - X1.2850 Z-0.3093 - X1.2800 Z-0.2779 -G2 X1.2600 Y0.3150 Z-0.2202 I-0.1831 K-0.0311 -G2 X1.1850 Y0.3150 Z-0.1445 I-0.1803 K-0.1036 -G3 X1.1650 Y0.3150 Z-0.1347 I0.0208 K0.0677 -G1 X1.1350 Z-0.1232 - X1.0750 Z-0.1118 - X1.0000 - X0.9800 Z-0.1137 -G3 X0.9400 Y0.3150 Z-0.1185 I-0.0543 K0.2873 -G2 X0.8900 Y0.3150 Z-0.1280 I0.1653 K-1.0018 -G1 X0.8250 Z-0.1376 - X0.7350 Z-0.1465 - X0.6750 Z-0.1455 - X0.6350 Z-0.1416 - X0.5400 Z-0.1280 -G2 X0.4900 Y0.3150 Z-0.1185 I-0.2158 K-0.9947 -G1 X0.4300 Z-0.1118 - X0.3550 - X0.2900 Z-0.1250 - X0.2650 Z-0.1347 -G3 X0.2450 Y0.3150 Z-0.1445 I-0.0408 K0.0579 -G1 X0.2100 Z-0.1708 -G2 X0.1650 Y0.3150 Z-0.2300 I0.1264 K-0.1428 -G2 X0.1500 Y0.3150 Z-0.2779 I0.1695 K-0.0793 -G1 X0.1450 Z-0.3093 - X0.1250 Z-0.4917 - X0.1200 Z-0.5000 - X0.0000 - Y0.2750 - X0.1500 - X0.1550 Z-0.4947 - X0.1600 Z-0.4642 - X0.1750 Z-0.3191 - X0.1800 Z-0.2858 -G3 X0.2100 Y0.2750 Z-0.2084 I0.1676 K-0.0205 -G3 X0.2900 Y0.2750 Z-0.1406 I0.1759 K-0.1263 -G1 X0.3450 Z-0.1213 - X0.4050 Z-0.1118 - X0.4950 - X0.6050 Z-0.1202 - X0.6150 Z-0.1221 - X0.6300 - X0.6400 Z-0.1241 - X0.6600 - X0.6700 Z-0.1260 - X0.7600 - X0.7700 Z-0.1241 - X0.7900 - X0.8000 Z-0.1221 - X0.8150 - X0.8250 Z-0.1202 - X0.9350 Z-0.1118 - X1.0250 - X1.0850 Z-0.1213 -G2 X1.1200 Y0.2750 Z-0.1327 I0.0718 K0.1601 -G3 X1.1600 Y0.2750 Z-0.1523 I-0.0415 K-0.1353 -G3 X1.1900 Y0.2750 Z-0.1747 I-0.0939 K-0.1571 -G3 X1.2350 Y0.2750 Z-0.2339 I-0.1273 K-0.1434 -G3 X1.2500 Y0.2750 Z-0.2858 I-0.1558 K-0.0732 -G1 X1.2550 Z-0.3191 - X1.2700 Z-0.4642 - X1.2750 Z-0.4947 - X1.2800 Z-0.5000 - X1.4300 - Y0.2350 - X1.2450 - X1.2400 Z-0.4928 - X1.2350 Z-0.4653 -G3 X1.2200 Y0.2350 Z-0.3339 I3.4995 K0.4661 -G2 X1.2100 Y0.2350 Z-0.2692 I-0.6965 K-0.0745 -G2 X1.1850 Y0.2350 Z-0.2124 I-0.1561 K-0.0347 -G2 X1.1500 Y0.2350 Z-0.1761 I-0.1487 K-0.1084 -G2 X1.1050 Y0.2350 Z-0.1465 I-0.1268 K-0.1436 -G1 X1.0600 Z-0.1289 - X1.0450 Z-0.1250 -G2 X0.9550 Y0.2350 Z-0.1118 I-0.1216 K-0.5155 -G1 X0.8200 - X0.7550 Z-0.1145 - X0.6750 - X0.6100 Z-0.1118 - X0.4750 -G2 X0.3850 Y0.2350 Z-0.1250 I0.0316 K-0.5287 -G1 X0.3700 Z-0.1289 - X0.3250 Z-0.1465 -G2 X0.3000 Y0.2350 Z-0.1610 I0.0636 K-0.1382 -G2 X0.2600 Y0.2350 Z-0.1943 I0.1578 K-0.2300 -G2 X0.2250 Y0.2350 Z-0.2516 I0.1092 K-0.1061 -G2 X0.2150 Y0.2350 Z-0.2967 I0.2293 K-0.0745 -G1 X0.1950 Z-0.4653 - X0.1900 Z-0.4928 - X0.1850 Z-0.5000 - X0.0000 - Y0.1950 - X0.2250 - X0.2300 Z-0.4928 - X0.2350 Z-0.4653 -G2 X0.2500 Y0.1950 Z-0.3378 I-2.7633 K0.3898 -G3 X0.2600 Y0.1950 Z-0.2771 I0.5783 K-0.0639 -G3 X0.2950 Y0.1950 Z-0.2065 I0.1507 K-0.0308 -G3 X0.3250 Y0.1950 Z-0.1786 I0.2080 K-0.1941 -G1 X0.3650 Z-0.1535 -G3 X0.4250 Y0.1950 Z-0.1309 I0.2024 K-0.4460 -G3 X0.4950 Y0.1950 Z-0.1182 I0.1565 K-0.6657 -G1 X0.5900 Z-0.1118 - X0.8400 - X0.9200 Z-0.1165 -G3 X1.0050 Y0.1950 Z-0.1309 I-0.0591 K-0.6085 -G3 X1.0650 Y0.1950 Z-0.1535 I-0.1424 K-0.4686 -G1 X1.1000 Z-0.1750 - X1.1250 Z-0.1963 -G3 X1.1600 Y0.1950 Z-0.2457 I-0.0982 K-0.1067 -G3 X1.1750 Y0.1950 Z-0.3026 I-0.2253 K-0.0898 -G1 X1.1950 Z-0.4653 - X1.2000 Z-0.4928 - X1.2050 Z-0.5000 - X1.4300 - Y0.1550 - X1.1600 - X1.1550 Z-0.4967 - X1.1500 Z-0.4740 - X1.1300 Z-0.3309 -G2 X1.1200 Y0.1550 Z-0.2779 I-0.6503 K-0.0954 -G2 X1.0850 Y0.1550 Z-0.2124 I-0.1433 K-0.0344 -G2 X1.0250 Y0.1550 Z-0.1672 I-0.1527 K-0.1404 -G1 X0.9600 Z-0.1416 - X0.8850 Z-0.1260 - X0.8450 Z-0.1221 - X0.8300 - X0.8200 Z-0.1202 - X0.7950 - X0.7800 Z-0.1182 - X0.6500 - X0.6350 Z-0.1202 - X0.6100 - X0.6000 Z-0.1221 - X0.5850 - X0.5450 Z-0.1260 -G2 X0.4700 Y0.1550 Z-0.1416 I0.0820 K-0.5854 -G1 X0.4050 Z-0.1672 -G2 X0.3450 Y0.1550 Z-0.2124 I0.0927 K-0.1855 -G2 X0.3150 Y0.1550 Z-0.2623 I0.0958 K-0.0916 -G2 X0.3050 Y0.1550 Z-0.3015 I0.1920 K-0.0699 -G1 X0.2800 Z-0.4740 - X0.2750 Z-0.4967 - X0.2700 Z-0.5000 - X0.0000 - Y0.1150 - X0.3300 Z-0.4995 -G2 X0.3400 Y0.1150 Z-0.4564 I-0.5300 K0.1456 -G1 X0.3600 Z-0.3309 -G3 X0.3700 Y0.1150 Z-0.2838 I0.3430 K-0.0483 -G3 X0.4050 Y0.1150 Z-0.2230 I0.1178 K-0.0274 -G3 X0.4600 Y0.1150 Z-0.1820 I0.1504 K-0.1439 -G1 X0.5200 Z-0.1578 - X0.5450 Z-0.1514 - X0.6050 Z-0.1406 - X0.6450 Z-0.1367 - X0.7000 Z-0.1347 - X0.7850 Z-0.1367 - X0.8250 Z-0.1406 - X0.8850 Z-0.1514 -G3 X0.9650 Y0.1150 Z-0.1793 I-0.0727 K-0.3365 -G3 X1.0150 Y0.1150 Z-0.2133 I-0.0933 K-0.1909 -G3 X1.0600 Y0.1150 Z-0.2838 I-0.0890 K-0.1064 -G3 X1.0700 Y0.1150 Z-0.3309 I-0.3330 K-0.0953 -G1 X1.0900 Z-0.4564 -G2 X1.1000 Y0.1150 Z-0.4995 I0.5400 K0.1025 -G1 X1.4300 Z-0.5000 - Y0.0750 - X1.0300 -G3 X1.0200 Y0.0750 Z-0.4740 I0.0429 K0.0314 -G1 X0.9900 Z-0.3230 -G2 X0.9600 Y0.0750 Z-0.2505 I-0.2142 K-0.0461 -G2 X0.9250 Y0.0750 Z-0.2182 I-0.1063 K-0.0802 -G1 X0.9050 Z-0.2084 - X0.9000 Z-0.2045 - X0.8600 Z-0.1888 - X0.8300 Z-0.1801 - X0.7900 Z-0.1723 - X0.7500 Z-0.1683 - X0.6800 - X0.6700 Z-0.1703 - X0.6550 -G2 X0.5950 Y0.0750 Z-0.1813 I0.0505 K-0.4447 -G1 X0.5800 Z-0.1852 - X0.5300 Z-0.2045 - X0.5250 Z-0.2084 - X0.5050 Z-0.2182 - X0.4850 Z-0.2339 -G2 X0.4500 Y0.0750 Z-0.2897 I0.0820 K-0.0903 -G2 X0.4350 Y0.0750 Z-0.3446 I0.4231 K-0.1451 -G3 X0.4100 Y0.0750 Z-0.4740 I-3.8695 K0.6804 -G3 X0.4000 Y0.0750 Z-0.5000 I-0.0529 K0.0055 -G1 X0.0000 - Y0.0350 - X0.5000 -G2 X0.5100 Y0.0350 Z-0.4878 I-0.0032 K0.0128 -G3 X0.5300 Y0.0350 Z-0.4348 I0.5135 K-0.1637 -G3 X0.5650 Y0.0350 Z-0.3289 I1.3000 K-0.3710 -G2 X0.5900 Y0.0350 Z-0.2878 I-0.3832 K0.2608 -G1 X0.6000 Z-0.2771 -G3 X0.6350 Y0.0350 Z-0.2516 I0.1125 K-0.1177 -G1 X0.6650 Z-0.2419 - X0.6850 Z-0.2398 - X0.7000 Z-0.2360 - X0.7300 - X0.7800 Z-0.2458 - X0.7950 Z-0.2516 - X0.8050 Z-0.2575 - X0.8350 Z-0.2819 - X0.8450 Z-0.2956 -G2 X0.8650 Y0.0350 Z-0.3289 I0.1832 K0.0873 -G3 X0.9000 Y0.0350 Z-0.4348 I-1.2650 K-0.4769 -G1 X0.9250 Z-0.4976 - X0.9300 Z-0.5000 - X1.4300 - Y0.0050 - X0.7450 - X0.7300 Z-0.4888 - X0.7250 Z-0.4870 - X0.7050 - X0.7000 Z-0.4888 - X0.6850 Z-0.5000 - X0.0000 -G19 -G0 Z0.0120 - X1.4000 Y1.0650 -G1 Z-0.3750 -G2 Y0.940000 Z-0.500000 R0.125000 -G1 Y0.9350 - Y0.9300 Z-0.4976 - Y0.9250 Z-0.4878 - Y0.9050 Z-0.4348 -G3 X1.4000 Y0.8700 Z-0.3289 J-1.3000 K-0.3710 -G1 Y0.8450 Z-0.2858 - Y0.8100 Z-0.2575 -G0 Z0.0120 - Y0.6301 -G1 Z-0.2562 -G2 Y0.625000 Z-0.261378 R0.005148 -G1 Y0.5950 Z-0.2858 - Y0.5700 Z-0.3289 -G3 X1.4000 Y0.5350 Z-0.4348 J1.2650 K-0.4769 -G1 Y0.5150 Z-0.4878 - Y0.5100 Z-0.4976 - Y0.5050 Z-0.5000 - Y0.4950 -G0 Z0.0120 - X1.3600 Y0.2850 -G1 Z-0.3667 -G3 Y0.410000 Z-0.491673 R0.125000 -G3 X1.3600 Y0.4200 Z-0.4505 J-0.3032 K0.0954 -G1 Y0.4400 Z-0.3446 -G2 X1.3600 Y0.4550 Z-0.2897 J0.4381 K-0.0902 -G2 X1.3600 Y0.4950 Z-0.2300 J0.1159 K-0.0344 -G0 Z0.0120 - Y0.9450 -G1 Z-0.2298 -G3 Y0.950000 Z-0.234810 R0.005004 -G2 X1.3600 Y0.9850 Z-0.2897 J-0.0740 K-0.0858 -G2 X1.3600 Y1.0000 Z-0.3446 J-0.4231 K-0.1451 -G1 Y1.0250 Z-0.4740 -G3 X1.3600 Y1.0350 Z-0.5000 J0.0529 K0.0055 -G0 Z0.0120 - X1.3200 Y1.2450 -G1 Z-0.3750 -G2 Y1.120000 Z-0.500000 R0.125000 -G1 Y1.1050 Z-0.4995 - Y1.1000 Z-0.4819 -G2 X1.3200 Y1.0750 Z-0.3309 J3.8276 K0.7114 -G3 X1.3200 Y1.0650 Z-0.2838 J-0.3430 K-0.0483 -G3 X1.3200 Y1.0350 Z-0.2289 J-0.1104 K-0.0247 -G0 Z0.0120 - Y0.4100 -G1 Z-0.2239 -G2 Y0.400000 Z-0.233927 R0.010000 -G3 X1.3200 Y0.3700 Z-0.3034 J0.1057 K-0.0868 -G1 Y0.3450 Z-0.4544 -G2 X1.3200 Y0.3350 Z-0.4995 J-0.2393 K0.0294 -G1 Y0.3150 Z-0.5000 -G0 Z0.0120 - X1.2800 Y0.1500 -G1 Z-0.3750 -G3 Y0.275000 Z-0.500000 R0.125000 -G1 Y0.2800 Z-0.4967 - Y0.2850 Z-0.4740 - Y0.3050 Z-0.3309 -G2 X1.2800 Y0.3150 Z-0.2779 J0.6503 K-0.0954 -G2 X1.2800 Y0.3600 Z-0.2026 J0.1430 K-0.0342 -G0 Z0.0120 - Y1.0800 -G1 Z-0.2024 -G3 Y1.085000 Z-0.207445 R0.005001 -G1 Y1.0950 Z-0.2182 - Y1.1100 Z-0.2398 -G2 X1.2800 Y1.1250 Z-0.2779 J-0.1358 K-0.0754 -G2 X1.2800 Y1.1350 Z-0.3309 J-0.6403 K-0.1484 -G1 Y1.1550 Z-0.4740 - Y1.1600 Z-0.4967 - Y1.1650 Z-0.5000 - Y1.1700 -G0 Z0.0120 - X1.2400 Y1.3350 -G1 Z-0.3750 -G2 Y1.210000 Z-0.500000 R0.125000 -G1 Y1.2050 Z-0.4928 - Y1.2000 Z-0.4673 -G2 X1.2400 Y1.1850 Z-0.3378 J3.9383 K0.5221 -G3 X1.2400 Y1.1750 Z-0.2751 J-0.8001 K-0.0953 -G3 X1.2400 Y1.1250 Z-0.1918 J-0.1549 K-0.0363 -G0 Z0.0120 - Y0.3150 -G1 Z-0.1913 -G2 Y0.310000 Z-0.196283 R0.005025 -G3 X1.2400 Y0.2650 Z-0.2751 J0.1028 K-0.1109 -G3 X1.2400 Y0.2550 Z-0.3378 J0.7901 K-0.1581 -G2 X1.2400 Y0.2400 Z-0.4673 J-3.9533 K0.3926 -G1 Y0.2350 Z-0.4928 - Y0.2300 Z-0.5000 - Y0.2250 -G0 Z0.0120 - X1.2000 Y0.0600 -G1 Z-0.3750 -G3 Y0.185000 Z-0.500000 R0.125000 -G1 Y0.1900 - Y0.1950 Z-0.4928 - Y0.2000 Z-0.4653 -G3 X1.2000 Y0.2150 Z-0.3339 J-3.4995 K0.4661 -G2 X1.2000 Y0.2250 Z-0.2692 J0.6965 K-0.0745 -G2 X1.2000 Y0.2700 Z-0.1898 J0.1505 K-0.0328 -G0 Z0.0120 - Y1.1700 -G1 Z-0.1893 -G3 Y1.175000 Z-0.194322 R0.005025 -G2 X1.2000 Y1.2150 Z-0.2692 J-0.1105 K-0.1071 -G2 X1.2000 Y1.2250 Z-0.3339 J-0.6865 K-0.1392 -G3 X1.2000 Y1.2400 Z-0.4653 J3.5145 K0.3347 -G1 Y1.2450 Z-0.4928 - Y1.2500 Z-0.5000 - Y1.2600 -G0 Z0.0120 - X1.1600 Y1.4250 -G1 Z-0.3750 -G2 Y1.300000 Z-0.500000 R0.125000 -G1 Y1.2850 - Y1.2800 Z-0.4928 - Y1.2750 Z-0.4642 - Y1.2600 Z-0.3191 -G3 X1.1600 Y1.2500 Z-0.2614 J-0.5479 K-0.0652 -G3 X1.1600 Y1.2150 Z-0.1963 J-0.1504 K-0.0389 -G0 Z0.0120 - Y0.2250 -G1 Z-0.1972 -G2 Y0.220000 Z-0.202165 R0.005000 -G3 X1.1600 Y0.1900 Z-0.2614 J0.1104 K-0.0931 -G3 X1.1600 Y0.1800 Z-0.3191 J0.5379 K-0.1229 -G1 Y0.1650 Z-0.4642 - Y0.1600 Z-0.4928 - Y0.1550 Z-0.5000 - Y0.1350 -G0 Z0.0120 - X1.1200 Y-0.0300 -G1 Z-0.3750 -G3 Y0.095000 Z-0.500000 R0.125000 -G1 Y0.1250 - Y0.1300 Z-0.4917 - Y0.1500 Z-0.3093 - Y0.1550 Z-0.2779 -G2 X1.1200 Y0.1800 Z-0.2124 J0.1727 K-0.0283 -G0 Z0.0120 - Y1.2600 -G1 Z-0.2152 -G3 Y1.265000 Z-0.220201 R0.005000 -G2 X1.1200 Y1.2850 Z-0.2779 J-0.1598 K-0.0877 -G1 Y1.2900 Z-0.3093 - Y1.3100 Z-0.4917 - Y1.3150 Z-0.5000 - Y1.3500 -G0 Z0.0120 - X1.0800 Y1.4700 -G1 Z-0.3750 -G2 Y1.345000 Z-0.500000 R0.125000 -G1 Y1.3400 - Y1.3350 Z-0.4897 -G2 X1.0800 Y1.3200 Z-0.3387 J2.5067 K0.3253 -G3 X1.0800 Y1.3100 Z-0.2681 J-0.5491 K-0.0418 -G3 X1.0800 Y1.2600 Z-0.1761 J-0.1894 K-0.0433 -G0 Z0.0120 - Y0.1800 -G1 Z-0.1770 -G2 Y0.175000 Z-0.181955 R0.005000 -G3 X1.0800 Y0.1500 Z-0.2163 J0.0910 K-0.0925 -G3 X1.0800 Y0.1300 Z-0.2681 J0.1780 K-0.0984 -G3 X1.0800 Y0.1200 Z-0.3387 J0.5391 K-0.1124 -G2 X1.0800 Y0.1050 Z-0.4897 J-2.5217 K0.1743 -G1 Y0.1000 Z-0.5000 - Y0.0900 -G0 Z0.0120 - X1.0400 Y-0.0750 -G1 Z-0.3750 -G3 Y0.050000 Z-0.500000 R0.125000 -G1 Y0.0800 - Y0.0850 Z-0.4740 - Y0.1000 Z-0.3132 - Y0.1050 Z-0.2779 -G2 X1.0400 Y0.1350 Z-0.2016 J0.1940 K-0.0321 -G0 Z0.0120 - Y1.3050 -G1 Z-0.2054 -G3 Y1.310000 Z-0.210398 R0.005000 -G2 X1.0400 Y1.3350 Z-0.2779 J-0.1559 K-0.0961 -G1 Y1.3400 Z-0.3132 - Y1.3550 Z-0.4740 - Y1.3600 Z-0.5000 - Y1.3950 -G0 Z0.0120 - X1.0000 Y1.5150 -G1 Z-0.3750 -G2 Y1.390000 Z-0.500000 R0.125000 -G1 Y1.3800 - Y1.3750 Z-0.4779 - Y1.3600 Z-0.3172 -G3 X1.0000 Y1.3500 Z-0.2564 J-0.4043 K-0.0353 -G3 X1.0000 Y1.3050 Z-0.1741 J-0.1963 K-0.0539 -G0 Z0.0120 - Y0.1350 -G1 Z-0.1750 -G2 Y0.130000 Z-0.179994 R0.005000 -G3 X1.0000 Y0.0900 Z-0.2564 J0.1737 K-0.1396 -G3 X1.0000 Y0.0800 Z-0.3172 J0.3943 K-0.0961 -G1 Y0.0650 Z-0.4779 - Y0.0600 Z-0.5000 - Y0.0450 -G0 Z0.0120 - X0.9600 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0450 - Y0.0500 Z-0.4740 - Y0.0650 Z-0.3074 - Y0.0700 Z-0.2721 -G2 X0.9600 Y0.0900 Z-0.2143 J0.1831 K-0.0311 -G0 Z0.0120 - Y1.3450 -G1 Z-0.2093 -G3 Y1.350000 Z-0.214319 R0.005000 -G2 X0.9600 Y1.3700 Z-0.2721 J-0.1598 K-0.0877 -G1 Y1.3750 Z-0.3074 - Y1.3900 Z-0.4740 - Y1.3950 Z-0.5000 - Y1.4350 -G0 Z0.0120 - X0.9200 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4100 - Y1.4050 Z-0.4858 -G2 X0.9200 Y1.3900 Z-0.3211 J3.0412 K0.3600 -G1 Y1.3850 Z-0.2819 -G3 X0.9200 Y1.3500 Z-0.1908 J-0.1937 K-0.0222 -G0 Z0.0120 - Y0.9000 -G1 Z-0.3172 -G2 Y0.895000 Z-0.322162 R0.005000 -G3 X0.9200 Y0.8650 Z-0.3858 J0.5285 K-0.2881 -G1 Y0.8300 Z-0.4917 - Y0.8250 Z-0.5000 - Y0.8100 -G0 Z0.0120 - Y0.7500 -G1 Z-0.3750 -G2 Y0.625000 Z-0.500000 R0.125000 -G1 Y0.6150 - Y0.6100 Z-0.4917 - Y0.5750 Z-0.3858 -G3 X0.9200 Y0.5400 Z-0.3143 J-0.4659 K-0.1838 -G0 Z0.0120 - Y0.0850 -G1 Z-0.2015 -G2 Y0.080000 Z-0.206476 R0.005000 -G3 X0.9200 Y0.0550 Z-0.2819 J0.1653 K-0.0966 -G1 Y0.0500 Z-0.3211 -G2 X0.9200 Y0.0350 Z-0.4858 J-3.0562 K0.1953 -G1 Y0.0300 Z-0.5000 - Y0.0050 -G0 Z0.0120 - X0.8800 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0200 - Y0.0250 Z-0.4838 -G3 X0.8800 Y0.0400 Z-0.3152 J-4.6155 K0.4955 -G2 X0.8800 Y0.0500 Z-0.2516 J0.4273 K-0.0345 -G2 X0.8800 Y0.0900 Z-0.1754 J0.1935 K-0.0530 -G0 Z0.0120 - Y0.4500 -G1 Z-0.2358 -G3 Y0.455000 Z-0.240779 R0.005000 -G2 X0.8800 Y0.4900 Z-0.2908 J-0.1806 K-0.1637 -G2 X0.8800 Y0.5150 Z-0.3418 J-0.2462 K-0.1523 -G2 X0.8800 Y0.5300 Z-0.3849 J-0.5212 K-0.2054 -G1 Y0.5550 Z-0.4908 - Y0.5600 Z-0.5000 - Y0.5850 -G0 Z0.0120 - Y0.7350 -G1 Z-0.3750 -G3 Y0.860000 Z-0.500000 R0.125000 -G1 Y0.8800 - Y0.8850 Z-0.4908 - Y0.9100 Z-0.3849 -G2 X0.8800 Y0.9250 Z-0.3418 J0.5362 K-0.1623 -G2 X0.8800 Y0.9500 Z-0.2908 J0.2712 K-0.1014 -G2 X0.8800 Y0.9900 Z-0.2329 J0.4097 K-0.2405 -G0 Z0.0120 - Y1.3650 -G1 Z-0.1995 -G3 Y1.370000 Z-0.204515 R0.005000 -G2 X0.8800 Y1.3950 Z-0.2760 J-0.1624 K-0.0969 -G1 Y1.4000 Z-0.3152 -G3 X0.8800 Y1.4150 Z-0.4838 J4.6305 K0.3269 -G1 Y1.4200 Z-0.5000 - Y1.4350 -G0 Z0.0120 - X0.8400 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 - Y1.4250 Z-0.4936 -G2 X0.8400 Y1.4100 Z-0.3309 J2.0906 K0.2748 -G3 X0.8400 Y1.4000 Z-0.2594 J-0.6072 K-0.0485 -G3 X0.8400 Y1.3500 Z-0.1656 J-0.1791 K-0.0352 -G0 Z0.0120 - Y0.9900 -G1 Z-0.2672 -G2 Y0.985000 Z-0.272151 R0.005000 -G3 X0.8400 Y0.9400 Z-0.3760 J0.3975 K-0.2339 -G3 X0.8400 Y0.9300 Z-0.4191 J0.5300 K-0.1456 -G2 X0.8400 Y0.9150 Z-0.4976 J-1.3493 K0.2174 -G1 Y0.9100 Z-0.5000 - Y0.9000 -G0 Z0.0120 - Y0.6600 -G1 Z-0.3750 -G2 Y0.535000 Z-0.500000 R0.125000 -G1 Y0.5300 - Y0.5250 Z-0.4976 - Y0.5050 Z-0.3956 -G3 X0.8400 Y0.4950 Z-0.3603 J-0.3043 K-0.0672 -G3 X0.8400 Y0.4500 Z-0.2633 J-0.4346 K-0.1428 -G0 Z0.0120 - Y0.0650 -G1 Z-0.2015 -G2 Y0.060000 Z-0.206476 R0.005000 -G3 X0.8400 Y0.0400 Z-0.2594 J0.1413 K-0.0836 -G3 X0.8400 Y0.0300 Z-0.3309 J0.5972 K-0.1200 -G2 X0.8400 Y0.0150 Z-0.4936 J-2.1056 K0.1120 -G1 Y0.0100 Z-0.5000 - Y0.0050 -G0 Z0.0120 - X0.8000 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0100 Z-0.4908 -G3 X0.8000 Y0.0250 Z-0.3230 J-2.6266 K0.3194 -G2 X0.8000 Y0.0350 Z-0.2555 J0.5120 K-0.0413 -G2 X0.8000 Y0.0900 Z-0.1582 J0.1850 K-0.0404 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2214 -G3 Y0.410000 Z-0.226387 R0.005000 -G2 X0.8000 Y0.4700 Z-0.3387 J-0.3148 K-0.2403 -G2 X0.8000 Y0.4850 Z-0.3897 J-0.4248 K-0.1527 -G1 Y0.5050 Z-0.4976 - Y0.5100 Z-0.5000 - Y0.5400 -G0 Z0.0120 - Y0.7800 -G1 Z-0.3750 -G3 Y0.905000 Z-0.500000 R0.125000 -G1 Y0.9300 - Y0.9350 Z-0.4976 - Y0.9550 Z-0.3897 -G2 X0.8000 Y0.9700 Z-0.3387 J0.4398 K-0.1017 -G2 X0.8000 Y1.0350 Z-0.2203 J0.3683 K-0.1252 -G0 Z0.0120 - Y1.3800 -G1 Z-0.1995 -G3 Y1.385000 Z-0.204515 R0.005000 -G2 X0.8000 Y1.4000 Z-0.2378 J-0.0821 K-0.0570 -G2 X0.8000 Y1.4100 Z-0.2819 J-0.2424 K-0.0782 -G1 Y1.4150 Z-0.3230 -G3 X0.8000 Y1.4300 Z-0.4908 J2.6416 K0.1517 -G1 Y1.4350 Z-0.5000 -G0 Z0.0120 - X0.7600 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 Z-0.4811 - Y1.4150 Z-0.3045 -G3 X0.7600 Y1.4050 Z-0.2418 J-0.3841 K-0.0290 -G3 X0.7600 Y1.3850 Z-0.1968 J-0.1489 K-0.0393 -G3 X0.7600 Y1.3500 Z-0.1543 J-0.1622 K-0.0979 -G0 Z0.0120 - Y1.0350 -G1 Z-0.2330 -G2 Y1.030000 Z-0.237987 R0.005000 -G3 X0.7600 Y0.9800 Z-0.3398 J0.3107 K-0.2158 -G3 X0.7600 Y0.9650 Z-0.3967 J0.3961 K-0.1349 -G1 Y0.9500 Z-0.4858 - Y0.9450 Z-0.5000 -G0 Z0.0120 - Y0.6150 -G1 Z-0.3608 -G2 Y0.490000 Z-0.485791 R0.125000 -G1 Y0.4750 Z-0.3967 -G3 X0.7600 Y0.4600 Z-0.3398 J-0.4111 K-0.0780 -G3 X0.7600 Y0.4050 Z-0.2311 J-0.3725 K-0.1202 -G0 Z0.0120 - Y0.0600 -G1 Z-0.1918 -G2 Y0.055000 Z-0.196811 R0.005000 -G3 X0.7600 Y0.0300 Z-0.2653 J0.1399 K-0.0899 -G1 Y0.0250 Z-0.3045 - Y0.0100 Z-0.4811 - Y0.0050 Z-0.5000 -G0 Z0.0120 - X0.7200 Y-0.1200 -G1 Z-0.3620 -G3 Y0.005000 Z-0.487007 R0.125000 -G3 X0.7200 Y0.0200 Z-0.3223 J-3.0412 K0.3600 -G2 X0.7200 Y0.0300 Z-0.2537 J0.4423 K-0.0294 -G2 X0.7200 Y0.0900 Z-0.1543 J0.1818 K-0.0420 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2389 -G3 Y0.410000 Z-0.243869 R0.005000 -G2 X0.7200 Y0.4550 Z-0.3378 J-0.2598 K-0.1821 -G2 X0.7200 Y0.4700 Z-0.3948 J-0.3439 K-0.1210 -G1 Y0.4850 Z-0.4850 - Y0.4900 Z-0.5000 - Y0.4950 -G0 Z0.0120 - Y0.8250 -G1 Z-0.3750 -G3 Y0.950000 Z-0.500000 R0.125000 -G1 Y0.9550 Z-0.4850 - Y0.9750 Z-0.3713 -G2 X0.7200 Y0.9850 Z-0.3378 J0.4897 K-0.1281 -G2 X0.7200 Y1.0350 Z-0.2360 J0.3464 K-0.1069 -G0 Z0.0120 - Y1.3800 -G1 Z-0.1898 -G3 Y1.385000 Z-0.194850 R0.005000 -G2 X0.7200 Y1.4150 Z-0.2792 J-0.1697 K-0.1079 -G1 Y1.4350 Z-0.4870 -G0 Z0.0120 - X0.6800 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 Z-0.4740 - Y1.4150 Z-0.2995 -G3 X0.6800 Y1.4050 Z-0.2399 J-0.3112 K-0.0216 -G3 X0.6800 Y1.3500 Z-0.1543 J-0.1826 K-0.0568 -G0 Z0.0120 - Y1.0350 -G1 Z-0.2349 -G2 Y1.030000 Z-0.239948 R0.005000 -G3 X0.6800 Y0.9850 Z-0.3300 J0.2882 K-0.2003 -G3 X0.6800 Y0.9700 Z-0.3810 J0.4248 K-0.1527 -G1 Y0.9500 Z-0.4947 - Y0.9450 Z-0.5000 -G0 Z0.0120 - Y0.6150 -G1 Z-0.3697 -G2 Y0.490000 Z-0.494711 R0.125000 -G1 Y0.4700 Z-0.3810 -G3 X0.6800 Y0.4550 Z-0.3300 J-0.4398 K-0.1017 -G3 X0.6800 Y0.4050 Z-0.2321 J-0.3765 K-0.1306 -G0 Z0.0120 - Y0.0550 -G1 Z-0.1997 -G2 Y0.050000 Z-0.204654 R0.005000 -G3 X0.6800 Y0.0300 Z-0.2615 J0.1517 K-0.0853 -G1 Y0.0250 Z-0.2995 - Y0.0100 Z-0.4740 - Y0.0050 Z-0.5000 -G0 Z0.0120 - X0.6400 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0100 Z-0.4849 -G3 X0.6400 Y0.0250 Z-0.3143 J-3.3931 K0.3843 -G2 X0.6400 Y0.0350 Z-0.2496 J0.4880 K-0.0423 -G2 X0.6400 Y0.0900 Z-0.1566 J0.1922 K-0.0509 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2251 -G3 Y0.410000 Z-0.230144 R0.005000 -G2 X0.6400 Y0.4700 Z-0.3485 J-0.3119 K-0.2324 -G2 X0.6400 Y0.4850 Z-0.4074 J-0.3558 K-0.1221 -G1 Y0.5000 Z-0.4936 - Y0.5050 Z-0.5000 - Y0.5400 -G0 Z0.0120 - Y0.7800 -G1 Z-0.3750 -G3 Y0.905000 Z-0.500000 R0.125000 -G1 Y0.9350 - Y0.9400 Z-0.4936 -G2 X0.6400 Y0.9600 Z-0.3838 J2.1792 K-0.3402 -G2 X0.6400 Y0.9750 Z-0.3339 J0.3678 K-0.0833 -G2 X0.6400 Y1.0350 Z-0.2232 J0.3591 K-0.1231 -G0 Z0.0120 - Y1.3800 -G1 Z-0.1976 -G3 Y1.385000 Z-0.202554 R0.005000 -G2 X0.6400 Y1.4000 Z-0.2339 J-0.0925 K-0.0635 -G2 X0.6400 Y1.4100 Z-0.2751 J-0.1734 K-0.0639 -G1 Y1.4150 Z-0.3143 -G3 X0.6400 Y1.4300 Z-0.4849 J3.4081 K0.2137 -G1 Y1.4350 Z-0.5000 -G0 Z0.0120 - X0.6000 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4300 - Y1.4250 Z-0.4869 -G2 X0.6000 Y1.4100 Z-0.3172 J3.4555 K0.3909 -G3 X0.6000 Y1.4000 Z-0.2516 J-0.4751 K-0.0389 -G3 X0.6000 Y1.3500 Z-0.1636 J-0.1827 K-0.0457 -G0 Z0.0120 - Y1.0350 -G1 Z-0.2096 -G2 Y1.030000 Z-0.214622 R0.005000 -G3 X0.6000 Y0.9950 Z-0.2623 J0.2278 K-0.2037 -G3 X0.6000 Y0.9600 Z-0.3328 J0.3779 K-0.2315 -G3 X0.6000 Y0.9400 Z-0.3976 J0.3605 K-0.1469 -G1 Y0.9200 Z-0.4995 - Y0.9000 Z-0.5000 -G0 Z0.0120 - Y0.6600 -G1 Z-0.3750 -G2 Y0.535000 Z-0.500000 R0.125000 -G1 Y0.5200 Z-0.4995 - Y0.5000 Z-0.3976 - Y0.4900 Z-0.3603 -G3 X0.6000 Y0.4500 Z-0.2712 J-0.4493 K-0.1481 -G0 Z0.0120 - Y0.0650 -G1 Z-0.1977 -G2 Y0.060000 Z-0.202693 R0.005000 -G3 X0.6000 Y0.0350 Z-0.2771 J0.1639 K-0.0965 -G1 Y0.0300 Z-0.3172 -G2 X0.6000 Y0.0150 Z-0.4869 J-3.4705 K0.2212 -G1 Y0.0100 Z-0.5000 - Y0.0050 -G0 Z0.0120 - X0.5600 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0200 Z-0.4995 - Y0.0250 Z-0.4623 -G2 X0.5600 Y0.0400 Z-0.2936 J4.6305 K-0.3269 -G2 X0.5600 Y0.0500 Z-0.2437 J0.3561 K-0.0454 -G2 X0.5600 Y0.0900 Z-0.1715 J0.2014 K-0.0643 -G0 Z0.0120 - Y0.4500 -G1 Z-0.2436 -G3 Y0.455000 Z-0.248622 R0.005000 -G1 Y0.4850 Z-0.2947 -G2 X0.5600 Y0.5100 Z-0.3477 J-0.3664 K-0.2054 -G2 X0.5600 Y0.5250 Z-0.3967 J-0.7470 K-0.2554 -G1 Y0.5450 Z-0.4888 - Y0.5500 Z-0.5000 - Y0.5850 -G0 Z0.0120 - Y0.7350 -G1 Z-0.3750 -G3 Y0.860000 Z-0.500000 R0.125000 -G1 Y0.8900 - Y0.8950 Z-0.4888 - Y0.9150 Z-0.3967 -G2 X0.5600 Y0.9300 Z-0.3477 J0.7620 K-0.2064 -G2 X0.5600 Y0.9900 Z-0.2408 J0.4134 K-0.1618 -G0 Z0.0120 - Y1.3700 -G1 Z-0.2034 -G3 Y1.375000 Z-0.208437 R0.005000 -G2 X0.5600 Y1.4000 Z-0.2936 J-0.1851 K-0.1006 -G2 X0.5600 Y1.4150 Z-0.4623 J-4.6155 K-0.4955 -G1 Y1.4200 Z-0.4995 - Y1.4350 Z-0.5000 -G0 Z0.0120 - X0.5200 Y1.5600 -G1 Z-0.3750 -G2 Y1.435000 Z-0.500000 R0.125000 -G1 Y1.4100 Z-0.4995 -G2 X0.5200 Y1.3950 Z-0.3427 J1.8579 K0.2568 -G3 X0.5200 Y1.3850 Z-0.2642 J-0.7557 K-0.0565 -G3 X0.5200 Y1.3500 Z-0.1869 J-0.1831 K-0.0362 -G0 Z0.0120 - Y0.9450 -G1 Z-0.2658 -G2 Y0.940000 Z-0.270792 R0.005000 -G1 Y0.9100 Z-0.3163 -G3 X0.5200 Y0.8850 Z-0.3673 J0.6673 K-0.3589 -G1 Y0.8450 Z-0.4976 - Y0.8400 Z-0.5000 - Y0.8100 -G0 Z0.0120 - Y0.7500 -G1 Z-0.3750 -G2 Y0.625000 Z-0.500000 R0.125000 -G1 Y0.6000 - Y0.5950 Z-0.4976 - Y0.5550 Z-0.3673 -G3 X0.5200 Y0.4950 Z-0.2633 J-0.4330 K-0.1807 -G0 Z0.0120 - Y0.0800 -G1 Z-0.2054 -G2 Y0.075000 Z-0.210398 R0.005000 -G3 X0.5200 Y0.0500 Z-0.2956 J0.1785 K-0.0986 -G3 X0.5200 Y0.0350 Z-0.4603 J3.9029 K-0.4385 -G1 Y0.0300 Z-0.4995 - Y0.0050 Z-0.5000 -G0 Z0.0120 - X0.4800 Y-0.1200 -G1 Z-0.3750 -G3 Y0.005000 Z-0.500000 R0.125000 -G1 Y0.0400 - Y0.0450 Z-0.4858 - Y0.0650 Z-0.2799 -G2 X0.4800 Y0.0900 Z-0.2084 J0.1712 K-0.0198 -G0 Z0.0120 - Y0.6300 -G1 Z-0.3847 -G3 Y0.635000 Z-0.389712 R0.005000 -G3 X0.4800 Y0.6750 Z-0.4564 J0.6606 K0.3510 -G0 Z0.0120 - Y0.6450 -G1 Z-0.3235 -G3 Y0.770000 Z-0.448536 R0.125000 -G1 Y0.8100 Z-0.3819 -G0 Z0.0120 - Y1.3500 -G1 Z-0.2113 -G3 Y1.355000 Z-0.216280 R0.005000 -G2 X0.4800 Y1.3750 Z-0.2799 J-0.1638 K-0.0864 -G1 Y1.3950 Z-0.4858 - Y1.4000 Z-0.5000 - Y1.4350 -G0 Z0.0120 - X0.4400 Y1.5150 -G1 Z-0.3750 -G2 Y1.390000 Z-0.500000 R0.125000 -G1 Y1.3850 - Y1.3800 Z-0.4858 -G2 X0.4400 Y1.3650 Z-0.3230 J3.1676 K0.3740 -G3 X0.4400 Y1.3550 Z-0.2583 J-0.4880 K-0.0423 -G3 X0.4400 Y1.3050 Z-0.1695 J-0.2026 K-0.0556 -G0 Z0.0120 - Y0.1350 -G1 Z-0.1704 -G2 Y0.130000 Z-0.175407 R0.005000 -G3 X0.4400 Y0.0850 Z-0.2583 J0.1388 K-0.1290 -G3 X0.4400 Y0.0750 Z-0.3230 J0.4780 K-0.1070 -G2 X0.4400 Y0.0600 Z-0.4858 J-3.1826 K0.2113 -G1 Y0.0550 Z-0.5000 - Y0.0450 -G0 Z0.0120 - X0.4000 Y-0.0750 -G1 Z-0.3750 -G3 Y0.050000 Z-0.500000 R0.125000 -G1 Y0.0750 - Y0.0800 Z-0.4740 - Y0.0950 Z-0.3132 - Y0.1000 Z-0.2779 -G2 X0.4000 Y0.1350 Z-0.1937 J0.1827 K-0.0265 -G0 Z0.0120 - Y1.3050 -G1 Z-0.1966 -G3 Y1.310000 Z-0.201563 R0.005000 -G2 X0.4000 Y1.3400 Z-0.2779 J-0.1531 K-0.1042 -G1 Y1.3450 Z-0.3132 - Y1.3600 Z-0.4740 - Y1.3650 Z-0.5000 - Y1.3950 -G0 Z0.0120 - X0.3600 Y1.4700 -G1 Z-0.3750 -G2 Y1.345000 Z-0.500000 R0.125000 -G1 Y1.3400 Z-0.4799 -G2 X0.3600 Y1.3250 Z-0.3309 J3.5018 K0.4277 -G3 X0.3600 Y1.3150 Z-0.2642 J-0.4617 K-0.0352 -G3 X0.3600 Y1.2600 Z-0.1692 J-0.1826 K-0.0422 -G0 Z0.0120 - Y0.1800 -G1 Z-0.1691 -G2 Y0.175000 Z-0.174112 R0.005000 -G3 X0.3600 Y0.1250 Z-0.2642 J0.1310 K-0.1316 -G3 X0.3600 Y0.1150 Z-0.3309 J0.4517 K-0.1018 -G2 X0.3600 Y0.1000 Z-0.4799 J-3.5168 K0.2787 -G1 Y0.0950 Z-0.5000 - Y0.0900 -G0 Z0.0120 - X0.3200 Y-0.0300 -G1 Z-0.3750 -G3 Y0.095000 Z-0.500000 R0.125000 -G1 Y0.1200 - Y0.1250 Z-0.4799 -G3 X0.3200 Y0.1400 Z-0.3328 J-2.7487 K0.3547 -G2 X0.3200 Y0.1500 Z-0.2681 J0.4880 K-0.0423 -G2 X0.3200 Y0.1800 Z-0.2006 J0.1697 K-0.0349 -G0 Z0.0120 - Y1.2600 -G1 Z-0.2034 -G3 Y1.265000 Z-0.208437 R0.005000 -G2 X0.3200 Y1.2950 Z-0.2936 J-0.1485 K-0.1002 -G2 X0.3200 Y1.3100 Z-0.4387 J-2.2137 K-0.3022 -G3 X0.3200 Y1.3200 Z-0.5000 J0.2527 K0.0098 -G1 Y1.3500 -G0 Z0.0120 - X0.2800 Y1.4250 -G1 Z-0.3750 -G2 Y1.300000 Z-0.500000 R0.125000 -G1 Y1.2900 - Y1.2850 Z-0.4740 - Y1.2650 Z-0.2986 -G3 X0.2800 Y1.2550 Z-0.2516 J-0.3430 K-0.0483 -G3 X0.2800 Y1.2150 Z-0.1859 J-0.1523 K-0.0477 -G0 Z0.0120 - Y0.2250 -G1 Z-0.1868 -G2 Y0.220000 Z-0.191759 R0.005000 -G3 X0.2800 Y0.1850 Z-0.2516 J0.1129 K-0.1062 -G3 X0.2800 Y0.1750 Z-0.2986 J0.3330 K-0.0953 -G1 Y0.1550 Z-0.4740 - Y0.1500 Z-0.5000 - Y0.1350 -G0 Z0.0120 - X0.2400 Y0.0600 -G1 Z-0.3678 -G3 Y0.185000 Z-0.492750 R0.125000 -G3 X0.2400 Y0.1950 Z-0.4280 J-0.6865 K0.1392 -G2 X0.2400 Y0.2100 Z-0.2967 J3.5145 K-0.3347 -G2 X0.2400 Y0.2200 Z-0.2516 J0.4074 K-0.0667 -G2 X0.2400 Y0.2400 Z-0.2124 J0.2090 K-0.0819 -G2 X0.2400 Y0.2700 Z-0.1786 J0.1849 K-0.1343 -G0 Z0.0120 - Y1.1700 -G1 Z-0.1795 -G3 Y1.175000 Z-0.184518 R0.005000 -G1 Y1.1800 Z-0.1884 - Y1.2000 Z-0.2124 -G2 X0.2400 Y1.2200 Z-0.2516 J-0.1505 K-0.1015 -G2 X0.2400 Y1.2300 Z-0.2967 J-0.3974 K-0.1118 -G1 Y1.2500 Z-0.4653 - Y1.2550 Z-0.4928 - Y1.2600 Z-0.5000 -G0 Z0.0120 - X0.2000 Y1.3800 -G1 Z-0.3750 -G2 Y1.255000 Z-0.500000 R0.125000 -G1 Y1.2200 - Y1.2150 Z-0.4928 - Y1.2100 Z-0.4653 - Y1.1900 Z-0.2986 -G3 X0.2000 Y1.1700 Z-0.2320 J-0.1995 K-0.0235 -G0 Z0.0120 - Y0.3151 -G1 Z-0.1794 -G2 Y0.310000 Z-0.184518 R0.005148 -G3 X0.2000 Y0.2550 Z-0.2731 J0.0935 K-0.1194 -G1 Y0.2500 Z-0.2986 - Y0.2300 Z-0.4653 - Y0.2250 Z-0.4928 -G0 Z0.0120 - X0.1600 Y0.1050 -G1 Z-0.3750 -G3 Y0.230000 Z-0.500000 R0.125000 -G1 Y0.2650 - Y0.2700 Z-0.4897 - Y0.2750 Z-0.4642 - Y0.2950 Z-0.3093 -G2 X0.1600 Y0.3150 Z-0.2418 J0.2180 K-0.0278 -G0 Z0.0120 - Y1.0799 -G1 Z-0.1886 -G3 Y1.085000 Z-0.193720 R0.005148 -G2 X0.1600 Y1.1350 Z-0.2662 J-0.0815 K-0.1097 -G2 X0.1600 Y1.1450 Z-0.3093 J-0.2562 K-0.0821 -G1 Y1.1650 Z-0.4642 - Y1.1700 Z-0.4897 -G0 Z0.0120 - X0.1200 Y1.2450 -G1 Z-0.3745 -G2 Y1.120000 Z-0.499516 R0.125000 -G2 X0.1200 Y1.1100 Z-0.4544 J0.3974 K0.1118 -G3 X0.1200 Y1.0900 Z-0.3191 J-5.3775 K-0.7258 -G3 X0.1200 Y1.0800 Z-0.2779 J-0.3132 K-0.0543 -G3 X0.1200 Y1.0350 Z-0.2094 J-0.1299 K-0.0363 -G0 Z0.0120 - Y0.4050 -G1 Z-0.2093 -G2 Y0.400000 Z-0.214319 R0.005001 -G3 X0.1200 Y0.3600 Z-0.2779 J0.0856 K-0.0982 -G3 X0.1200 Y0.3500 Z-0.3191 J0.3032 K-0.0954 -G1 Y0.3300 Z-0.4544 - Y0.3250 Z-0.4799 - Y0.3200 Z-0.4995 - Y0.3150 Z-0.5000 -G0 Z0.0120 - X0.0800 Y0.2400 -G1 Z-0.3750 -G3 Y0.365000 Z-0.500000 R0.125000 -G1 Y0.3850 - Y0.3900 Z-0.4917 -G3 X0.0800 Y0.4000 Z-0.4505 J-0.3032 K0.0954 -G2 X0.0800 Y0.4250 Z-0.3152 J2.3368 K-0.3618 -G2 X0.0800 Y0.4500 Z-0.2505 J0.1759 K-0.0308 -G0 Z0.0120 - Y0.9900 -G1 Z-0.2533 -G3 Y0.995000 Z-0.258340 R0.005000 -G2 X0.0800 Y1.0150 Z-0.3152 J-0.1628 K-0.0892 -G2 X0.0800 Y1.0400 Z-0.4505 J-2.0803 K-0.4544 -G3 X0.0800 Y1.0500 Z-0.4917 J0.3132 K0.0542 -G1 Y1.0550 Z-0.5000 - Y1.0800 -G0 Z0.0120 - X0.0400 Y1.1100 -G1 Z-0.3750 -G2 Y0.985000 Z-0.500000 R0.125000 -G1 Y0.9650 - Y0.9600 Z-0.4976 -G2 X0.0400 Y0.9450 Z-0.4564 J0.2697 K0.1216 -G3 X0.0400 Y0.9100 Z-0.3270 J-1.7704 K-0.4094 -G3 X0.0400 Y0.8850 Z-0.2760 J-0.2712 K-0.1014 -G3 X0.0400 Y0.8550 Z-0.2457 J-0.1055 K-0.0745 -G0 Z0.0120 - Y0.5851 -G1 Z-0.2445 -G2 Y0.580000 Z-0.249613 R0.005148 -G3 X0.0400 Y0.5550 Z-0.2760 J0.0608 K-0.0827 -G3 X0.0400 Y0.5300 Z-0.3270 J0.1400 K-0.1003 -G1 Y0.4950 Z-0.4564 -G2 X0.0400 Y0.4800 Z-0.4976 J-0.3224 K0.0941 -G1 Y0.4750 Z-0.5000 - Y0.4500 -G0 Z0.0120 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc index eafe2f8a565..01dd5fff6ab 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/line-arc-burnin.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc index 8910b6202fc..460a43cd46c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-convex.ngc @@ -1,6 +1,4 @@ -G20 G17 G40 -G90 G91.1 -G64 (Q0.001) +o call F999.0000 G0 X0 Y0 Z0 G1 X-1 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc index 59c09770daa..f03391768b8 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-end-overrun.ngc @@ -1,8 +1,5 @@ -G20 +o call G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) F999.0000 G18 G0 X3.3390 Y4.8580 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc index 3b1d03a084a..ca78e8acf5a 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc-stellabee.ngc @@ -1,8 +1,5 @@ -G20 +o call G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) F999.0000 G18 G0 X3.3390 Y4.8580 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc index ee3a4a3d3f2..9845a9948a8 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/linearc.ngc @@ -1,4 +1,4 @@ -G20 G90 +o call G64 P0.0 Q0.0 F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc index c368be598fb..494e099bbb9 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc @@ -1,9 +1,8 @@ -g21 -G64 P0 (Q0.004) +o call G0 X0 Y0 Z20 G0 X5.394910 Y13.495489 Z20.332291 G19 G2 F12700 (225 29) J5.656854 K5.656854 X10.394910 Y26.080547 Z29.989145 G19 G2 (75 14) J-0.258819 K-0.965926 X6.894910 Y26.787653 Z29.282038 G17 G2 (329 285) I-4.330127 J2.500000 X3.858878 Y24.458024 Z31.282038 G0 X0 Y0 Z20 -m30 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc index f22259bfbdd..f6111b240ca 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/abc_tangent.ngc @@ -1,4 +1,4 @@ -G20 G90 G17 +o call G0 Z1 G0 X0 Y0 A0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc index 995988e53dc..810c56ca5ed 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/adjustment.ngc @@ -1,5 +1,5 @@ -(Adjustment) -G20 G90 +(Test blend behavior due to alternating long and short moves) +o call G64 P0 Q0.005 F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc index 021162ebb8f..4aacedf571d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/arc-with-spiral.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc index 913f3a558b1..fdbbeb73b21 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_1.ngc @@ -1,7 +1,4 @@ -G90 G20 G40 - -G64 P0 Q0 -/G61.1 +o call G0 Z1 G0 X0 Y0 Z0 @@ -19,4 +16,3 @@ G1 X1 Y-1 M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc index ff7a636e189..f72de568fc1 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_comp.ngc @@ -1,7 +1,4 @@ -G90 G20 G40 - -G64 P0 Q0 -/G61.1 +o call G10 L1 P1 Z1 R.5 T1 M6 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc index c60f1d54329..e100da7a0af 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/bad_spirals_lathe.ngc @@ -1,7 +1,5 @@ -G90 G20 G40 +o call G18 -G64 P0 Q0 -/G61.1 G0 X0 Z0 G1 Z0 F100 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc index 24afba1a7cd..66bdebeb923 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/default_tolerance_check.ngc @@ -19,7 +19,7 @@ X0.0 Y1 X0 Y0 o100 endsub -G20 G90 G17 +o call ("Default" blending with no G64 words) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc index d08b9674a65..08ed4475168 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/feed-tracking-test.ngc @@ -1,4 +1,4 @@ -G90 G20 +o call G64 P0.005 Q0.0 @@ -10,4 +10,3 @@ G1 X5 F1000 G1 X6 F1000 G1 X7 F1000 M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc index 6e53b4604a3..bfd5fa0c737 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/helix_slowdown.ngc @@ -1,4 +1,4 @@ -G90 G20 +o call G0 Z.01 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc index cf9ade9710f..2c863b638c4 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/modal_state_test.ngc @@ -1,5 +1,4 @@ -G20 G90 - +o call F5 G0 Z1 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc index ba58533b729..593ca153d82 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/parabolic_split1.ngc @@ -1,5 +1,4 @@ -G90 G20 -G64 P0 Q0 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc index 58aac4a4b80..9e085e3c65c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/ripple1.ngc @@ -1,5 +1,5 @@ -G20 G90 -G64 P0.0 Q0.0 +(Demonstrate low acceleration in last segment due to G64) +o call F1000 G0 X0 Y0 Z0 G1 X1 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc index 92b431c2975..4016ba686e9 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/simple_blend.ngc @@ -1,4 +1,4 @@ -G90 G20 G64 +o call F1000 G0 X0 Y0 Z0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc index dc2391bb444..003b28dd193 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/spiral_aggressive.ngc @@ -1,7 +1,4 @@ -G90 G20 G40 - -G64 P0 Q0 -/G61.1 +o call G0 Z1 G0 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc index b9d70e9dce2..98a1812c329 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/tangent_check.ngc @@ -1,4 +1,5 @@ -G61 G90 +o call +G61 G0 X-.1 Y0 Z0 F20 G1 X0 Y0 Z0 G64 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc index 12e4ae53c49..fd99f8fcb2e 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/diagnostic/test-active-states.ngc @@ -1,4 +1,5 @@ -G90 G20 G17 G54 +o call + G0 Z1 X0 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh b/tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh new file mode 100644 index 00000000000..bebfe60b0d7 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/find_dupes.sh @@ -0,0 +1,9 @@ +#!/bin/sh + +# Finds duplicates based on this SO suggestion: https://stackoverflow.com/a/16278407 +dirname=${1:-'./'} +find $dirname -type f | sed 's_.*/__' | sort| uniq -d| +while read fileName +do +find $dirname -type f | grep "$fileName" +done diff --git a/tests/trajectory-planner/circular-arcs/nc_files/speed-tests/sam-tail-slowdown.ngc b/tests/trajectory-planner/circular-arcs/nc_files/performance/sam-tail-slowdown.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/speed-tests/sam-tail-slowdown.ngc rename to tests/trajectory-planner/circular-arcs/nc_files/performance/sam-tail-slowdown.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc b/tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc deleted file mode 100644 index c368be598fb..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/performance/tort-sam-mach3.ngc +++ /dev/null @@ -1,9 +0,0 @@ -g21 -G64 P0 (Q0.004) -G0 X0 Y0 Z20 -G0 X5.394910 Y13.495489 Z20.332291 -G19 G2 F12700 (225 29) J5.656854 K5.656854 X10.394910 Y26.080547 Z29.989145 -G19 G2 (75 14) J-0.258819 K-0.965926 X6.894910 Y26.787653 Z29.282038 -G17 G2 (329 285) I-4.330127 J2.500000 X3.858878 Y24.458024 Z31.282038 -G0 X0 Y0 Z20 -m30 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc deleted file mode 100644 index eca82217ad6..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-concave-convex.ngc +++ /dev/null @@ -1,12 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.2 R.1 -G3 X.3 R.2 -G1 X.8 -G1 Y.2 -X0 -Y0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc deleted file mode 100644 index 5c6e86b0bb8..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-concave.ngc +++ /dev/null @@ -1,12 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G3 X.2 R.1 -G2 X.3 R.2 -G1 X.8 -G1 Y.2 -X0 -Y0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc deleted file mode 100644 index 5c0067bacfa..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex-convex.ngc +++ /dev/null @@ -1,10 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.2 R.1 -G2 X.1 R.1 -G1 X0 -Y0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc deleted file mode 100644 index eca82217ad6..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-convex.ngc +++ /dev/null @@ -1,12 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.2 R.1 -G3 X.3 R.2 -G1 X.8 -G1 Y.2 -X0 -Y0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc deleted file mode 100644 index d31b544c5ff..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-neartangent.ngc +++ /dev/null @@ -1,10 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.2 Y.1 R.1 -G2 X.2999 Y0 I0 J-.1 -G1 X1 -Y0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc deleted file mode 100644 index 53f9f1706e7..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-planeswitch.ngc +++ /dev/null @@ -1,13 +0,0 @@ -G90 G20 G64 -F1000 -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.2 R.1 -G3 X.3 R.1 -G18 -G3 X.4 R.1 -G17 -G3 X.5 R.1 -G1 X.8 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc deleted file mode 100644 index 9f5966af77d..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-arc-sawtooth.ngc +++ /dev/null @@ -1,56 +0,0 @@ -G90 G20 -G64 -F1000 -G0 X0 Y0 -Z0 - -G2 X0 Y0.1 R0.2 -G3 X0.1 Y-0.1 R0.2 -G2 X0.2 Y0.1 R0.2 -G3 X0.3 Y-0.1 R0.2 -G2 X0.4 Y0.1 R0.2 -G3 X0.5 Y-0.1 R0.2 -G2 X0.6 Y0.1 R0.2 -G3 X0.7 Y-0.1 R0.2 -G2 X0.8 Y0.1 R0.2 -G3 X0.9 Y-0.1 R0.2 -G2 X1 Y0.1 R0.2 -G3 X1.1 Y-0.1 R0.2 -G2 X1.2 Y0.1 R0.2 -G3 X1.3 Y-0.1 R0.2 -G2 X1.4 Y0.1 R0.2 -G3 X1.5 Y-0.1 R0.2 -G2 X1.6 Y0.1 R0.2 -G3 X1.7 Y-0.1 R0.2 -G2 X1.8 Y0.1 R0.2 -G3 X1.9 Y-0.1 R0.2 -G2 X2 Y0.1 R0.2 -G3 X2.1 Y-0.1 R0.2 -G2 X2.2 Y0.1 R0.2 -G3 X2.3 Y-0.1 R0.2 -G2 X2.4 Y0.1 R0.2 -G3 X2.5 Y-0.1 R0.2 -G2 X2.6 Y0.1 R0.2 -G3 X2.7 Y-0.1 R0.2 -G2 X2.8 Y0.1 R0.2 -G3 X2.9 Y-0.1 R0.2 -G2 X3 Y0.1 R0.2 -G3 X3.1 Y-0.1 R0.2 -G2 X3.2 Y0.1 R0.2 -G3 X3.3 Y-0.1 R0.2 -G2 X3.4 Y0.1 R0.2 -G3 X3.5 Y-0.1 R0.2 -G2 X3.6 Y0.1 R0.2 -G3 X3.7 Y-0.1 R0.2 -G2 X3.8 Y0.1 R0.2 -G3 X3.9 Y-0.1 R0.2 -G2 X4 Y0.1 R0.2 -G3 X4.1 Y-0.1 R0.2 -G2 X4.2 Y0.1 R0.2 -G3 X4.3 Y-0.1 R0.2 -G2 X4.4 Y0.1 R0.2 -G3 X4.5 Y-0.1 R0.2 -G2 X4.6 Y0.1 R0.2 -G3 X4.7 Y-0.1 R0.2 -M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc deleted file mode 100644 index adb38ff00cf..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/arc-terminal.ngc +++ /dev/null @@ -1,14 +0,0 @@ -G20 G90 -G64 -F1000 - -G0 X0 Y0 -Z0 -G1 X.1 -G2 X.3 R.1 -G3 X.4 R.05 -G1 Y.1 -G3 X.3 R0.05 -G2 X.1 R.1 -G1 X0 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc deleted file mode 100644 index 8910b6202fc..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-convex.ngc +++ /dev/null @@ -1,11 +0,0 @@ -G20 G17 G40 -G90 G91.1 -G64 (Q0.001) -F999.0000 -G0 X0 Y0 Z0 -G1 X-1 -G1 X2 -G2 X1 R.5 -G1 X3 -G1 X4 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc deleted file mode 100644 index 59c09770daa..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-end-overrun.ngc +++ /dev/null @@ -1,55 +0,0 @@ -G20 -G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) -F999.0000 -G18 -G0 X3.3390 Y4.8580 -G1 Z-0.0319 -G1 X3.2100 Z-0.1530 - X3.2040 Z-0.1539 - X3.2000 Z-0.1569 - X3.1940 Z-0.1539 - X3.1840 Z-0.1588 - X3.1680 Z-0.1549 - X3.1620 Z-0.1565 - X3.1380 Z-0.1539 - X3.1280 Z-0.1589 - X3.1260 Z-0.1575 - X3.1240 Z-0.1520 - X3.1200 Z-0.1510 - X3.1160 Z-0.1549 - X3.1060 Z-0.1588 -G3 X3.0940 Y4.8580 Z-0.1540 I0.0013 K0.0205 -G1 X3.0860 Z-0.1556 - X3.0800 Z-0.1539 - X3.0760 Z-0.1569 - X3.0700 Z-0.1539 - X3.0660 Z-0.1549 - X3.0620 Z-0.1529 - X3.0560 Z-0.1539 - X3.0520 Z-0.1520 - X3.0440 Z-0.1520 - X3.0400 Z-0.1549 - X3.0360 Z-0.1556 - X3.0280 Z-0.1549 - X3.0240 Z-0.1529 - X3.0040 Z-0.1549 - X2.9900 Z-0.1490 - X2.9860 Z-0.1529 - X2.9800 Z-0.1510 - X2.9740 Z-0.1546 - X2.9380 Z-0.1510 - X2.9300 Z-0.1526 - X2.9220 Z-0.1520 - X2.9180 Z-0.1559 - X2.9100 Z-0.1556 - X2.9080 Z-0.1536 - X2.9040 Z-0.1530 - X2.9000 Z-0.1559 - X2.8940 Z-0.1529 - X2.8860 Z-0.1520 - X2.8780 Z-0.1546 - X2.8760 Z-0.1569 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc deleted file mode 100644 index 3b1d03a084a..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc-stellabee.ngc +++ /dev/null @@ -1,132 +0,0 @@ -G20 -G0 Z0.0120 -G17 G40 -G80 G90 G91.1 -G64 (Q0.001) -F999.0000 -G18 -G0 X3.3390 Y4.8580 -G1 Z-0.0319 -G3 X3.214000 Z-0.156892 R0.125000 -G1 X3.2100 Z-0.1530 - X3.2040 Z-0.1539 - X3.2000 Z-0.1569 - X3.1940 Z-0.1539 - X3.1840 Z-0.1588 - X3.1680 Z-0.1549 - X3.1620 Z-0.1565 - X3.1380 Z-0.1539 - X3.1280 Z-0.1589 - X3.1260 Z-0.1575 - X3.1240 Z-0.1520 - X3.1200 Z-0.1510 - X3.1160 Z-0.1549 - X3.1060 Z-0.1588 -G3 X3.0940 Y4.8580 Z-0.1540 I0.0013 K0.0205 -G1 X3.0860 Z-0.1556 - X3.0800 Z-0.1539 - X3.0760 Z-0.1569 - X3.0700 Z-0.1539 - X3.0660 Z-0.1549 - X3.0620 Z-0.1529 - X3.0560 Z-0.1539 - X3.0520 Z-0.1520 - X3.0440 Z-0.1520 - X3.0400 Z-0.1549 - X3.0360 Z-0.1556 - X3.0280 Z-0.1549 - X3.0240 Z-0.1529 - X3.0040 Z-0.1549 - X2.9900 Z-0.1490 - X2.9860 Z-0.1529 - X2.9800 Z-0.1510 - X2.9740 Z-0.1546 - X2.9380 Z-0.1510 - X2.9300 Z-0.1526 - X2.9220 Z-0.1520 - X2.9180 Z-0.1559 - X2.9100 Z-0.1556 - X2.9080 Z-0.1536 - X2.9040 Z-0.1530 - X2.9000 Z-0.1559 - X2.8940 Z-0.1529 - X2.8860 Z-0.1520 - X2.8780 Z-0.1546 - X2.8760 Z-0.1569 - X2.8640 -G2 X2.8540 Y4.8580 Z-0.1510 I-0.0110 K-0.0073 -G1 X2.8500 Z-0.1516 - X2.8480 Z-0.1546 - X2.8320 Z-0.1539 - X2.8220 Z-0.1556 - X2.8200 Z-0.1536 - X2.8160 Z-0.1530 - X2.8140 Z-0.1536 - X2.8100 Z-0.1598 - X2.8060 Z-0.1526 - X2.8040 Z-0.1520 - X2.8000 Z-0.1526 - X2.7980 Z-0.1556 - X2.7960 Z-0.1559 - X2.7920 Z-0.1529 - X2.7740 Z-0.1559 - X2.7700 Z-0.1529 - X2.7600 Z-0.1549 - X2.7560 Z-0.1490 - X2.7520 Z-0.1480 - X2.7500 Z-0.1490 - X2.7480 Z-0.1539 - X2.7420 Z-0.1559 - X2.7200 Z-0.1539 - X2.7040 Z-0.1575 - X2.6980 Z-0.1529 - X2.6920 Z-0.1549 - X2.6820 Z-0.1530 - X2.6740 Z-0.1559 - X2.6600 Z-0.1529 - X2.6560 Z-0.1579 - X2.6540 Z-0.1556 - X2.6480 Z-0.1546 - X2.6460 Z-0.1487 - X2.6440 Z-0.1481 - X2.6400 Z-0.1487 - X2.6360 Z-0.1540 - X2.6300 Z-0.1529 - X2.6220 Z-0.1559 - X2.6140 Z-0.1539 - X2.6120 Z-0.1497 - X2.6080 Z-0.1490 - X2.6040 Z-0.1530 - X2.6000 Z-0.1520 - X2.5960 Z-0.1559 - X2.5920 Z-0.1559 - X2.5900 Z-0.1500 - X2.5860 Z-0.1471 - X2.5820 Z-0.1481 - X2.5800 Z-0.1530 - X2.5760 Z-0.1539 - X2.5720 Z-0.1530 - X2.5700 Z-0.1471 - X2.5680 Z-0.1461 - X2.5640 Z-0.1471 - X2.5600 Z-0.1539 - X2.5540 Z-0.1540 - X2.5460 Z-0.1510 - X2.5420 Z-0.1546 - X2.5360 Z-0.1530 - X2.5320 Z-0.1549 - X2.5260 Z-0.1520 - X2.5140 Z-0.1529 - X2.5080 Z-0.1510 - X2.5040 Z-0.1526 - X2.4920 Z-0.1490 - X2.4760 Z-0.1510 - X2.4640 Z-0.1480 - X2.4580 Z-0.1510 - X2.4480 Z-0.1510 - X2.4420 Z-0.1536 - X2.4380 Z-0.1487 - X2.4340 Z-0.1480 - X2.4240 Z-0.1529 - X2.4180 Z-0.1490 -M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc deleted file mode 100644 index ee3a4a3d3f2..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/linearc.ngc +++ /dev/null @@ -1,12 +0,0 @@ -G20 G90 -G64 P0.0 Q0.0 -F1000 -G0 X0 Y0 Z0 -G1 X-.1 -G1 X.1 -G2 X.3 R.1 -G1 X.4 -G1 X.5 -G1 X.0 -M2 - diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc deleted file mode 100644 index 7991e4ac16c..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/spindle-wait.ngc +++ /dev/null @@ -1,33 +0,0 @@ -(Test of atspeed) -(Setup code) -G20 G90 G64 G17 -(Move to standard initial position) -G0 x0 y0 z1 -(Tangent segments allow speed above "triangle" velocity) -X0.01 -X0.02 -X0.03 -X0.04 -X0.05 -X0.06 -X0.07 (Should momentarily stop here) -S3000 M3 -X0.08 -X0.09 -X0.10 -X0.11 -X0.12 -X0.13 -G0 X1.0 -M8 (Coolant too just in case) -G1 X0 Y1 F20 -G1 X-1 Y0 -G1 Y0 -Y0.1 -Y0.2 -M5 M9 -Z.1 -Z.2 -G0 Z1 -M2 - 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 a7aa197e668..48add847aaf 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,9 +1,8 @@ -G90 G20 G64 (absolute distance mode) +o call G0 X1 Z0.1 (rapid to position) 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 M5 M2 (end program) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc index 62f3cd27615..53f9dbb3ad7 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/rigidtap_test.ngc @@ -1,4 +1,4 @@ -G20 G90 G40 +o call M3 S200 G0 X1.000 Y1.000 Z0.100 G33.1 Z-0.750 K0.05 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc index 7991e4ac16c..e0fd5af447c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spindle-wait.ngc @@ -1,6 +1,6 @@ +o call (Test of atspeed) (Setup code) -G20 G90 G64 G17 (Move to standard initial position) G0 x0 y0 z1 (Tangent segments allow speed above "triangle" velocity) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc index c2ccba8a909..f764b689d03 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/spiral_spindleon.ngc @@ -1,5 +1,4 @@ -G20 -G64 +o call G0 X1 Y0 Z0 G0 X0 G1 X1.724638 Y-1.012731 F999 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc index aa31044674a..7e39396622a 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/straight_spindleon.ngc @@ -1,6 +1,6 @@ (Simple straight line case to test "perfect" tangent case) -(Setup code) -G20 G90 G64 G17 +o call + (Test code begins here) O100 sub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc b/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc index ff0b511aae4..c7ec7344c2b 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/spindle/tapping.ngc @@ -1,4 +1,4 @@ -G20 G90 G40 +o call M3 S200 G0 X1.000 Y1.000 Z0.100 G33.1 Z-0.750 K0.05 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc new file mode 100644 index 00000000000..245a4940714 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_inch.ngc @@ -0,0 +1,4 @@ +o sub +o call +G20 +o endsub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc new file mode 100644 index 00000000000..740977777ae --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_mm.ngc @@ -0,0 +1,4 @@ +o sub +o call +G21 +o endsub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc new file mode 100644 index 00000000000..ff5f147f21f --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/nc_files/test_subs/default_modal_state.ngc @@ -0,0 +1,19 @@ +o sub +G10 L2 P0 R0 (Clear any coordinate system rotation) +G17 +G20 +G40 (clear cutter compensation) +G49 (cancel tool length compensation) +G54 (Start in G54 work offset) +G64 P0 Q0 (Blend mode with no tolerance applied) +G90 (Absolute positioning) +G92.1 (Reset G92 offsets) +G94 (Feed-per-minute mode) +G97 (RPM spindle mode) +G98 (Return to the initial value) + +M5 (Turn off spindle) +M9 (Turn off coolant) +S0 +F0 +o endsub diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc index e151d129431..3da76282f89 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207L.ngc @@ -1,7 +1,5 @@ -G92.1 G54 G49 -M111 +o call G53 G0 Z0 W0 -G90 G64 P0.005 Q0.005 F80 M3 S18000 @@ -670,7 +668,6 @@ G1 Z0. G1 X9.6003 Y2.0173 Z-0.015 G1 X9.6014 Y2.0385 Z0. G1 X9.6003 Y2.0173 Z-0.015 -G1 X9.7859 Y1.8111 G1 X9.8071 Y1.81 Z0. G1 X9.7859 Y1.8111 Z-0.015 G1 X9.7848 Y1.7898 Z0. @@ -3937,5 +3934,4 @@ G53 G0 Z0 W0 G53 G0 X0 Y65 G49 G92.1 M5 -M30 -% +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc index 99ef5f8452e..6f8c0c75cff 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/130207LZW.ngc @@ -1,7 +1,5 @@ -G20 -G92.1 G54 G49 +o call G53 G0 Z0 W0 -G90 G64 P0.005 F80 M3 S18000 @@ -3857,5 +3855,4 @@ G0 Z0.25 W0.25 G53 G0 Z0 W0 G49 G92.1 M5 -M30 -% +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc index 78d17268534..4da66c4f5f1 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xw.ngc @@ -1,6 +1,6 @@ -G20 g64 -g0x0y0w1 -g1w-.1f3000 -x1 -w1 -m30 +o call +G0 X0 Y0 W1 +G1 W-.1 F3000 +X1 +W1 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc index ba9fe55251f..5270694059c 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/wz-tests/xz.ngc @@ -1,6 +1,6 @@ -g64 -g0x0y0z1 -g1z-.1f3000 -x1 -z1 -m30 +o call +G0 X0 Y0 Z1 +G1 Z-.1 F3000 +X1 +Z1 +M30 From 75029f03b8a17ddb2c36fb659b2fea3a1a42a38b Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 7 Feb 2019 09:31:11 -0500 Subject: [PATCH 087/129] tp: Clean up some more warnings --- src/emc/tp/tc.c | 8 ++++---- src/emc/tp/tc.h | 4 ++-- src/emc/tp/tc_types.h | 4 ++-- src/emc/tp/tp.c | 5 ++++- src/emc/tp/tp_types.h | 2 +- 5 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index aeead876f11..2c28e26754d 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -430,7 +430,7 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) * Set the terminal condition (i.e. blend or stop) for the given motion segment. * Also sets flags on the next segment relevant to blending (e.g. parabolic blend sets the blend_prev flag). */ -int tcSetTermCond(TC_STRUCT *tc, TC_STRUCT *nexttc, int term_cond) +int tcSetTermCond(TC_STRUCT *tc, TC_STRUCT *nexttc, tc_term_cond_t term_cond) { if (tc) { tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, tc->id, tc->motion_type); @@ -520,10 +520,10 @@ int tcFindBlendTolerance(TC_STRUCT const * const prev_tc, double T1 = prev_tc->tolerance; double T2 = tc->tolerance; //Detect zero tolerance = no tolerance and force to reasonable maximum - if (T1 == 0) { + if (T1 <= 0) { T1 = prev_tc->nominal_length * tolerance_ratio; } - if (T2 == 0) { + if (T2 <= 0) { T2 = tc->nominal_length * tolerance_ratio; } *nominal_tolerance = fmin(T1,T2); @@ -593,7 +593,7 @@ double pmLine9Target(PmLine9 * const line9, int pure_angular) * the struct is properly initialized BEFORE calling this function. */ int tcInit(TC_STRUCT * const tc, - int motion_type, + tc_motion_type_t motion_type, int canon_motion_type, double cycle_time, unsigned char enables, diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 48eb5e9be07..fde8e983611 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -46,7 +46,7 @@ int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, int tcCanConsume(TC_STRUCT const * const tc); -int tcSetTermCond(TC_STRUCT * tc, TC_STRUCT * nexttc, int term_cond); +int tcSetTermCond(TC_STRUCT * tc, TC_STRUCT * nexttc, tc_term_cond_t term_cond); int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, PmCartesian const * const circ_start, @@ -86,7 +86,7 @@ int pmRigidTapInit(PmRigidTap * const tap, double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev); int tcInit(TC_STRUCT * const tc, - int motion_type, + tc_motion_type_t motion_type, int canon_motion_type, double cycle_time, unsigned char enables, diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 3cf94ea7e67..b73829a3812 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -150,12 +150,12 @@ typedef struct { Arc9 arc; } coords; - int motion_type; // TC_LINEAR (coords.line) or + tc_motion_type_t motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or // TC_RIGIDTAP (coords.rigidtap) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? - int term_cond; // gcode requests continuous feed at the end of + tc_term_cond_t term_cond; // gcode requests continuous feed at the end of // this segment (g64 mode) int blending_next; // segment is being blended into following segment diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index d9aaab2e372..eab7d3821f4 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -758,12 +758,15 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, return NO_BLEND; } - // Can't blend segments that are explicitly disallowed switch (prev_tc->term_cond) { case TC_TERM_COND_EXACT: case TC_TERM_COND_STOP: + // Can't blend segments that are explicitly disallowed return NO_BLEND; + case TC_TERM_COND_PARABOLIC: + case TC_TERM_COND_TANGENT: + break; } // Compute performance measures ("perf_xxx") for each method. This is diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 5e1aa0e47f6..b5dffa3324d 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -109,7 +109,7 @@ typedef struct { double wDotMax; /* rotational accelleration max */ int nextId; int execId; - int termCond; + tc_term_cond_t termCond; int done; int depth; /* number of total queued motions */ int activeDepth; /* number of motions blending */ From a13f89af601cd9977cdc792c3f57feb720355670 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 7 Feb 2019 11:58:24 -0500 Subject: [PATCH 088/129] Extend blendmath unit tests to include test cases for end condition check --- .gitignore | 1 + meson.build | 2 +- src/emc/tp/blendmath.c | 13 +--- src/emc/tp/tp_types.h | 2 +- unit_tests/tp/test_blendmath.c | 113 +++++++++++++++++++++++++++++++++ 5 files changed, 118 insertions(+), 13 deletions(-) diff --git a/.gitignore b/.gitignore index d3d37422074..9d8fbbb7fc9 100644 --- a/.gitignore +++ b/.gitignore @@ -44,3 +44,4 @@ position.txt *.glade.h # expanded INI files *.ini.expanded +build diff --git a/meson.build b/meson.build index 60de2e574cd..c5ac938c3db 100644 --- a/meson.build +++ b/meson.build @@ -17,7 +17,7 @@ inc_dir = include_directories([ cc = meson.get_compiler('c') m_dep = cc.find_library('m', required : true) -add_global_arguments(['-DULAPI','-DUNIT_TEST'], language : 'c') +add_global_arguments(['-DULAPI','-DUNIT_TEST','-DTP_PEDANTIC_DEBUG'], language : 'c') pm = static_library('posemath', 'src/libnml/posemath/_posemath.c', diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 5909efa7f79..cc017904f19 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1903,16 +1903,6 @@ EndCondition checkEndCondition(double cycleTime, TP_BIG_NUM * cycleTime, }; - // This block essentially ignores split cycles for exact-stop moves - if (dx <= TP_POS_EPSILON) { - //If the segment is close to the target position, then we assume that it's done. - tc_pdebug_print("close to target, dx = %.12f\n",dx); - //Force progress to land exactly on the target to prevent numerical errors. - out.dt = 0.0; - out.v_f = currentvel; - return out; - } - double v_avg = (currentvel + v_f) / 2.0; //Check that we have a non-zero "average" velocity between now and the @@ -1938,7 +1928,7 @@ EndCondition checkEndCondition(double cycleTime, // Assuming a full timestep: double dv = v_f - currentvel; - double a_f = dv / dt; + double a_f = dv / fmax(dt, TP_TIME_EPSILON); //If this is a valid acceleration, then we're done. If not, then we solve //for v_f and dt given the max acceleration allowed. @@ -1962,6 +1952,7 @@ EndCondition checkEndCondition(double cycleTime, tc_pdebug_print("using positive sqrt\n"); dt = -currentvel / a + pmSqrt(disc); } else { + tc_pdebug_print("using negative sqrt\n"); dt = -currentvel / a - pmSqrt(disc); } diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index b5dffa3324d..32b24b15c11 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -37,7 +37,7 @@ /* "neighborhood" size (if two values differ by less than the epsilon, * then they are effectively equal.)*/ #define TP_ACCEL_EPSILON 1e-4 -#define TP_VEL_EPSILON 1e-8 +#define TP_VEL_EPSILON DOUBLE_FUZZ #define TP_POS_EPSILON 1e-12 #define TP_TIME_EPSILON 1e-12 #define TP_ANGLE_EPSILON 1e-6 diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 35c41ab4adb..34b1cb3d474 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -22,6 +22,15 @@ void rtapi_print_msg(msg_level_t level, const char *fmt, ...) va_end(args); } +void rtapi_print(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); +} + TEST pmCartCartParallel_numerical() { PmCartesian u0 = {1,0,0}; @@ -94,11 +103,115 @@ SUITE(joint_utils) { } +static const double a_max_mock = 10.0; +static const double v_max_mock = 2.0; +static const double cycle_time = 0.001; +static const double target_dist_mock = 2.0; + +static const double SMALL_DISTANCE = 1e-10; +static const double SMALL_VELOCITY = 1e-12; + +TEST checkEndCondition_zero_velocity() +{ + EndCondition ec_zero = checkEndCondition(cycle_time, + 0.0, + target_dist_mock, + 0.0, + 0.0, + a_max_mock + ); + // When both initial and final velocity are zero, assume that at least one cycle is left + ASSERT(ec_zero.dt >= cycle_time); + + EndCondition ec_small = checkEndCondition(cycle_time, + 0.0, + target_dist_mock, + SMALL_VELOCITY, + 0.0, + a_max_mock + ); + // when "average" velocity is very small and distance to go is large, at least one cycle is left + ASSERT(ec_small.dt >= cycle_time); + PASS(); +} +TEST checkEndCondition_small_velocity() +{ + EndCondition ec_small_close = checkEndCondition(cycle_time, + target_dist_mock - SMALL_VELOCITY*cycle_time / 2.0, + target_dist_mock, + SMALL_VELOCITY, + 0.0, + a_max_mock + ); + ASSERT(ec_small_close.dt < cycle_time); + + EndCondition ec_small_far = checkEndCondition(cycle_time, + target_dist_mock - SMALL_VELOCITY*cycle_time * 2.0, + target_dist_mock, + SMALL_VELOCITY, + 0.0, + a_max_mock + ); + // when "average" velocity is very small and distance to go is large, at least one cycle is left + ASSERT(ec_small_far.dt >= cycle_time); + PASS(); +} + + +TEST checkEndCondition_at_velocity() +{ + double v_final = 1.0; + EndCondition ec_close = checkEndCondition(cycle_time, + target_dist_mock - 0.001 + SMALL_DISTANCE, + target_dist_mock, + v_final, + v_final, + a_max_mock + ); + ASSERT(ec_close.dt < cycle_time); + ASSERT_IN_RANGEm("final velocity", ec_close.v_f, v_final - DOUBLE_FUZZ, v_final + DOUBLE_FUZZ); + + EndCondition ec_far = checkEndCondition(cycle_time, + target_dist_mock - 0.001 - SMALL_DISTANCE, + target_dist_mock, + v_final, + v_final, + a_max_mock + ); + ASSERT(ec_far.dt >= cycle_time); + ASSERT_IN_RANGEm("final velocity", ec_close.v_f, v_final - DOUBLE_FUZZ, v_final + DOUBLE_FUZZ); + PASS(); +} + +TEST checkEndCondition_below_final_velocity() +{ + const double v_final = v_max_mock; + const double v_current = 1.0; + const double nominal_cycle_dist = v_current * cycle_time; + + EndCondition ec_close = checkEndCondition(cycle_time, + target_dist_mock - nominal_cycle_dist, + target_dist_mock, + v_current, + v_max_mock, + a_max_mock + ); + ASSERT(ec_close.dt < cycle_time); + ASSERT_IN_RANGEm("final velocity", ec_close.v_f, 1.0, v_final); + PASS(); +} +SUITE(tc_functions) { + RUN_TEST(checkEndCondition_zero_velocity); + RUN_TEST(checkEndCondition_small_velocity); + RUN_TEST(checkEndCondition_at_velocity); + RUN_TEST(checkEndCondition_below_final_velocity); +} int main(int argc, char **argv) { GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ RUN_SUITE(blendmath); RUN_SUITE(joint_utils); + RUN_SUITE(tc_functions); GREATEST_MAIN_END(); /* display results */ } From bb05bb11c8a950be11df977e053cadb40e38c557 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 7 Feb 2019 11:58:53 -0500 Subject: [PATCH 089/129] Add an inline wrapper function to extract max accel from emcmotDebug --- src/emc/tp/joint_util.c | 27 +++++++++++++++++---------- src/emc/tp/joint_util.h | 2 ++ 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/emc/tp/joint_util.c b/src/emc/tp/joint_util.c index df306d3747a..a055af4a8f5 100644 --- a/src/emc/tp/joint_util.c +++ b/src/emc/tp/joint_util.c @@ -22,6 +22,11 @@ PmCartesian getXYZVelBounds() { return vel_bound; } +inline double jointMaxAccel(int joint_idx) +{ + return emcmotDebug->joints[joint_idx].acc_limit; +} + /** * Checks for any acceleration violations based on axis limits. * @@ -32,17 +37,19 @@ unsigned findAccelViolations(EmcPose axis_accel) // Allow some numerical tolerance here since max acceleration is a bit // fuzzy anyway. Torque is the true limiting factor, so there has to be // some slack here anyway due to variations in table load, friction, etc. - static const double sf = 1.0001; + static const double ABS_ACCEL_TOL = 1e-4; + const double REL_ACC_TOL = 1.0 + ABS_ACCEL_TOL; // Bit-mask each failure so we can report all failed axes - int fail_bits = (fabs(axis_accel.tran.x) > (emcmotDebug->joints[0].acc_limit * sf + TP_ACCEL_EPSILON)) << 0 - | (fabs(axis_accel.tran.y) > (emcmotDebug->joints[1].acc_limit * sf + TP_ACCEL_EPSILON)) << 1 - | (fabs(axis_accel.tran.z) > (emcmotDebug->joints[2].acc_limit * sf + TP_ACCEL_EPSILON)) << 2 - | (fabs(axis_accel.a) > (emcmotDebug->joints[3].acc_limit * sf + TP_ACCEL_EPSILON)) << 3 - | (fabs(axis_accel.b) > (emcmotDebug->joints[4].acc_limit * sf + TP_ACCEL_EPSILON)) << 4 - | (fabs(axis_accel.c) > (emcmotDebug->joints[5].acc_limit * sf + TP_ACCEL_EPSILON)) << 5 - | (fabs(axis_accel.u) > (emcmotDebug->joints[6].acc_limit * sf + TP_ACCEL_EPSILON)) << 6 - | (fabs(axis_accel.v) > (emcmotDebug->joints[7].acc_limit * sf + TP_ACCEL_EPSILON)) << 7 - | (fabs(axis_accel.w) > (emcmotDebug->joints[8].acc_limit * sf + TP_ACCEL_EPSILON)) << 8; + unsigned fail_bits = (unsigned)(0x0 + | (fabs(axis_accel.tran.x) > (jointMaxAccel(0) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 0 + | (fabs(axis_accel.tran.y) > (jointMaxAccel(1) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 1 + | (fabs(axis_accel.tran.z) > (jointMaxAccel(2) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 2 + | (fabs(axis_accel.a) > (jointMaxAccel(3) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 3 + | (fabs(axis_accel.b) > (jointMaxAccel(4) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 4 + | (fabs(axis_accel.c) > (jointMaxAccel(5) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 5 + | (fabs(axis_accel.u) > (jointMaxAccel(6) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 6 + | (fabs(axis_accel.v) > (jointMaxAccel(7) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 7 + | (fabs(axis_accel.w) > (jointMaxAccel(8) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 8); return fail_bits; } diff --git a/src/emc/tp/joint_util.h b/src/emc/tp/joint_util.h index 592bc3c6e2d..416918184ee 100644 --- a/src/emc/tp/joint_util.h +++ b/src/emc/tp/joint_util.h @@ -10,6 +10,8 @@ PmCartesian getXYZVelBounds(); unsigned findAccelViolations(EmcPose axis_accel); +double jointMaxAccel(int joint_idx); + /** * Finds the smallest non-zero component in a non-negative "bounds" vector. * Used to identify the "slowest" axis, but ignore axes From 84234e4966a9f1c544346f0fdfb1fd672443ede3 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 7 Feb 2019 12:00:54 -0500 Subject: [PATCH 090/129] tp: migrate more debug output to use new JSON5 printing --- src/emc/tp/tp.c | 103 +++++++++++++++++++++++++++++------------------- 1 file changed, 63 insertions(+), 40 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index eab7d3821f4..18f1885e3db 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -649,10 +649,14 @@ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_S double acc_scaled = tcGetTangentialMaxAccel(tc); double triangle_vel = findVPeak(acc_scaled, tc->target); double max_vel = tpGetMaxTargetVel(tp, tc); - tp_debug_json_log_start(tpCalculateOptimizationInitialVel, Command); - tp_debug_json_double(triangle_vel); - tp_debug_json_log_end(); - return fmin(triangle_vel, max_vel); + double optim_init_vel = fmin(triangle_vel, max_vel); +#ifdef TP_DEBUG + print_json5_log_start(tpCalculateOptimizationInitialVel, Command); + print_json5_double(triangle_vel); + print_json5_double(optim_init_vel); + print_json5_log_end(); +#endif + return optim_init_vel; } @@ -742,6 +746,21 @@ static inline int find_max_element(double arr[], int sz) return max_idx; } +const char *blendTypeAsString(tc_blend_type_t c) +{ + switch(c) { + case NO_BLEND: + return "NO_BLEND"; + case PARABOLIC_BLEND: + return "PARABOLIC_BLEND"; + case TANGENT_SEGMENTS_BLEND: + return "TANGENT_SEGMENTS"; + case ARC_BLEND: + return "ARC_BLEND"; + } + return ""; +} + /** * Compare performance of blend arc and equivalent tangent speed. * If we can go faster by assuming the segments are already tangent (and @@ -778,34 +797,37 @@ STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, double perf_tangent = prev_tc->kink_vel; double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0; - tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ", - perf_parabolic, - perf_tangent, - perf_arc_blend); - // KLUDGE Order the performance measurements so that they match the enum values double perf[3] = {perf_parabolic, perf_tangent, perf_arc_blend}; tc_blend_type_t best_blend = find_max_element(perf, 3); switch (best_blend) { case PARABOLIC_BLEND: // parabolic - tp_debug_print("using parabolic blend\n"); tcRemoveKinkProperties(prev_tc, tc); tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC); break; case TANGENT_SEGMENTS_BLEND: // tangent - tp_debug_print("using approximate tangent blend\n"); // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); break; case ARC_BLEND: // arc blend - tp_debug_print("using blend arc\n"); tcRemoveKinkProperties(prev_tc, tc); - break; case NO_BLEND: + tcRemoveKinkProperties(prev_tc, tc); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); break; } + +#ifdef TP_DEBUG + print_json5_log_start(CheckBlendPerformance,Command); + print_json5_double(perf_parabolic); + print_json5_double(perf_tangent); + print_json5_double(perf_arc_blend); + print_json5_string_("best_blend", blendTypeAsString(best_blend)); + print_json5_log_end(); +#endif + return best_blend; } @@ -1955,21 +1977,21 @@ int tpAddLine(TP_STRUCT * const tp, #ifdef TP_DEBUG { // macros use the variable name, need a plain name to please the JSON5 parser - tp_debug_json_log_start(tpAddLine, Command); + print_json5_log_start(tpAddLine, Command); print_json5_long_("id", tp->nextId); print_json5_EmcPose_("goal", tp->goalPos); - tp_debug_json_EmcPose(end); - tp_debug_json_double(vel); - tp_debug_json_double(ini_maxvel); - tp_debug_json_double(acc); - tp_debug_json_unsigned(enables); - tp_debug_json_long(indexrotary); - tp_debug_json_long(atspeed); - tp_debug_json_long(canon_motion_type); + print_json5_EmcPose(end); + print_json5_double(vel); + print_json5_double(ini_maxvel); + print_json5_double(acc); + print_json5_unsigned(enables); + print_json5_long(indexrotary); + print_json5_long(atspeed); + print_json5_long(canon_motion_type); EmcPose delta = tp->goalPos; emcPoseSub(&end, &tp->goalPos, &delta); - tp_debug_json_EmcPose(delta); - tp_debug_json_log_end(); + print_json5_EmcPose(delta); + print_json5_log_end(); } #endif @@ -2046,23 +2068,23 @@ int tpAddCircle(TP_STRUCT * const tp, // macros use the variable name, need a plain name to please the JSON5 parser long nextId = tp->nextId; EmcPose goal = tp->goalPos; - tp_debug_json_log_start(tpAddCircle, Command); - tp_debug_json_long(nextId); - tp_debug_json_EmcPose(goal); - tp_debug_json_PmCartesian(center); - tp_debug_json_PmCartesian(normal); - tp_debug_json_long(turn); - tp_debug_json_double(vel); - tp_debug_json_double(ini_maxvel); - tp_debug_json_double(acc); - tp_debug_json_double(acc_normal); - tp_debug_json_unsigned(enables); - tp_debug_json_long(atspeed); - tp_debug_json_long(canon_motion_type); + print_json5_log_start(tpAddCircle, Command); + print_json5_long(nextId); + print_json5_EmcPose(goal); + print_json5_PmCartesian(center); + print_json5_PmCartesian(normal); + print_json5_long(turn); + print_json5_double(vel); + print_json5_double(ini_maxvel); + print_json5_double(acc); + print_json5_double(acc_normal); + print_json5_unsigned(enables); + print_json5_long(atspeed); + print_json5_long(canon_motion_type); EmcPose delta = tp->goalPos; emcPoseSub(&end, &tp->goalPos, &delta); - tp_debug_json_EmcPose(delta); - tp_debug_json_log_end(); + print_json5_EmcPose(delta); + print_json5_log_end(); } #endif @@ -2857,9 +2879,10 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { rtapi_print_msg(RTAPI_MSG_ERR, "TP Position violation on axes [%s] at segment %d\n", failed_axes_list, tc->id); print_json5_start_(); - print_json5_object_start_("accel_violation"); + print_json5_object_start_("position_violation"); print_json5_ll_("time_ticks", tp->time_elapsed_ticks); print_json5_int_("id", tc->id); + print_json5_string_("failed_axes", failed_axes_list); print_json5_EmcPose(tp_position_error); print_json5_object_end_(); print_json5_end_(); From 03a21291aa186859e60f1743588cec001f2e9548 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 7 Feb 2019 12:01:09 -0500 Subject: [PATCH 091/129] Add default conditions to a TP test case --- .../circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc index 5e94a6e1612..44ecb67417d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/quick-tests/dodecagon_0.1in.ngc @@ -1,7 +1,6 @@ -G61 G90 G20 - G0 X-.1 Y0 Z0 F20 - G1 X0 Y0 Z0 -G64 +o call +G0 X-.1 Y0 Z0 F20 +G1 X0 Y0 Z0 G1 X0.086603 Y-0.050000 Z0.000000 F10.000000 G1 X0.136603 Y-0.136603 Z0.000000 F10.000000 G1 X0.136603 Y-0.236603 Z0.000000 F10.000000 From 59edeb7f2edccece8354ca0cd3d36f60961dcf59 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 7 Feb 2019 12:31:24 -0500 Subject: [PATCH 092/129] tp: add some skeletal unit tests for joint_utils and spherical_arc --- meson.build | 26 ++++++++++ src/emc/tp/tc.c | 1 + unit_tests/mock/mock_rtapi.inc | 22 ++++++++ unit_tests/tp/test_blendmath.c | 20 +------ unit_tests/tp/test_joint_util.c | 83 ++++++++++++++++++++++++++++++ unit_tests/tp/test_spherical_arc.c | 39 ++++++++++++++ 6 files changed, 172 insertions(+), 19 deletions(-) create mode 100644 unit_tests/mock/mock_rtapi.inc create mode 100644 unit_tests/tp/test_joint_util.c create mode 100644 unit_tests/tp/test_spherical_arc.c diff --git a/meson.build b/meson.build index c5ac938c3db..af9e936b357 100644 --- a/meson.build +++ b/meson.build @@ -5,6 +5,7 @@ tp_src_dir = join_paths(src_root, 'emc/tp') inc_dir = include_directories([ 'unit_tests', + 'unit_tests/mock', src_root, join_paths(src_root, 'emc/kinematics'), tp_src_dir, @@ -12,6 +13,7 @@ inc_dir = include_directories([ 'src/emc/nml_intf', 'src/emc/motion', 'src/rtapi', + 'src/hal', ]) cc = meson.get_compiler('c') @@ -40,3 +42,27 @@ test_blendmath=executable('test_blendmath', ) test('blendmath test',test_blendmath) + +test_joint_util=executable('test_joint_util', + ['unit_tests/tp/test_joint_util.c', + join_paths(tp_src_dir, 'joint_util.c'), + join_paths(src_root, 'emc/nml_intf/emcpose.c'), + ], + dependencies : [m_dep], + include_directories : inc_dir, + link_with : pm + ) + +test('joint_util test',test_joint_util) + +test_spherical_arc=executable('test_spherical_arc', + ['unit_tests/tp/test_spherical_arc.c', + join_paths(tp_src_dir, 'spherical_arc.c'), + join_paths(src_root, 'emc/nml_intf/emcpose.c'), + ], + dependencies : [m_dep], + include_directories : inc_dir, + link_with : pm + ) + +test('spherical_arc test',test_spherical_arc) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 2c28e26754d..6780d4badc6 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -470,6 +470,7 @@ int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, tp_debug_print(" L1 end : %f %f %f\n",prev_tc->coords.line.xyz.end.x, prev_tc->coords.line.xyz.end.y, prev_tc->coords.line.xyz.end.z); + } else { tp_debug_print("connect: consume prev_tc\n"); } diff --git a/unit_tests/mock/mock_rtapi.inc b/unit_tests/mock/mock_rtapi.inc new file mode 100644 index 00000000000..a630d3fedab --- /dev/null +++ b/unit_tests/mock/mock_rtapi.inc @@ -0,0 +1,22 @@ +static struct emcmot_debug_t lcl = {0}; +struct emcmot_debug_t *emcmotDebug = &lcl; + +// KLUDGE fix link error the ugly way +void rtapi_print_msg(msg_level_t level, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); +} + +void rtapi_print(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); +} + diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 34b1cb3d474..7e696e521f8 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -6,30 +6,12 @@ #include "rtapi.h" #include "joint_util.h" #include "motion_debug.h" -struct emcmot_debug_t emcmotDebug = {0}; /* Expand to all the definitions that need to be in the test runner's main file. */ GREATEST_MAIN_DEFS(); -// KLUDGE fix link error the ugly way -void rtapi_print_msg(msg_level_t level, const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - vprintf(fmt, args); - va_end(args); -} - -void rtapi_print(const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - vprintf(fmt, args); - va_end(args); -} +#include "mock_rtapi.inc" TEST pmCartCartParallel_numerical() { diff --git a/unit_tests/tp/test_joint_util.c b/unit_tests/tp/test_joint_util.c new file mode 100644 index 00000000000..d454564850a --- /dev/null +++ b/unit_tests/tp/test_joint_util.c @@ -0,0 +1,83 @@ +#include "tp_debug.h" +#include "greatest.h" +#include "math.h" +#include "rtapi.h" +#include "joint_util.h" +#include "motion_debug.h" +#include "mot_priv.h" + +static const struct emcmot_debug_t emcmotDebug_empty = {0}; + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +#include "mock_rtapi.inc" + +static void emc_globals_setup(void *arg) { + printf("-- in EMC global vars setup callback\n"); + emcmotDebug->joints[0].acc_limit = 10.0; + emcmotDebug->joints[1].acc_limit = 5.0; + emcmotDebug->joints[2].acc_limit = 2.0; + (void)arg; +} + +static void emc_globals_teardown(void *arg) { + printf("-- in EMC global vars teardown callback\n"); + *emcmotDebug = emcmotDebug_empty; + (void)arg; +} + +TEST findAccelViolations_basic() +{ + const EmcPose bad_accel = {{10.01,5.01,2.01},0,0,0,0,0,0}; + const EmcPose good_accel = {{10,5,2},0,0,0,0,0,0}; + unsigned bad_axes = findAccelViolations(bad_accel); + + ASSERT_EQ(bad_axes, 1 | (1<<1) | (1<<2)); + unsigned good_axes = findAccelViolations(good_accel); + + ASSERT_FALSE(good_axes); + PASS(); +} + +TEST findAccelViolations_single() +{ + const EmcPose accel = {{10.0,5.01,2.0},0,0,0,0,0,0}; + + unsigned bad_axes = findAccelViolations(accel); + ASSERT_EQ(bad_axes, (1<<1)); + PASS(); +} + +TEST findMinNonZero_cases() +{ + { + PmCartesian v1 = {1,2,0}; + ASSERT_IN_RANGE(findMinNonZero(&v1), 1.0,1.0); + } + + { + PmCartesian v2 = {3,1,4}; + ASSERT_IN_RANGE(findMinNonZero(&v2), 1.0,1.0); + } + { + PmCartesian v3 = {3,0,0}; + ASSERT_IN_RANGE(findMinNonZero(&v3), 3.0,3.0); + } + PASS(); +} + +SUITE(joint_util) { + GREATEST_SET_SETUP_CB(emc_globals_setup, NULL); + GREATEST_SET_TEARDOWN_CB(emc_globals_teardown, NULL); + RUN_TEST(findAccelViolations_basic); + RUN_TEST(findAccelViolations_single); + RUN_TEST(findMinNonZero_cases); +} + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ + RUN_SUITE(joint_util); + GREATEST_MAIN_END(); /* display results */ +} diff --git a/unit_tests/tp/test_spherical_arc.c b/unit_tests/tp/test_spherical_arc.c new file mode 100644 index 00000000000..ffc5cdce663 --- /dev/null +++ b/unit_tests/tp/test_spherical_arc.c @@ -0,0 +1,39 @@ +#include "tp_debug.h" +#include "spherical_arc.h" +#include "greatest.h" +#include "blendmath.h" +#include "tp_types.h" +#include "math.h" +#include "rtapi.h" +#include "motion_debug.h" + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +#include "mock_rtapi.inc" + +TEST arcInitFromPoints_simple() { + SphericalArc arc; + PmCartesian start = {0,0,0}; + PmCartesian end = {1,1,0}; + PmCartesian center = {0,1,0}; + + int res = arcInitFromPoints(&arc, &start, &end, ¢er); + ASSERT_FALSE(res); + + ASSERT(pmCartCartCompare(&arc.end, &end)); + ASSERT(pmCartCartCompare(&arc.start, &start)); + ASSERT_EQ(arc.radius, 1.0); + PASS(); +} + +SUITE(arc_shape) { + RUN_TEST(arcInitFromPoints_simple); +} + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); + RUN_SUITE(arc_shape); + GREATEST_MAIN_END(); +} From 3e19ce067589410a62feda227b322dd2e1449df2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 8 Feb 2019 12:53:55 -0500 Subject: [PATCH 093/129] tp: add a test for the ray intersection check --- src/emc/tp/blendmath.c | 8 +++--- src/emc/tp/spherical_arc.c | 30 +++++++++++++++++----- src/emc/tp/spherical_arc.h | 4 +-- unit_tests/greatest.h | 1 + unit_tests/tp/test_joint_util.c | 6 ++--- unit_tests/tp/test_spherical_arc.c | 41 +++++++++++++++++++++++++++--- 6 files changed, 70 insertions(+), 20 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index cc017904f19..11d9b357d7e 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -734,7 +734,7 @@ int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param &geom->center2, &geom->radius2); // Handle convexity - param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); + param->convex2 = checkRayIntersectsArc(&geom->center2, &geom->P, &geom->u_tan1, true); tp_debug_print("circ2 convex: %d\n", param->convex2); @@ -820,7 +820,7 @@ int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param &geom->center1, &geom->radius1); - param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); + param->convex1 = checkRayIntersectsArc(&geom->center1, &geom->P, &geom->u_tan2, false); tp_debug_print("circ1 convex: %d\n", param->convex1); @@ -936,8 +936,8 @@ int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, geom->P.y, geom->P.z); - param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); - param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); + param->convex1 = checkRayIntersectsArc(&geom->center1, &geom->P, &geom->u_tan2, false); + param->convex2 = checkRayIntersectsArc(&geom->center2, &geom->P, &geom->u_tan1, true); tp_debug_print("circ1 convex: %d, circ2 convex: %d\n", param->convex1, param->convex2); diff --git a/src/emc/tp/spherical_arc.c b/src/emc/tp/spherical_arc.c index 75012d5ec93..a1151eb4468 100644 --- a/src/emc/tp/spherical_arc.c +++ b/src/emc/tp/spherical_arc.c @@ -161,16 +161,32 @@ int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, return arcInitFromPoints(arc, start, end, ¢er); } -int arcConvexTest(PmCartesian const * const center, - PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir) + +/** + * Tests if a ray from the specified point / direction intersects the circle with the given center. + * + * @note that the circle is assumed to be coindicent with P (so the radius is || P - center ||). + * + * For blending, this is used to determine if a given segment forms a convex or + * concave intersection with another segment. "Convex" means that the tangent + * vector of the other segment (at P) is in the interior of the circle + * containing this segment. For an arc-arc intersection, there are actually 4 possibilities: + * + * convex-convex (each arc's tangent vector is in the other's interior) + * concave-convex + * convex-concave + * concave-concave + */ +int checkRayIntersectsArc(PmCartesian const * const center, + PmCartesian const * const P, + PmCartesian const * const u_ray, + int reverse_dir) { - //Check if an arc-line intersection is concave or convex double dot; - PmCartesian diff; - pmCartCartSub(P, center, &diff); - pmCartCartDot(&diff, uVec, &dot); + PmCartesian r_CP; + pmCartCartSub(P, center, &r_CP); + pmCartCartDot(&r_CP, u_ray, &dot); - tp_debug_print("convex test: dot = %f, reverse_dir = %d\n", dot, reverse_dir); int convex = (reverse_dir != 0) ^ (dot < 0); return convex; } diff --git a/src/emc/tp/spherical_arc.h b/src/emc/tp/spherical_arc.h index cea336909b9..a670538b8eb 100644 --- a/src/emc/tp/spherical_arc.h +++ b/src/emc/tp/spherical_arc.h @@ -60,8 +60,8 @@ int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, PmCartLine const * const line2, double radius, double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume); -int arcConvexTest(PmCartesian const * const center, - PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir); +int checkRayIntersectsArc(PmCartesian const * const center, + PmCartesian const * const P, PmCartesian const * const u_ray, int reverse_dir); int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end); #endif diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index fbab23c9c95..ad6e370f99b 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -1183,6 +1183,7 @@ greatest_run_info greatest_info #define ASSERT_FALSE GREATEST_ASSERT_FALSE #define ASSERT_EQ GREATEST_ASSERT_EQ #define ASSERT_EQ_FMT GREATEST_ASSERT_EQ_FMT +#define ASSERT_FLOAT_EQ(EXP, GOT) GREATEST_ASSERT_IN_RANGE(EXP, GOT, 1e-17) #define ASSERT_IN_RANGE GREATEST_ASSERT_IN_RANGE #define ASSERT_EQUAL_T GREATEST_ASSERT_EQUAL_T #define ASSERT_STR_EQ GREATEST_ASSERT_STR_EQ diff --git a/unit_tests/tp/test_joint_util.c b/unit_tests/tp/test_joint_util.c index d454564850a..35ea22b990c 100644 --- a/unit_tests/tp/test_joint_util.c +++ b/unit_tests/tp/test_joint_util.c @@ -54,16 +54,16 @@ TEST findMinNonZero_cases() { { PmCartesian v1 = {1,2,0}; - ASSERT_IN_RANGE(findMinNonZero(&v1), 1.0,1.0); + ASSERT_IN_RANGE(findMinNonZero(&v1), 1.0,DOUBLE_FUZZ); } { PmCartesian v2 = {3,1,4}; - ASSERT_IN_RANGE(findMinNonZero(&v2), 1.0,1.0); + ASSERT_IN_RANGE(findMinNonZero(&v2), 1.0,DOUBLE_FUZZ); } { PmCartesian v3 = {3,0,0}; - ASSERT_IN_RANGE(findMinNonZero(&v3), 3.0,3.0); + ASSERT_IN_RANGE(findMinNonZero(&v3), 3.0,DOUBLE_FUZZ); } PASS(); } diff --git a/unit_tests/tp/test_spherical_arc.c b/unit_tests/tp/test_spherical_arc.c index ffc5cdce663..8c166b15352 100644 --- a/unit_tests/tp/test_spherical_arc.c +++ b/unit_tests/tp/test_spherical_arc.c @@ -15,21 +15,54 @@ GREATEST_MAIN_DEFS(); TEST arcInitFromPoints_simple() { SphericalArc arc; - PmCartesian start = {0,0,0}; - PmCartesian end = {1,1,0}; - PmCartesian center = {0,1,0}; + PmCartesian start = {1,2,0}; + PmCartesian end = {2,3,0}; + PmCartesian center = {1,3,0}; int res = arcInitFromPoints(&arc, &start, &end, ¢er); ASSERT_FALSE(res); ASSERT(pmCartCartCompare(&arc.end, &end)); ASSERT(pmCartCartCompare(&arc.start, &start)); - ASSERT_EQ(arc.radius, 1.0); + ASSERT(pmCartCartCompare(&arc.center, ¢er)); + + double mag; + pmCartMag(&arc.rStart, &mag); + ASSERT_FLOAT_EQ(mag, 1.0); + pmCartMag(&arc.rEnd, &mag); + ASSERT_FLOAT_EQ(mag, 1.0); + + ASSERT_FLOAT_EQ(arc.radius, 1.0); + ASSERT_FLOAT_EQ(arc.Sangle, 1.0); + ASSERT_FLOAT_EQ(arc.spiral, 0.0); + ASSERT_FLOAT_EQ(arc.angle, M_PI_2); + + PmCartesian pt={0}; + arcPoint(&arc, 0, &pt); + ASSERT(pmCartCartCompare(&arc.center, ¢er)); + + PASS(); +} + +/** + * Tests the orientation of a line-arc intersection + * @return + */ +TEST test_rayIntersectsArc() { + PmCartesian center = {DOUBLE_FUZZ,1,0}; + PmCartesian P = {0,0,0}; + PmCartesian uVec = {1,0,0}; + int reverse_dir = 0; + + ASSERT(checkRayIntersectsArc(¢er, &P, &uVec, reverse_dir)); + center.x*=-1.0;; + ASSERT_FALSE(checkRayIntersectsArc(¢er, &P, &uVec, reverse_dir)); PASS(); } SUITE(arc_shape) { RUN_TEST(arcInitFromPoints_simple); + RUN_TEST(test_rayIntersectsArc); } int main(int argc, char **argv) { From d2bf890afed64bcf2d229d956217e92b3b9ce6cf Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 8 Feb 2019 13:54:47 -0500 Subject: [PATCH 094/129] tp: Clean up some pedantic warnings about default cases --- src/emc/tp/tp.c | 26 ++++++++------------------ src/emc/tp/tp.h | 2 +- 2 files changed, 9 insertions(+), 19 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 18f1885e3db..09026ebff44 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -117,10 +117,8 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { } case TC_SPHERICAL: return true; - default: - tp_debug_print("Unknown motion type!\n"); - return false; } + return false; } @@ -511,11 +509,9 @@ int tpGetExecId(TP_STRUCT * const tp) * begins. If cond is TC_TERM_COND_PARABOLIC, the following move is begun when the * current move slows below a calculated blend velocity. */ -int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) +int tpSetTermCond(TP_STRUCT * const tp, tc_term_cond_t cond, double tolerance) { - if (!tp) { - return TP_ERR_FAIL; - } + tp_err_t res = TP_ERR_FAIL; switch (cond) { //Purposeful waterfall for now @@ -523,15 +519,15 @@ int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) case TC_TERM_COND_TANGENT: case TC_TERM_COND_EXACT: case TC_TERM_COND_STOP: + if (tp) { tp->termCond = cond; tp->tolerance = tolerance; + res = TP_ERR_OK; + } break; - default: - //Invalid condition - return -1; } - return TP_ERR_OK; + return res; } /** @@ -1464,7 +1460,7 @@ STATIC int tcUpdateArcLengthFit(TC_STRUCT * const tc) case TC_LINEAR: case TC_RIGIDTAP: case TC_SPHERICAL: - default: + // No update necessary break; } return 0; @@ -3285,16 +3281,10 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, } break; case TC_TERM_COND_STOP: - break; case TC_TERM_COND_EXACT: break; - default: - rtapi_print_msg(RTAPI_MSG_ERR,"unknown term cond %d in segment %d\n", - tc->term_cond, - tc->id); } - // Update status for the split portion // FIXME redundant tangent check, refactor to switch if (tc->term_cond == TC_TERM_COND_STOP diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index a17bb5ab544..f6b5571188f 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -28,7 +28,7 @@ int tpSetVmax(TP_STRUCT * const tp, double vmax, double ini_maxvel); int tpSetVlimit(TP_STRUCT * const tp, double vLimit); int tpSetId(TP_STRUCT * const tp, int id); int tpGetExecId(TP_STRUCT * const tp); -int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance); +int tpSetTermCond(TP_STRUCT * const tp, tc_term_cond_t cond, double tolerance); int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos); int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); From 79547195309070241c0364d9eb178bc7c0e7cfbb Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 8 Feb 2019 13:56:15 -0500 Subject: [PATCH 095/129] tp: Factor out code handling spindle-sync into a separate function --- src/emc/tp/tp.c | 38 ++++++++++++++++++++------------------ 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 09026ebff44..6ba6333a254 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -3350,6 +3350,24 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, * status; I think those are spelled out here correctly and I can't clean it up * without breaking the API that the TP presents to motion. */ +tp_err_t updateSyncTargets(TP_STRUCT *tp, TC_STRUCT *tc, TC_STRUCT *nexttc) +{ + switch (tc->synchronized) { + case TC_SYNC_NONE: + emcmotStatus->spindle_fb.synced = 0; + return TP_ERR_OK; + case TC_SYNC_VELOCITY: + tc_debug_print("sync velocity\n"); + tpSyncVelocityMode(tc, nexttc); + return TP_ERR_OK; + case TC_SYNC_POSITION: + tc_debug_print("sync position\n"); + tpSyncPositionMode(tp, tc, nexttc); + return TP_ERR_OK; + } + return TP_ERR_INVALID; +} + int tpRunCycle(TP_STRUCT * const tp, long period) { //Pointers to current and next trajectory component @@ -3408,24 +3426,8 @@ int tpRunCycle(TP_STRUCT * const tp, long period) // Assume zero tracking error unless position sync is active emcmotStatus->pos_tracking_error = 0.0; - /** If synchronized with spindle, calculate requested velocity to track - * spindle motion.*/ - switch (tc->synchronized) { - case TC_SYNC_NONE: - emcmotStatus->spindle_fb.synced = 0; - break; - case TC_SYNC_VELOCITY: - tc_debug_print("sync velocity\n"); - tpSyncVelocityMode(tc, nexttc); - break; - case TC_SYNC_POSITION: - tc_debug_print("sync position\n"); - tpSyncPositionMode(tp, tc, nexttc); - break; - default: - rtapi_print_msg(RTAPI_MSG_WARN, "Unrecognized spindle sync state %d, no sync applied\n", tc->synchronized); - break; - } + // If synchronized with spindle, calculate requested velocity to track spindle motion + updateSyncTargets(tp, tc, nexttc); EmcPose const axis_pos_old = tp->currentPos; EmcPose const axis_vel_old = tp->currentVel; From 249016bd803ea9a0baf5366463c8346be9fd6339 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 18 Feb 2019 15:22:35 -0500 Subject: [PATCH 096/129] greatest: Tweak the console output to be more gcc-like * Move error outputs to stderr to avoid clobbering stdout verbose results * Add file / line information in GCC format to failure notes, for more consistency (especially in IDE displays) (cherry picked from commit 63f0ff0b8473bde53602f0a29f1c865f0bbba4cc) --- unit_tests/greatest.h | 48 +++++++++++++++++++++++++------------------ 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index ad6e370f99b..09b63bd6951 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -112,6 +112,10 @@ int main(int argc, char **argv) { #define GREATEST_STDOUT stdout #endif +#ifndef GREATEST_STDERR +#define GREATEST_STDERR stderr +#endif + /* Remove GREATEST_ prefix from most commonly used symbols? */ #ifndef GREATEST_USE_ABBREVS #define GREATEST_USE_ABBREVS 1 @@ -503,9 +507,13 @@ typedef enum greatest_test_res { int greatest_GOT = (int)(GOT); \ greatest_enum_str_fun *greatest_ENUM_STR = ENUM_STR; \ if (greatest_EXP != greatest_GOT) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: %s", \ - greatest_ENUM_STR(greatest_EXP)); \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: %s\n", \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + "%s:%u: In %s:\n", \ + __FILE__, __LINE__, greatest_info.name_buf); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + "%s:%u: expected %s, got %s", \ + __FILE__, __LINE__, \ + greatest_ENUM_STR(greatest_EXP), \ greatest_ENUM_STR(greatest_GOT)); \ GREATEST_FAILm(MSG); \ } \ @@ -522,12 +530,19 @@ typedef enum greatest_test_res { greatest_EXP - greatest_GOT > greatest_TOL) || \ (greatest_EXP < greatest_GOT && \ greatest_GOT - greatest_EXP > greatest_TOL)) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, \ - "\nExpected: " GREATEST_FLOAT_FMT \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + "%s:%u: In %s:\n", \ + __FILE__, __LINE__, greatest_info.name_buf); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + "%s:%u: expected " GREATEST_FLOAT_FMT \ " +/- " GREATEST_FLOAT_FMT \ - "\n Got: " GREATEST_FLOAT_FMT \ - "\n", \ - greatest_EXP, greatest_TOL, greatest_GOT); \ + ", got " GREATEST_FLOAT_FMT \ + ", difference " GREATEST_FLOAT_FMT "\n", \ + __FILE__, __LINE__, \ + greatest_EXP, \ + greatest_TOL, \ + greatest_GOT, \ + greatest_EXP-greatest_GOT); \ GREATEST_FAILm(MSG); \ } \ } while (0) @@ -759,24 +774,17 @@ static void greatest_do_pass(void) { \ \ static void greatest_do_fail(void) { \ struct greatest_run_info *g = &greatest_info; \ - if (GREATEST_IS_VERBOSE()) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, \ - "%s:%u: error: test %s failed: %s", \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + "%s:%u: error: %s failed: %s\n", \ g->fail_file, g->fail_line, \ g->name_buf, \ g->msg ? g->msg : ""); \ + if (GREATEST_IS_VERBOSE()) { \ + GREATEST_FPRINTF(GREATEST_STDOUT, "FAIL %s: %s", \ + g->name_buf, g->msg ? g->msg : ""); \ } else { \ GREATEST_FPRINTF(GREATEST_STDOUT, "F"); \ g->col++; /* add linebreak if in line of '.'s */ \ - if (g->col != 0) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ - g->col = 0; \ - } \ - GREATEST_FPRINTF(GREATEST_STDOUT, \ - "%s:%u: error: test %s failed: %s", \ - g->fail_file, g->fail_line, \ - g->name_buf, \ - g->msg ? g->msg : ""); \ } \ g->suite.failed++; \ } \ From 2d0555e8fb2f77b98645b08a3295f0020a32a9c2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 8 Feb 2019 18:13:14 -0500 Subject: [PATCH 097/129] posemath: Overhaul and extend unit tests (PmCartLine, PmCircle, and PmCartesian) Add smoke tests for the posemath arithmetic functions, and some logic checks for the various vector operations. Not all math is tested here, but the basic conditions should be enforced. --- meson.build | 62 ++-- src/libnml/posemath/_posemath.c | 9 +- src/libnml/posemath/posemath.h | 12 +- unit_tests/tp/gen_posemath_boilerplate.m | 23 ++ unit_tests/tp/test_blendmath.c | 26 ++ unit_tests/tp/test_posemath.c | 433 +++++++++++++++++++++++ 6 files changed, 522 insertions(+), 43 deletions(-) create mode 100644 unit_tests/tp/gen_posemath_boilerplate.m create mode 100644 unit_tests/tp/test_posemath.c diff --git a/meson.build b/meson.build index af9e936b357..b72cb4f9c64 100644 --- a/meson.build +++ b/meson.build @@ -21,48 +21,44 @@ m_dep = cc.find_library('m', required : true) add_global_arguments(['-DULAPI','-DUNIT_TEST','-DTP_PEDANTIC_DEBUG'], language : 'c') -pm = static_library('posemath', - 'src/libnml/posemath/_posemath.c', - include_directories : inc_dir) - -# TODO implement subdir builds for tp, motion, etc +# Define static libraries for easier linking with unit tests -# KLUDGE just shove all the source files into the test build -test_blendmath=executable('test_blendmath', - ['unit_tests/tp/test_blendmath.c', - join_paths(tp_src_dir, 'blendmath.c'), - join_paths(tp_src_dir, 'spherical_arc.c'), - join_paths(tp_src_dir, 'joint_util.c'), - join_paths(src_root, 'emc/nml_intf/emcpose.c'), - join_paths(tp_src_dir, 'tc.c'), - ], - dependencies : [m_dep], +posemath_obj = static_library('posemath', + 'src/libnml/posemath/_posemath.c', include_directories : inc_dir, - link_with : pm + dependencies : [m_dep] ) -test('blendmath test',test_blendmath) +emcpose_obj = static_library('emcpose', + join_paths(src_root, 'emc/nml_intf/emcpose.c'), + include_directories : inc_dir + ) -test_joint_util=executable('test_joint_util', - ['unit_tests/tp/test_joint_util.c', +utlib_tp_deps = static_library('tp_deps', + [ join_paths(tp_src_dir, 'joint_util.c'), - join_paths(src_root, 'emc/nml_intf/emcpose.c'), - ], - dependencies : [m_dep], + join_paths(tp_src_dir, 'blendmath.c'), + join_paths(tp_src_dir, 'spherical_arc.c'), + join_paths(tp_src_dir, 'tc.c'), + join_paths(tp_src_dir, 'tcq.c'), + ], include_directories : inc_dir, - link_with : pm - ) + link_with: [emcpose_obj, posemath_obj]) -test('joint_util test',test_joint_util) +tp_test_files = [ + 'test_blendmath', + 'test_joint_util', + 'test_spherical_arc', + 'test_posemath' + ] -test_spherical_arc=executable('test_spherical_arc', - ['unit_tests/tp/test_spherical_arc.c', - join_paths(tp_src_dir, 'spherical_arc.c'), - join_paths(src_root, 'emc/nml_intf/emcpose.c'), - ], +foreach n : tp_test_files + +test(n, executable(n, + join_paths('unit_tests/tp', n+'.c'), dependencies : [m_dep], include_directories : inc_dir, - link_with : pm - ) + link_with : [ utlib_tp_deps] + )) +endforeach -test('spherical_arc test',test_spherical_arc) diff --git a/src/libnml/posemath/_posemath.c b/src/libnml/posemath/_posemath.c index 2d6ba394263..4922a1f9c04 100644 --- a/src/libnml/posemath/_posemath.c +++ b/src/libnml/posemath/_posemath.c @@ -777,12 +777,9 @@ int pmHomPoseConvert(PmHomogeneous const * const h, PmPose * const p) int pmCartCartCompare(PmCartesian const * const v1, PmCartesian const * const v2) { - if (fabs(v1->x - v2->x) >= V_FUZZ || - fabs(v1->y - v2->y) >= V_FUZZ || fabs(v1->z - v2->z) >= V_FUZZ) { - return 0; - } - - return 1; + return fabs(v1->x - v2->x) < V_FUZZ + && fabs(v1->y - v2->y) < V_FUZZ + && fabs(v1->z - v2->z) < V_FUZZ; } int pmCartCartDot(PmCartesian const * const v1, PmCartesian const * const v2, double *d) diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index 610a7d761fd..c08043bd6bf 100644 --- a/src/libnml/posemath/posemath.h +++ b/src/libnml/posemath/posemath.h @@ -771,10 +771,14 @@ extern "C" { /* global error number and errors */ extern int pmErrno; extern void pmPerror(const char *fmt); -#define PM_ERR -1 /* unspecified error */ -#define PM_IMPL_ERR -2 /* not implemented */ -#define PM_NORM_ERR -3 /* arg should have been norm */ -#define PM_DIV_ERR -4 /* divide by zero error */ + + typedef enum { + PM_DIV_ERR = -4, /* divide by zero error */ + PM_NORM_ERR = -3, /* arg should have been norm */ + PM_IMPL_ERR = -2, /* not implemented */ + PM_ERR = -1, /* unspecified error */ + PM_OK = 0 + } PosemathErrCode; /* Scalar functions */ diff --git a/unit_tests/tp/gen_posemath_boilerplate.m b/unit_tests/tp/gen_posemath_boilerplate.m new file mode 100644 index 00000000000..a4b23810f68 --- /dev/null +++ b/unit_tests/tp/gen_posemath_boilerplate.m @@ -0,0 +1,23 @@ +%% Generate arithmetic test examples for Posemath library +% Suitable for copy/paste into a unit test +s = (rand(3,1)-0.5)*rand(1)*10; +e = (rand(3,1)-0.5)*rand(1)*10; +k = (rand(1)-0.5)*2.0; + + +fprintf(' static const PmCartesian v1 = {%0.17g, %0.17g, %0.17g};\n', s); +fprintf(' static const PmCartesian v2 = {%0.17g, %0.17g, %0.17g};\n', e); +fprintf(' static const double k = %0.17g;\n', k); + +fprintf(' static const double mag_v1 = %0.17g;\n',norm(s)); +fprintf(' static const double mag_v2 = %0.17g;\n',norm(e)); +fprintf(' static const double mag_diff = %0.17g;\n',sqrt(sum((e-s).^2))); +fprintf(' static const double dot = %0.17g;\n', e'*s); +fprintf(' static const PmCartesian cross = {%0.17g, %0.17g, %0.17g};\n', cross(s,e)); +fprintf(' static const PmCartesian diff = {%0.17g, %0.17g, %0.17g};\n', e-s); +fprintf(' static const PmCartesian sum = {%0.17g, %0.17g, %0.17g};\n', e+s); +fprintf(' static const PmCartesian v1_neg = {%0.17g, %0.17g, %0.17g};\n', -s); +fprintf(' static const PmCartesian v1_mult_k = {%0.17g, %0.17g, %0.17g};\n', s*k); +fprintf(' static const PmCartesian v1_div_k = {%0.17g, %0.17g, %0.17g};\n', s/k); +fprintf(' static const PmCartesian elem_mult = {%0.17g, %0.17g, %0.17g};\n', s.*e); +fprintf(' static const PmCartesian elem_div = {%0.17g, %0.17g, %0.17g};\n', s./e); \ No newline at end of file diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 7e696e521f8..4b9655016d0 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -190,10 +190,36 @@ SUITE(tc_functions) { RUN_TEST(checkEndCondition_below_final_velocity); } +TEST test_pmCircleActualMaxVel_cutoff() +{ + double const v_max = 8.0; + double const a_max = 30.0; + double const r_cutoff = pmSq(v_max)/(a_max * BLEND_ACC_RATIO_NORMAL); + + PmCircle c; + PmCartesian start = {r_cutoff,0, 0}; + PmCartesian end = start; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 2); + + PmCircleLimits lim = pmCircleActualMaxVel(&c, v_max, a_max); + + ASSERT_IN_RANGE(lim.v_max, v_max, CART_FUZZ); + ASSERT_IN_RANGE(lim.acc_ratio, 0.5, CART_FUZZ); + PASS(); +} + +SUITE(circle_funcs) +{ + RUN_TEST(test_pmCircleActualMaxVel_cutoff); +} + int main(int argc, char **argv) { GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ RUN_SUITE(blendmath); RUN_SUITE(joint_utils); RUN_SUITE(tc_functions); + RUN_SUITE(circle_funcs); GREATEST_MAIN_END(); /* display results */ } diff --git a/unit_tests/tp/test_posemath.c b/unit_tests/tp/test_posemath.c new file mode 100644 index 00000000000..62c7a502973 --- /dev/null +++ b/unit_tests/tp/test_posemath.c @@ -0,0 +1,433 @@ +#include "posemath.h" +#include "greatest.h" +#include "rtapi.h" +#include "math.h" +#include "motion_debug.h" + +/* Expand to all the definitions that need to be in + the test runner's main file. */ +GREATEST_MAIN_DEFS(); + +#include "mock_rtapi.inc" + +TEST test_pmCartCartCompare_self() +{ + PmCartesian start = {10.0, 2.0, -3.45}; + + // Always self-compare positive + ASSERT(pmCartCartCompare(&start, &start)); + PASS(); +} + +TEST test_pmCartCartCompare_small() +{ + PmCartesian start = {DOUBLE_FUZZ, DOUBLE_FUZZ, DOUBLE_FUZZ}; + + PmCartesian large_diff=start; + large_diff.z += 1.0; + ASSERT(pmCartCartCompare(&start, &large_diff) == 0); + + // Always self-compare positive + ASSERT(pmCartCartCompare(&start, &start)); + + // NOTE: the initial point is <0,0,0> so that initial magnitude is small, + // and adding the fuzz value does not round away the difference. + PmCartesian too_small=start; + too_small.y += nextafter(CART_FUZZ, 0.0); + ASSERT(pmCartCartCompare(&start, &too_small) == 1); + + PmCartesian big_enough=start; + big_enough.x += nextafter(CART_FUZZ, 1.0); + ASSERT(pmCartCartCompare(&start, &big_enough) == 0); + + PASS(); +} + +TEST test_pmCartCartDot_parallel() +{ + PmCartesian zero = {0.0,0.0,0.0}; + double pmdot; + pmCartCartDot(&zero, &zero, &pmdot); + ASSERT_FLOAT_EQ(pmdot, 0.0); + + PmCartesian one = {1.0,-2.0,0.5}; + pmCartCartDot(&one, &one, &pmdot); + ASSERT_FLOAT_EQ(pmdot, 5.25); + PASS(); +} + +TEST test_pmCartCartDot_perpendicular() +{ + PmCartesian v1 = {cos(M_PI_4), sin(M_PI_4), 0.0}; + PmCartesian v2 = {sin(M_PI_4), -cos(M_PI_4), 0.0}; + double pmdot; + pmCartCartDot(&v1, &v2, &pmdot); + ASSERT_FLOAT_EQ(pmdot, 0.0); + + PASS(); +} + +TEST test_pmCartInfNorm() +{ + PmCartesian v1 ={3.25, -1, 2.0}; + PmCartesian v2 = v1; + double v1_inf; + pmCartInfNorm(&v1, &v1_inf); + ASSERT_FLOAT_EQ(v1_inf, 3.25); + + v2.y = -4; + double v2_inf; + pmCartInfNorm(&v2, &v2_inf); + ASSERT_FLOAT_EQ(v2_inf, 4); + + PASS(); +} + +static const PmCartesian v1 = {0.0011892247147802617, 0.019855106334368509, -0.00050476166977278807}; +static const PmCartesian v2 = {-0.15453530685917072, -0.28437966952533078, -0.15744435569802101}; +static const double k = -0.21253452789880767; +static const double mag_v1 = 0.019897092433684791; +static const double mag_v2 = 0.35991899456205328; +static const double mag_diff = 0.37608372040023119; +static const double dot = -0.0057506939081026628; +static const PmCartesian cross = {-0.0032696183809693856, 0.00026524021852782629, 0.0027301236187225355}; +static const PmCartesian diff = {-0.15572453157395097, -0.30423477585969927, -0.15693959402824822}; +static const PmCartesian sum = {-0.15334608214439047, -0.26452456319096229, -0.1579491173677938}; +static const PmCartesian v1_neg = {-0.0011892247147802617, -0.019855106334368509, 0.00050476166977278807}; +static const PmCartesian v1_mult_k = {-0.0002527513133214171, -0.0042198956511556368, 0.00010727928318657337}; +static const PmCartesian v1_div_k = {-0.0055954424278133177, -0.093420615137988119, 0.002374963140168529}; +static const PmCartesian elem_mult = {-0.00018377720622307753, -0.005646388577758019, 7.9471875878433867e-05}; +static const PmCartesian elem_div = {-0.0076954887459084794, -0.069819007693164015, 0.0032059686581646868}; + +TEST test_PmCartesian_arithmetic() +{ + double pm_mag_v1=0.0; + pmCartMag(&v1, &pm_mag_v1); + ASSERT_IN_RANGE(1e-16, mag_v1, pm_mag_v1); + + double pm_mag_v2=0.0; + pmCartMag(&v2, &pm_mag_v2); + ASSERT_IN_RANGE(mag_v2, 1e-16, pm_mag_v2); + + double pm_mag_sq=0.0; + pmCartMagSq(&v1, &pm_mag_sq); + ASSERT_FLOAT_EQ(pow(mag_v1, 2), pm_mag_sq); + + double pm_disp; + ASSERT_EQ(PM_OK, pmCartCartDisp(&v1, &v2, &pm_disp)); + ASSERT_FLOAT_EQ(mag_diff, pm_disp); + + PmCartesian pmsum; + ASSERT_EQ(PM_OK, pmCartCartAdd(&v1, &v2, &pmsum)); + ASSERT(pmCartCartCompare(&sum, &pmsum)); + + PmCartesian pm_diff; + ASSERT_EQ(PM_OK, pmCartCartSub(&v2, &v1, &pm_diff)); + ASSERT(pmCartCartCompare(&diff, &pm_diff)); + + PmCartesian pm_mult_k; + ASSERT_EQ(PM_OK, pmCartScalMult(&v1, k, &pm_mult_k)); + ASSERT(pmCartCartCompare(&v1_mult_k, &pm_mult_k)); + + PmCartesian pm_div_k; + ASSERT_EQ(PM_OK, pmCartScalDiv(&v1, k, &pm_div_k)); + ASSERT(pmCartCartCompare(&v1_div_k, &pm_div_k)); + + PmCartesian pm_neg; + ASSERT_EQ(PM_OK, pmCartNeg(&v1, &pm_neg)); + ASSERT(pmCartCartCompare(&v1_neg, &pm_neg)); + + double pm_dot=0.0; + pmCartCartDot(&v1, &v2, &pm_dot); + ASSERT_FLOAT_EQ(pm_dot, dot); + + PmCartesian pm_cross; + pmCartCartCross(&v1, &v2, &pm_cross); + ASSERT(pmCartCartCompare(&pm_cross, &cross)); + + PmCartesian pm_elem_mult; + pmCartCartMult(&v1, &v2, &pm_elem_mult); + ASSERT(pmCartCartCompare(&pm_elem_mult, &elem_mult)); + + PmCartesian pm_elem_div; + pmCartCartDiv(&v1, &v2, &pm_elem_div); + ASSERT(pmCartCartCompare(&pm_elem_div, &elem_div)); + + PASS(); +} + +TEST test_pmCartUnit() +{ + PmCartesian pm_u1; + ASSERT_FALSE(pmCartUnit(&v1, &pm_u1)); + + double rx = pm_u1.x / v1.x; + double ry = pm_u1.y / v1.y; + double rz = pm_u1.z / v1.z; + + // Verify that unit vector is in the same direction + ASSERT_FLOAT_EQ(rx, ry); + ASSERT_FLOAT_EQ(ry, rz); + ASSERT_FLOAT_EQ(rz, rx); + + double pm_umag; + ASSERT_FALSE(pmCartMag(&pm_u1, &pm_umag)); + + // And that the magnitude is close enough + ASSERT_IN_RANGE(1.0, 1e-16, pm_umag); + + // Show that the self-modifying version is the same output + PmCartesian ueq = v1; + ASSERT_FALSE(pmCartUnitEq(&ueq)); + ASSERT(pmCartCartCompare(&ueq, &pm_u1)); + PASS(); +} + +TEST test_pmCartAbs() +{ + PmCartesian mixed = {-0.1, 2.0, -5.0}; + + PmCartesian pm_abs; + ASSERT_FALSE(pmCartAbs(&mixed, &pm_abs)); + ASSERT(pm_abs.x >= 0.0); + ASSERT(pm_abs.y >= 0.0); + ASSERT(pm_abs.z >= 0.0); + PASS(); +} + +SUITE(pm_cart_basics) +{ + RUN_TEST(test_pmCartCartCompare_self); + RUN_TEST(test_pmCartCartCompare_small); + RUN_TEST(test_pmCartCartDot_parallel); + RUN_TEST(test_pmCartCartDot_perpendicular); + RUN_TEST(test_pmCartInfNorm); + RUN_TEST(test_PmCartesian_arithmetic); + RUN_TEST(test_pmCartUnit); + RUN_TEST(test_pmCartAbs); +} + +TEST test_pmCircleInit_simple() +{ + PmCircle c; + PmCartesian start = {10.0,0, 0}; + PmCartesian end = start; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + ASSERT_FLOAT_EQ(c.angle, 2.0 * M_PI); + ASSERT_FLOAT_EQ(c.radius, 10.0); + + PASS(); +} + +TEST test_pmCircleInit() +{ + PmCircle c; + PmCartesian center = {2.2, 3.3, 4.4}; + + const double nominal_radius = 0.5; + const double theta1 = 0.123; + const double theta2 = 1.456; + + PmCartesian start = {0.0, nominal_radius * cos(theta1), nominal_radius * sin(theta1)}; + pmCartCartAddEq(&start, ¢er); + PmCartesian end = {0.0, nominal_radius * cos(theta2), nominal_radius * sin(theta2)}; + pmCartCartAddEq(&end, ¢er); + + PmCartesian normal = {1,0,0}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + ASSERT_IN_RANGE(c.angle, theta2-theta1, CART_FUZZ); + ASSERT_IN_RANGE(c.radius, nominal_radius, CART_FUZZ); + + PASS(); +} + +TEST test_pmCircleStretch() +{ + PmCircle c; + PmCartesian center = {0.0, 0.0, 0.0}; + + const double nominal_radius = 0.5; + const double theta1 = M_PI_4; + const double theta2 = 3.0 / 8.0 * M_PI; + + PmCartesian start = {nominal_radius * cos(theta1), nominal_radius * sin(theta1), 0.0}; + PmCartesian end = {nominal_radius * cos(theta2), nominal_radius * sin(theta2), 0.0}; + PmCartesian normal = {0,0,1}; + + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + PmCartesian sample_before; + pmCirclePoint(&c, M_PI / 8.0, &sample_before); + + // Stretch to extend past the end point + pmCircleStretch(&c, M_PI, 0); + + PmCartesian sample_end_stretch; + pmCirclePoint(&c, M_PI / 8.0, &sample_end_stretch); + + ASSERT(pmCartCartCompare(&sample_before, &sample_end_stretch) == 1); + + // Stretch to extend past the starting point + pmCircleStretch(&c, M_PI * 5.0/4.0, 1); + + PmCartesian sample_start_stretch; + pmCirclePoint(&c, M_PI / 8.0 + M_PI_4, &sample_start_stretch); + + ASSERT(pmCartCartCompare(&sample_before, &sample_start_stretch) == 1); + PASS(); +} + +SUITE(pm_circle) { + RUN_TEST(test_pmCircleInit_simple); + RUN_TEST(test_pmCircleInit); + RUN_TEST(test_pmCircleStretch); +} + +TEST test_pmCartLineInit_simple() +{ + PmCartLine tst; + PmCartesian start = {0,0,0}; + PmCartesian end = {1,2,3}; + + pmCartLineInit(&tst, &start, &end); + + ASSERT_FLOAT_EQ(tst.tmag, sqrt(1 + 4 + 9)); + ASSERT(pmCartCartCompare(&start, &tst.start)); + ASSERT(pmCartCartCompare(&end, &tst.end)); + PASS(); +} + +TEST test_pmCartLineInit_ex1() +{ + PmCartLine tst; + const PmCartesian start = { + 0.51320891452372031, + 0.3513817033641376, + 0.30203143034775765, + }; + const PmCartesian end = { + 0.96211392505131188, + 0.20784727108932183, + 0.73266325488481165, + }; + const double mag = 0.63840552161564434; + pmCartLineInit(&tst, &start, &end); + + ASSERT_FLOAT_EQ(tst.tmag, mag); + ASSERT(pmCartCartCompare(&start, &tst.start)); + ASSERT(pmCartCartCompare(&end, &tst.end)); + PASS(); +} + +TEST test_pmCartLineInit_ex2() +{ + PmCartLine tst; + const PmCartesian start = { + 1.2196110251384671, + -0.99958732370407333, + 0.41726932038630371, + }; + const PmCartesian end = { + 0.29786615882106027, + 0.91903453415863678, + 0.7079824705329727, + }; + const double mag = 2.1483103983640843; + pmCartLineInit(&tst, &start, &end); + + ASSERT_FLOAT_EQ(tst.tmag, mag); + ASSERT(pmCartCartCompare(&start, &tst.start)); + ASSERT(pmCartCartCompare(&end, &tst.end)); + PASS(); +} + +TEST test_pmCartLineInit_small_belowfuzz() +{ + PmCartLine tst; + const PmCartesian start = { + 0,0,0 + }; + const double almost_fuzz = nextafter(CART_FUZZ, 0.0); + const PmCartesian end = { + almost_fuzz, + -almost_fuzz, + almost_fuzz, + }; + pmCartLineInit(&tst, &start, &end); + double mag=0.0; + pmCartMag(&end, &mag); + + // Store the actual magnitude + ASSERT_FLOAT_EQ(tst.tmag, mag); + ASSERT(tst.tmag_zero > 0); + ASSERT(pmCartCartCompare(&start, &tst.start)); + ASSERT(pmCartCartCompare(&end, &tst.end)); + PASS(); +} + +TEST test_pmCartLineInit_small_abovefuzz() +{ + PmCartLine tst; + const PmCartesian start = { + 0,0,0 + }; + const double above_fuzz = nextafter(CART_FUZZ, 1.0); + const PmCartesian end = { + above_fuzz, + -above_fuzz, + above_fuzz, + }; + pmCartLineInit(&tst, &start, &end); + + ASSERT(tst.tmag > 0.0); + ASSERT(tst.tmag_zero == 0); + ASSERT(pmCartCartCompare(&start, &tst.start)); + ASSERT(pmCartCartCompare(&end, &tst.end)); + PASS(); +} + +TEST test_pmCartLinePoint() +{ + PmCartLine tst; + const PmCartesian start = { + 0.020215567195396768, + -0.01189543243460452, + 0.025741132979321263, + }; + const PmCartesian end = { + 4.6752714631010335, + -2.6867637044820922, + -3.6444762156871819, + }; + + pmCartLineInit(&tst, &start, &end); + PmCartesian point = {0,0,0}; + pmCartLinePoint(&tst, 1.4828560169111997, &point); + + ASSERT(tst.tmag > 0.0); + ASSERT(tst.tmag_zero == 0); + ASSERT(pmCartCartCompare(&start, &tst.start)); + ASSERT(pmCartCartCompare(&end, &tst.end)); + PASS(); +} + +SUITE(pm_line) { + RUN_TEST(test_pmCartLineInit_simple); + RUN_TEST(test_pmCartLineInit_ex1); + RUN_TEST(test_pmCartLineInit_ex2); + RUN_TEST(test_pmCartLineInit_small_belowfuzz); + RUN_TEST(test_pmCartLineInit_small_abovefuzz); + RUN_TEST(test_pmCartLinePoint); +} + +int main(int argc, char **argv) { + GREATEST_MAIN_BEGIN(); + RUN_SUITE(pm_cart_basics); + RUN_SUITE(pm_circle); + RUN_SUITE(pm_line); + GREATEST_MAIN_END(); +} From acfdc67e4691f8d579b570ccd2d8cda8f1bb311c Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 14 Feb 2019 16:00:46 -0500 Subject: [PATCH 098/129] canon: Fix a pedantic unused variable warning (cherry picked from commit 716689ee1084584991fbe8f6f7b25395ad6726c2) --- src/emc/task/emccanon.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 22b353a0932..30190df1dc3 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -1847,7 +1847,9 @@ void ARC_FEED(int line_number, double a_max = total_xyz_length / tt_max; // Limit velocity by maximum +#ifdef EMCCANON_DEBUG double nominal_vel = getActiveFeedRate(FEED_LINEAR); +#endif // Make sure spindle speed is within range (for spindle_sync motion only) double vel = limitSpindleSpeedByActiveFeedRate(v_max); From 26e00c127ac277989774074ebe8777666e84d000 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 19 Feb 2019 13:23:30 -0500 Subject: [PATCH 099/129] posemath: Remove extraneous semicolons from definitions in posemath (cherry picked from commit a34a6220a973065503d1a4970331b8e1cf139a54) --- src/libnml/posemath/posemath.h | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index c08043bd6bf..86a1446d20f 100644 --- a/src/libnml/posemath/posemath.h +++ b/src/libnml/posemath/posemath.h @@ -123,7 +123,7 @@ struct PM_HOMOGENEOUS; /* Hom */ struct PM_CARTESIAN { /* ctors/dtors */ PM_CARTESIAN() { - }; + } PM_CARTESIAN(double _x, double _y, double _z); #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_CARTESIAN(PM_CCONST PM_CARTESIAN & cart); // added 7-May-1997 @@ -151,7 +151,7 @@ struct PM_CARTESIAN { struct PM_SPHERICAL { /* ctors/dtors */ PM_SPHERICAL() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s); #endif @@ -171,7 +171,7 @@ struct PM_SPHERICAL { struct PM_CYLINDRICAL { /* ctors/dtors */ PM_CYLINDRICAL() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c); #endif @@ -191,7 +191,7 @@ struct PM_CYLINDRICAL { struct PM_ROTATION_VECTOR { /* ctors/dtors */ PM_ROTATION_VECTOR() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r); #endif @@ -211,7 +211,7 @@ struct PM_ROTATION_VECTOR { struct PM_ROTATION_MATRIX { /* ctors/dtors */ PM_ROTATION_MATRIX() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & mat); /* added 7-May-1997 @@ -244,7 +244,7 @@ enum PM_AXIS { PM_X, PM_Y, PM_Z }; struct PM_QUATERNION { /* ctors/dtors */ PM_QUATERNION() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_QUATERNION(PM_CCONST PM_QUATERNION & quat); /* added 7-May-1997 by WPS */ @@ -274,7 +274,7 @@ struct PM_QUATERNION { struct PM_EULER_ZYZ { /* ctors/dtors */ PM_EULER_ZYZ() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz); #endif @@ -294,7 +294,7 @@ struct PM_EULER_ZYZ { struct PM_EULER_ZYX { /* ctors/dtors */ PM_EULER_ZYX() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx); #endif @@ -314,7 +314,7 @@ struct PM_EULER_ZYX { struct PM_RPY { /* ctors/dtors */ PM_RPY() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_RPY(PM_CCONST PM_RPY PM_REF rpy); /* added 7-May-1997 by WPS */ #endif @@ -334,7 +334,7 @@ struct PM_RPY { struct PM_POSE { /* ctors/dtors */ PM_POSE() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_POSE(PM_CCONST PM_POSE & p); #endif @@ -356,7 +356,7 @@ struct PM_POSE { struct PM_HOMOGENEOUS { /* ctors/dtors */ PM_HOMOGENEOUS() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h); #endif @@ -376,7 +376,7 @@ struct PM_HOMOGENEOUS { struct PM_LINE { /* ctors/dtors */ PM_LINE() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_LINE(PM_CCONST PM_LINE &); #endif @@ -396,7 +396,7 @@ struct PM_LINE { struct PM_CIRCLE { /* ctors/dtors */ PM_CIRCLE() { - }; + } #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS PM_CIRCLE(PM_CCONST PM_CIRCLE &); #endif From 8eecb7fd3bc0db96fcf4fcbc5b28c355a7f0f840 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 19 Feb 2019 14:01:46 -0500 Subject: [PATCH 100/129] tp: Pedantic rename of a function to match its result (cherry picked from commit ac66a3296e9444c49577fb7aeb36f4aceac88b72) --- src/emc/tp/blendmath.c | 18 +++++++++--------- src/emc/tp/blendmath.h | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 11d9b357d7e..50dac32f402 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -405,13 +405,13 @@ double pmCartMin(PmCartesian const * const in) * Calculate the diameter of a circle incscribed on a central cross section of a 3D * rectangular prism. * - * @param normal normal direction of plane slicing prism. + * @param normal normal direction of plane slicing prism (must be unit length!). * @param extents distance from center to one corner of the prism. * @param diameter diameter of inscribed circle on cross section. * */ -int calculateInscribedDiameter(PmCartesian const * const normal, - PmCartesian const * const bounds, double * const diameter) +int calculateInscribedRadius(PmCartesian const * const normal, + PmCartesian const * const bounds, double * const radius) { if (!normal ) { return TP_ERR_MISSING_INPUT; @@ -473,17 +473,17 @@ int calculateInscribedDiameter(PmCartesian const * const normal, } // Find the highest value to start from - *diameter = fmax(fmax(x_extent, y_extent),z_extent); + *radius = fmax(fmax(x_extent, y_extent),z_extent); // Only for active axes, find the minimum extent if (bounds->x > 0) { - *diameter = fmin(*diameter, x_extent); + *radius = fmin(*radius, x_extent); } if (bounds->y > 0) { - *diameter = fmin(*diameter, y_extent); + *radius = fmin(*radius, y_extent); } if (bounds->z > 0) { - *diameter = fmin(*diameter, z_extent); + *radius = fmin(*radius, z_extent); } return TP_ERR_OK; @@ -632,7 +632,7 @@ int blendParamKinematics(BlendGeom3 * const geom, tcFindBlendTolerance(prev_tc, tc, ¶m->tolerance, &nominal_tolerance); // Calculate max acceleration based on plane containing lines - int res_dia = calculateInscribedDiameter(&geom->binormal, acc_bound, ¶m->a_max); + int res_dia = calculateInscribedRadius(&geom->binormal, acc_bound, ¶m->a_max); // Store max normal acceleration param->a_n_max = param->a_max * BLEND_ACC_RATIO_NORMAL; @@ -652,7 +652,7 @@ int blendParamKinematics(BlendGeom3 * const geom, // Calculate the maximum planar velocity double v_planar_max; //FIXME sloppy handling of return value - res_dia |= calculateInscribedDiameter(&geom->binormal, vel_bound, &v_planar_max); + res_dia |= calculateInscribedRadius(&geom->binormal, vel_bound, &v_planar_max); tp_debug_print("v_planar_max = %f\n", v_planar_max); // Clip the angle at a reasonable value (less than 90 deg), to prevent div by zero diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 7b3d6ca5a60..b41f79b0724 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -141,8 +141,8 @@ int findIntersectionAngle(PmCartesian const * const u1, double pmCartMin(PmCartesian const * const in); -int calculateInscribedDiameter(PmCartesian const * const normal, - PmCartesian const * const bounds, double * const diameter); +int calculateInscribedRadius(PmCartesian const * const normal, + PmCartesian const * const bounds, double * const radius); int findAccelScale(PmCartesian const * const acc, PmCartesian const * const bounds, From 42183fdabe0731a605530d84be708d3417380237 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 19 Feb 2019 14:02:03 -0500 Subject: [PATCH 101/129] tp: Remove an unnecessary special case in tangent angle check (cherry picked from commit 668dda5844eb0eee2f8fd6ac3d2e6530bf6ceed9) --- src/emc/tp/blendmath.c | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 50dac32f402..dd857cf6ac6 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -38,13 +38,7 @@ double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) //TODO somewhat redundant with findKinkAccel, should refactor double acc_margin = BLEND_ACC_RATIO_NORMAL * BLEND_KINK_FACTOR * acc_limit; double dx = v_plan / cycle_time; - if (dx > 0.0) { - return (acc_margin / dx); - } else { - tp_debug_print(" Velocity or period is negative!\n"); - //Should not happen... - return TP_ANGLE_EPSILON; - } + return fabs(acc_margin / fmax(dx, TP_POS_EPSILON)); } From 259aa406bd10926afc0314cc38c3a5446aa4cc78 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 19 Feb 2019 14:02:54 -0500 Subject: [PATCH 102/129] tp: Add more unit tests for blendmath (cherry picked from commit 2742974108113ae5995f34e2c73ff5ce60c9212c) --- unit_tests/tp/test_blendmath.c | 71 ++++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 4b9655016d0..14c06f05c65 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -13,6 +13,44 @@ GREATEST_MAIN_DEFS(); #include "mock_rtapi.inc" +TEST test_findMaxTangentAngle() { + double acc_limit = 30.0; + double cycle_time = 0.001; + + // Don't bother testing arithmetic, just make sure that the output is sane + double max_angle_out = findMaxTangentAngle(1.0, acc_limit, cycle_time); + ASSERT(max_angle_out <= acc_limit / 1000); + ASSERT(max_angle_out >= 0.0); + + double max_angle_at_zero = findMaxTangentAngle(0.0, acc_limit, cycle_time); + ASSERT(max_angle_at_zero > 0.0); + + PASS(); +} + +TEST test_findKinkAccel() +{ + double acc_out = findKinkAccel(M_PI / 8, 2.0, 0.001); + ASSERT(acc_out > 0.0); + double acc_tangent = findKinkAccel(0, 2.0, 0.001); + ASSERT_FLOAT_EQ(0.0, acc_tangent); + PASS(); +} + +TEST test_findAccelScale() +{ + PmCartesian bounds = {5.0, 0.0, 20.0}; + PmCartesian acc = {200.0, 100.0, -1.0}; + + PmCartesian scale_out; + findAccelScale(&acc, &bounds, &scale_out); + + ASSERT_FLOAT_EQ(40, scale_out.x); + ASSERT_FLOAT_EQ(0, scale_out.y); + ASSERT_FLOAT_EQ(1/20.0, scale_out.z); + PASS(); +} + TEST pmCartCartParallel_numerical() { PmCartesian u0 = {1,0,0}; @@ -58,6 +96,9 @@ TEST pmCartCartAntiParallel_numerical() { } SUITE(blendmath) { + RUN_TEST(test_findMaxTangentAngle); + RUN_TEST(test_findKinkAccel); + RUN_TEST(test_findAccelScale); RUN_TEST(pmCartCartParallel_numerical); RUN_TEST(pmCartCartAntiParallel_numerical); } @@ -215,11 +256,41 @@ SUITE(circle_funcs) RUN_TEST(test_pmCircleActualMaxVel_cutoff); } +TEST test_calculateInscribedRadius() +{ + const PmCartesian xy = {0,0,1}; + const PmCartesian bounds = {3,4,5}; + double d_xy; + calculateInscribedRadius(&xy, &bounds, &d_xy); + ASSERT_FLOAT_EQ(3.0, d_xy); + + double d_yz; + const PmCartesian xz = {1,0,0}; + calculateInscribedRadius(&xz, &bounds, &d_yz); + ASSERT_FLOAT_EQ(4.0, d_yz); + + // Test the case where the normal vector is aligned just so the plane slices the prism along the XY diagonal + double d_xycorner; + PmCartesian corner = {-4.0, 3.0, 0.0}; + pmCartUnitEq(&corner); + + calculateInscribedRadius(&corner, &bounds, &d_xycorner); + ASSERT_FLOAT_EQ(5, d_xycorner); + + PASS(); +} + +SUITE(geom_funcs) +{ + RUN_TEST(test_calculateInscribedRadius); +} + int main(int argc, char **argv) { GREATEST_MAIN_BEGIN(); /* command-line arguments, initialization. */ RUN_SUITE(blendmath); RUN_SUITE(joint_utils); RUN_SUITE(tc_functions); RUN_SUITE(circle_funcs); + RUN_SUITE(geom_funcs); GREATEST_MAIN_END(); /* display results */ } From 833bdc94183a32c0583ac49736592f44d8c284fc Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 19 Feb 2019 15:54:03 -0500 Subject: [PATCH 103/129] unit_test: Document setup for unit test build in README --- README | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README b/README index 673760e5258..abe1706d425 100644 --- a/README +++ b/README @@ -46,3 +46,14 @@ to run the software go back to the top level directory, and issue: . scripts/rip-environment linuxcnc + + +Unit Tests +--------- + +Building unit tests requires Meson (see https://mesonbuild.com/Getting-meson.html for details). + +To build and run the unit tests (from the repositor root directory): + + meson build && cd build + ninja test From 089dee6d51e4d02da9e58dcc2ba8aa1895ee52c7 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 20 Feb 2019 13:58:37 -0500 Subject: [PATCH 104/129] Make greatest float printing more precise to help troubleshoot numerical errors --- unit_tests/greatest.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index 09b63bd6951..cc936ae5520 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -148,7 +148,7 @@ int main(int argc, char **argv) { /* Floating point type, for ASSERT_IN_RANGE. */ #ifndef GREATEST_FLOAT #define GREATEST_FLOAT double -#define GREATEST_FLOAT_FMT "%g" +#define GREATEST_FLOAT_FMT "%0.17g" #endif /* Size of buffer for test name + optional '_' separator and suffix */ From 8e1ac6bfd0c42c89ac7c096089a2491c31ca3f83 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 28 Feb 2019 23:14:24 -0500 Subject: [PATCH 105/129] tp: make TP axis limit error checks more robust * Refactor TP code to be more careful to check errors even in cases when the TP is waiting (checks for accidental slam-to-a-stop due to spindle-at-speed, for example). * Add position limit checks (will complain loudly if limits are violated but won't stop the machine) * Extend debug output when an error is reported so that more of the TP state can be inspected in the logs. --- src/emc/tp/joint_util.c | 86 ++++++++++++++++---- src/emc/tp/joint_util.h | 4 +- src/emc/tp/tp.c | 174 +++++++++++++++++++++++----------------- 3 files changed, 173 insertions(+), 91 deletions(-) diff --git a/src/emc/tp/joint_util.c b/src/emc/tp/joint_util.c index a055af4a8f5..34910932755 100644 --- a/src/emc/tp/joint_util.c +++ b/src/emc/tp/joint_util.c @@ -22,9 +22,35 @@ PmCartesian getXYZVelBounds() { return vel_bound; } -inline double jointMaxAccel(int joint_idx) +unsigned jointAccelViolation(int joint_idx, double acc) { - return emcmotDebug->joints[joint_idx].acc_limit; + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double ABS_TOL = 1e-4; + const double REL_TOL = 1.0 + ABS_TOL; + const double a_max_nominal = emcmotDebug->joints[joint_idx].acc_limit; + const double a_limit = fmax(a_max_nominal * REL_TOL, a_max_nominal + ABS_TOL); + return (unsigned)(fabs(acc) > a_limit) << joint_idx; +} + +unsigned jointVelocityViolation(int joint_idx, double v_actual) +{ + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double ABS_TOL = 1e-2; + const double REL_ACC_TOL = 1.0 + ABS_TOL; + const double v_max_nominal = emcmotDebug->joints[joint_idx].vel_limit; + const double v_limit = fmax(v_max_nominal * REL_ACC_TOL, v_max_nominal + ABS_TOL); + return (unsigned)(fabs(v_actual) > v_limit) << joint_idx; +} + +unsigned jointPositionViolation(int joint_idx, double position) +{ + static const double ABS_TOL = 1e-6; + return (unsigned)(position < emcmotDebug->joints[joint_idx].min_pos_limit - ABS_TOL && + position > emcmotDebug->joints[joint_idx].max_pos_limit + ABS_TOL) << joint_idx; } /** @@ -34,25 +60,53 @@ inline double jointMaxAccel(int joint_idx) */ unsigned findAccelViolations(EmcPose axis_accel) { - // Allow some numerical tolerance here since max acceleration is a bit - // fuzzy anyway. Torque is the true limiting factor, so there has to be - // some slack here anyway due to variations in table load, friction, etc. - static const double ABS_ACCEL_TOL = 1e-4; - const double REL_ACC_TOL = 1.0 + ABS_ACCEL_TOL; // Bit-mask each failure so we can report all failed axes unsigned fail_bits = (unsigned)(0x0 - | (fabs(axis_accel.tran.x) > (jointMaxAccel(0) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 0 - | (fabs(axis_accel.tran.y) > (jointMaxAccel(1) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 1 - | (fabs(axis_accel.tran.z) > (jointMaxAccel(2) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 2 - | (fabs(axis_accel.a) > (jointMaxAccel(3) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 3 - | (fabs(axis_accel.b) > (jointMaxAccel(4) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 4 - | (fabs(axis_accel.c) > (jointMaxAccel(5) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 5 - | (fabs(axis_accel.u) > (jointMaxAccel(6) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 6 - | (fabs(axis_accel.v) > (jointMaxAccel(7) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 7 - | (fabs(axis_accel.w) > (jointMaxAccel(8) * REL_ACC_TOL + ABS_ACCEL_TOL)) << 8); + | jointAccelViolation(0, axis_accel.tran.x) + | jointAccelViolation(1, axis_accel.tran.y) + | jointAccelViolation(2, axis_accel.tran.z) + | jointAccelViolation(3, axis_accel.a) + | jointAccelViolation(4, axis_accel.b) + | jointAccelViolation(5, axis_accel.c) + | jointAccelViolation(6, axis_accel.u) + | jointAccelViolation(7, axis_accel.v) + | jointAccelViolation(8, axis_accel.w)); + return fail_bits; +} + +unsigned findVelocityViolations(EmcPose axis_vel) +{ + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0 + | jointVelocityViolation(0, axis_vel.tran.x) + | jointVelocityViolation(1, axis_vel.tran.y) + | jointVelocityViolation(2, axis_vel.tran.z) + | jointVelocityViolation(3, axis_vel.a) + | jointVelocityViolation(4, axis_vel.b) + | jointVelocityViolation(5, axis_vel.c) + | jointVelocityViolation(6, axis_vel.u) + | jointVelocityViolation(7, axis_vel.v) + | jointVelocityViolation(8, axis_vel.w)); return fail_bits; } +unsigned findPositionLimitViolations(EmcPose axis_pos) +{ + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0 + | jointPositionViolation(0, axis_pos.tran.x) + | jointPositionViolation(1, axis_pos.tran.y) + | jointPositionViolation(2, axis_pos.tran.z) + | jointPositionViolation(3, axis_pos.a) + | jointPositionViolation(4, axis_pos.b) + | jointPositionViolation(5, axis_pos.c) + | jointPositionViolation(6, axis_pos.u) + | jointPositionViolation(7, axis_pos.v) + | jointPositionViolation(8, axis_pos.w)); + return fail_bits; +} + + double findMinNonZero(const PmCartesian * const bounds) { //Start with max accel value double act_limit = fmax(fmax(bounds->x, bounds->y), bounds->z); diff --git a/src/emc/tp/joint_util.h b/src/emc/tp/joint_util.h index 416918184ee..76b470b0e3d 100644 --- a/src/emc/tp/joint_util.h +++ b/src/emc/tp/joint_util.h @@ -9,8 +9,8 @@ PmCartesian getXYZAccelBounds(); PmCartesian getXYZVelBounds(); unsigned findAccelViolations(EmcPose axis_accel); - -double jointMaxAccel(int joint_idx); +unsigned findVelocityViolations(EmcPose axis_vel); +unsigned findPositionLimitViolations(EmcPose axis_pos); /** * Finds the smallest non-zero component in a non-negative "bounds" vector. diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 6ba6333a254..207ac6eb9eb 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -366,6 +366,8 @@ int tpClear(TP_STRUCT * const tp) tp->queueSize = 0; tp->goalPos = tp->currentPos; ZERO_EMC_POSE(tp->currentVel); + tp->time_elapsed_sec = 0.0; + tp->time_elapsed_ticks = 0; tp->nextId = 0; tp->execId = 0; tp->motionType = 0; @@ -2862,7 +2864,7 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { ZERO_EMC_POSE(tp_position_error); tcGetPos(tc, &tp_position_error); emcPoseSelfSub(&tp_position_error, &tp->currentPos); - int failed_axes = findAbsThresholdViolations(tp_position_error, TP_POS_EPSILON); + int failed_axes = findAbsThresholdViolations(tp_position_error, 1e-6); if (failed_axes) { // KLUDGE mask out good axes from print list with space char failed_axes_list[9] = "XYZABCUVW"; @@ -2874,37 +2876,35 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { } rtapi_print_msg(RTAPI_MSG_ERR, "TP Position violation on axes [%s] at segment %d\n", failed_axes_list, tc->id); - print_json5_start_(); - print_json5_object_start_("position_violation"); - print_json5_ll_("time_ticks", tp->time_elapsed_ticks); - print_json5_int_("id", tc->id); - print_json5_string_("failed_axes", failed_axes_list); - print_json5_EmcPose(tp_position_error); - print_json5_object_end_(); - print_json5_end_(); } #ifdef TP_DEBUG - print_json5_log_start(ActivateSegment,Run); - print_json5_ll_("time_ticks", tp->time_elapsed_ticks); - print_json5_int_("id", tc->id); - // Position settings - print_json5_double_("target", tc->target); - print_json5_double_("progress", tc->progress); - // Velocity settings - print_json5_double_("reqvel", tc->reqvel); - print_json5_double_("target_vel", tc->target_vel); - print_json5_double_("finalvel", tc->finalvel); - // Acceleration settings - print_json5_double_("accel_scale", tcGetAccelScale(tc)); - print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); - print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); - print_json5_bool_("accel_ramp", tc->accel_mode); - print_json5_bool_("blend_prev", tc->blend_prev); - print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); - print_json5_log_end(); + int need_print = true; +#else + int need_print = failed_axes; #endif + if (need_print) { + print_json5_log_start(ActivateSegment,Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_int_("id", tc->id); + // Position settings + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + // Velocity settings + print_json5_double_("reqvel", tc->reqvel); + print_json5_double_("target_vel", tc->target_vel); + print_json5_double_("finalvel", tc->finalvel); + // Acceleration settings + print_json5_double_("accel_scale", tcGetAccelScale(tc)); + print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); + print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); + print_json5_bool_("accel_ramp", tc->accel_mode); + print_json5_bool_("blend_prev", tc->blend_prev); + print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); + print_json5_log_end(); + } + return res; } @@ -3368,7 +3368,7 @@ tp_err_t updateSyncTargets(TP_STRUCT *tp, TC_STRUCT *tc, TC_STRUCT *nexttc) return TP_ERR_INVALID; } -int tpRunCycle(TP_STRUCT * const tp, long period) +int tpRunCycleInternal(TP_STRUCT * const tp) { //Pointers to current and next trajectory component TC_STRUCT *tc; @@ -3383,11 +3383,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) //Set GUI status to "zero" state tpUpdateInitialStatus(tp); - //Hack debug output for timesteps - // NOTE: need to track every timestep, even those where the trajectory planner is idle - tp->time_elapsed_sec+=tp->cycleTime; - ++tp->time_elapsed_ticks; - //If we have a NULL pointer, then the queue must be empty, so we're done. if(!tc) { tpHandleEmptyQueue(tp); @@ -3428,9 +3423,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) // If synchronized with spindle, calculate requested velocity to track spindle motion updateSyncTargets(tp, tc, nexttc); - - EmcPose const axis_pos_old = tp->currentPos; - EmcPose const axis_vel_old = tp->currentVel; tcClearFlags(tc); @@ -3442,41 +3434,84 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpHandleRegularCycle(tp, tc, nexttc); } + // If TC is complete, remove it from the queue. + if (tc->remove) { + tpCompleteSegment(tp, tc); + } + + return TP_ERR_OK; +} + +typedef struct { + char axes[9]; +} AxisMaskString; + +inline AxisMaskString axisBitMaskToString(unsigned failed_axes) +{ + AxisMaskString axis_str = {{"XYZABCUVW"}}; + for (int i = 0; i < 9;++i) { + if (failed_axes & (1 << i)) { + continue; + } + axis_str.axes[i] =' '; + } + return axis_str; +} + + +static inline void reportTPAxisError(TP_STRUCT const *tp, unsigned failed_axes, const char *msg_prefix) +{ + if (failed_axes) { - EmcPose const axis_pos = tp->currentPos; + AxisMaskString failed_axes_str = axisBitMaskToString(failed_axes); + rtapi_print_msg(RTAPI_MSG_ERR, "%s, axes [%s] at %g sec\n", + msg_prefix ?: "unknown error", + failed_axes_str.axes, + tp->time_elapsed_sec); + } +} - EmcPose axis_vel; - emcPoseSub(&axis_pos, &axis_pos_old, &axis_vel); - emcPoseMultScalar(&axis_vel, 1.0 / tp->cycleTime); - tp->currentVel = axis_vel; - EmcPose axis_accel; - emcPoseSub(&axis_vel, &axis_vel_old, &axis_accel); - emcPoseMultScalar(&axis_accel, 1.0 / tp->cycleTime); +int tpRunCycle(TP_STRUCT *tp, long period) +{ + // Before every TP update, ensure that elapsed time and + // TP measurements are stored for error checks + tp->time_elapsed_sec+=tp->cycleTime; + ++tp->time_elapsed_ticks; + EmcPose const axis_pos_old = tp->currentPos; + EmcPose const axis_vel_old = tp->currentVel; - unsigned failed_axes = findAccelViolations(axis_accel); - if (failed_axes) - { - // KLUDGE mask out good axes from print list with space - char failed_axes_list[9] = "XYZABCUVW"; - for (int i = 0; i < 9;++i) { - if (failed_axes & (1 << i)) { - continue; - } - failed_axes_list[i]=' '; - } + int res = tpRunCycleInternal(tp); - rtapi_print_msg(RTAPI_MSG_ERR, "Acceleration violation on axes [%s] at %g sec\n", failed_axes_list, tp->time_elapsed_sec); - print_json5_start_(); - print_json5_object_start_("accel_violation"); - print_json5_ll_("time_ticks", tp->time_elapsed_ticks); - print_json5_int_("id", tc->id); - print_json5_EmcPose(axis_accel); - print_json5_object_end_(); - print_json5_end_(); - } + // After update (even a no-op), update pos / vel / accel + EmcPose const axis_pos = tp->currentPos; -#ifdef TC_DEBUG + EmcPose axis_vel; + emcPoseSub(&axis_pos, &axis_pos_old, &axis_vel); + emcPoseMultScalar(&axis_vel, 1.0 / tp->cycleTime); + tp->currentVel = axis_vel; + + EmcPose axis_accel; + emcPoseSub(&axis_vel, &axis_vel_old, &axis_accel); + emcPoseMultScalar(&axis_accel, 1.0 / tp->cycleTime); + + unsigned accel_error_mask = findAccelViolations(axis_accel); + unsigned vel_error_mask = findVelocityViolations(axis_vel); + unsigned pos_limit_error_mask = findPositionLimitViolations(axis_pos); + + reportTPAxisError(tp, accel_error_mask, "Acceleration limit exceeded"); + reportTPAxisError(tp, vel_error_mask, "Velocity limit exceeded"); + reportTPAxisError(tp, pos_limit_error_mask, "Position limits exceeded"); + + #ifdef TC_DEBUG + unsigned debug_mask = (unsigned)(-1); + #else + unsigned debug_mask = 0; + #endif + + if (debug_mask || ( + accel_error_mask | vel_error_mask | pos_limit_error_mask) + ) { print_json5_log_start(tpRunCycle, Run); print_json5_ll_("time_ticks", tp->time_elapsed_ticks); print_json5_EmcPose(axis_pos); @@ -3486,15 +3521,8 @@ int tpRunCycle(TP_STRUCT * const tp, long period) print_json5_double(current_vel); print_json5_double_("time", tp->time_elapsed_sec); print_json5_end_(); -#endif } - - // If TC is complete, remove it from the queue. - if (tc->remove) { - tpCompleteSegment(tp, tc); - } - - return TP_ERR_OK; + return res; } int tpSetSpindleSync(TP_STRUCT * const tp, double sync, int mode) { From f976b2f6b387a393a0e64a0f906067cb5d2b8246 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 28 Feb 2019 23:17:39 -0500 Subject: [PATCH 106/129] tp: remove some obsolete debug output (to be replaced with unit testing) --- src/emc/tp/spherical_arc.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/emc/tp/spherical_arc.c b/src/emc/tp/spherical_arc.c index a1151eb4468..532632baa2b 100644 --- a/src/emc/tp/spherical_arc.c +++ b/src/emc/tp/spherical_arc.c @@ -86,18 +86,19 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * const out) { - //TODO pedantic +#ifdef TP_PEDANTIC + if (!arc) {return TP_ERR_MISSING_INPUT;} + if (!out) {return TP_ERR_MISSING_OUTPUT;} +#endif //Convert progress to actual progress around the arc double net_progress = progress - arc->line_length; if (net_progress <= 0.0 && arc->line_length > 0) { - tc_debug_print("net_progress = %f, line_length = %f\n", net_progress, arc->line_length); //Get position on line (not actually an angle in this case) pmCartScalMult(&arc->uTan, net_progress, out); pmCartCartAdd(out, &arc->start, out); } else { double angle_in = net_progress / arc->radius; - tc_debug_print("angle_in = %f, angle_total = %f\n", angle_in, arc->angle); double scale0 = sin(arc->angle - angle_in) / arc->Sangle; double scale1 = sin(angle_in) / arc->Sangle; From 78e2c0cea602bff87f372b83f4ec35f08ef39987 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 28 Feb 2019 23:16:06 -0500 Subject: [PATCH 107/129] tp: refactor TP functional tests to run headless * Make TP batch test script run headless (instead of using axis GUI). Along with the "nogui" versions of test configs, this makes it possible to execute a batch test, which will log TP errors to stderr, and return with a nonzero exit code if any errors are found. --- .../circular-arcs/configs/mill_XYZA.ini | 8 +- .../circular-arcs/configs/nogui_mill_XYZA.ini | 235 ++++++++++++++++++ .../circular-arcs/linuxcnc_control.py | 170 ++++++++----- .../circular-arcs/run_all_tests.py | 118 ++++----- 4 files changed, 404 insertions(+), 127 deletions(-) create mode 100644 tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini diff --git a/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini index e8be7d32172..30c30b8e880 100644 --- a/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini +++ b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini @@ -18,9 +18,8 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] -PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc -DISPLAY = axis +DISPLAY = dummy # Cycle time, in seconds, that display will sleep between polls CYCLE_TIME = 0.066666666666 @@ -115,7 +114,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -POSTGUI_HALFILE = postgui-XYZA.tcl +# POSTGUI_HALFILE = postgui-XYZA.tcl HALUI = halui @@ -230,3 +229,6 @@ CYCLE_TIME = 0.066666666666 TOOL_TABLE = sim.tbl TOOL_CHANGE_POSITION = 0 0 0 TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini b/tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini new file mode 100644 index 00000000000..f284c133ab8 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini @@ -0,0 +1,235 @@ +# EMC controller parameters for a simulated machine. + +# General note: Comments can either be preceded with a # or ; - either is +# acceptable, although # is in keeping with most linux config files. + +# General section ------------------------------------------------------------- +[EMC] + +# Version of this INI file +VERSION = $Revision$ + +# Name of machine, for use with display, etc. +MACHINE = LinuxCNC-Circular-Blend-Tester-XYZ + +# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others +#DEBUG = 0x7FFFFFFF +DEBUG = 0 + +# Sections for display options ------------------------------------------------ +[DISPLAY] +# Name of display program, e.g., xemc +DISPLAY = dummy + +# Cycle time, in seconds, that display will sleep between polls +CYCLE_TIME = 0.066666666666 + +# Path to help file +HELP_FILE = doc/help.txt + +# Initial display setting for position, RELATIVE or MACHINE +POSITION_OFFSET = RELATIVE + +# Initial display setting for position, COMMANDED or ACTUAL +POSITION_FEEDBACK = ACTUAL + +# Highest value that will be allowed for feed override, 1.0 = 100% +MAX_FEED_OVERRIDE = 2.0 +MAX_SPINDLE_OVERRIDE = 1.0 + +MAX_LINEAR_VELOCITY = 12 +DEFAULT_LINEAR_VELOCITY = .25 +# Prefix to be used +PROGRAM_PREFIX = ../nc_files/ + +EDITOR = gedit +TOOL_EDITOR = tooledit + +INCREMENTS = 1 in, 0.1 in, 10 mil, 1 mil, 1mm, .1mm, 1/8000 in + +GEOMETRY = XYZA + +[FILTER] +PROGRAM_EXTENSION = .png,.gif,.jpg Grayscale Depth Image +PROGRAM_EXTENSION = .py Python Script + +png = image-to-gcode +gif = image-to-gcode +jpg = image-to-gcode +py = python + +# Task controller section ----------------------------------------------------- +[TASK] + +# Name of task controller program, e.g., milltask +TASK = milltask + +# Cycle time, in seconds, that task controller will sleep between polls +CYCLE_TIME = 0.001 + +# Part program interpreter section -------------------------------------------- +[RS274NGC] + +# File containing interpreter variables +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs + +# Motion control section ------------------------------------------------------ +[EMCMOT] + +EMCMOT = motmod + +# Timeout for comm to emcmot, in seconds +COMM_TIMEOUT = 1.0 + +# Interval between tries to emcmot, in seconds +COMM_WAIT = 0.010 + +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 +# Servo task period, in nano-seconds +SERVO_PERIOD = 1000000 + +# Hardware Abstraction Layer section -------------------------------------------------- +[HAL] + +# The run script first uses halcmd to execute any HALFILE +# files, and then to execute any individual HALCMD commands. +# + +# list of hal config files to run through halcmd +# files are executed in the order in which they appear +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +# Other HAL files +(??) +HALFILE = sim_spindle_encoder.hal + +# list of halcmd commands to execute +# commands are executed in the order in which they appear +#HALCMD = save neta + +# Single file that is executed after the GUI has started. Only supported by +# AXIS at this time (only AXIS creates a HAL component of its own) +# POSTGUI_HALFILE = postgui-XYZA.tcl + +HALUI = halui + +# Trajectory planner section -------------------------------------------------- +[TRAJ] + +AXES = 4 +COORDINATES = X Y Z A +HOME = 0 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +POSITION_FILE = position.txt +MAX_LINEAR_VELOCITY = 12 + +ARC_BLEND_ENABLE = 1 +ARC_BLEND_FALLBACK_ENABLE = 0 +(??) +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + + +# Axes sections --------------------------------------------------------------- + +# First axis +[AXIS_0] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Second axis +[AXIS_1] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Third axis +[AXIS_2] + +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 25 +MAX_ACCELERATION = 800 +SCALE = 500.0 +FERROR = 5.0 +MIN_FERROR = 2.5 +MIN_LIMIT = -9999.0 +MAX_LIMIT = 9999.0 +HOME_OFFSET = 0.000000 +HOME_SEARCH_VEL = 0.00000 +HOME_LATCH_VEL = 0.00000 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# section for main IO controller parameters ----------------------------------- +[EMCIO] + +# Name of IO controller program, e.g., io +EMCIO = io + +# cycle time, in seconds +CYCLE_TIME = 0.066666666666 + +# tool table file +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/circular-arcs/linuxcnc_control.py b/tests/trajectory-planner/circular-arcs/linuxcnc_control.py index 33196931680..52dc835c173 100644 --- a/tests/trajectory-planner/circular-arcs/linuxcnc_control.py +++ b/tests/trajectory-planner/circular-arcs/linuxcnc_control.py @@ -1,22 +1,22 @@ #!/usr/bin/env python -'''Copied from m61-test''' +"""Copied from m61-test""" import linuxcnc import os +import re from time import sleep -# this is how long we wait for linuxcnc to do our bidding - class LinuxcncError(Exception): - pass -# def __init__(self, value): -# self.value = value -# def __str__(self): -# return repr(self.value) + def __init__(self, value): + self.value = value + + def __str__(self): + return repr(self.value) + class LinuxcncControl: - ''' + """ issue G-Code commands make sure important modes are saved and restored mode is saved only once, and can be restored only once @@ -27,29 +27,57 @@ class LinuxcncControl: any internal sub using e.g("G0.....") e.finish_mdi() - ''' + """ - def __init__(self,timeout=2): - self.c = linuxcnc.command() - self.e = linuxcnc.error_channel() - self.s = linuxcnc.stat() + def __init__(self, timeout=2, throw_exceptions=False): + self.c = None + self.e = None + self.s = None self.timeout = timeout + self.error_list = [] + self.raise_on_error = throw_exceptions + # Ignore blank errors by default + self.err_ignore_mask = '^$' + self.try_init(ignore_error=True) + + def try_init(self, ignore_error=False): + try: + self.c = linuxcnc.command() + self.e = linuxcnc.error_channel() + self.s = linuxcnc.stat() + self.s.poll() + except linuxcnc.error as e: + if ignore_error: + return False + + return True + + def wait_for_backend(self, timeout_sec): + k = 0 + print('Waiting for LinuxCNC...') + while k < 25: + k += 1 + sleep(1) + if self.try_init(ignore_error=True): + return True + + return False def running(self, do_poll=True): - ''' - check wether interpreter is running. + """ + check if interpreter is running. If so, cant switch to MDI mode. - ''' + """ if do_poll: self.s.poll() return (self.s.task_mode == linuxcnc.MODE_AUTO and self.s.interp_state != linuxcnc.INTERP_IDLE) - def set_mode(self,m): - ''' + def set_mode(self, m): + """ set EMC mode if possible, else throw LinuxcncError return current mode - ''' + """ self.s.poll() if self.s.task_mode == m : return m @@ -60,11 +88,11 @@ def set_mode(self,m): return m - def set_state(self,m): - ''' + def set_state(self, m): + """ set EMC mode if possible, else throw LinuxcncError return current mode - ''' + """ self.s.poll() if self.s.task_mode == m : return m @@ -72,25 +100,24 @@ def set_state(self,m): self.c.wait_complete(self.timeout) return m - def do_home(self,axismask): + def do_home(self, axismask): self.s.poll() self.c.home(axismask) self.c.wait_complete(self.timeout) - def ok_for_mdi(self): - ''' + """ check wether ok to run MDI commands. - ''' + """ self.s.poll() return not self.s.estop and self.s.enabled and self.s.homed def prepare_for_mdi(self): - ''' + """ check wether ok to run MDI commands. throw LinuxcncError if told so. return current mode - ''' + """ self.s.poll() if self.s.estop: @@ -109,11 +136,11 @@ def prepare_for_mdi(self): g_raise_except = True - def g(self,code,wait=False): - ''' + def g(self, code, wait=False): + """ issue G-Code as MDI command. wait for completion if reqested - ''' + """ self.c.mdi(code) if wait: @@ -122,19 +149,10 @@ def g(self,code,wait=False): pass return True except KeyboardInterrupt: - print "interrupted by keyboard in c.wait_complete(self.timeout)" + self.error_list.append("warning: interrupted by keyboard in c.wait_complete(self.timeout)") return False - self.error = self.e.poll() - if self.error: - kind, text = self.error - if kind in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR): - if self.g_raise_except: - raise LinuxcncError(text) - else: - print ("error " + text) - else: - print ("info " + text) + self.poll_and_log_error(code) return False def get_current_tool(self): @@ -142,7 +160,7 @@ def get_current_tool(self): return self.s.tool_in_spindle def active_codes(self): - self.e.poll() + self.s.poll() return self.s.gcodes def get_current_system(self): @@ -154,48 +172,80 @@ def get_current_system(self): return i - 584 return 1 - def open_program(self,filename): - '''Open an nc file''' + def open_program(self, filename): + """Open an nc file""" self.s.poll() self.set_mode(linuxcnc.MODE_AUTO) self.c.wait_complete() sleep(.25) - self.c.program_open(filename) + full_file = os.path.join(os.getcwd(), filename) + self.c.program_open(full_file) self.c.wait_complete() def run_full_program(self): - '''Start a loaded program''' + """Start a loaded program""" self.s.poll() self.c.auto(linuxcnc.AUTO_RUN, 0) - self.c.wait_complete(self.timeout) - return self.check_rcs_error() + return self.c.wait_complete(self.timeout) - def set_feed_scale(self,scale): - '''Assign a feed scale''' + def set_feed_scale(self, scale): + """Assign a feed scale""" self.s.poll() self.c.feedrate(scale) self.c.wait_complete(self.timeout) - def wait_on_program(self): + def wait_on_program(self, f): self.s.poll() while self.s.exec_state != linuxcnc.EXEC_DONE or self.s.state != linuxcnc.RCS_DONE and self.s.task_state == linuxcnc.STATE_ON: - sleep(.25) + sleep(.01) self.s.poll() + self.flush_errors(f) if self.s.task_state != linuxcnc.STATE_ON: + self.error_list.append('{}: error: motion disabled'.format(f)) return False - if self.check_rcs_error(): - print "Found RCS error while waiting, running again" - self.run_full_program() return True + def run_until_done(self, f): + self.set_mode(linuxcnc.MODE_AUTO) + self.open_program(f) + self.set_mode(linuxcnc.MODE_AUTO) + self.run_full_program() + return self.wait_on_program(f) + def check_rcs_error(self): self.s.poll() if self.s.state == linuxcnc.RCS_ERROR: - print "detected RCS error" return True return False -def introspect(): - os.system("halcmd show pin python-ui") + def set_error_ignore_pattern(self, pattern): + self.err_ignore_mask = pattern + + def poll_and_log_error(self, f): + error = self.e.poll() + if error is None: + return False + + err_type, msg_text = error + err_type_str = "error" if err_type in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) else "info" + err_str = "{}: {}: {}".format(f, err_type_str, msg_text) + self.error_list.append(err_str) + if self.raise_on_error: + raise LinuxcncError(err_str) + return True + + def flush_errors(self, filename=""): + while self.poll_and_log_error(filename): + sleep(0.001) + continue + + def has_error(self): + for msg in self.error_list: + if re.search(self.err_ignore_mask, msg): + continue + if re.search('error: ', msg): + return True + else: + return False diff --git a/tests/trajectory-planner/circular-arcs/run_all_tests.py b/tests/trajectory-planner/circular-arcs/run_all_tests.py index 3757fd9999e..d9750f2c850 100755 --- a/tests/trajectory-planner/circular-arcs/run_all_tests.py +++ b/tests/trajectory-planner/circular-arcs/run_all_tests.py @@ -1,9 +1,11 @@ #!/usr/bin/env python -'''Copied from m61-test''' +""" +Based on the control script in m61_test +""" import linuxcnc from linuxcnc_control import LinuxcncControl -import hal +#import hal import sys from time import sleep @@ -11,7 +13,8 @@ from os import listdir from os.path import isfile, join import re -import Tkinter, os +import glob + def query_yes_no(question, default="yes"): """Ask a yes/no question via raw_input() and return their answer. @@ -45,71 +48,58 @@ def query_yes_no(question, default="yes"): sys.stdout.write("Please respond with 'yes' or 'no' "\ "(or 'y' or 'n').\n") + def find_test_nc_files(testpath='nc_files', show=False): files = [ testpath + '/' + f for f in listdir(testpath) if isfile(join(testpath,f)) and re.search('.ngc$',f) is not None ] if show: print "In folder {0}, found {1} files:".format(testpath,len(files)) - for f in files: - print f + for fname in files: + print fname return files -def axis_open_program(t,f): - return t.tk.call("send", "axis", ("remote","open_file_name", os.path.abspath(f))) - -t = Tkinter.Tk() -t.wm_withdraw() -"""Run the test""" - -raw_input("Press any key when the LinuxCNC GUI has loaded") - -import glob -#KLUDGE this lists all subfolders in the auto-test directory -# this dir should be populated with symlinks to any folders of test files to -# run -test_folders=glob.glob('nc_files/auto-test/*') - -test_files = [] - - -for f in test_folders: - run_quick = query_yes_no("Run test cases in %s?" % f ) - if run_quick: - test_files.extend(find_test_nc_files(f,True)) - - -h = hal.component("python-ui") -h.ready() # mark the component as 'ready' - -#Don't need this with axis? -#os.system("halcmd source ./postgui.hal") - -# -# connect to LinuxCNC -# - -e = LinuxcncControl(1) -e.g_raise_except = False -e.set_mode(linuxcnc.MODE_MANUAL) -e.set_state(linuxcnc.STATE_ESTOP_RESET) -e.set_state(linuxcnc.STATE_ON) -e.do_home(-1) -sleep(1) -e.set_mode(linuxcnc.MODE_AUTO) -sleep(1) -for f in test_files: - if re.search('\.ngc$',f) is not None: - print "Loading program {0}".format(f) - e.set_mode(linuxcnc.MODE_AUTO) - sleep(1) - axis_open_program(t,f) - sleep(1) - e.set_mode(linuxcnc.MODE_AUTO) - while e.run_full_program(): - sleep(.5) - res = e.wait_on_program() - if res == False: - print "Program {0} failed to complete!".format(f) - sys.exit(1) - -print "All tests completed!" +if __name__ == '__main__': + """ Run batch tests (user specifies which ones interactively) """ + run_sts = 0 + + # raw_input("Press any key when the LinuxCNC GUI has loaded") + # KLUDGE this lists all subfolders in the auto-test directory + # this dir should be populated with symlinks to any folders of test files to + # run + test_folders = glob.glob('nc_files/auto-test/*') + + test_files = [] + + for f in test_folders: + test_files.extend(find_test_nc_files(f, True)) + + e = LinuxcncControl(1) + + load_timeout = 5 + if not e.wait_for_backend(load_timeout): + sys.stderr.write("error: LinuxCNC failed to load in {} seconds".format(load_timeout)) + sys.exit(2) + + e.g_raise_except = False + e.set_mode(linuxcnc.MODE_MANUAL) + e.set_state(linuxcnc.STATE_ESTOP_RESET) + e.set_state(linuxcnc.STATE_ON) + e.do_home(-1) + sleep(0.5) + e.set_mode(linuxcnc.MODE_AUTO) + sleep(0.5) + + error_list = [] + for f in test_files: + if re.search('.ngc$|.TAP$|.nc$', f) is not None: + print "Loading program {0}".format(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.open_program(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.run_full_program() + e.wait_on_program(f) + + # KLUDGE just in case any others creep in + e.flush_errors() + sys.stderr.writelines(e.error_list) + sys.exit(e.has_error()) From bc7d51054a8989dd8f24ce1736b3fdac23a67bad Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 28 Feb 2019 09:34:00 -0500 Subject: [PATCH 108/129] tp: Control new consistency checks with INI file settings to avoid console spam in production Also fix some functional tests broken by the INI change. --- src/emc/ini/initraj.cc | 8 + src/emc/motion-logger/motion-logger.c | 6 + src/emc/motion/command.c | 44 ++-- src/emc/motion/motion.h | 10 + src/emc/nml_intf/emc.hh | 2 + src/emc/nml_intf/emcpose.c | 7 +- src/emc/nml_intf/emcpose.h | 2 +- src/emc/task/taskintf.cc | 9 + src/emc/tp/tp.c | 217 +++++++++--------- .../mountaindew/expected.motion-logger | 1 + 10 files changed, 176 insertions(+), 130 deletions(-) diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index fd293d12ddd..258833d5d74 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -193,6 +193,14 @@ static int loadTraj(EmcIniFile *trajInifile) old_inihal_data.traj_arc_blend_tangent_kink_ratio = arcBlendTangentKinkRatio; //TODO update inihal + int extraConsistencyChecks = 0; + double maxPositionDriftError = 0.0001; + + trajInifile->Find(&extraConsistencyChecks, "EXTRA_CONSISTENCY_CHECKS", "TRAJ"); + trajInifile->Find(&maxPositionDriftError, "MAX_POSITION_DRIFT_ERROR", "TRAJ"); + + emcSetupConsistencyChecks(extraConsistencyChecks, maxPositionDriftError); + double maxFeedScale = 1.0; trajInifile->Find(&maxFeedScale, "MAX_FEED_OVERRIDE", "DISPLAY"); diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index fead99ebc45..fd9ec671431 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -625,6 +625,12 @@ int main(int argc, char* argv[]) { log_print("SETUP_ARC_BLENDS\n"); break; + case EMCMOT_SETUP_CONSISTENCY_CHECKS: + log_print("SETUP_CONSISTENCY_CHECKS enabled=%d, position_drift=%f\n", + c->consistencyCheckConfig.extraConsistencyChecks, + c->consistencyCheckConfig.maxPositionDriftError); + break; + default: log_print("ERROR: unknown command %d\n", c->command); break; diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 2cdfceae9bd..e48b9d7bc9f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1646,26 +1646,32 @@ check_stuff ( "before command_handler()" ); joint->comp.entries++; break; - case EMCMOT_SET_OFFSET: - emcmotStatus->tool_offset = emcmotCommand->tool_offset; - break; + case EMCMOT_SET_OFFSET: + emcmotStatus->tool_offset = emcmotCommand->tool_offset; + break; - default: - rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); - reportError(_("unrecognized command %d"), emcmotCommand->command); - emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND; - break; - case EMCMOT_SET_MAX_FEED_OVERRIDE: - emcmotConfig->maxFeedScale = emcmotCommand->maxFeedScale; - break; - case EMCMOT_SETUP_ARC_BLENDS: - emcmotConfig->arcBlendEnable = emcmotCommand->arcBlendEnable; - emcmotConfig->arcBlendFallbackEnable = emcmotCommand->arcBlendFallbackEnable; - emcmotConfig->arcBlendOptDepth = emcmotCommand->arcBlendOptDepth; - emcmotConfig->arcBlendGapCycles = emcmotCommand->arcBlendGapCycles; - emcmotConfig->arcBlendRampFreq = emcmotCommand->arcBlendRampFreq; - emcmotConfig->arcBlendTangentKinkRatio = emcmotCommand->arcBlendTangentKinkRatio; - break; + case EMCMOT_SET_MAX_FEED_OVERRIDE: + emcmotConfig->maxFeedScale = emcmotCommand->maxFeedScale; + break; + + case EMCMOT_SETUP_ARC_BLENDS: + emcmotConfig->arcBlendEnable = emcmotCommand->arcBlendEnable; + emcmotConfig->arcBlendFallbackEnable = emcmotCommand->arcBlendFallbackEnable; + emcmotConfig->arcBlendOptDepth = emcmotCommand->arcBlendOptDepth; + emcmotConfig->arcBlendGapCycles = emcmotCommand->arcBlendGapCycles; + emcmotConfig->arcBlendRampFreq = emcmotCommand->arcBlendRampFreq; + emcmotConfig->arcBlendTangentKinkRatio = emcmotCommand->arcBlendTangentKinkRatio; + break; + + case EMCMOT_SETUP_CONSISTENCY_CHECKS: + emcmotConfig->consistencyCheckConfig = emcmotCommand->consistencyCheckConfig; + break; + + default: + rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN"); + reportError(_("unrecognized command %d"), emcmotCommand->command); + emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND; + break; } /* end of: command switch */ if (emcmotStatus->commandStatus != EMCMOT_COMMAND_OK) { diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index bff0816a831..1efaab035f6 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -177,6 +177,7 @@ extern "C" { EMCMOT_SET_OFFSET, /* set tool offsets */ EMCMOT_SET_MAX_FEED_OVERRIDE, EMCMOT_SETUP_ARC_BLENDS, + EMCMOT_SETUP_CONSISTENCY_CHECKS, } cmd_code_t; /* this enum lists the possible results of a command */ @@ -194,6 +195,11 @@ extern "C" { #define EMCMOT_TERM_COND_BLEND 2 #define EMCMOT_TERM_COND_TANGENT 3 +typedef struct { + // Consistency checking within TP + int extraConsistencyChecks; + double maxPositionDriftError; +} consistency_check_config_t; /********************************* COMMAND STRUCTURE *********************************/ @@ -261,6 +267,9 @@ extern "C" { double arcBlendRampFreq; double arcBlendTangentKinkRatio; double maxFeedScale; + + consistency_check_config_t consistencyCheckConfig; + } emcmot_command_t; /*! \todo FIXME - these packed bits might be replaced with chars @@ -764,6 +773,7 @@ typedef enum { double arcBlendRampFreq; double arcBlendTangentKinkRatio; double maxFeedScale; + consistency_check_config_t consistencyCheckConfig; } emcmot_config_t; /********************************* diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 385999799b2..bdbd9b43b02 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -556,6 +556,8 @@ int emcSetupArcBlends(int arcBlendEnable, double arcBlendRampFreq, double arcBlendTangentKinkRatio); +int emcSetupConsistencyChecks(int consistency_checks, double max_position_drift_error); + extern int emcUpdate(EMC_STAT * stat); // full EMC status extern EMC_STAT *emcStatus; diff --git a/src/emc/nml_intf/emcpose.c b/src/emc/nml_intf/emcpose.c index 6ddf333a7f9..5daa750806b 100644 --- a/src/emc/nml_intf/emcpose.c +++ b/src/emc/nml_intf/emcpose.c @@ -332,11 +332,12 @@ int emcPoseValid(EmcPose const * const pose) * out-of-limit axes set their corresponding bit to 1 in the returned value (X * is 0th bit, Y is 1st, etc.). */ -int findAbsThresholdViolations(EmcPose vec, double threshold) +unsigned int findAbsThresholdViolations(EmcPose vec, double threshold) { threshold = fabs(threshold); // Bit-mask each failure so we can report all failed axes - int fail_bits = (fabs(vec.tran.x) > threshold) << 0 + unsigned fail_bits = (unsigned)(0x0 + | (fabs(vec.tran.x) > threshold) << 0 | (fabs(vec.tran.y) > threshold) << 1 | (fabs(vec.tran.z) > threshold) << 2 | (fabs(vec.a) > threshold) << 3 @@ -344,6 +345,6 @@ int findAbsThresholdViolations(EmcPose vec, double threshold) | (fabs(vec.c) > threshold) << 5 | (fabs(vec.u) > threshold) << 6 | (fabs(vec.v) > threshold) << 7 - | (fabs(vec.w) > threshold) << 8; + | (fabs(vec.w) > threshold) << 8); return fail_bits; } diff --git a/src/emc/nml_intf/emcpose.h b/src/emc/nml_intf/emcpose.h index f0f5750c740..5aa378de170 100644 --- a/src/emc/nml_intf/emcpose.h +++ b/src/emc/nml_intf/emcpose.h @@ -49,7 +49,7 @@ int emcPoseGetUVW(EmcPose const * const pose, PmCartesian * const uvw); int emcPoseMagnitude(EmcPose const * const pose, double * const out); -int findAbsThresholdViolations(EmcPose vec, double threshold); +unsigned int findAbsThresholdViolations(EmcPose vec, double threshold); int emcPoseValid(EmcPose const * const pose); diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 0f50242ff5a..55ed1494982 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1582,6 +1582,15 @@ int emcSetupArcBlends(int arcBlendEnable, return usrmotWriteEmcmotCommand(&emcmotCommand); } +int emcSetupConsistencyChecks(int consistency_checks, + double max_position_drift_error) +{ + emcmotCommand.command = EMCMOT_SETUP_CONSISTENCY_CHECKS; + emcmotCommand.consistencyCheckConfig.extraConsistencyChecks = consistency_checks; + emcmotCommand.consistencyCheckConfig.maxPositionDriftError = max_position_drift_error; + return usrmotWriteEmcmotCommand(&emcmotCommand); +} + int emcSetMaxFeedOverride(double maxFeedScale) { emcmotCommand.command = EMCMOT_SET_MAX_FEED_OVERRIDE; emcmotCommand.maxFeedScale = maxFeedScale; diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 207ac6eb9eb..40a51c571ff 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -89,6 +89,37 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc); + +typedef struct { + char axes[9]; +} AxisMaskString; + +inline AxisMaskString axisBitMaskToString(unsigned failed_axes) +{ + AxisMaskString axis_str = {{"XYZABCUVW"}}; + for (int i = 0; i < 9;++i) { + if (failed_axes & (1 << i)) { + continue; + } + axis_str.axes[i] =' '; + } + return axis_str; +} + + +static inline void reportTPAxisError(TP_STRUCT const *tp, unsigned failed_axes, const char *msg_prefix) +{ + if (failed_axes) + { + AxisMaskString failed_axes_str = axisBitMaskToString(failed_axes); + rtapi_print_msg(RTAPI_MSG_ERR, "%s, axes [%s] at %g sec\n", + msg_prefix ?: "unknown error", + failed_axes_str.axes, + tp->time_elapsed_sec); + } +} + + /** * @section tpcheck Internal state check functions. * These functions compartmentalize some of the messy state checks. @@ -2783,13 +2814,57 @@ STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) return TP_ERR_OK; } + +void checkPositionMatch(TP_STRUCT const *tp, TC_STRUCT const *tc) +{ + if (!emcmotConfig->consistencyCheckConfig.extraConsistencyChecks || tc->is_blending){ + return; + } + + EmcPose tp_position_error; + ZERO_EMC_POSE(tp_position_error); + tcGetPos(tc, &tp_position_error); + emcPoseSelfSub(&tp_position_error, &tp->currentPos); + + unsigned position_mismatch_axes = findAbsThresholdViolations(tp_position_error, emcmotConfig->consistencyCheckConfig.maxPositionDriftError); + reportTPAxisError(tp, position_mismatch_axes, "Motion start position difference exceeds threshold"); + +#ifdef TP_DEBUG + int need_print = true; +#else + int need_print = position_mismatch_axes != 0; +#endif + + // Log a bunch of TP internal state if required by debug level or position error + if (need_print) { + print_json5_log_start(ActivateSegment,Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_int_("id", tc->id); + // Position settings + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + // Velocity settings + print_json5_double_("reqvel", tc->reqvel); + print_json5_double_("target_vel", tc->target_vel); + print_json5_double_("finalvel", tc->finalvel); + // Acceleration settings + print_json5_double_("accel_scale", tcGetAccelScale(tc)); + print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); + print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); + print_json5_bool_("accel_ramp", tc->accel_mode); + print_json5_bool_("blend_prev", tc->blend_prev); + print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); + print_json5_log_end(); + } +} + + /** * "Activate" a segment being read for the first time. * This function handles initial setup of a new segment read off of the queue * for the first time. */ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { - //Check if already active if (!tc || tc->active) { return TP_ERR_OK; @@ -2859,52 +2934,7 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { res = TP_ERR_WAITING; } - - EmcPose tp_position_error; - ZERO_EMC_POSE(tp_position_error); - tcGetPos(tc, &tp_position_error); - emcPoseSelfSub(&tp_position_error, &tp->currentPos); - int failed_axes = findAbsThresholdViolations(tp_position_error, 1e-6); - if (failed_axes) { - // KLUDGE mask out good axes from print list with space - char failed_axes_list[9] = "XYZABCUVW"; - for (int i = 0; i < 9;++i) { - if (failed_axes & (1 << i)) { - continue; - } - failed_axes_list[i]=' '; - } - - rtapi_print_msg(RTAPI_MSG_ERR, "TP Position violation on axes [%s] at segment %d\n", failed_axes_list, tc->id); - } - -#ifdef TP_DEBUG - int need_print = true; -#else - int need_print = failed_axes; -#endif - - if (need_print) { - print_json5_log_start(ActivateSegment,Run); - print_json5_ll_("time_ticks", tp->time_elapsed_ticks); - print_json5_int_("id", tc->id); - // Position settings - print_json5_double_("target", tc->target); - print_json5_double_("progress", tc->progress); - // Velocity settings - print_json5_double_("reqvel", tc->reqvel); - print_json5_double_("target_vel", tc->target_vel); - print_json5_double_("finalvel", tc->finalvel); - // Acceleration settings - print_json5_double_("accel_scale", tcGetAccelScale(tc)); - print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); - print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); - print_json5_bool_("accel_ramp", tc->accel_mode); - print_json5_bool_("blend_prev", tc->blend_prev); - print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); - print_json5_log_end(); - } - + checkPositionMatch(tp, tc); return res; } @@ -3442,35 +3472,6 @@ int tpRunCycleInternal(TP_STRUCT * const tp) return TP_ERR_OK; } -typedef struct { - char axes[9]; -} AxisMaskString; - -inline AxisMaskString axisBitMaskToString(unsigned failed_axes) -{ - AxisMaskString axis_str = {{"XYZABCUVW"}}; - for (int i = 0; i < 9;++i) { - if (failed_axes & (1 << i)) { - continue; - } - axis_str.axes[i] =' '; - } - return axis_str; -} - - -static inline void reportTPAxisError(TP_STRUCT const *tp, unsigned failed_axes, const char *msg_prefix) -{ - if (failed_axes) - { - AxisMaskString failed_axes_str = axisBitMaskToString(failed_axes); - rtapi_print_msg(RTAPI_MSG_ERR, "%s, axes [%s] at %g sec\n", - msg_prefix ?: "unknown error", - failed_axes_str.axes, - tp->time_elapsed_sec); - } -} - int tpRunCycle(TP_STRUCT *tp, long period) { @@ -3479,48 +3480,50 @@ int tpRunCycle(TP_STRUCT *tp, long period) tp->time_elapsed_sec+=tp->cycleTime; ++tp->time_elapsed_ticks; EmcPose const axis_pos_old = tp->currentPos; - EmcPose const axis_vel_old = tp->currentVel; int res = tpRunCycleInternal(tp); - // After update (even a no-op), update pos / vel / accel - EmcPose const axis_pos = tp->currentPos; + if (emcmotConfig->consistencyCheckConfig.extraConsistencyChecks) { + // After update (even a no-op), update pos / vel / accel + EmcPose const axis_vel_old = tp->currentVel; + EmcPose const axis_pos = tp->currentPos; - EmcPose axis_vel; - emcPoseSub(&axis_pos, &axis_pos_old, &axis_vel); - emcPoseMultScalar(&axis_vel, 1.0 / tp->cycleTime); - tp->currentVel = axis_vel; + EmcPose axis_vel; + emcPoseSub(&axis_pos, &axis_pos_old, &axis_vel); + emcPoseMultScalar(&axis_vel, 1.0 / tp->cycleTime); + tp->currentVel = axis_vel; - EmcPose axis_accel; - emcPoseSub(&axis_vel, &axis_vel_old, &axis_accel); - emcPoseMultScalar(&axis_accel, 1.0 / tp->cycleTime); + EmcPose axis_accel; + emcPoseSub(&axis_vel, &axis_vel_old, &axis_accel); + emcPoseMultScalar(&axis_accel, 1.0 / tp->cycleTime); - unsigned accel_error_mask = findAccelViolations(axis_accel); - unsigned vel_error_mask = findVelocityViolations(axis_vel); - unsigned pos_limit_error_mask = findPositionLimitViolations(axis_pos); + unsigned accel_error_mask = findAccelViolations(axis_accel); + unsigned vel_error_mask = findVelocityViolations(axis_vel); + unsigned pos_limit_error_mask = findPositionLimitViolations(axis_pos); - reportTPAxisError(tp, accel_error_mask, "Acceleration limit exceeded"); - reportTPAxisError(tp, vel_error_mask, "Velocity limit exceeded"); - reportTPAxisError(tp, pos_limit_error_mask, "Position limits exceeded"); + reportTPAxisError(tp, accel_error_mask, "Acceleration limit exceeded"); + reportTPAxisError(tp, vel_error_mask, "Velocity limit exceeded"); + reportTPAxisError(tp, pos_limit_error_mask, "Position limits exceeded"); - #ifdef TC_DEBUG +#ifdef TC_DEBUG unsigned debug_mask = (unsigned)(-1); - #else +#else unsigned debug_mask = 0; - #endif +#endif - if (debug_mask || ( - accel_error_mask | vel_error_mask | pos_limit_error_mask) - ) { - print_json5_log_start(tpRunCycle, Run); - print_json5_ll_("time_ticks", tp->time_elapsed_ticks); - print_json5_EmcPose(axis_pos); - print_json5_EmcPose(axis_vel); - print_json5_EmcPose(axis_accel); - double current_vel = emcmotStatus->current_vel; - print_json5_double(current_vel); - print_json5_double_("time", tp->time_elapsed_sec); - print_json5_end_(); + if (debug_mask || ( + accel_error_mask | vel_error_mask | pos_limit_error_mask) + ) { + print_json5_log_start(tpRunCycle, Run); + print_json5_ll_("time_ticks", tp->time_elapsed_ticks); + print_json5_EmcPose(axis_pos); + print_json5_EmcPose(axis_vel); + print_json5_EmcPose(axis_accel); + double current_vel = emcmotStatus->current_vel; + print_json5_double(current_vel); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_end_(); + } } return res; } diff --git a/tests/motion-logger/mountaindew/expected.motion-logger b/tests/motion-logger/mountaindew/expected.motion-logger index de9e2a505a6..61b08859daf 100644 --- a/tests/motion-logger/mountaindew/expected.motion-logger +++ b/tests/motion-logger/mountaindew/expected.motion-logger @@ -3,6 +3,7 @@ SET_VEL vel=0.000000, ini_maxvel=120.000000 SET_VEL_LIMIT vel=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SET_ACC acc=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SETUP_ARC_BLENDS +SETUP_CONSISTENCY_CHECKS enabled=0, position_drift=0.000100 SET_MAX_FEED_OVERRIDE 1.000000 SET_WORLD_HOME x=0.000000, y=0.000000, z=0.000000, a=0.000000, b=0.000000, c=0.000000, u=0.000000, v=0.000000, w=0.000000 SET_BACKLASH joint=0, backlash=0.000000 From ce4f630f719f9922d79043bbfa545081572ea62f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 28 Feb 2019 23:32:09 -0500 Subject: [PATCH 109/129] test: Functional tests for TP from NGC snippets known to reproduce past issues This adds de-facto regression / functional tests for the TP. The batch-test runs a small set of NGC programs that have previously revealed bugs in TP / motion, and looks for any reported errors, including: * Violations of any axis acceleration, velocity, or position limits * Position "drift" in the trajectory planner (i.e. mismatch between TP and motion commands) The batch test provides a standard test runner for the functional tests, as well as error / result checking. This also replaces a lot of the hacked-together test functionality in the "circular-arcs" test folder (which was not designed for standalone / automated testing). Note that the circular-arcs folder still has utilities for manual testing, but a lot of the NGC files have been moved over to the batch test directory. --- .../trajectory-planner/batch-test/.gitignore | 5 + .../batch-test/batch-test.ini | 7 + .../trajectory-planner/batch-test/checkresult | 3 + .../batch-test/headless_batch_test.py | 51 +++ .../nc_files/arc-feed/arc_maxvel_xy.ngc | 0 .../nc_files/arc-feed/arc_time.ngc | 0 .../nc_files/arc-feed/basic_xy.ngc | 0 .../nc_files/arc-feed/full_xy.ngc | 0 .../nc_files/arc-feed/full_xz.ngc | 0 .../nc_files/arc-feed/full_yz.ngc | 0 .../arc-feed/optimal_circle_acc_ratio.ngc | 0 .../nc_files/arc-feed/simple_arc2.ngc | 0 .../nc_files/arc-feed/simple_arc3.ngc | 0 .../arc-arc-concave-convex.ngc | 13 + .../arc-arc-convex-concave.ngc | 13 + .../arc-arc-convex-convex.ngc | 10 + .../arc-intersections/arc-arc-convex.ngc | 12 + .../arc-intersections/arc-arc-neartangent.ngc | 10 + .../arc-intersections/arc-arc-planeswitch.ngc | 13 + .../arc-intersections/arc-arc-sawtooth.ngc | 38 +++ .../arc-intersections/arc-arc-test.ngc | 12 + .../arc-intersections/arc-large-radii.ngc | 13 + .../arc-intersections/arc-terminal.ngc | 13 + .../arc-intersections/arc-with-spiral.ngc | 10 + .../arc-intersections/line-arc-burnin.ngc | 47 +++ .../arc-intersections/linearc-convex.ngc | 9 + .../arc-intersections/linearc-end-overrun.ngc | 52 ++++ .../nc_files/arc-intersections/linearc.ngc | 12 + .../arc-intersections/tort-sam-mach3.ngc | 8 + .../nc_files/regressions}/546_simplfied.ngc | 0 .../regressions}/arc_vel_violation2.ngc | 0 .../blend_arc_simple_pos_test.ngc | 0 .../nc_files/regressions}/bug424_long.ngc | 0 .../regressions}/bug550_short_moves.ngc | 0 .../regressions}/circular_tolerance_check.ngc | 0 .../nc_files/regressions/corner_decel.ngc | 12 + .../regressions/cradek_accel_issue.ngc | 9 + .../nc_files/regressions}/feedtest.ngc | 0 .../nc_files/regressions}/full_reverse.ngc | 0 .../nc_files/regressions}/machinekit_412.ngc | 0 .../regressions}/maxvel_violation_helical.ngc | 0 .../maxvel_violation_helical2.ngc | 0 .../regressions}/maxvel_violation_line.ngc | 0 .../regressions}/maxvel_violation_line2.ngc | 0 .../nc_files/regressions/parabolic_bump.ngc | 10 + .../nc_files/regressions/parabolic_test.ngc | 9 + .../regressions/parabolic_varying.ngc | 12 + .../nc_files/regressions}/quick_arc.ngc | 0 .../nc_files/regressions}/rickg-arcs.ngc | 2 +- .../regressions}/sam-master-violation.ngc | 0 .../regressions}/sam_vel_violation.ngc | 0 .../nc_files/regressions}/sam_violation1.ngc | 0 .../nc_files/regressions}/sam_violation2.ngc | 0 .../regressions}/sam_violation2_verbatim.ngc | 0 .../nc_files/regressions}/sam_violation3.ngc | 3 +- .../regressions}/sam_zero_length_move.ngc | 0 .../regressions}/split-cycle-check.ngc | 0 .../regressions}/stellabee-kink-test.ngc | 0 .../stellabee-violation-spirals.ngc | 0 .../nc_files/regressions}/stop-err.ngc | 0 .../regressions}/tangent_lines_smooth.ngc | 0 .../tap-discrimant-error-orig.ngc | 2 +- .../regressions}/tkalcevic_realtime_error.ngc | 0 .../nc_files/regressions/warnings_g81.ngc | 20 ++ .../z_jump_tangent_improvement.ngc | 0 .../batch-test/nc_files/spindle/g33_blend.ngc | 24 ++ .../nc_files/spindle/g33_simple.ngc | 8 + .../batch-test/nc_files/spindle/g95_blend.ngc | 30 ++ .../nc_files/spindle/rigidtap_test.ngc | 11 + .../nc_files/spindle/spindle-wait.ngc | 33 ++ .../batch-test/nc_files/spindle/tapping.ngc | 10 + .../batch-test/position.blank | 9 + .../batch-test/print_load_times.py | 20 ++ tests/trajectory-planner/batch-test/sim.tbl | 1 + tests/trajectory-planner/batch-test/test.sh | 4 + .../configs/apply_limits_to_enable.tcl | 1 + ...i_mill_XYZA.ini => autotest_mill_XYZA.ini} | 18 +- .../configs/autotest_mill_XYZA_template.ini | 192 ++++++++++++ .../configs/load_constraint_components.tcl | 2 - .../circular-arcs/configs/mill_XYZA.ini | 5 +- .../configs/test_XYZA_all_programs.ini | 7 + .../configs/test_run_all_programs.py | 61 ++++ .../circular-arcs/linuxcnc_control.py | 90 ++++-- .../arc-intersections/tort-sam-mach3.ngc | 2 +- .../circular-arcs/nc_files/auto-test/XYtests | 1 - .../nc_files/auto-test/performance | 1 - .../violation_checks/spreadsheeterrors.ngc | 66 ---- .../circular-arcs/run_all_tests.py | 105 ------- .../common-config/apply_limits_to_enable.tcl | 1 + .../common-config/axis-A.tcl | 35 +++ .../common-config/axis-X.tcl | 35 +++ .../common-config/axis-Y.tcl | 35 +++ .../common-config/axis-Z.tcl | 34 ++ .../common-config/core_sim_components.hal | 19 ++ .../common-config/linuxcnc_control.py | 291 ++++++++++++++++++ .../load_constraint_components.tcl | 57 ++++ .../common-config/mill_XYZA_base.ini | 177 +++++++++++ .../trajectory-planner/common-config/sim.tbl | 6 + .../common-subs/default_inch.ngc | 4 + .../common-subs/default_mm.ngc | 4 + .../common-subs/default_modal_state.ngc | 19 ++ 101 files changed, 1628 insertions(+), 220 deletions(-) create mode 100644 tests/trajectory-planner/batch-test/.gitignore create mode 100644 tests/trajectory-planner/batch-test/batch-test.ini create mode 100755 tests/trajectory-planner/batch-test/checkresult create mode 100755 tests/trajectory-planner/batch-test/headless_batch_test.py rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/arc_maxvel_xy.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/arc_time.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/basic_xy.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/full_xy.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/full_xz.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/full_yz.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/optimal_circle_acc_ratio.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/simple_arc2.ngc (100%) rename tests/trajectory-planner/{circular-arcs => batch-test}/nc_files/arc-feed/simple_arc3.ngc (100%) create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-test.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/546_simplfied.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/arc_vel_violation2.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/blend_arc_simple_pos_test.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/bug424_long.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/bug550_short_moves.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/circular_tolerance_check.ngc (100%) create mode 100644 tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/feedtest.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/full_reverse.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/machinekit_412.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/maxvel_violation_helical.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/maxvel_violation_helical2.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/maxvel_violation_line.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/maxvel_violation_line2.ngc (100%) create mode 100644 tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/quick_arc.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/rickg-arcs.ngc (89%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam-master-violation.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam_vel_violation.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam_violation1.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam_violation2.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam_violation2_verbatim.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam_violation3.ngc (86%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/sam_zero_length_move.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/split-cycle-check.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/stellabee-kink-test.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/stellabee-violation-spirals.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/stop-err.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/tangent_lines_smooth.ngc (100%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/tap-discrimant-error-orig.ngc (98%) rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/tkalcevic_realtime_error.ngc (100%) create mode 100644 tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc rename tests/trajectory-planner/{circular-arcs/nc_files/violation_checks => batch-test/nc_files/regressions}/z_jump_tangent_improvement.ngc (100%) create mode 100644 tests/trajectory-planner/batch-test/nc_files/spindle/g33_blend.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/spindle/g33_simple.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/spindle/g95_blend.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc create mode 100644 tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc create mode 100644 tests/trajectory-planner/batch-test/position.blank create mode 100755 tests/trajectory-planner/batch-test/print_load_times.py create mode 120000 tests/trajectory-planner/batch-test/sim.tbl create mode 100755 tests/trajectory-planner/batch-test/test.sh create mode 100644 tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl rename tests/trajectory-planner/circular-arcs/configs/{nogui_mill_XYZA.ini => autotest_mill_XYZA.ini} (90%) create mode 100644 tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini create mode 100644 tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini create mode 100755 tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py delete mode 120000 tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests delete mode 120000 tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance delete mode 100644 tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc delete mode 100755 tests/trajectory-planner/circular-arcs/run_all_tests.py create mode 100644 tests/trajectory-planner/common-config/apply_limits_to_enable.tcl create mode 100644 tests/trajectory-planner/common-config/axis-A.tcl create mode 100644 tests/trajectory-planner/common-config/axis-X.tcl create mode 100644 tests/trajectory-planner/common-config/axis-Y.tcl create mode 100644 tests/trajectory-planner/common-config/axis-Z.tcl create mode 100644 tests/trajectory-planner/common-config/core_sim_components.hal create mode 100644 tests/trajectory-planner/common-config/linuxcnc_control.py create mode 100644 tests/trajectory-planner/common-config/load_constraint_components.tcl create mode 100644 tests/trajectory-planner/common-config/mill_XYZA_base.ini create mode 100644 tests/trajectory-planner/common-config/sim.tbl create mode 100644 tests/trajectory-planner/common-subs/default_inch.ngc create mode 100644 tests/trajectory-planner/common-subs/default_mm.ngc create mode 100644 tests/trajectory-planner/common-subs/default_modal_state.ngc diff --git a/tests/trajectory-planner/batch-test/.gitignore b/tests/trajectory-planner/batch-test/.gitignore new file mode 100644 index 00000000000..47e12288fc1 --- /dev/null +++ b/tests/trajectory-planner/batch-test/.gitignore @@ -0,0 +1,5 @@ +.idea +sim.var +axis*csv +test*.log +generated diff --git a/tests/trajectory-planner/batch-test/batch-test.ini b/tests/trajectory-planner/batch-test/batch-test.ini new file mode 100644 index 00000000000..a01f5158ce3 --- /dev/null +++ b/tests/trajectory-planner/batch-test/batch-test.ini @@ -0,0 +1,7 @@ +#INCLUDE ../common-config/mill_XYZA_base.ini + +# Choose the test program to run as the "display" which +# runs a headless instance, to test everything downstream of the GUI +[DISPLAY] +DISPLAY = ./headless_batch_test.py + diff --git a/tests/trajectory-planner/batch-test/checkresult b/tests/trajectory-planner/batch-test/checkresult new file mode 100755 index 00000000000..e534a8cf61d --- /dev/null +++ b/tests/trajectory-planner/batch-test/checkresult @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +# Passthrough since the batch test catches errors as it reports them +exit 0 \ No newline at end of file diff --git a/tests/trajectory-planner/batch-test/headless_batch_test.py b/tests/trajectory-planner/batch-test/headless_batch_test.py new file mode 100755 index 00000000000..f082229238f --- /dev/null +++ b/tests/trajectory-planner/batch-test/headless_batch_test.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +""" +Based on the control script in m61_test +""" + +# KLUDGE bake-in the path to common configs folder +import sys +sys.path.append('../common-config') + +import linuxcnc +from linuxcnc_control import LinuxcncControl + +from os import listdir +from os.path import isfile, join +import re +import glob + + +def find_test_nc_files(testpath='../nc_files', show=False): + files = [join(testpath, f) for f in listdir(testpath) if isfile(join(testpath, f)) and re.search('.ngc$|.nc$|.TAP$', f) is not None] + if show: + for fname in files: + print fname + return files + + +if __name__ == '__main__': + test_folders = glob.glob(join('nc_files', '*')) + + test_files = [] + + for f in test_folders: + test_files.extend(find_test_nc_files(f, True)) + + e = LinuxcncControl() + + e.do_startup(need_home=True) + + error_list = [] + for f in test_files: + e.log(f, 'info', 'loading program') + e.set_mode(linuxcnc.MODE_AUTO) + e.open_program(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.run_full_program() + if not e.wait_on_program(f): + e.do_startup(need_home=False) + + # KLUDGE just in case any others creep in + e.flush_errors() + sys.exit(e.has_error()) diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_maxvel_xy.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_maxvel_xy.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_maxvel_xy.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_time.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/arc_time.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/arc_time.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/basic_xy.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/basic_xy.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/basic_xy.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xy.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xy.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xy.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xz.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_xz.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/full_xz.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/full_yz.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/full_yz.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/full_yz.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/optimal_circle_acc_ratio.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/optimal_circle_acc_ratio.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/optimal_circle_acc_ratio.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc2.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc2.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc3.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/arc-feed/simple_arc3.ngc rename to tests/trajectory-planner/batch-test/nc_files/arc-feed/simple_arc3.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc new file mode 100644 index 00000000000..1ca8428f31a --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-concave-convex.ngc @@ -0,0 +1,13 @@ +(Intersection between two arcs where 1st is concave, 2nd is convex wrt intersection) +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.2 R.1 +G3 X.3 R.2 +G1 X.8 +G1 Y.2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc new file mode 100644 index 00000000000..5dce8b71164 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-concave.ngc @@ -0,0 +1,13 @@ +(Intersection between two arcs, concave concave, tests arc blend creation) +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G3 X.2 R.1 +G2 X.3 R.2 +G1 X.8 +G1 Y.2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc new file mode 100644 index 00000000000..006375c8834 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex-convex.ngc @@ -0,0 +1,10 @@ +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.2 R.1 +G2 X.1 R.1 +G1 X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc new file mode 100644 index 00000000000..022c4b24d83 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-convex.ngc @@ -0,0 +1,12 @@ +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.2 R.1 +G3 X.3 R.2 +G1 X.8 +G1 Y.2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc new file mode 100644 index 00000000000..b5f68a270cd --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-neartangent.ngc @@ -0,0 +1,10 @@ +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.2 Y.1 R.1 +G2 X.2999 Y0 I0 J-.1 +G1 X1 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc new file mode 100644 index 00000000000..a25e7d1ba89 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-planeswitch.ngc @@ -0,0 +1,13 @@ +o call +F1000 +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.2 R.1 +G3 X.3 R.1 +G18 +G3 X.4 R.1 +G17 +G3 X.5 R.1 +G1 X.8 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc new file mode 100644 index 00000000000..d8530b78867 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-sawtooth.ngc @@ -0,0 +1,38 @@ +o call +F1000 +G0 X0 Y0 +Z0 + +G2 X0 Y0.1 R0.2 +G3 X0.1 Y-0.1 R0.2 +G2 X0.2 Y0.1 R0.2 +G3 X0.3 Y-0.1 R0.2 +G2 X0.4 Y0.1 R0.2 +G3 X0.5 Y-0.1 R0.2 +G2 X0.6 Y0.1 R0.2 +G3 X0.7 Y-0.1 R0.2 +G2 X0.8 Y0.1 R0.2 +G3 X0.9 Y-0.1 R0.2 +G2 X1 Y0.1 R0.2 +G3 X1.1 Y-0.1 R0.2 +G2 X1.2 Y0.1 R0.2 +G3 X1.3 Y-0.1 R0.2 +G2 X1.4 Y0.1 R0.2 +G3 X1.5 Y-0.1 R0.2 +G2 X1.6 Y0.1 R0.2 +G3 X1.7 Y-0.1 R0.2 +G2 X1.8 Y0.1 R0.2 +G3 X1.9 Y-0.1 R0.2 +G2 X2 Y0.1 R0.2 +G3 X2.1 Y-0.1 R0.2 +G2 X2.2 Y0.1 R0.2 +G3 X2.3 Y-0.1 R0.2 +G2 X2.4 Y0.1 R0.2 +G3 X2.5 Y-0.1 R0.2 +G2 X2.6 Y0.1 R0.2 +G3 X2.7 Y-0.1 R0.2 +G2 X2.8 Y0.1 R0.2 +G3 X2.9 Y-0.1 R0.2 +G2 X3 Y0.1 R0.2 +G0 X0 Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-test.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-test.ngc new file mode 100644 index 00000000000..412eb027305 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-arc-test.ngc @@ -0,0 +1,12 @@ +G90 G20 G64 +F1000 +G0 X0 Y0 +Z0 +G1 X1 +G2 X2 R1 +G2 X3 R2 +G1 X4 +G1 Y2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc new file mode 100644 index 00000000000..cdf4fb777ce --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-large-radii.ngc @@ -0,0 +1,13 @@ +o call +G64 P0 Q0 +F1000 +G0 X0 Y0 +Z0 +G1 X1 +G2 X2 R1 +G2 X3 R2 +G1 X4 +G1 Y2 +X0 +Y0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc new file mode 100644 index 00000000000..ccb6e51eb32 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-terminal.ngc @@ -0,0 +1,13 @@ +o call +F1000 + +G0 X0 Y0 +Z0 +G1 X.1 +G2 X.3 R.1 +G3 X.4 R.05 +G1 Y.1 +G3 X.3 R0.05 +G2 X.1 R.1 +G1 X0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc new file mode 100644 index 00000000000..021162ebb8f --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/arc-with-spiral.ngc @@ -0,0 +1,10 @@ +G90 G20 G64 +F1000 +G0 X0 Y0 +Z0 +G1 X-.1 +G1 X.1 +G3 X.2 R.1 +G2 X.3 Y-.1 I.0503535 J-.05 +G1 X.4 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc new file mode 100644 index 00000000000..01dd5fff6ab --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/line-arc-burnin.ngc @@ -0,0 +1,47 @@ +o call +F1000 +G0 X0 Y0 +Z0 +G1 X0.0 +G1 X0.1 +G2 X0.2 R0.1 +G1 X0.3 +G3 X0.4 R0.1 +G1 X0.5 +G2 X0.6 R0.1 +G1 X0.7 +G3 X0.8 R0.1 +G1 X0.9 +G2 X1.0 R0.1 +G1 X1.1 +G3 X1.2 R0.1 +G1 X1.3 +G2 X1.4 R0.1 +G1 X1.5 +G3 X1.6 R0.1 +G1 X1.7 +G2 X1.8 R0.1 +G1 X1.9 +G3 X2.0 R0.1 +G1 X2.1 +G2 X2.2 R0.1 +G1 X2.3 +G3 X2.4 R0.1 +G1 X2.5 +G2 X2.6 R0.1 +G1 X2.7 +G3 X2.8 R0.1 +G1 X2.9 +G2 X3.0 R0.1 +G1 X3.1 +G3 X3.2 R0.1 +G1 X3.3 +G2 X3.4 R0.1 +G1 X3.5 +G3 X3.6 R0.1 +G1 X3.7 +G2 X3.8 R0.1 +G1 X3.9 +G3 X4.0 R0.1 +G0 X0 Y0 Z0 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc new file mode 100644 index 00000000000..460a43cd46c --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-convex.ngc @@ -0,0 +1,9 @@ +o call +F999.0000 +G0 X0 Y0 Z0 +G1 X-1 +G1 X2 +G2 X1 R.5 +G1 X3 +G1 X4 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc new file mode 100644 index 00000000000..f03391768b8 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc-end-overrun.ngc @@ -0,0 +1,52 @@ +o call +G0 Z0.0120 +F999.0000 +G18 +G0 X3.3390 Y4.8580 +G1 Z-0.0319 +G1 X3.2100 Z-0.1530 + X3.2040 Z-0.1539 + X3.2000 Z-0.1569 + X3.1940 Z-0.1539 + X3.1840 Z-0.1588 + X3.1680 Z-0.1549 + X3.1620 Z-0.1565 + X3.1380 Z-0.1539 + X3.1280 Z-0.1589 + X3.1260 Z-0.1575 + X3.1240 Z-0.1520 + X3.1200 Z-0.1510 + X3.1160 Z-0.1549 + X3.1060 Z-0.1588 +G3 X3.0940 Y4.8580 Z-0.1540 I0.0013 K0.0205 +G1 X3.0860 Z-0.1556 + X3.0800 Z-0.1539 + X3.0760 Z-0.1569 + X3.0700 Z-0.1539 + X3.0660 Z-0.1549 + X3.0620 Z-0.1529 + X3.0560 Z-0.1539 + X3.0520 Z-0.1520 + X3.0440 Z-0.1520 + X3.0400 Z-0.1549 + X3.0360 Z-0.1556 + X3.0280 Z-0.1549 + X3.0240 Z-0.1529 + X3.0040 Z-0.1549 + X2.9900 Z-0.1490 + X2.9860 Z-0.1529 + X2.9800 Z-0.1510 + X2.9740 Z-0.1546 + X2.9380 Z-0.1510 + X2.9300 Z-0.1526 + X2.9220 Z-0.1520 + X2.9180 Z-0.1559 + X2.9100 Z-0.1556 + X2.9080 Z-0.1536 + X2.9040 Z-0.1530 + X2.9000 Z-0.1559 + X2.8940 Z-0.1529 + X2.8860 Z-0.1520 + X2.8780 Z-0.1546 + X2.8760 Z-0.1569 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc new file mode 100644 index 00000000000..9845a9948a8 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/linearc.ngc @@ -0,0 +1,12 @@ +o call +G64 P0.0 Q0.0 +F1000 +G0 X0 Y0 Z0 +G1 X-.1 +G1 X.1 +G2 X.3 R.1 +G1 X.4 +G1 X.5 +G1 X.0 +M2 + diff --git a/tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc new file mode 100644 index 00000000000..0e5a5b4214d --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/arc-intersections/tort-sam-mach3.ngc @@ -0,0 +1,8 @@ +o call +G0 X0 Y0 Z20 +G0 X5.394910 Y13.495489 Z20.332291 +G19 G2 F12700 (225 29) J5.656854 K5.656854 X10.394910 Y26.080547 Z29.989145 +G19 G2 (75 14) J-0.258819 K-0.965926 X6.894910 Y26.787653 Z29.282038 +G17 G2 (329 285) I-4.330127 J2.500000 X3.858878 Y24.458024 Z31.282038 +G0 X0 Y0 Z20 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/546_simplfied.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/546_simplfied.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/546_simplfied.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/546_simplfied.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/arc_vel_violation2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/arc_vel_violation2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/arc_vel_violation2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/arc_vel_violation2.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/blend_arc_simple_pos_test.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/blend_arc_simple_pos_test.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/blend_arc_simple_pos_test.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/blend_arc_simple_pos_test.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug424_long.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/bug424_long.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug424_long.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/bug424_long.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug550_short_moves.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/bug550_short_moves.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/bug550_short_moves.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/bug550_short_moves.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/circular_tolerance_check.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/circular_tolerance_check.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/circular_tolerance_check.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/circular_tolerance_check.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc new file mode 100644 index 00000000000..18cb58ec6d3 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/corner_decel.ngc @@ -0,0 +1,12 @@ +G21 G90 G64 G40 G17 +G0 Z3.0 +M3 S10000 +G0 X1.75781 Y0.5 +G1 F100.0 Z-2.0 +G1 F1200.0 X5.95508 Y20.54297 +G1 X10.07031 +G1 X6.58398 Y3.84961 +G1 X16.7832 +G1 X30 +X20 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc new file mode 100644 index 00000000000..622d369ddb9 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/cradek_accel_issue.ngc @@ -0,0 +1,9 @@ +G20 G40 G80 G90 G94 G17 +G64 +F999 +G0 X0 Y0 Z0 +G0 X0.2320 Z-0.0771 Y0 +G1 X0.2560 Z-0.1203 +G18 G2 X0.2620 Y0 Z-0.1134 I-0.0025 K0.0082 +G1 X1 +M2 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/feedtest.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/feedtest.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/feedtest.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/feedtest.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/full_reverse.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/full_reverse.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/full_reverse.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/full_reverse.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/machinekit_412.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/machinekit_412.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/machinekit_412.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/machinekit_412.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_helical2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_helical2.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/maxvel_violation_line2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/maxvel_violation_line2.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc new file mode 100644 index 00000000000..51351807631 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_bump.ngc @@ -0,0 +1,10 @@ +G20 G64 G90 +G61 +G0 X0 Y0 Z0 +G64 +F9999 +G1 X4 +G1 X4.25 Y0.25 +G1 X4.5 Y0 +G1 X8 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc new file mode 100644 index 00000000000..80eb16b2cdd --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_test.ngc @@ -0,0 +1,9 @@ +(Test code for new blend velocity function) +G20 G90 G61 +G0 X0 Y0 Z0 +G64 +G1 X5 Y5 F9999 +G1 Y6 +G1 X0 Y11 +G0 X0 Y0 Z0 +M30 diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc new file mode 100644 index 00000000000..825148349eb --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/parabolic_varying.ngc @@ -0,0 +1,12 @@ +(Test code for new blend velocity function) +G20 G90 G64 +G0 X0 Y0 Z0 +G64 +G1 X5 Y5 F9999 +G1 Y6 +G1 X0 Y11 +G1 Y10 +G1 X-.5 Y9.5 +G1 Y5 +G0 X0 Y0 Z0 +M30 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/quick_arc.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/quick_arc.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/quick_arc.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/quick_arc.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/rickg-arcs.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/rickg-arcs.ngc similarity index 89% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/rickg-arcs.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/rickg-arcs.ngc index 5bcf9f952a0..b963c958f09 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/rickg-arcs.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/rickg-arcs.ngc @@ -1,4 +1,4 @@ -G90 G20 +o call G64 P0.001 Q0.001 F100 G0 X2 Y0 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam-master-violation.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam-master-violation.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam-master-violation.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam-master-violation.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_vel_violation.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_vel_violation.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_vel_violation.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_vel_violation.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation1.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation1.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation1.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation1.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2_verbatim.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2_verbatim.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation2_verbatim.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation2_verbatim.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation3.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation3.ngc similarity index 86% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation3.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation3.ngc index 869255fb836..5e11d466515 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_violation3.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_violation3.ngc @@ -1,4 +1,5 @@ -G21 G64p.127 +o call +G64p.127 G0 Z0 S6000 M4 G1 X-11.952 Y112.062 F10000000 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_zero_length_move.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/sam_zero_length_move.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/sam_zero_length_move.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/sam_zero_length_move.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/split-cycle-check.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/split-cycle-check.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/split-cycle-check.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/split-cycle-check.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-kink-test.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-kink-test.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-kink-test.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-kink-test.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-violation-spirals.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-violation-spirals.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stellabee-violation-spirals.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/stellabee-violation-spirals.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stop-err.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/stop-err.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/stop-err.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/stop-err.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tangent_lines_smooth.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/tangent_lines_smooth.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tangent_lines_smooth.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/tangent_lines_smooth.ngc diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tap-discrimant-error-orig.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/tap-discrimant-error-orig.ngc similarity index 98% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tap-discrimant-error-orig.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/tap-discrimant-error-orig.ngc index 211bc595941..d328d4dfc82 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tap-discrimant-error-orig.ngc +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/tap-discrimant-error-orig.ngc @@ -18,7 +18,7 @@ # = #5 (Spindle RPM) # = #6 (Tool Number) # = #7 (=8 Coolant 8 On 9 Off) - G7 G17 G20 G40 G80 + G17 G20 G40 G80 T# M6 G43 o<053o110> if [# EQ 0] # = [1 / #] diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tkalcevic_realtime_error.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/tkalcevic_realtime_error.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/tkalcevic_realtime_error.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/tkalcevic_realtime_error.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc new file mode 100644 index 00000000000..faa1c30199d --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/regressions/warnings_g81.ngc @@ -0,0 +1,20 @@ +G90 G20 G40 + +G64 P0 Q0 +/G61.1 + +G0 Z1 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +G81 X0 Z-1 R1 F100 P2 +X1 +G0 X0 + +M2 + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/z_jump_tangent_improvement.ngc b/tests/trajectory-planner/batch-test/nc_files/regressions/z_jump_tangent_improvement.ngc similarity index 100% rename from tests/trajectory-planner/circular-arcs/nc_files/violation_checks/z_jump_tangent_improvement.ngc rename to tests/trajectory-planner/batch-test/nc_files/regressions/z_jump_tangent_improvement.ngc diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/g33_blend.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_blend.ngc new file mode 100644 index 00000000000..40847f81382 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_blend.ngc @@ -0,0 +1,24 @@ +(From TP Issue #68) +G20 G8 G18 +G64 P0.01 Q0.0 +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-0.75 +G33 K0.1 X1.0 Z-1.0 +(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/batch-test/nc_files/spindle/g33_simple.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_simple.ngc new file mode 100644 index 00000000000..48add847aaf --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/g33_simple.ngc @@ -0,0 +1,8 @@ +o call +G0 X1 Z0.1 (rapid to position) +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) +M5 +M2 (end program) diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/g95_blend.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/g95_blend.ngc new file mode 100644 index 00000000000..b29e907260f --- /dev/null +++ b/tests/trajectory-planner/batch-test/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 + diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc new file mode 100644 index 00000000000..53f9dbb3ad7 --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/rigidtap_test.ngc @@ -0,0 +1,11 @@ +o call +M3 S200 +G0 X1.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +S500 +G0 X2 Y1 Z0.100 +G33.1 Z-0.750 K0.05 +G0 X3 Y1 Z0.100 +G33.1 Z-0.750 K0.1 +M5 +M2 diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc new file mode 100644 index 00000000000..e0fd5af447c --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/spindle-wait.ngc @@ -0,0 +1,33 @@ +o call +(Test of atspeed) +(Setup code) +(Move to standard initial position) +G0 x0 y0 z1 +(Tangent segments allow speed above "triangle" velocity) +X0.01 +X0.02 +X0.03 +X0.04 +X0.05 +X0.06 +X0.07 (Should momentarily stop here) +S3000 M3 +X0.08 +X0.09 +X0.10 +X0.11 +X0.12 +X0.13 +G0 X1.0 +M8 (Coolant too just in case) +G1 X0 Y1 F20 +G1 X-1 Y0 +G1 Y0 +Y0.1 +Y0.2 +M5 M9 +Z.1 +Z.2 +G0 Z1 +M2 + diff --git a/tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc b/tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc new file mode 100644 index 00000000000..c7ec7344c2b --- /dev/null +++ b/tests/trajectory-planner/batch-test/nc_files/spindle/tapping.ngc @@ -0,0 +1,10 @@ +o call +M3 S200 +G0 X1.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +G0 X2.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +G0 X3.000 Y1.000 Z0.100 +G33.1 Z-0.750 K0.05 +M5 +M2 diff --git a/tests/trajectory-planner/batch-test/position.blank b/tests/trajectory-planner/batch-test/position.blank new file mode 100644 index 00000000000..9bdc9d4bca4 --- /dev/null +++ b/tests/trajectory-planner/batch-test/position.blank @@ -0,0 +1,9 @@ +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 +0.00000000000000000 diff --git a/tests/trajectory-planner/batch-test/print_load_times.py b/tests/trajectory-planner/batch-test/print_load_times.py new file mode 100755 index 00000000000..5c169188f23 --- /dev/null +++ b/tests/trajectory-planner/batch-test/print_load_times.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python +import datetime +import re + +if __name__ == '__main__': + with open('batch-test-err.log') as err_log: + t_old = datetime.datetime(1970, 1, 1) + for l in err_log.readlines(): + dt_str = l.split('|')[0].rstrip() + try: + t = datetime.datetime.strptime(dt_str, "%Y-%m-%d %H:%M:%S.%f") + msg = l.split('|')[1].rstrip() + except ValueError: + continue + + if re.search('loading', l): + # print dt before recalculating (since dt here is for the next program + dt = (t-t_old).total_seconds() + print("{} seconds | {}".format(dt, msg)) + t_old = t diff --git a/tests/trajectory-planner/batch-test/sim.tbl b/tests/trajectory-planner/batch-test/sim.tbl new file mode 120000 index 00000000000..7ad328ea5e4 --- /dev/null +++ b/tests/trajectory-planner/batch-test/sim.tbl @@ -0,0 +1 @@ +../common-config/sim.tbl \ No newline at end of file diff --git a/tests/trajectory-planner/batch-test/test.sh b/tests/trajectory-planner/batch-test/test.sh new file mode 100755 index 00000000000..e027086322b --- /dev/null +++ b/tests/trajectory-planner/batch-test/test.sh @@ -0,0 +1,4 @@ +#!/usr/bin/env bash +cp position.blank position.txt +linuxcnc batch-test.ini 2> batch-test-err.log | tee batch-test-out.log +exit $? diff --git a/tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl b/tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl new file mode 100644 index 00000000000..d7ce71c27e7 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/apply_limits_to_enable.tcl @@ -0,0 +1 @@ +net constraints-ok <= match_all.out -> motion.enable diff --git a/tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA.ini similarity index 90% rename from tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini rename to tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA.ini index f284c133ab8..4ec415df1ce 100644 --- a/tests/trajectory-planner/circular-arcs/configs/nogui_mill_XYZA.ini +++ b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA.ini @@ -19,7 +19,7 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] # Name of display program, e.g., xemc -DISPLAY = dummy +DISPLAY = ../run_all_tests.py # Cycle time, in seconds, that display will sleep between polls CYCLE_TIME = 0.066666666666 @@ -93,30 +93,14 @@ SERVO_PERIOD = 1000000 # Hardware Abstraction Layer section -------------------------------------------------- [HAL] -# The run script first uses halcmd to execute any HALFILE -# files, and then to execute any individual HALCMD commands. -# - -# list of hal config files to run through halcmd -# files are executed in the order in which they appear HALFILE = core_sim_components.hal HALFILE = load_constraint_components.tcl HALFILE = axis-X.tcl HALFILE = axis-Y.tcl HALFILE = axis-Z.tcl HALFILE = axis-A.tcl -# Other HAL files -(??) HALFILE = sim_spindle_encoder.hal -# list of halcmd commands to execute -# commands are executed in the order in which they appear -#HALCMD = save neta - -# Single file that is executed after the GUI has started. Only supported by -# AXIS at this time (only AXIS creates a HAL component of its own) -# POSTGUI_HALFILE = postgui-XYZA.tcl - HALUI = halui # Trajectory planner section -------------------------------------------------- diff --git a/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini new file mode 100644 index 00000000000..8189e8e97f5 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/autotest_mill_XYZA_template.ini @@ -0,0 +1,192 @@ +# LinuxCNC controller parameters for a simulated 4-axis milling machine + +# General section ------------------------------------------------------------- +[EMC] + +# Version of this INI file +VERSION = $Revision$ + +# Name of machine, for use with display, etc. +MACHINE = LinuxCNC-Auto-Test-Config + +# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others +#DEBUG = 0x7FFFFFFF +DEBUG = 0 + +# NOTE can only handle raw G-code files + +# Task controller section ----------------------------------------------------- +[TASK] + +# Name of task controller program, e.g., milltask +TASK = milltask + +# Cycle time, in seconds, that task controller will sleep between polls +CYCLE_TIME = 0.001 + +# Part program interpreter section -------------------------------------------- +[RS274NGC] + +# File containing interpreter variables +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../nc_files/test_subs + +# Motion control section ------------------------------------------------------ +[EMCMOT] + +EMCMOT = motmod + +# Timeout for comm to emcmot, in seconds +COMM_TIMEOUT = 1.0 + +# Interval between tries to emcmot, in seconds +COMM_WAIT = 0.010 + +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 +# Servo task period, in nano-seconds +SERVO_PERIOD = 1000000 + +# Hardware Abstraction Layer section -------------------------------------------------- +[HAL] + +# The run script first uses halcmd to execute any HALFILE +# files, and then to execute any individual HALCMD commands. +# + +# list of hal config files to run through halcmd +# files are executed in the order in which they appear +HALFILE = core_sim_components.hal +HALFILE = load_constraint_components.tcl +HALFILE = axis-X.tcl +HALFILE = axis-Y.tcl +HALFILE = axis-Z.tcl +HALFILE = axis-A.tcl +# Other HAL files +(??) +HALFILE = sim_spindle_encoder.hal + +# list of halcmd commands to execute +# commands are executed in the order in which they appear +#HALCMD = save neta + +# Single file that is executed after the GUI has started. Only supported by +# AXIS at this time (only AXIS creates a HAL component of its own) +# POSTGUI_HALFILE = postgui-XYZA.tcl + +HALUI = halui + +# Trajectory planner section -------------------------------------------------- +[TRAJ] + +AXES = 4 +COORDINATES = X Y Z A +HOME = 0 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +POSITION_FILE = position.txt +MAX_LINEAR_VELOCITY = 12 + +ARC_BLEND_ENABLE = 1 +ARC_BLEND_FALLBACK_ENABLE = 0 +(??) +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + + +# Axes sections --------------------------------------------------------------- + +# First axis +[AXIS_0] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Second axis +[AXIS_1] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Third axis +[AXIS_2] + +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 25 +MAX_ACCELERATION = 800 +SCALE = 500.0 +FERROR = 5.0 +MIN_FERROR = 2.5 +MIN_LIMIT = -9999.0 +MAX_LIMIT = 9999.0 +HOME_OFFSET = 0.000000 +HOME_SEARCH_VEL = 0.00000 +HOME_LATCH_VEL = 0.00000 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# section for main IO controller parameters ----------------------------------- +[EMCIO] + +# Name of IO controller program, e.g., io +EMCIO = io + +# cycle time, in seconds +CYCLE_TIME = 0.066666666666 + +# tool table file +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl b/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl index 22f08405d55..5fa13c938db 100644 --- a/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl +++ b/tests/trajectory-planner/circular-arcs/configs/load_constraint_components.tcl @@ -55,5 +55,3 @@ setp match_all.b4 0 setp match_all.b5 0 setp match_all.b6 0 setp match_all.b7 0 - -net constraints-ok <= match_all.out diff --git a/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini index 30c30b8e880..2a656822e84 100644 --- a/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini +++ b/tests/trajectory-planner/circular-arcs/configs/mill_XYZA.ini @@ -18,8 +18,9 @@ DEBUG = 0 # Sections for display options ------------------------------------------------ [DISPLAY] +PYVCP = vcp-XYZA.xml # Name of display program, e.g., xemc -DISPLAY = dummy +DISPLAY = mini # Cycle time, in seconds, that display will sleep between polls CYCLE_TIME = 0.066666666666 @@ -114,7 +115,7 @@ HALFILE = sim_spindle_encoder.hal # Single file that is executed after the GUI has started. Only supported by # AXIS at this time (only AXIS creates a HAL component of its own) -# POSTGUI_HALFILE = postgui-XYZA.tcl +POSTGUI_HALFILE = postgui-XYZA.tcl HALUI = halui diff --git a/tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini b/tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini new file mode 100644 index 00000000000..f38662b63c8 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/test_XYZA_all_programs.ini @@ -0,0 +1,7 @@ +#INCLUDE autotest_mill_XYZA_template.ini + +# Choose the test program to run as the "display" which +# runs a headless instance, to test everything downstream of the GUI +[DISPLAY] +DISPLAY = ./test_run_all_programs.py --base-dir ../nc_files/auto_test + diff --git a/tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py b/tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py new file mode 100755 index 00000000000..2664aba0790 --- /dev/null +++ b/tests/trajectory-planner/circular-arcs/configs/test_run_all_programs.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +""" +Based on the control script in m61_test +""" + +import linuxcnc +from linuxcnc_control import LinuxcncControl +import sys + +from os import listdir +from os.path import isfile, join +import re +import glob + + +def find_test_nc_files(testpath='../nc_files', show=False): + files = [join(testpath, f) for f in listdir(testpath) if isfile(join(testpath, f)) and re.search('.ngc$|.nc$|.TAP$', f) is not None] + if show: + print "In folder {0}, found {1} files:".format(testpath,len(files)) + for fname in files: + print fname + return files + + +if __name__ == '__main__': + """ Run batch tests (user specifies which ones interactively) """ + run_sts = 0 + + # raw_input("Press any key when the LinuxCNC GUI has loaded") + # KLUDGE this lists all subfolders in the auto-test directory + # this dir should be populated with symlinks to any folders of test files to + # run + test_folders = glob.glob(join('nc_files', '*')) + + test_files = [] + + for f in test_folders: + test_files.extend(find_test_nc_files(f, True)) + + e = LinuxcncControl(continue_after_error=args.continue_after_error) + + e.do_startup(need_home=True) + + error_list = [] + for f in test_files: + e.write_error_log() + + print "Loading program {0}".format(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.open_program(f) + e.set_mode(linuxcnc.MODE_AUTO) + e.run_full_program() + if not e.wait_on_program(f) and e.continue_after_error: + e.do_startup(need_home=False) + else: + break + + # KLUDGE just in case any others creep in + e.flush_errors() + e.write_error_log() + sys.exit(e.has_error()) diff --git a/tests/trajectory-planner/circular-arcs/linuxcnc_control.py b/tests/trajectory-planner/circular-arcs/linuxcnc_control.py index 52dc835c173..43daed38280 100644 --- a/tests/trajectory-planner/circular-arcs/linuxcnc_control.py +++ b/tests/trajectory-planner/circular-arcs/linuxcnc_control.py @@ -3,6 +3,7 @@ import linuxcnc import os +import sys import re from time import sleep @@ -15,6 +16,15 @@ def __str__(self): return repr(self.value) +class CommandTimeoutError(Exception): + def __init__(self, message): + # Call the base class constructor with the parameters it needs + self.message = message + + def __str__(self): + return self.message + + class LinuxcncControl: """ issue G-Code commands @@ -29,16 +39,20 @@ class LinuxcncControl: """ - def __init__(self, timeout=2, throw_exceptions=False): + def __init__(self, timeout=2, max_runtime=24 * 60 * 60, throw_lcnc_exceptions=False, continue_after_error=True): self.c = None self.e = None self.s = None - self.timeout = timeout + self.cmd_timeout = timeout self.error_list = [] - self.raise_on_error = throw_exceptions + self.raise_on_error = throw_lcnc_exceptions # Ignore blank errors by default self.err_ignore_mask = '^$' self.try_init(ignore_error=True) + # Don't allow a test to run for more than + self.max_runtime = max_runtime + self.continue_after_error = continue_after_error + self.has_error_ = False def try_init(self, ignore_error=False): try: @@ -63,6 +77,12 @@ def wait_for_backend(self, timeout_sec): return False + def await_done(self, timeout=None): + sts = self.c.wait_complete(timeout if timeout is not None else self.cmd_timeout) + if sts == -1: + raise CommandTimeoutError("command exceeded timeout of {}".format(self.cmd_timeout)) + return sts + def running(self, do_poll=True): """ check if interpreter is running. @@ -76,34 +96,30 @@ def running(self, do_poll=True): def set_mode(self, m): """ set EMC mode if possible, else throw LinuxcncError - return current mode """ self.s.poll() if self.s.task_mode == m : - return m + return 1 if self.running(do_poll=False): raise LinuxcncError("interpreter running - cant change mode") self.c.mode(m) - self.c.wait_complete() + return self.await_done() - return m def set_state(self, m): """ set EMC mode if possible, else throw LinuxcncError - return current mode """ self.s.poll() if self.s.task_mode == m : - return m + return 1 self.c.state(m) - self.c.wait_complete(self.timeout) - return m + return self.await_done() def do_home(self, axismask): self.s.poll() self.c.home(axismask) - self.c.wait_complete(self.timeout) + return self.await_done() def ok_for_mdi(self): """ @@ -145,7 +161,7 @@ def g(self, code, wait=False): self.c.mdi(code) if wait: try: - while self.c.wait_complete(self.timeout) == -1: + while self.c.wait_complete(self.cmd_timeout) == -1: pass return True except KeyboardInterrupt: @@ -176,36 +192,45 @@ def open_program(self, filename): """Open an nc file""" self.s.poll() self.set_mode(linuxcnc.MODE_AUTO) - self.c.wait_complete() - sleep(.25) full_file = os.path.join(os.getcwd(), filename) self.c.program_open(full_file) - self.c.wait_complete() + return self.await_done() def run_full_program(self): """Start a loaded program""" self.s.poll() self.c.auto(linuxcnc.AUTO_RUN, 0) - return self.c.wait_complete(self.timeout) + # WARNING: does not await completion of the command (use wait_on_program for this) def set_feed_scale(self, scale): """Assign a feed scale""" self.s.poll() self.c.feedrate(scale) - self.c.wait_complete(self.timeout) + return self.await_done() def wait_on_program(self, f): - self.s.poll() - while self.s.exec_state != linuxcnc.EXEC_DONE or self.s.state != linuxcnc.RCS_DONE and self.s.task_state == linuxcnc.STATE_ON: - sleep(.01) + iter_count = 0 + update_rate = 50. # Hz + max_iter = int(update_rate * self.max_runtime) + while iter_count < max_iter: self.s.poll() self.flush_errors(f) if self.s.task_state != linuxcnc.STATE_ON: self.error_list.append('{}: error: motion disabled'.format(f)) return False - return True + if self.s.exec_state == linuxcnc.EXEC_ERROR or self.s.state == linuxcnc.RCS_ERROR: + self.error_list.append('{}: warning: unhandled linuxcnc error, exec_state = {}, rcs_state = {}, aborting test'.format(f, self.s.exec_state, self.s.state)) + return False + + if self.s.exec_state == linuxcnc.EXEC_DONE and self.s.state == linuxcnc.RCS_DONE: + return True + + sleep(1. / update_rate) + else: + self.error_list.append('{}: error: exceeded max allowed run time of {} seconds'.format(f, self.max_runtime)) + return False def run_until_done(self, f): self.set_mode(linuxcnc.MODE_AUTO) @@ -242,6 +267,10 @@ def flush_errors(self, filename=""): continue def has_error(self): + self.has_error_ = self.has_error_ or self.find_new_error() + return self.has_error_ + + def find_new_error(self): for msg in self.error_list: if re.search(self.err_ignore_mask, msg): continue @@ -249,3 +278,20 @@ def has_error(self): return True else: return False + + def write_error_log(self): + sys.stderr.writelines(self.error_list) + # Cache the error state since we flush the error list to std err + self.has_error_ = self.has_error() + self.error_list = [] + + def do_startup(self, need_home=False): + self.set_mode(linuxcnc.MODE_MANUAL) + self.set_state(linuxcnc.STATE_ESTOP_RESET) + self.set_state(linuxcnc.STATE_ON) + if need_home: + self.do_home(-1) + sleep(0.1) + self.set_mode(linuxcnc.MODE_AUTO) + + diff --git a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc index 494e099bbb9..0e5a5b4214d 100644 --- a/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc +++ b/tests/trajectory-planner/circular-arcs/nc_files/arc-intersections/tort-sam-mach3.ngc @@ -1,4 +1,4 @@ -o call +o call G0 X0 Y0 Z20 G0 X5.394910 Y13.495489 Z20.332291 G19 G2 F12700 (225 29) J5.656854 K5.656854 X10.394910 Y26.080547 Z29.989145 diff --git a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests b/tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests deleted file mode 120000 index 47c072572fb..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/XYtests +++ /dev/null @@ -1 +0,0 @@ -../XYtests \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance b/tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance deleted file mode 120000 index 6b52c1e72d4..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/auto-test/performance +++ /dev/null @@ -1 +0,0 @@ -../performance/ \ No newline at end of file diff --git a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc b/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc deleted file mode 100644 index ec8ea42362d..00000000000 --- a/tests/trajectory-planner/circular-arcs/nc_files/violation_checks/spreadsheeterrors.ngc +++ /dev/null @@ -1,66 +0,0 @@ -g21 - -#8=0 -#9=0 -#10=0 -#11=0 -# = 5 -o140 do -(DEBUG, Deg [#8]) -g10l2p1x#9y#10z#11r#8f100000 -(# = .7) -g64p.01q.01 - -G1 X[#*41.394] Y[#*88.961] -G2 X[#*42.768] Y[#*88.508] I[#*-5.181] J[#*-18.046] -X[#*44.707] Y[#*87.733] I[#*-10.076] J[#*-27.999] -X[#*49.036] Y[#*85.098] I[#*-9.45] J[#*-20.404] -X[#*51.404] Y[#*82.809] I[#*-10.941] J[#*-13.681] -X[#*53.138] Y[#*80.405] I[#*-14.853] J[#*-12.548] -X[#*54.031] Y[#*78.718] I[#*-13.122] J[#*-8.02] -X[#*55.009] Y[#*75.787] I[#*-14.431] J[#*-6.445] -X[#*55.332] Y[#*73.785] I[#*-14.376] J[#*-3.348] -X[#*55.384] Y[#*71.711] I[#*-18.936] J[#*-1.513] -X[#*54.714] Y[#*67.79] I[#*-16.786] J[#*0.852] -X[#*53.315] Y[#*64.487] I[#*-14.195] J[#*4.067] -X[#*50.7] Y[#*61.041] I[#*-13.786] J[#*7.745] -X[#*48.588] Y[#*59.2] I[#*-15.748] J[#*15.936] -X[#*46.275] Y[#*57.738] I[#*-11.693] J[#*15.933] -X[#*45.966] Y[#*57.603] I[#*-0.656] J[#*1.083] -G3 X[#*45.882] Y[#*57.673] I[#*-0.131] J[#*-0.074] -X[#*45.836] Y[#*57.491] I[#*-0.023] J[#*-0.091] -X[#*45.959] Y[#*57.554] I[#*0.02] J[#*0.111] -G1 X[#*45.967] Y[#*57.564] -G2 X[#*46.417] Y[#*57.372] I[#*-0.597] J[#*-2.031] -X[#*48.505] Y[#*56.011] I[#*-5.52] J[#*-10.749] -X[#*50.414] Y[#*54.369] I[#*-22.264] J[#*-27.826] -X[#*51.883] Y[#*52.934] I[#*-37.762] J[#*-40.113] -X[#*54.037] Y[#*50.531] I[#*-34.816] J[#*-33.364] -X[#*57.418] Y[#*46.006] I[#*-41.891] J[#*-34.84] -X[#*58.904] Y[#*43.164] I[#*-15.621] J[#*-9.974] -X[#*61.158] Y[#*35.798] I[#*-41.283] J[#*-16.66] -X[#*61.822] Y[#*31.33] I[#*-35.828] J[#*-7.606] -X[#*61.951] Y[#*28.768] I[#*-33.99] J[#*-2.994] -X[#*61.941] Y[#*26.703] I[#*-41.425] J[#*-0.847] -X[#*61.583] Y[#*22.416] I[#*-41.847] J[#*1.342] -X[#*60.815] Y[#*18.286] I[#*-40.791] J[#*5.443] -X[#*59.747] Y[#*14.756] I[#*-33.542] J[#*8.223] -X[#*58.73] Y[#*12.322] I[#*-29.525] J[#*10.903] -X[#*57.307] Y[#*9.681] I[#*-25.104] J[#*11.828] -X[#*56.009] Y[#*7.837] I[#*-12.665] J[#*7.534] -X[#*54.075] Y[#*5.821] I[#*-11.615] J[#*9.211] -X[#*52.734] Y[#*4.711] I[#*-14.655] J[#*16.335] -G1 X[#*51.442] Y[#*3.758] -G2 X[#*48.906] Y[#*2.121] I[#*-28.308] J[#*41.068] -G1 X[#*47.145] Y[#*1.135] - -G0 Z[#*5.0] -G0 Z[#*30.0] -#8=[#8+20.2] -#9=[#9+5] -#10=[#10+5] -#11=[#11+20] -#=[#*.9] -o140 while [#8 LT 22] -g10l2p1r0 -m30 diff --git a/tests/trajectory-planner/circular-arcs/run_all_tests.py b/tests/trajectory-planner/circular-arcs/run_all_tests.py deleted file mode 100755 index d9750f2c850..00000000000 --- a/tests/trajectory-planner/circular-arcs/run_all_tests.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python -""" -Based on the control script in m61_test -""" - -import linuxcnc -from linuxcnc_control import LinuxcncControl -#import hal -import sys - -from time import sleep - -from os import listdir -from os.path import isfile, join -import re -import glob - - -def query_yes_no(question, default="yes"): - """Ask a yes/no question via raw_input() and return their answer. - - "question" is a string that is presented to the user. - "default" is the presumed answer if the user just hits . - It must be "yes" (the default), "no" or None (meaning - an answer is required of the user). - - The "answer" return value is one of "yes" or "no". - """ - valid = {"yes":True, "y":True, "ye":True, - "no":False, "n":False} - if default == None: - prompt = " [y/n] " - elif default == "yes": - prompt = " [Y/n] " - elif default == "no": - prompt = " [y/N] " - else: - raise ValueError("invalid default answer: '%s'" % default) - - while True: - sys.stdout.write(question + prompt) - choice = raw_input().lower() - if default is not None and choice == '': - return valid[default] - elif choice in valid: - return valid[choice] - else: - sys.stdout.write("Please respond with 'yes' or 'no' "\ - "(or 'y' or 'n').\n") - - -def find_test_nc_files(testpath='nc_files', show=False): - files = [ testpath + '/' + f for f in listdir(testpath) if isfile(join(testpath,f)) and re.search('.ngc$',f) is not None ] - if show: - print "In folder {0}, found {1} files:".format(testpath,len(files)) - for fname in files: - print fname - return files - - -if __name__ == '__main__': - """ Run batch tests (user specifies which ones interactively) """ - run_sts = 0 - - # raw_input("Press any key when the LinuxCNC GUI has loaded") - # KLUDGE this lists all subfolders in the auto-test directory - # this dir should be populated with symlinks to any folders of test files to - # run - test_folders = glob.glob('nc_files/auto-test/*') - - test_files = [] - - for f in test_folders: - test_files.extend(find_test_nc_files(f, True)) - - e = LinuxcncControl(1) - - load_timeout = 5 - if not e.wait_for_backend(load_timeout): - sys.stderr.write("error: LinuxCNC failed to load in {} seconds".format(load_timeout)) - sys.exit(2) - - e.g_raise_except = False - e.set_mode(linuxcnc.MODE_MANUAL) - e.set_state(linuxcnc.STATE_ESTOP_RESET) - e.set_state(linuxcnc.STATE_ON) - e.do_home(-1) - sleep(0.5) - e.set_mode(linuxcnc.MODE_AUTO) - sleep(0.5) - - error_list = [] - for f in test_files: - if re.search('.ngc$|.TAP$|.nc$', f) is not None: - print "Loading program {0}".format(f) - e.set_mode(linuxcnc.MODE_AUTO) - e.open_program(f) - e.set_mode(linuxcnc.MODE_AUTO) - e.run_full_program() - e.wait_on_program(f) - - # KLUDGE just in case any others creep in - e.flush_errors() - sys.stderr.writelines(e.error_list) - sys.exit(e.has_error()) diff --git a/tests/trajectory-planner/common-config/apply_limits_to_enable.tcl b/tests/trajectory-planner/common-config/apply_limits_to_enable.tcl new file mode 100644 index 00000000000..5409fbf48ad --- /dev/null +++ b/tests/trajectory-planner/common-config/apply_limits_to_enable.tcl @@ -0,0 +1 @@ +net constraints-ok <= match_all.out => motion.enable diff --git a/tests/trajectory-planner/common-config/axis-A.tcl b/tests/trajectory-planner/common-config/axis-A.tcl new file mode 100644 index 00000000000..9a593cc73b9 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-A.tcl @@ -0,0 +1,35 @@ +set av_id 3 +set aa_id 13 + +# Add axis components to servo thread +addf wcomp.$av_id servo-thread +addf wcomp.$aa_id servo-thread +addf minmax.$av_id servo-thread +addf minmax.$aa_id servo-thread +addf d2dt2.$av_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net a_pos axis.$av_id.motor-pos-cmd => axis.$av_id.motor-pos-fb d2dt2.$av_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net a_vel <= d2dt2.$av_id.out1 => wcomp.$av_id.in minmax.$av_id.in +net a_acc <= d2dt2.$av_id.out2 => wcomp.$aa_id.in minmax.$aa_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$aa_id.max $::AXIS_3(MAX_ACCELERATION)*$acc_limit +setp wcomp.$aa_id.min $::AXIS_3(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$av_id.max $::AXIS_3(MAX_VELOCITY)*$vel_limit +setp wcomp.$av_id.min $::AXIS_3(MAX_VELOCITY)*-1.0*$vel_limit + +net a_acc-ok <= wcomp.$av_id.out => match_abc.a0 +net a_vel-ok <= wcomp.$aa_id.out => match_abc.a1 + +# Enable match_all pins for axis +setp match_abc.b0 1 +setp match_abc.b1 1 + diff --git a/tests/trajectory-planner/common-config/axis-X.tcl b/tests/trajectory-planner/common-config/axis-X.tcl new file mode 100644 index 00000000000..0c549683595 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-X.tcl @@ -0,0 +1,35 @@ +set xv_id 0 +set xa_id 10 + +# Add axis components to servo thread +addf wcomp.$xv_id servo-thread +addf wcomp.$xa_id servo-thread +addf minmax.$xv_id servo-thread +addf minmax.$xa_id servo-thread +addf d2dt2.$xv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net x_pos axis.$xv_id.motor-pos-cmd => axis.$xv_id.motor-pos-fb d2dt2.$xv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net x_vel <= d2dt2.$xv_id.out1 => vel_xyz.in$xv_id wcomp.$xv_id.in minmax.$xv_id.in +net x_acc <= d2dt2.$xv_id.out2 => wcomp.$xa_id.in minmax.$xa_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$xa_id.max $::AXIS_0(MAX_ACCELERATION)*$acc_limit +setp wcomp.$xa_id.min $::AXIS_0(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$xv_id.max $::AXIS_0(MAX_VELOCITY)*$vel_limit +setp wcomp.$xv_id.min $::AXIS_0(MAX_VELOCITY)*-1.0*$vel_limit + +net x_acc-ok <= wcomp.$xv_id.out => match_xyz.a0 +net x_vel-ok <= wcomp.$xa_id.out => match_xyz.a1 + +# Enable match_all pins for axis +setp match_xyz.b0 1 +setp match_xyz.b1 1 + diff --git a/tests/trajectory-planner/common-config/axis-Y.tcl b/tests/trajectory-planner/common-config/axis-Y.tcl new file mode 100644 index 00000000000..0c393ed57c6 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-Y.tcl @@ -0,0 +1,35 @@ +set yv_id 1 +set ya_id 11 + +# Add axis components to servo thread +addf wcomp.$yv_id servo-thread +addf wcomp.$ya_id servo-thread +addf minmax.$yv_id servo-thread +addf minmax.$ya_id servo-thread +addf d2dt2.$yv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net y_pos axis.$yv_id.motor-pos-cmd => axis.$yv_id.motor-pos-fb d2dt2.$yv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net y_vel <= d2dt2.$yv_id.out1 => vel_xyz.in$yv_id wcomp.$yv_id.in minmax.$yv_id.in +net y_acc <= d2dt2.$yv_id.out2 => wcomp.$ya_id.in minmax.$ya_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$ya_id.max $::AXIS_1(MAX_ACCELERATION)*$acc_limit +setp wcomp.$ya_id.min $::AXIS_1(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$yv_id.max $::AXIS_1(MAX_VELOCITY)*$vel_limit +setp wcomp.$yv_id.min $::AXIS_1(MAX_VELOCITY)*-1.0*$vel_limit + +net y_acc-ok <= wcomp.$yv_id.out => match_xyz.a2 +net y_vel-ok <= wcomp.$ya_id.out => match_xyz.a3 + +# Enable match_all pins for axis +setp match_xyz.b2 1 +setp match_xyz.b3 1 + diff --git a/tests/trajectory-planner/common-config/axis-Z.tcl b/tests/trajectory-planner/common-config/axis-Z.tcl new file mode 100644 index 00000000000..87e186ce971 --- /dev/null +++ b/tests/trajectory-planner/common-config/axis-Z.tcl @@ -0,0 +1,34 @@ +set zv_id 2 +set za_id 12 + +# Add axis components to servo thread +addf wcomp.$zv_id servo-thread +addf wcomp.$za_id servo-thread +addf minmax.$zv_id servo-thread +addf minmax.$za_id servo-thread +addf d2dt2.$zv_id servo-thread + +# create HAL signals for position commands from motion module +# loop position commands back to motion module feedback +net z_pos axis.$zv_id.motor-pos-cmd => axis.$zv_id.motor-pos-fb d2dt2.$zv_id.in + +# send the position commands thru differentiators to +# generate velocity and accel signals +net z_vel <= d2dt2.$zv_id.out1 => vel_xyz.in$zv_id wcomp.$zv_id.in minmax.$zv_id.in +net z_acc <= d2dt2.$zv_id.out2 => wcomp.$za_id.in minmax.$za_id.in + +#Conservative limits +set acc_limit 1.0001 +set vel_limit 1.01 + +setp wcomp.$za_id.max $::AXIS_2(MAX_ACCELERATION)*$acc_limit +setp wcomp.$za_id.min $::AXIS_2(MAX_ACCELERATION)*-1.0*$acc_limit +setp wcomp.$zv_id.max $::AXIS_2(MAX_VELOCITY)*$vel_limit +setp wcomp.$zv_id.min $::AXIS_2(MAX_VELOCITY)*-1.0*$vel_limit + +net z_acc-ok <= wcomp.$zv_id.out => match_xyz.a4 +net z_vel-ok <= wcomp.$za_id.out => match_xyz.a5 + +# Enable match_all pins for axis +setp match_xyz.b4 1 +setp match_xyz.b5 1 diff --git a/tests/trajectory-planner/common-config/core_sim_components.hal b/tests/trajectory-planner/common-config/core_sim_components.hal new file mode 100644 index 00000000000..7f32d4a7384 --- /dev/null +++ b/tests/trajectory-planner/common-config/core_sim_components.hal @@ -0,0 +1,19 @@ +# core HAL config file for simulation + +# first load all the RT modules that will be needed +# kinematics +loadrt trivkins +# motion controller, get name and thread periods from ini file +loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES + +# add motion controller functions to servo thread +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# estop loopback +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in + +# create signals for tool loading loopback +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed + diff --git a/tests/trajectory-planner/common-config/linuxcnc_control.py b/tests/trajectory-planner/common-config/linuxcnc_control.py new file mode 100644 index 00000000000..c9316f8c41f --- /dev/null +++ b/tests/trajectory-planner/common-config/linuxcnc_control.py @@ -0,0 +1,291 @@ +#!/usr/bin/env python +"""Copied from m61-test""" + +import linuxcnc +import os +import sys +import re +from time import sleep +import datetime +import string + + +class LinuxcncError(Exception): + def __init__(self, value): + self.value = value + + def __str__(self): + return repr(self.value) + + +class CommandTimeoutError(Exception): + def __init__(self, message): + # Call the base class constructor with the parameters it needs + self.message = message + + def __str__(self): + return self.message + + +class LinuxcncControl: + """ + issue G-Code commands + make sure important modes are saved and restored + mode is saved only once, and can be restored only once + + usage example: + e = emc_control() + e.prepare_for_mdi() + any internal sub using e.g("G0.....") + e.finish_mdi() + + """ + + def __init__(self, timeout=2, max_runtime=24 * 60 * 60, throw_lcnc_exceptions=False): + self.c = None + self.e = None + self.s = None + self.cmd_timeout = timeout + self.raise_on_error = throw_lcnc_exceptions + # Ignore blank errors by default + self.err_ignore_mask = '^$' + self.try_init(ignore_error=True) + # Don't allow a test to run for more than + self.max_runtime = max_runtime + self.has_error_ = False + + def try_init(self, ignore_error=False): + try: + self.c = linuxcnc.command() + self.e = linuxcnc.error_channel() + self.s = linuxcnc.stat() + self.s.poll() + except linuxcnc.error as e: + if ignore_error: + return False + + return True + + def wait_for_backend(self, timeout_sec): + k = 0 + print('Waiting for LinuxCNC...') + while k < 25: + k += 1 + sleep(1) + if self.try_init(ignore_error=True): + return True + + return False + + def await_done(self, timeout=None): + sts = self.c.wait_complete(timeout if timeout is not None else self.cmd_timeout) + if sts == -1: + raise CommandTimeoutError("command exceeded timeout of {}".format(self.cmd_timeout)) + return sts + + def running(self, do_poll=True): + """ + check if interpreter is running. + If so, cant switch to MDI mode. + """ + if do_poll: + self.s.poll() + return (self.s.task_mode == linuxcnc.MODE_AUTO and + self.s.interp_state != linuxcnc.INTERP_IDLE) + + def set_mode(self, m): + """ + set EMC mode if possible, else throw LinuxcncError + """ + self.s.poll() + if self.s.task_mode == m : + return 1 + if self.running(do_poll=False): + raise LinuxcncError("interpreter running - cant change mode") + self.c.mode(m) + return self.await_done() + + + def set_state(self, m): + """ + set EMC mode if possible, else throw LinuxcncError + """ + self.s.poll() + if self.s.task_mode == m : + return 1 + self.c.state(m) + return self.await_done() + + def do_home(self, axismask): + self.s.poll() + self.c.home(axismask) + return self.await_done() + + def ok_for_mdi(self): + """ + check wether ok to run MDI commands. + """ + self.s.poll() + return not self.s.estop and self.s.enabled and self.s.homed + + def prepare_for_mdi(self): + """ + check wether ok to run MDI commands. + throw LinuxcncError if told so. + return current mode + """ + + self.s.poll() + if self.s.estop: + raise LinuxcncError("machine in ESTOP") + + if not self.s.enabled: + raise LinuxcncError("machine not enabled") + + if not self.s.homed: + raise LinuxcncError("machine not homed") + + if self.running(): + raise LinuxcncError("interpreter not idle") + + return self.set_mode(linuxcnc.MODE_MDI) + + g_raise_except = True + + def g(self, code, wait=False): + """ + issue G-Code as MDI command. + wait for completion if reqested + """ + + self.c.mdi(code) + if wait: + try: + while self.c.wait_complete(self.cmd_timeout) == -1: + pass + return True + except KeyboardInterrupt: + self.log('mdi', 'warning', 'interrupted by keyboard in c.wait_complete(self.timeout)') + return False + + self.poll_and_log_error(code) + return False + + def get_current_tool(self): + self.s.poll() + return self.s.tool_in_spindle + + def active_codes(self): + self.s.poll() + return self.s.gcodes + + def get_current_system(self): + g = self.active_codes() + for i in g: + if i >= 540 and i <= 590: + return i/10 - 53 + elif i >= 590 and i <= 593: + return i - 584 + return 1 + + def open_program(self, filename): + """Open an nc file""" + self.s.poll() + self.set_mode(linuxcnc.MODE_AUTO) + full_file = os.path.join(os.getcwd(), filename) + self.c.program_open(full_file) + return self.await_done() + + def run_full_program(self): + """Start a loaded program""" + self.s.poll() + self.c.auto(linuxcnc.AUTO_RUN, 0) + # WARNING: does not await completion of the command (use wait_on_program for this) + + def set_feed_scale(self, scale): + """Assign a feed scale""" + + self.s.poll() + self.c.feedrate(scale) + return self.await_done() + + def wait_on_program(self, f): + iter_count = 0 + update_rate = 50. # Hz + max_iter = int(update_rate * self.max_runtime) + while iter_count < max_iter: + self.s.poll() + self.flush_errors(f) + if self.s.task_state != linuxcnc.STATE_ON: + self.log_error(f, 'motion disabled') + return False + + if self.s.exec_state == linuxcnc.EXEC_ERROR or self.s.state == linuxcnc.RCS_ERROR: + self.log_error(f, 'unhandled linuxcnc error with exec_state = {}, rcs_state = {}'.format( + self.s.exec_state, self.s.state)) + return False + + if self.s.exec_state == linuxcnc.EXEC_DONE and self.s.state == linuxcnc.RCS_DONE: + return True + + sleep(1. / update_rate) + else: + self.log_error(f, 'exceeded max allowed run time of {} seconds'.format(self.max_runtime)) + return False + + def run_until_done(self, f): + self.set_mode(linuxcnc.MODE_AUTO) + self.open_program(f) + self.set_mode(linuxcnc.MODE_AUTO) + self.run_full_program() + return self.wait_on_program(f) + + def check_rcs_error(self): + self.s.poll() + if self.s.state == linuxcnc.RCS_ERROR: + return True + return False + + def set_error_ignore_pattern(self, pattern): + self.err_ignore_mask = pattern + + def log(self, ngc_file, level, msg_text): + full_msg = '{} | {}: {}: {}\n'.format(datetime.datetime.now(), ngc_file, level, msg_text) + sys.stderr.write(full_msg) + if string.lower(level) is 'error': + self.has_error_ = True + return full_msg + + def log_error(self, ngc_file, msg_text): + err_str = self.log(ngc_file, 'error', msg_text) + if self.raise_on_error: + raise LinuxcncError(err_str) + + def poll_and_log_error(self, f): + error = self.e.poll() + if error is None: + return False + + err_type, msg_text = error + err_type_str = 'error' if err_type in (linuxcnc.NML_ERROR, linuxcnc.OPERATOR_ERROR) else 'info' + self.log(f, err_type_str, msg_text) + return True + + def flush_errors(self, filename=""): + while self.poll_and_log_error(filename): + sleep(0.001) + continue + + def has_error(self): + return self.has_error_ + + def do_startup(self, need_home=False): + self.set_mode(linuxcnc.MODE_MANUAL) + self.set_state(linuxcnc.STATE_ESTOP_RESET) + self.set_state(linuxcnc.STATE_ON) + if need_home: + self.do_home(-1) + sleep(0.1) + self.set_mode(linuxcnc.MODE_AUTO) + + diff --git a/tests/trajectory-planner/common-config/load_constraint_components.tcl b/tests/trajectory-planner/common-config/load_constraint_components.tcl new file mode 100644 index 00000000000..5fa13c938db --- /dev/null +++ b/tests/trajectory-planner/common-config/load_constraint_components.tcl @@ -0,0 +1,57 @@ +# Load all constraint-checking components used in HAL files for various sim configurations +# NOTE: add only the actively used ones to the RT thread in axis TCL files + +loadrt d2dt2 count=10 +# load additional blocks +loadrt hypot names=vel_xy,vel_xyz +addf vel_xy servo-thread +addf vel_xyz servo-thread + +# Compares computed velocities / accelerations to limits +loadrt wcomp count=20 + +# Track the overall min / max value of each signal reached (for VCP display) +loadrt minmax count=20 + +# Ties all limit checks together (used to force a motion enable / disable) +loadrt match8 names=match_xyz,match_abc,match_uvw,match_all + +net XYZ-limits-ok <= match_xyz.out => match_all.a0 +net ABC-limits-ok <= match_abc.out => match_all.a1 +net UVW-limits-ok <= match_uvw.out => match_all.a2 + +setp match_xyz.b0 0 +setp match_xyz.b1 0 +setp match_xyz.b2 0 +setp match_xyz.b3 0 +setp match_xyz.b4 0 +setp match_xyz.b5 0 +setp match_xyz.b6 0 +setp match_xyz.b7 0 + +setp match_abc.b0 0 +setp match_abc.b1 0 +setp match_abc.b2 0 +setp match_abc.b3 0 +setp match_abc.b4 0 +setp match_abc.b5 0 +setp match_abc.b6 0 +setp match_abc.b7 0 + +setp match_uvw.b0 0 +setp match_uvw.b1 0 +setp match_uvw.b2 0 +setp match_uvw.b3 0 +setp match_uvw.b4 0 +setp match_uvw.b5 0 +setp match_uvw.b6 0 +setp match_uvw.b7 0 + +setp match_all.b0 1 +setp match_all.b1 1 +setp match_all.b2 1 +setp match_all.b3 0 +setp match_all.b4 0 +setp match_all.b5 0 +setp match_all.b6 0 +setp match_all.b7 0 diff --git a/tests/trajectory-planner/common-config/mill_XYZA_base.ini b/tests/trajectory-planner/common-config/mill_XYZA_base.ini new file mode 100644 index 00000000000..c9d3d2bdf6f --- /dev/null +++ b/tests/trajectory-planner/common-config/mill_XYZA_base.ini @@ -0,0 +1,177 @@ +# LinuxCNC controller parameters for a simulated 4-axis milling machine + +# General section ------------------------------------------------------------- +[EMC] + +# Version of this INI file +VERSION = $Revision$ + +# Name of machine, for use with display, etc. +MACHINE = LinuxCNC-Auto-Test-Config + +# Debug level, 0 means no messages. See src/emc/nml_int/emcglb.h for others +DEBUG = 0 + +# NOTE can only handle raw G-code files + +# Task controller section ----------------------------------------------------- +[TASK] + +# Name of task controller program, e.g., milltask +TASK = milltask + +# Cycle time, in seconds, that task controller will sleep between polls +CYCLE_TIME = 0.001 + +# Part program interpreter section -------------------------------------------- +[RS274NGC] + +# File containing interpreter variables +PARAMETER_FILE = sim.var +SUBROUTINE_PATH = ../common-subs + +# Motion control section ------------------------------------------------------ +[EMCMOT] + +EMCMOT = motmod + +# Timeout for comm to emcmot, in seconds +COMM_TIMEOUT = 1.0 + +# Interval between tries to emcmot, in seconds +COMM_WAIT = 0.010 + +# BASE_PERIOD is unused in this configuration but specified in core_sim.hal +BASE_PERIOD = 0 + +# Servo task period, in nano-seconds +SERVO_PERIOD = 1000000 + +# Hardware Abstraction Layer section ----------------------------------------- +[HAL] +# Sim config with individual axis files to set up velocity / acceleration calcs in HAL +# KLUDGE ini file is called from the test directory, hence the prefix +HALFILE = ../common-config/core_sim_components.hal +HALFILE = ../common-config/load_constraint_components.tcl +HALFILE = ../common-config/axis-X.tcl +HALFILE = ../common-config/axis-Y.tcl +HALFILE = ../common-config/axis-Z.tcl +HALFILE = ../common-config/axis-A.tcl +HALFILE = sim_spindle_encoder.hal + +HALUI = halui + +# Trajectory planner section -------------------------------------------------- +[TRAJ] + +AXES = 4 +COORDINATES = X Y Z A +HOME = 0 0 0 0 +LINEAR_UNITS = inch +ANGULAR_UNITS = degree +CYCLE_TIME = 0.010 +DEFAULT_VELOCITY = 1.2 +POSITION_FILE = position.txt +MAX_LINEAR_VELOCITY = 12 + +ARC_BLEND_ENABLE = 1 +ARC_BLEND_FALLBACK_ENABLE = 0 +ARC_BLEND_GAP_CYCLES = 4 +ARC_BLEND_RAMP_FREQ = 100 + +EXTRA_CONSISTENCY_CHECKS = 1 + +# Axes sections --------------------------------------------------------------- + +# First axis +[AXIS_0] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Second axis +[AXIS_1] + +TYPE = LINEAR +HOME = 0.000 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.000 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -40.0 +MAX_LIMIT = 40.0 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# Third axis +[AXIS_2] + +TYPE = LINEAR +HOME = 0.0 +MAX_VELOCITY = 8 +MAX_ACCELERATION = 30 +BACKLASH = 0.0 +INPUT_SCALE = 2000 +OUTPUT_SCALE = 1.000 +MIN_LIMIT = -10.0 +MAX_LIMIT = 10.0001 +FERROR = 0.050 +MIN_FERROR = 0.010 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = 0.0 +HOME_LATCH_VEL = 0.0 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +[AXIS_3] +TYPE = ANGULAR +HOME = 0.0 +MAX_VELOCITY = 25 +MAX_ACCELERATION = 800 +SCALE = 500.0 +FERROR = 5.0 +MIN_FERROR = 2.5 +MIN_LIMIT = -9999.0 +MAX_LIMIT = 9999.0 +HOME_OFFSET = 0.000000 +HOME_SEARCH_VEL = 0.00000 +HOME_LATCH_VEL = 0.00000 +HOME_USE_INDEX = NO +HOME_SEQUENCE = 0 + +# section for main IO controller parameters ----------------------------------- +[EMCIO] + +# Name of IO controller program, e.g., io +EMCIO = io + +# cycle time, in seconds +CYCLE_TIME = 0.066666666666 + +# tool table file +TOOL_TABLE = sim.tbl +TOOL_CHANGE_POSITION = 0 0 0 +TOOL_CHANGE_QUILL_UP = 1 + +[REDIS] +DISABLE_SERVER=1 diff --git a/tests/trajectory-planner/common-config/sim.tbl b/tests/trajectory-planner/common-config/sim.tbl new file mode 100644 index 00000000000..1947e260753 --- /dev/null +++ b/tests/trajectory-planner/common-config/sim.tbl @@ -0,0 +1,6 @@ +T1 P1 D1.000000 Z+1.000000 ;1/8 end mill +T2 P2 D0.062500 Z+0.100000 ;1/16 end mill +T3 P3 D0.201000 Z+1.273000 ;#7 tap drill +T4 P4 D0.031250 Z+0.511000 ;1/32 end mill +T99999 P99999 Z+0.100000 ;big tool number +T15 P15 D0.062500 Z+1.000000 ;dummy tool diff --git a/tests/trajectory-planner/common-subs/default_inch.ngc b/tests/trajectory-planner/common-subs/default_inch.ngc new file mode 100644 index 00000000000..245a4940714 --- /dev/null +++ b/tests/trajectory-planner/common-subs/default_inch.ngc @@ -0,0 +1,4 @@ +o sub +o call +G20 +o endsub diff --git a/tests/trajectory-planner/common-subs/default_mm.ngc b/tests/trajectory-planner/common-subs/default_mm.ngc new file mode 100644 index 00000000000..740977777ae --- /dev/null +++ b/tests/trajectory-planner/common-subs/default_mm.ngc @@ -0,0 +1,4 @@ +o sub +o call +G21 +o endsub diff --git a/tests/trajectory-planner/common-subs/default_modal_state.ngc b/tests/trajectory-planner/common-subs/default_modal_state.ngc new file mode 100644 index 00000000000..ff5f147f21f --- /dev/null +++ b/tests/trajectory-planner/common-subs/default_modal_state.ngc @@ -0,0 +1,19 @@ +o sub +G10 L2 P0 R0 (Clear any coordinate system rotation) +G17 +G20 +G40 (clear cutter compensation) +G49 (cancel tool length compensation) +G54 (Start in G54 work offset) +G64 P0 Q0 (Blend mode with no tolerance applied) +G90 (Absolute positioning) +G92.1 (Reset G92 offsets) +G94 (Feed-per-minute mode) +G97 (RPM spindle mode) +G98 (Return to the initial value) + +M5 (Turn off spindle) +M9 (Turn off coolant) +S0 +F0 +o endsub From e82b64ca7ae46c04e79a7eb1c39ad8b17294be33 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 28 Feb 2019 23:38:59 -0500 Subject: [PATCH 110/129] test: remove an unused config file from previous versions of test configs --- .../circular-arcs/configs/core_sim9.tcl | 64 ------------------- 1 file changed, 64 deletions(-) delete mode 100644 tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl diff --git a/tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl b/tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl deleted file mode 100644 index 2e191cd6adf..00000000000 --- a/tests/trajectory-planner/circular-arcs/configs/core_sim9.tcl +++ /dev/null @@ -1,64 +0,0 @@ -# core HAL config file for simulation - -# first load all the RT modules that will be needed -# kinematics -loadrt trivkins -# motion controller, get name and thread periods from ini file -loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[TRAJ]AXES -# load 6 differentiators (for velocity and accel signals - -# add motion controller functions to servo thread -addf motion-command-handler servo-thread -addf motion-controller servo-thread - -set next_ddt_idx = 0 - -# link the differentiator functions into the code -addf ddt_x servo-thread -addf ddt_xv servo-thread -addf ddt_y servo-thread -addf ddt_yv servo-thread -addf ddt_z servo-thread -addf ddt_zv servo-thread -addf vel_xy servo-thread -addf vel_xyz servo-thread - -# create HAL signals for position commands from motion module -# loop position commands back to motion module feedback -net Xpos axis.0.motor-pos-cmd => axis.0.motor-pos-fb ddt_x.in -net Ypos axis.1.motor-pos-cmd => axis.1.motor-pos-fb ddt_y.in -net Zpos axis.2.motor-pos-cmd => axis.2.motor-pos-fb ddt_z.in -net Apos axis.3.motor-pos-cmd => axis.3.motor-pos-fb -net Bpos axis.4.motor-pos-cmd => axis.4.motor-pos-fb -net Cpos axis.5.motor-pos-cmd => axis.5.motor-pos-fb -net Upos axis.6.motor-pos-cmd => axis.6.motor-pos-fb -net Vpos axis.7.motor-pos-cmd => axis.7.motor-pos-fb -net Wpos axis.8.motor-pos-cmd => axis.8.motor-pos-fb - -# send the position commands thru differentiators to -# generate velocity and accel signals -net Xvel ddt_x.out => ddt_xv.in vel_xy.in0 -net Xacc <= ddt_xv.out -net Yvel ddt_y.out => ddt_yv.in vel_xy.in1 -net Yacc <= ddt_yv.out -net Zvel ddt_z.out => ddt_zv.in vel_xyz.in0 -net Zacc <= ddt_zv.out - -# Cartesian 2- and 3-axis velocities -net XYvel vel_xy.out => vel_xyz.in1 -net XYZvel <= vel_xyz.out - -# estop loopback -net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in - -# create signals for tool loading loopback -net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared -net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed - -net spindle-fwd motion.spindle-forward -net spindle-rev motion.spindle-reverse -#net spindle-speed motion.spindle-speed-out - -net lube iocontrol.0.lube -net flood iocontrol.0.coolant-flood -net mist iocontrol.0.coolant-mist From c8b7a684faf4d51291ec9c96fb9acf53af398cb1 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Fri, 1 Mar 2019 11:36:09 -0500 Subject: [PATCH 111/129] unit_test: add some new cases for end condition check --- unit_tests/tp/test_blendmath.c | 70 +++++++++++++++++++++++++++++++--- 1 file changed, 64 insertions(+), 6 deletions(-) diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 14c06f05c65..203e1dae10c 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -206,29 +206,87 @@ TEST checkEndCondition_at_velocity() PASS(); } -TEST checkEndCondition_below_final_velocity() +TEST checkEndCondition_below_vf_close() { const double v_final = v_max_mock; const double v_current = 1.0; const double nominal_cycle_dist = v_current * cycle_time; - EndCondition ec_close = checkEndCondition(cycle_time, + EndCondition ec = checkEndCondition(cycle_time, target_dist_mock - nominal_cycle_dist, target_dist_mock, v_current, - v_max_mock, + v_final, a_max_mock ); - ASSERT(ec_close.dt < cycle_time); - ASSERT_IN_RANGEm("final velocity", ec_close.v_f, 1.0, v_final); + ASSERT(ec.dt < cycle_time); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + +TEST checkEndCondition_below_vf_far() +{ + const double v_final = v_max_mock; + const double v_current = 1.0; + const double nominal_cycle_dist = v_current * cycle_time; + + EndCondition ec = checkEndCondition(cycle_time, + target_dist_mock - 4*nominal_cycle_dist, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT(ec.dt > cycle_time); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + +TEST checkEndCondition_above_vf_far() +{ + const double v_final = 1.0; + const double v_current = v_max_mock; + + EndCondition ec = checkEndCondition(cycle_time, + 0.0, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT(ec.dt > cycle_time); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); PASS(); } + +TEST checkEndCondition_above_vf_close() +{ + const double v_final = 1.0; + const double v_current = v_final + a_max_mock * cycle_time; + const double nominal_cycle_dist = (v_current + v_final) / 2.0 * cycle_time; + + EndCondition ec = checkEndCondition(cycle_time, + target_dist_mock - nominal_cycle_dist, + target_dist_mock, + v_current, + v_final, + a_max_mock + ); + ASSERT_IN_RANGE(ec.dt, cycle_time, TP_TIME_EPSILON); + ASSERT_IN_RANGEm("final velocity", ec.v_f, 1.0, v_final); + PASS(); +} + + SUITE(tc_functions) { RUN_TEST(checkEndCondition_zero_velocity); RUN_TEST(checkEndCondition_small_velocity); RUN_TEST(checkEndCondition_at_velocity); - RUN_TEST(checkEndCondition_below_final_velocity); + RUN_TEST(checkEndCondition_below_vf_far); + RUN_TEST(checkEndCondition_below_vf_close); + RUN_TEST(checkEndCondition_above_vf_far); + RUN_TEST(checkEndCondition_above_vf_close); } TEST test_pmCircleActualMaxVel_cutoff() From f8bb8b1f1becebe6ce56d571b681e32b89d2d175 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Mar 2019 10:45:12 -0500 Subject: [PATCH 112/129] tp: hide some pedantic debug output that screws with unit tests --- src/emc/tp/blendmath.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index dd857cf6ac6..6e1ed0a4f94 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -307,7 +307,7 @@ int pmCartCartParallel(PmCartesian const * const u1, pmCartMagSq(&u_diff, &d_diff); } -#ifdef TP_DEBUG +#if defined(TP_DEBUG) && defined(TP_PEDANTIC) print_json5_log_start(pmCartCartParallel, Check); print_json5_double(d_diff); print_json5_end_(); @@ -333,9 +333,11 @@ int pmCartCartAntiParallel(PmCartesian const * const u1, pmCartMagSq(&u_sum, &d_sum); } - tp_debug_json_log_start(pmCartCartAntiParallel, Check); - tp_debug_json_double(d_sum); - tp_debug_json_log_end(); +#if defined(TP_DEBUG) && defined(TP_PEDANTIC) + print_json5_log_start(pmCartCartAntiParallel, Check); + print_json5_double(d_sum); + print_json5_end_(); +#endif return d_sum < tol; } From a88fd7b25b7179f73941da2f10a75f6b73e363eb Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Mar 2019 10:45:41 -0500 Subject: [PATCH 113/129] unit_test: add an assert macro to compare PmCartesians --- unit_tests/greatest.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index cc936ae5520..465536ff62b 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -1219,6 +1219,14 @@ greatest_run_info greatest_info #define SHUFFLE_TESTS GREATEST_SHUFFLE_TESTS #define SHUFFLE_SUITES GREATEST_SHUFFLE_SUITES +// Customized for LinuxCNC +#define ASSERT_PMCARTESIAN_IN_RANGE(EXP, GOT, tol) do { \ + ASSERT_IN_RANGE(EXP.x, GOT.x, tol); \ + ASSERT_IN_RANGE(EXP.y, GOT.y, tol); \ + ASSERT_IN_RANGE(EXP.z, GOT.z, tol); \ +} while (0) + + #ifdef GREATEST_VA_ARGS #define RUN_TESTp GREATEST_RUN_TESTp #endif From 563fef44d2fb448a156aed9efce5b578772a1815 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Mar 2019 10:46:08 -0500 Subject: [PATCH 114/129] tp: add more test cases for PmCircles with helix / spiral components --- unit_tests/tp/test_blendmath.c | 107 +++++++++++++++++++++++++++++++++ unit_tests/tp/test_posemath.c | 72 ++++++++++++++++++++++ 2 files changed, 179 insertions(+) diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 203e1dae10c..561f4d67f4b 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -6,11 +6,13 @@ #include "rtapi.h" #include "joint_util.h" #include "motion_debug.h" +#include "tc.h" /* Expand to all the definitions that need to be in the test runner's main file. */ GREATEST_MAIN_DEFS(); + #include "mock_rtapi.inc" TEST test_findMaxTangentAngle() { @@ -289,6 +291,109 @@ SUITE(tc_functions) { RUN_TEST(checkEndCondition_above_vf_close); } +TEST test_pmCircleTangentVector_unitcircle() +{ + PmCircle c; + PmCartesian start = {1,0, 0}; + PmCartesian end = {0,1,0}; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + { + PmCartesian utan_start; + const double angle = 0.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_start)); + PmCartesian const expect_utan_start = {0,1,0}; + ASSERT(pmCartCartCompare(&utan_start, &expect_utan_start)); + } + + { + PmCartesian utan_30deg; + const double angle = M_PI / 6.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_30deg)); + PmCartesian const expect_utan_30deg = {-0.5, sqrt(3)/2.0,0}; + ASSERT(pmCartCartCompare(&utan_30deg, &expect_utan_30deg)); + } + { + PmCartesian utan_60deg; + const double angle = M_PI / 3.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_60deg)); + PmCartesian const expect_utan_60deg = {-sqrt(3)/2.0, 0.5, 0}; + ASSERT(pmCartCartCompare(&utan_60deg, &expect_utan_60deg)); + } + { + PmCartesian utan_end; + const double angle = M_PI / 2.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_end)); + PmCartesian const expect_utan_end = {-1.0, 0, 0}; + ASSERT(pmCartCartCompare(&utan_end, &expect_utan_end)); + } + + PASS(); +} + + +/** + * Evaluate the derivative of the ideal XY spiral + * x(theta) = a*cos(theta) - (r0+a*theta)*sin(theta) + * y(theta) = a*sin(theta) + (r0+a*theta)*sin(theta) + * + * Where a is the spiral component, r0 is radius when theta is zero. + * + * Uses the derivative to produce the unit vector at the given theta. + * From https://math.stackexchange.com/questions/1078185/differentiate-archimedess-spiral + */ +static PmCartesian idealXYSpiralUnitVec(double a, double r0, double theta) +{ + PmCartesian out = {0}; + out.x = a*cos(theta) - (r0+a*theta)*sin(theta); + out.y = a*sin(theta) + (r0+a*theta)*cos(theta); + pmCartUnitEq(&out); + return out; +} + +TEST test_pmCircleTangentVector_spiralout() +{ + PmCircle c; + PmCartesian start = {1,0, 0}; + PmCartesian end = {1.0 + M_PI_2,0,0}; + PmCartesian center = {0,0,0}; + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + // a = (r1-r0)/theta = (pi/2) / (2pi) = 1/4 + double expect_spiral_a = 0.25; + ASSERT_FLOAT_EQ(c.angle, 2.0 * M_PI); + + { + PmCartesian utan_start; + const double angle = 0.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_start)); + PmCartesian const expect_utan_start = idealXYSpiralUnitVec(expect_spiral_a, 1.0, angle); + ASSERT_PMCARTESIAN_IN_RANGE(utan_start, expect_utan_start, CART_FUZZ); + } + + { + PmCartesian utan_30deg; + const double angle = M_PI / 6.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_30deg)); + PmCartesian const expect_utan_30deg = idealXYSpiralUnitVec(expect_spiral_a, 1.0, angle); + ASSERT_PMCARTESIAN_IN_RANGE(utan_30deg, expect_utan_30deg, CART_FUZZ); + } + + { + PmCartesian utan_end; + const double angle = M_PI / 6.0; + ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_end)); + PmCartesian const expect_utan_end = idealXYSpiralUnitVec(expect_spiral_a, 1.0, angle); + ASSERT_PMCARTESIAN_IN_RANGE(utan_end, expect_utan_end, CART_FUZZ); + } + + PASS(); +} + + TEST test_pmCircleActualMaxVel_cutoff() { double const v_max = 8.0; @@ -311,6 +416,8 @@ TEST test_pmCircleActualMaxVel_cutoff() SUITE(circle_funcs) { + RUN_TEST(test_pmCircleTangentVector_unitcircle); + RUN_TEST(test_pmCircleTangentVector_spiralout); RUN_TEST(test_pmCircleActualMaxVel_cutoff); } diff --git a/unit_tests/tp/test_posemath.c b/unit_tests/tp/test_posemath.c index 62c7a502973..374930484a7 100644 --- a/unit_tests/tp/test_posemath.c +++ b/unit_tests/tp/test_posemath.c @@ -245,6 +245,75 @@ TEST test_pmCircleInit() PASS(); } +TEST test_pmCircleInit_spiral_short() +{ + PmCircle c; + PmCartesian center = {0.0, 0.0, 0.0}; + PmCartesian start = {1,0, 0}; + PmCartesian end = {0, 1 + 0.3,0}; + + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + ASSERT_IN_RANGE(M_PI_2, c.angle, CART_FUZZ); + ASSERT_IN_RANGE(0.3, c.spiral, CART_FUZZ); + + PASS(); +} + +TEST test_pmCircleInit_spiral_long() +{ + PmCircle c; + PmCartesian center = {0.0, 0.0, 0.0}; + PmCartesian start = {1,0, 0}; + PmCartesian end = {-1-0.3, 0,0}; + + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 3); + + ASSERT_IN_RANGE(M_PI * 7, c.angle, CART_FUZZ); + ASSERT_IN_RANGE(0.3, c.spiral, CART_FUZZ); + + PASS(); +} + +TEST test_pmCircleInit_helix() +{ + PmCircle c; + // Assume center is zero for ease of writing the test + const PmCartesian center = {1.0, 2.0, 3.0}; + + PmCartesian start = {1,0, 0}; + PmCartesian end = {1.0,0,0.4}; + pmCartCartAddEq(&start, ¢er); + pmCartCartAddEq(&end, ¢er); + + PmCartesian normal = {0,0,1}; + pmCircleInit(&c, &start, &end, ¢er, &normal, 0); + + PmCartesian const expect_rHelix = {0,0,0.4}; + + ASSERT_IN_RANGE(2.0 * M_PI, c.angle, CART_FUZZ); + ASSERT(pmCartCartCompare(&expect_rHelix, &c.rHelix)); + const double expect_radius = 1.0; + + // Radius should be constant for an ideal helix + for (double angle = 0.0; angle <= M_PI * 2.0; angle += M_PI / 3.0) { + PmCartesian p = {0}; + pmCirclePoint(&c, 0.0, &p); + pmCartCartSubEq(&p, ¢er); + // Ignore perpendicular component + p.z=0; + double r; + pmCartMag(&p, &r); + + ASSERT_FLOAT_EQ(expect_radius, r); + } + + PASS(); +} + + TEST test_pmCircleStretch() { PmCircle c; @@ -284,6 +353,9 @@ TEST test_pmCircleStretch() SUITE(pm_circle) { RUN_TEST(test_pmCircleInit_simple); RUN_TEST(test_pmCircleInit); + RUN_TEST(test_pmCircleInit_spiral_short); + RUN_TEST(test_pmCircleInit_spiral_long); + RUN_TEST(test_pmCircleInit_helix); RUN_TEST(test_pmCircleStretch); } From d41eb13858d91714d8ddfea43eb38d566fc8da1f Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Mar 2019 10:56:03 -0500 Subject: [PATCH 115/129] unit_test: convert more PmCartesian asserts to use macro rather than compare function --- unit_tests/tp/test_blendmath.c | 8 ++--- unit_tests/tp/test_posemath.c | 50 ++++++++++++++++-------------- unit_tests/tp/test_spherical_arc.c | 8 ++--- 3 files changed, 34 insertions(+), 32 deletions(-) diff --git a/unit_tests/tp/test_blendmath.c b/unit_tests/tp/test_blendmath.c index 561f4d67f4b..ca96b1ecb3d 100644 --- a/unit_tests/tp/test_blendmath.c +++ b/unit_tests/tp/test_blendmath.c @@ -305,7 +305,7 @@ TEST test_pmCircleTangentVector_unitcircle() const double angle = 0.0; ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_start)); PmCartesian const expect_utan_start = {0,1,0}; - ASSERT(pmCartCartCompare(&utan_start, &expect_utan_start)); + ASSERT_PMCARTESIAN_IN_RANGE(utan_start, expect_utan_start, CART_FUZZ); } { @@ -313,21 +313,21 @@ TEST test_pmCircleTangentVector_unitcircle() const double angle = M_PI / 6.0; ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_30deg)); PmCartesian const expect_utan_30deg = {-0.5, sqrt(3)/2.0,0}; - ASSERT(pmCartCartCompare(&utan_30deg, &expect_utan_30deg)); + ASSERT_PMCARTESIAN_IN_RANGE(utan_30deg, expect_utan_30deg, CART_FUZZ); } { PmCartesian utan_60deg; const double angle = M_PI / 3.0; ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_60deg)); PmCartesian const expect_utan_60deg = {-sqrt(3)/2.0, 0.5, 0}; - ASSERT(pmCartCartCompare(&utan_60deg, &expect_utan_60deg)); + ASSERT_PMCARTESIAN_IN_RANGE(utan_60deg, expect_utan_60deg, CART_FUZZ); } { PmCartesian utan_end; const double angle = M_PI / 2.0; ASSERT_FALSE(pmCircleTangentVector(&c, angle, &utan_end)); PmCartesian const expect_utan_end = {-1.0, 0, 0}; - ASSERT(pmCartCartCompare(&utan_end, &expect_utan_end)); + ASSERT_PMCARTESIAN_IN_RANGE(utan_end, expect_utan_end, CART_FUZZ); } PASS(); diff --git a/unit_tests/tp/test_posemath.c b/unit_tests/tp/test_posemath.c index 374930484a7..9906c0acbc4 100644 --- a/unit_tests/tp/test_posemath.c +++ b/unit_tests/tp/test_posemath.c @@ -10,6 +10,8 @@ GREATEST_MAIN_DEFS(); #include "mock_rtapi.inc" +#define ASSERT_PMCARTESIAN_EQ(EXP, GOT) ASSERT_PMCARTESIAN_IN_RANGE(EXP, GOT, V_FUZZ) + TEST test_pmCartCartCompare_self() { PmCartesian start = {10.0, 2.0, -3.45}; @@ -119,23 +121,23 @@ TEST test_PmCartesian_arithmetic() PmCartesian pmsum; ASSERT_EQ(PM_OK, pmCartCartAdd(&v1, &v2, &pmsum)); - ASSERT(pmCartCartCompare(&sum, &pmsum)); + ASSERT_PMCARTESIAN_EQ(sum, pmsum); PmCartesian pm_diff; ASSERT_EQ(PM_OK, pmCartCartSub(&v2, &v1, &pm_diff)); - ASSERT(pmCartCartCompare(&diff, &pm_diff)); + ASSERT_PMCARTESIAN_EQ(diff, pm_diff); PmCartesian pm_mult_k; ASSERT_EQ(PM_OK, pmCartScalMult(&v1, k, &pm_mult_k)); - ASSERT(pmCartCartCompare(&v1_mult_k, &pm_mult_k)); + ASSERT_PMCARTESIAN_EQ(v1_mult_k, pm_mult_k); PmCartesian pm_div_k; ASSERT_EQ(PM_OK, pmCartScalDiv(&v1, k, &pm_div_k)); - ASSERT(pmCartCartCompare(&v1_div_k, &pm_div_k)); + ASSERT_PMCARTESIAN_EQ(v1_div_k, pm_div_k); PmCartesian pm_neg; ASSERT_EQ(PM_OK, pmCartNeg(&v1, &pm_neg)); - ASSERT(pmCartCartCompare(&v1_neg, &pm_neg)); + ASSERT_PMCARTESIAN_EQ(v1_neg, pm_neg); double pm_dot=0.0; pmCartCartDot(&v1, &v2, &pm_dot); @@ -143,15 +145,15 @@ TEST test_PmCartesian_arithmetic() PmCartesian pm_cross; pmCartCartCross(&v1, &v2, &pm_cross); - ASSERT(pmCartCartCompare(&pm_cross, &cross)); + ASSERT_PMCARTESIAN_EQ(pm_cross, cross); PmCartesian pm_elem_mult; pmCartCartMult(&v1, &v2, &pm_elem_mult); - ASSERT(pmCartCartCompare(&pm_elem_mult, &elem_mult)); + ASSERT_PMCARTESIAN_EQ(pm_elem_mult, elem_mult); PmCartesian pm_elem_div; pmCartCartDiv(&v1, &v2, &pm_elem_div); - ASSERT(pmCartCartCompare(&pm_elem_div, &elem_div)); + ASSERT_PMCARTESIAN_EQ(pm_elem_div, elem_div); PASS(); } @@ -179,7 +181,7 @@ TEST test_pmCartUnit() // Show that the self-modifying version is the same output PmCartesian ueq = v1; ASSERT_FALSE(pmCartUnitEq(&ueq)); - ASSERT(pmCartCartCompare(&ueq, &pm_u1)); + ASSERT_PMCARTESIAN_EQ(ueq, pm_u1); PASS(); } @@ -294,7 +296,7 @@ TEST test_pmCircleInit_helix() PmCartesian const expect_rHelix = {0,0,0.4}; ASSERT_IN_RANGE(2.0 * M_PI, c.angle, CART_FUZZ); - ASSERT(pmCartCartCompare(&expect_rHelix, &c.rHelix)); + ASSERT_PMCARTESIAN_EQ(expect_rHelix, c.rHelix); const double expect_radius = 1.0; // Radius should be constant for an ideal helix @@ -338,7 +340,7 @@ TEST test_pmCircleStretch() PmCartesian sample_end_stretch; pmCirclePoint(&c, M_PI / 8.0, &sample_end_stretch); - ASSERT(pmCartCartCompare(&sample_before, &sample_end_stretch) == 1); + ASSERT_PMCARTESIAN_EQ(sample_before, sample_end_stretch); // Stretch to extend past the starting point pmCircleStretch(&c, M_PI * 5.0/4.0, 1); @@ -346,7 +348,7 @@ TEST test_pmCircleStretch() PmCartesian sample_start_stretch; pmCirclePoint(&c, M_PI / 8.0 + M_PI_4, &sample_start_stretch); - ASSERT(pmCartCartCompare(&sample_before, &sample_start_stretch) == 1); + ASSERT_PMCARTESIAN_EQ(sample_before, sample_start_stretch); PASS(); } @@ -368,8 +370,8 @@ TEST test_pmCartLineInit_simple() pmCartLineInit(&tst, &start, &end); ASSERT_FLOAT_EQ(tst.tmag, sqrt(1 + 4 + 9)); - ASSERT(pmCartCartCompare(&start, &tst.start)); - ASSERT(pmCartCartCompare(&end, &tst.end)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); PASS(); } @@ -390,8 +392,8 @@ TEST test_pmCartLineInit_ex1() pmCartLineInit(&tst, &start, &end); ASSERT_FLOAT_EQ(tst.tmag, mag); - ASSERT(pmCartCartCompare(&start, &tst.start)); - ASSERT(pmCartCartCompare(&end, &tst.end)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); PASS(); } @@ -412,8 +414,8 @@ TEST test_pmCartLineInit_ex2() pmCartLineInit(&tst, &start, &end); ASSERT_FLOAT_EQ(tst.tmag, mag); - ASSERT(pmCartCartCompare(&start, &tst.start)); - ASSERT(pmCartCartCompare(&end, &tst.end)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); PASS(); } @@ -436,8 +438,8 @@ TEST test_pmCartLineInit_small_belowfuzz() // Store the actual magnitude ASSERT_FLOAT_EQ(tst.tmag, mag); ASSERT(tst.tmag_zero > 0); - ASSERT(pmCartCartCompare(&start, &tst.start)); - ASSERT(pmCartCartCompare(&end, &tst.end)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); PASS(); } @@ -457,8 +459,8 @@ TEST test_pmCartLineInit_small_abovefuzz() ASSERT(tst.tmag > 0.0); ASSERT(tst.tmag_zero == 0); - ASSERT(pmCartCartCompare(&start, &tst.start)); - ASSERT(pmCartCartCompare(&end, &tst.end)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); PASS(); } @@ -482,8 +484,8 @@ TEST test_pmCartLinePoint() ASSERT(tst.tmag > 0.0); ASSERT(tst.tmag_zero == 0); - ASSERT(pmCartCartCompare(&start, &tst.start)); - ASSERT(pmCartCartCompare(&end, &tst.end)); + ASSERT_PMCARTESIAN_EQ(start, tst.start); + ASSERT_PMCARTESIAN_EQ(end, tst.end); PASS(); } diff --git a/unit_tests/tp/test_spherical_arc.c b/unit_tests/tp/test_spherical_arc.c index 8c166b15352..235e23db485 100644 --- a/unit_tests/tp/test_spherical_arc.c +++ b/unit_tests/tp/test_spherical_arc.c @@ -22,9 +22,9 @@ TEST arcInitFromPoints_simple() { int res = arcInitFromPoints(&arc, &start, &end, ¢er); ASSERT_FALSE(res); - ASSERT(pmCartCartCompare(&arc.end, &end)); - ASSERT(pmCartCartCompare(&arc.start, &start)); - ASSERT(pmCartCartCompare(&arc.center, ¢er)); + ASSERT_PMCARTESIAN_IN_RANGE(arc.end, end, V_FUZZ); + ASSERT_PMCARTESIAN_IN_RANGE(arc.start, start, V_FUZZ); + ASSERT_PMCARTESIAN_IN_RANGE(arc.center, center, V_FUZZ); double mag; pmCartMag(&arc.rStart, &mag); @@ -39,7 +39,7 @@ TEST arcInitFromPoints_simple() { PmCartesian pt={0}; arcPoint(&arc, 0, &pt); - ASSERT(pmCartCartCompare(&arc.center, ¢er)); + ASSERT_PMCARTESIAN_IN_RANGE(arc.center, center, V_FUZZ); PASS(); } From 053beeb7c609e6ff85cbae4669813f0385becbed Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Mar 2019 11:50:42 -0500 Subject: [PATCH 116/129] posemath: Fix pointer type mismatches in posemath error messages --- src/libnml/posemath/_posemath.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/libnml/posemath/_posemath.c b/src/libnml/posemath/_posemath.c index 4922a1f9c04..abfd50946fe 100644 --- a/src/libnml/posemath/_posemath.c +++ b/src/libnml/posemath/_posemath.c @@ -921,7 +921,7 @@ int pmCartInvEq(PmCartesian * const v) if (size_sq == 0.0) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Zero vector in pmCartInv\n"); + pmPrintError("Zero vector in pmCartInv\n"); #endif return pmErrno = PM_NORM_ERR; } @@ -988,7 +988,7 @@ int pmCartScalDivEq(PmCartesian * const v, double d) if (d == 0.0) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Divide by 0 in pmCartScalDiv\n"); + pmPrintError("Divide by 0 in pmCartScalDiv\n"); #endif return pmErrno = PM_DIV_ERR; @@ -1452,7 +1452,7 @@ int pmQuatCartMult(PmQuaternion const * const q1, PmCartesian const * const v2, #ifdef PM_DEBUG if (!pmQuatIsNorm(q1)) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Bad quaternion in pmQuatCartMult\n"); + pmPrintError("Bad quaternion in pmQuatCartMult\n"); #endif return pmErrno = PM_NORM_ERR; } @@ -1505,7 +1505,7 @@ int pmPoseCartMult(PmPose const * const p1, PmCartesian const * const v2, PmCart #ifdef PM_DEBUG if (!pmQuatIsNorm(&p1->rot)) { #ifdef PM_PRINT_ERROR - pmPrintError(&"Bad quaternion in pmPoseCartMult\n"); + pmPrintError("Bad quaternion in pmPoseCartMult\n"); #endif return pmErrno = PM_NORM_ERR; } From 742cc2aed02728d06be692e9f89aae13596001a7 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Tue, 5 Mar 2019 10:44:51 -0500 Subject: [PATCH 117/129] motion: Convert #defines to enums --- src/emc/nml_intf/motion_types.h | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/emc/nml_intf/motion_types.h b/src/emc/nml_intf/motion_types.h index 57eb5b25fad..f0818dc6055 100644 --- a/src/emc/nml_intf/motion_types.h +++ b/src/emc/nml_intf/motion_types.h @@ -15,11 +15,14 @@ // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA #ifndef MOTION_TYPES_H #define MOTION_TYPES_H -#define EMC_MOTION_TYPE_TRAVERSE 1 -#define EMC_MOTION_TYPE_FEED 2 -#define EMC_MOTION_TYPE_ARC 3 -#define EMC_MOTION_TYPE_TOOLCHANGE 4 -#define EMC_MOTION_TYPE_PROBING 5 -#define EMC_MOTION_TYPE_INDEXROTARY 6 + +typedef enum { + EMC_MOTION_TYPE_TRAVERSE = 1, + EMC_MOTION_TYPE_FEED = 2, + EMC_MOTION_TYPE_ARC = 3, + EMC_MOTION_TYPE_TOOLCHANGE = 4, + EMC_MOTION_TYPE_PROBING = 5, + EMC_MOTION_TYPE_INDEXROTARY = 6, +} EMCMotionTypes; #endif From 4bcd4d7770075b10f7a5d551b85bbc4b63f6740e Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 1 May 2019 17:53:31 -0400 Subject: [PATCH 118/129] Fix some formatting mismatches between other branches for easier merging in the future --- src/emc/tp/tp.c | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 40a51c571ff..9f3c713f437 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -766,8 +766,7 @@ static inline int find_max_element(double arr[], int sz) } // Assumes at least one element int max_idx = 0; - int idx; - for (idx = 0; idx < sz; ++idx) { + for (int idx = 0; idx < sz; ++idx) { if (arr[idx] > arr[max_idx]) { max_idx = idx; } @@ -1531,8 +1530,12 @@ STATIC int tpFinalizeAndEnqueue(TP_STRUCT * const tp, TC_STRUCT * const tc) /** * Adds a rigid tap cycle to the motion queue. */ -int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxvel, - double acc, unsigned char enables) { +int tpAddRigidTap(TP_STRUCT * const tp, + EmcPose end, + double vel, + double ini_maxvel, + double acc, + unsigned char enables) { if (tpErrorCheck(tp)) { return TP_ERR_FAIL; } @@ -2392,7 +2395,8 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t double dx = tc->target - tc->progress; double maxaccel = tcGetTangentialMaxAccel(tc); - double maxnewvel = findTrapezoidalDesiredVel(maxaccel, dx, tc_finalvel, tc->currentvel, tc->cycle_time); + 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 @@ -2491,7 +2495,7 @@ static inline bool spindleReversed(spindle_origin_t origin, double prev_pos, dou return origin.direction * (current_pos - prev_pos) < 0; } -static inline bool cmdReverseSpindle() +static inline bool cmdReverseSpindle(void) { static bool reversed = false; // Flip sign on commanded velocity From de2249441dbbf66477bb01aebcc0275305452857 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 1 May 2019 17:57:54 -0400 Subject: [PATCH 119/129] Restore a debug message in case of future issues with zero-length moves --- src/emc/tp/tc.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 6780d4badc6..74ea3f07ef4 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -582,6 +582,13 @@ double pmLine9Target(PmLine9 * const line9, int pure_angular) } else if (!line9->abc.tmag_zero) { return line9->abc.tmag; } else { + rtapi_print_msg(RTAPI_MSG_DBG,"line can't have zero length! xyz start = %.12e,%.12e,%.12e, end = %.12e,%.12e,%.12e\n", + line9->xyz.start.x, + line9->xyz.start.y, + line9->xyz.start.z, + line9->xyz.end.x, + line9->xyz.end.y, + line9->xyz.end.z); return 0.0; } } From ea2be468abaa50910b9c3b2a80b6ca90a9b4a586 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 11 Mar 2019 20:01:48 -0400 Subject: [PATCH 120/129] unit_test: convert more asserts to produce GCC-style output --- unit_tests/greatest.h | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/unit_tests/greatest.h b/unit_tests/greatest.h index 465536ff62b..e3e6654d404 100644 --- a/unit_tests/greatest.h +++ b/unit_tests/greatest.h @@ -491,11 +491,15 @@ typedef enum greatest_test_res { do { \ greatest_info.assertions++; \ if ((EXP) != (GOT)) { \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\nExpected: "); \ - GREATEST_FPRINTF(GREATEST_STDOUT, FMT, EXP); \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n Got: "); \ - GREATEST_FPRINTF(GREATEST_STDOUT, FMT, GOT); \ - GREATEST_FPRINTF(GREATEST_STDOUT, "\n"); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: In %s:\n", \ + __LINE__, greatest_info.name_buf); \ + GREATEST_FPRINTF(GREATEST_STDERR, \ + __FILE__ ":%u: expected " FMT \ + ", got " FMT "\n", \ + __LINE__, \ + EXP, \ + GOT); \ GREATEST_FAILm(MSG); \ } \ } while (0) @@ -508,11 +512,11 @@ typedef enum greatest_test_res { greatest_enum_str_fun *greatest_ENUM_STR = ENUM_STR; \ if (greatest_EXP != greatest_GOT) { \ GREATEST_FPRINTF(GREATEST_STDERR, \ - "%s:%u: In %s:\n", \ - __FILE__, __LINE__, greatest_info.name_buf); \ + __FILE__ ":%u: In %s:\n", \ + __LINE__, greatest_info.name_buf); \ GREATEST_FPRINTF(GREATEST_STDERR, \ - "%s:%u: expected %s, got %s", \ - __FILE__, __LINE__, \ + __FILE__ ":%u: expected %s, got %s\n", \ + __LINE__, \ greatest_ENUM_STR(greatest_EXP), \ greatest_ENUM_STR(greatest_GOT)); \ GREATEST_FAILm(MSG); \ @@ -531,14 +535,14 @@ typedef enum greatest_test_res { (greatest_EXP < greatest_GOT && \ greatest_GOT - greatest_EXP > greatest_TOL)) { \ GREATEST_FPRINTF(GREATEST_STDERR, \ - "%s:%u: In %s:\n", \ - __FILE__, __LINE__, greatest_info.name_buf); \ + __FILE__ ":%u: In %s:\n", \ + __LINE__, greatest_info.name_buf); \ GREATEST_FPRINTF(GREATEST_STDERR, \ - "%s:%u: expected " GREATEST_FLOAT_FMT \ + __FILE__ ":%u: expected " GREATEST_FLOAT_FMT \ " +/- " GREATEST_FLOAT_FMT \ ", got " GREATEST_FLOAT_FMT \ ", difference " GREATEST_FLOAT_FMT "\n", \ - __FILE__, __LINE__, \ + __LINE__, \ greatest_EXP, \ greatest_TOL, \ greatest_GOT, \ From 5a815e0d46953bce5d1507eb798d6ac9b7011e7c Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Mon, 11 Mar 2019 20:02:35 -0400 Subject: [PATCH 121/129] unit_test: Hide pedantic debug ouput by default in unit tests --- meson.build | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/meson.build b/meson.build index b72cb4f9c64..1424bdaccfd 100644 --- a/meson.build +++ b/meson.build @@ -19,7 +19,7 @@ inc_dir = include_directories([ cc = meson.get_compiler('c') m_dep = cc.find_library('m', required : true) -add_global_arguments(['-DULAPI','-DUNIT_TEST','-DTP_PEDANTIC_DEBUG'], language : 'c') +add_global_arguments(['-DULAPI','-DUNIT_TEST'], language : 'c') # Define static libraries for easier linking with unit tests From b515f12697ce2803c5de2619f70e0ee84deadbc5 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 1 May 2019 18:59:42 -0400 Subject: [PATCH 122/129] tp: fix missing symbol --- src/emc/tp/tp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 9f3c713f437..657994726ef 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -94,7 +94,7 @@ typedef struct { char axes[9]; } AxisMaskString; -inline AxisMaskString axisBitMaskToString(unsigned failed_axes) +static inline AxisMaskString axisBitMaskToString(unsigned failed_axes) { AxisMaskString axis_str = {{"XYZABCUVW"}}; for (int i = 0; i < 9;++i) { From 24e6a35632b352b7ba5d13e0a8ddc8cc7b574d80 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 1 May 2019 19:12:56 -0400 Subject: [PATCH 123/129] Fix runtests for TP consistency checks --- tests/motion-logger/basic/expected.builtin-startup.in | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/motion-logger/basic/expected.builtin-startup.in b/tests/motion-logger/basic/expected.builtin-startup.in index c67ef0154bb..79b4144a6b9 100644 --- a/tests/motion-logger/basic/expected.builtin-startup.in +++ b/tests/motion-logger/basic/expected.builtin-startup.in @@ -8,6 +8,7 @@ SET_VEL vel=0.000000, ini_maxvel=1.200000 SET_VEL_LIMIT vel=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SET_ACC acc=999999999999999967336168804116691273849533185806555472917961779471295845921727862608739868455469056.000000 SETUP_ARC_BLENDS +SETUP_CONSISTENCY_CHECKS enabled=0, position_drift=0.000100 SET_MAX_FEED_OVERRIDE 1.000000 SET_WORLD_HOME x=0.000000, y=0.000000, z=0.000000, a=0.000000, b=0.000000, c=0.000000, u=0.000000, v=0.000000, w=0.000000 SET_BACKLASH joint=0, backlash=0.000000 From dd22d36d38c5bc537ecc6c73cf5c15b6255dc6f5 Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 18 Apr 2018 14:18:59 -0500 Subject: [PATCH 124/129] canon: fix feed rate problem in arc corner case When an arc is 360 degrees, the endpoint and the starting point are the same. Naively calculating the XYZ movement using only the difference between the endpoints overlooks XYZ motion. When such a move is combined with a rotary axis movement, the resulting feed rate will be incorrect. This patch fixes the problem by passing a parameter to the function calculating that distance that forces XYZ movement to be recognized. This may not address the entire issue, since later code in that function makes further use of the endpoint differences. --- src/emc/task/emccanon.cc | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 30190df1dc3..78fee99c5e0 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -700,9 +700,11 @@ static void applyMinDisplacement(double &dx, * Get the limiting acceleration for a displacement from the current position to the given position. * returns a single acceleration that is the minimum of all axis accelerations. */ -static AccelData getStraightAcceleration(double x, double y, double z, - double a, double b, double c, - double u, double v, double w) +static AccelData getStraightAcceleration( + double x, double y, double z, + double a, double b, double c, + double u, double v, double w, + int known_xyz_motion) { double dx, dy, dz, du, dv, dw, da, db, dc; double tx, ty, tz, tu, tv, tw, ta, tb, tc; @@ -726,12 +728,15 @@ static AccelData getStraightAcceleration(double x, double y, double z, applyMinDisplacement(dx, dy, dz, da, db, dc, du, dv, dw); if(debug_velacc) - printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", + printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g" + " dc %g du %g dv %g dw %g\n", dx, dy, dz, da, db, dc, du, dv, dw); // Figure out what kind of move we're making. This is used to determine // the units of vel/acc. - if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && + if (known_xyz_motion) + cartesian_move = 1; + else if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 && du <= 0.0 && dv <= 0.0 && dw <= 0.0) { cartesian_move = 0; } else { @@ -813,7 +818,17 @@ static AccelData getStraightAcceleration(double x, double y, double z, return out; } -static AccelData getStraightAcceleration(CANON_POSITION pos) +static AccelData getStraightAcceleration( + double x, double y, double z, + double a, double b, double c, + double u, double v, double w) +{ + return getStraightAcceleration( + x, y, z, a, b, c, u, v, w, 0); +} + +static AccelData getStraightAcceleration(CANON_POSITION pos, + int known_xyz_motion) { return getStraightAcceleration(pos.x, @@ -824,7 +839,8 @@ static AccelData getStraightAcceleration(CANON_POSITION pos) pos.c, pos.u, pos.v, - pos.w); + pos.w, + known_xyz_motion); } static VelData getStraightVelocity(double x, double y, double z, @@ -1836,7 +1852,8 @@ void ARC_FEED(int line_number, // Use "straight" acceleration measure to compute acceleration bounds due // to non-circular components (helical axis, other axes) - AccelData accdata = getStraightAcceleration(endpt); + AccelData accdata = getStraightAcceleration( + endpt, fabs(total_xyz_length) > 0.000001); double tt_max_motion = accdata.tmax; double tt_max_spiral = spiral_length / a_max_axes; From 0d08b10ab688f05fe0dd7f41527c61599819f5b2 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 1 May 2019 19:24:01 -0400 Subject: [PATCH 125/129] Fix formatting inconsistencies in canon --- src/emc/task/emccanon.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index 78fee99c5e0..7b6161fbd34 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -2231,11 +2231,13 @@ void CHANGE_TOOL(int slot) linearMoveMsg.indexrotary = -1; int old_feed_mode = feed_mode; - if(feed_mode) + if(feed_mode) { STOP_SPEED_FEED_SYNCH(); + } - if(vel && acc) + if(vel && acc) { interp_list.append(linearMoveMsg); + } if(old_feed_mode) START_SPEED_FEED_SYNCH(uuPerRev_vel, 1); From fa8c17d8d8eb284fbbf29f4d38bfd7a5e145c9aa Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Wed, 1 May 2019 19:24:45 -0400 Subject: [PATCH 126/129] canon: fix a merge / rebase error that dropped a variable rename in a previous commit --- 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 7b6161fbd34..b35d7750e1c 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -2850,7 +2850,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 From 347a6b5a709457a3ffd558d4c295e36cf46bcb3b Mon Sep 17 00:00:00 2001 From: John Morris Date: Wed, 18 Apr 2018 16:51:43 -0500 Subject: [PATCH 127/129] Fixup 80c92580: update motion-logger/basic test Update motion-logger/basic test after changing status buffer fields in 80c92580, "Add spindle-specific fields to emcmotCommand rather than reusing other fields (with confusing names)." --- src/emc/motion-logger/motion-logger.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/emc/motion-logger/motion-logger.c b/src/emc/motion-logger/motion-logger.c index fd9ec671431..e68c4621b49 100644 --- a/src/emc/motion-logger/motion-logger.c +++ b/src/emc/motion-logger/motion-logger.c @@ -571,8 +571,9 @@ int main(int argc, char* argv[]) { break; 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_cmd.velocity_rpm_out = c->vel; + log_print("SPINDLE_ON speed=%f, css_factor=%f, xoffset=%f\n", + c->spindle_speed, c->css_factor, c->css_xoffset); + emcmotStatus->spindle_cmd.velocity_rpm_out = c->spindle_speed; break; case EMCMOT_SPINDLE_OFF: From 7f1fa9f9f80e49c15514763ceb286bc58026b751 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 2 May 2019 22:16:14 -0400 Subject: [PATCH 128/129] Fix an illegal declaration in C89 --- src/emc/tp/tp.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 657994726ef..b28f04b7b90 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -766,7 +766,8 @@ static inline int find_max_element(double arr[], int sz) } // Assumes at least one element int max_idx = 0; - for (int idx = 0; idx < sz; ++idx) { + int idx; + for (idx = 0; idx < sz; ++idx) { if (arr[idx] > arr[max_idx]) { max_idx = idx; } From a0e15b6cbd5d05273aae3dfb0b1c99329bbc2cb9 Mon Sep 17 00:00:00 2001 From: "Robert W. Ellenberg" Date: Thu, 2 May 2019 22:29:33 -0400 Subject: [PATCH 129/129] Another C99 issue to satisfy travis (can we move to C99 already?) --- src/emc/tp/tp.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b28f04b7b90..07d2f5f3d5f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -97,7 +97,8 @@ typedef struct { static inline AxisMaskString axisBitMaskToString(unsigned failed_axes) { AxisMaskString axis_str = {{"XYZABCUVW"}}; - for (int i = 0; i < 9;++i) { + int i; + for (i = 0; i < 9;++i) { if (failed_axes & (1 << i)) { continue; }