From d8be94edc7fedc6d27bc6925ecd5bdf02394c726 Mon Sep 17 00:00:00 2001 From: andy pugh Date: Sat, 20 Oct 2018 23:15:23 +0100 Subject: [PATCH 37/37] TP patch for external offsets Signed-off-by: andy pugh --- src/emc/tp/blendmath.c | 21 ++- src/emc/tp/tc.c | 26 ++++ src/emc/tp/tc.h | 2 + src/emc/tp/tp.c | 414 ++++++++++++++++++++++++++++++++++--------------- src/emc/tp/tp_types.h | 13 ++ 5 files changed, 343 insertions(+), 133 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index b7deaf864..e8e402ed5 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -446,7 +446,13 @@ int calculateInscribedDiameter(PmCartesian const * const normal, PmCartesian planar_x,planar_y,planar_z; //Find perpendicular component of unit directions - // FIXME use plane project? + // FIXME Assumes normal is unit length + + /* This section projects the X / Y / Z unit vectors onto the plane + * containing the motions. The operation is done "backwards" here due to a + * quirk with posemath. + * + */ pmCartScalMult(normal, -normal->x, &planar_x); pmCartScalMult(normal, -normal->y, &planar_y); pmCartScalMult(normal, -normal->z, &planar_z); @@ -647,9 +653,15 @@ int blendParamKinematics(BlendGeom3 * const geom, tp_debug_print("a_max = %f, a_n_max = %f\n", param->a_max, param->a_n_max); - // Find common velocity and acceleration - param->v_req = fmax(prev_tc->reqvel, tc->reqvel); - param->v_goal = param->v_req * maxFeedScale; + // Find the nominal velocity for the blend segment with no overrides + double v_req_prev = tcGetMaxTargetVel(prev_tc, 1.0); + double v_req_this = tcGetMaxTargetVel(tc, 1.0); + tp_debug_print("vr_prev = %f, vr_this = %f\n", v_req_prev, v_req_this); + param->v_req = fmax(v_req_prev, v_req_this); + + // Find the worst-case velocity we should reach for either segment + param->v_goal = fmax(tcGetMaxTargetVel(prev_tc, maxFeedScale), + tcGetMaxTargetVel(tc, maxFeedScale)); // Calculate the maximum planar velocity double v_planar_max; @@ -693,7 +705,6 @@ int blendParamKinematics(BlendGeom3 * const geom, tp_debug_print("v_max = %f\n", v_max); param->v_goal = fmin(param->v_goal, v_max); - tp_debug_print("vr1 = %f, vr2 = %f\n", prev_tc->reqvel, tc->reqvel); tp_debug_print("v_goal = %f, max scale = %f\n", param->v_goal, maxFeedScale); return res_dia; diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 3207788c6..ca7784d66 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -28,6 +28,31 @@ #include "tp_debug.h" +double tcGetMaxTargetVel(TC_STRUCT const * const tc, + double max_scale) +{ + double v_max_target; + + switch (tc->synchronized) { + case TC_SYNC_NONE: + // Get maximum reachable velocity from max feed override + v_max_target = tc->reqvel * max_scale; + break; + + case TC_SYNC_VELOCITY: //Fallthrough + max_scale = 1.0; + case TC_SYNC_POSITION: + // Assume no spindle override during blend target + default: + v_max_target = tc->maxvel; + break; + } + + // Clip maximum velocity by the segment's own maximum velocity + return fmin(v_max_target, tc->maxvel); +} + + int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { PmCartesian startpoint; @@ -540,6 +565,7 @@ int tcSetupMotion(TC_STRUCT * const tc, double ini_maxvel, double acc) { + //FIXME assumes that state is already set up in TC_STRUCT, which depends on external order of function calls. tc->maxaccel = acc; diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 261d43873..5fe558634 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -22,6 +22,8 @@ #include "tc_types.h" #include "tp_types.h" +double tcGetMaxTargetVel(TC_STRUCT const * const tc, + double max_scale); 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/tp.c b/src/emc/tp/tp.c index fb9ab84ff..99bf59e21 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -50,8 +50,19 @@ extern emcmot_debug_t *emcmotDebug; extern emcmot_config_t *emcmotConfig; /** static function primitives (ugly but less of a pain than moving code around)*/ -STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc, - TC_STRUCT * const nexttc); +STATIC int tpComputeBlendVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double v_target_this, + double v_target_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net); + +STATIC double estimateParabolicBlendPerformance( + TP_STRUCT const *tp, + TC_STRUCT const *tc, + TC_STRUCT const *nexttc); STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc); @@ -64,7 +75,7 @@ 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(TP_STRUCT const * const, +STATIC int tpAdjustAccelForTangent( TC_STRUCT * const tc, double normal_acc_scale); /** @@ -91,7 +102,7 @@ STATIC int tcCheckLastParabolic(TC_STRUCT * const tc, /** * Returns true if there is motion along ABC or UVW axes, false otherwise. */ -STATIC int tpRotaryMotionCheck(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { +STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { switch (tc->motion_type) { //Note lack of break statements due to every path returning case TC_RIGIDTAP: @@ -188,6 +199,9 @@ STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * */ STATIC double tpGetFeedScale(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { + if (!tc) { + return 0.0; + } //All reasons to disable feed override go here bool pausing = tp->pausing && (tc->synchronized == TC_SYNC_NONE || tc->synchronized == TC_SYNC_VELOCITY); bool aborting = tp->aborting; @@ -215,6 +229,9 @@ STATIC double tpGetFeedScale(TP_STRUCT const * const tp, STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { + if (!tc) { + return 0.0; + } // Start with the scaled target velocity based on the current feed scale double v_target = tc->synchronized ? tc->target_vel : tc->reqvel; /*tc_debug_print("Initial v_target = %f\n",v_target);*/ @@ -224,8 +241,20 @@ STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, } +STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) +{ + //All reasons to disable feed override go here + if (tc && tc->synchronized == TC_SYNC_POSITION ) { + return 1.0; + } else { + return emcmotConfig->maxFeedScale; + } +} + + /** * Get the worst-case target velocity for a segment based on the trajectory planner state. + * Note that this factors in the user-specified velocity limit. */ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { @@ -249,7 +278,7 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con v_max_target = fmin(v_max_target,tp->vLimit); } - // Clip maximum velocity by the segment's own maximum velocity + // Apply maximum segment velocity limit (must always be respected) return fmin(v_max_target, tc->maxvel); } @@ -283,16 +312,19 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, } /** - * Get acceleration for a tc based on the trajectory planner state. + * 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 tpGetScaledAccel(TP_STRUCT const * const tp, - TC_STRUCT const * const tc) { +STATIC inline double planMaxSegmentAccel( + TC_STRUCT const * const tc, + tc_term_cond_t term_cond) +{ double a_scale = tc->maxaccel; /* 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->term_cond == TC_TERM_COND_PARABOLIC || tc->blend_prev) { + if (term_cond == TC_TERM_COND_PARABOLIC) { a_scale *= 0.5; } if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { @@ -302,6 +334,21 @@ STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp, 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 + * 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); +} + /** * Convert the 2-part spindle position and sign to a signed double. */ @@ -652,9 +699,9 @@ int tpErrorCheck(TP_STRUCT const * const tp) { * 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(TP_STRUCT const * const tp, TC_STRUCT * const tc) { +STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { //Compute peak velocity for blend calculations - double acc_scaled = tpGetScaledAccel(tp, tc); + double acc_scaled = tpGetScaledAccel(tc); double length = tc->target; if (!tc->finalized) { // blending may remove up to 1/2 of the segment @@ -677,7 +724,7 @@ STATIC double tpCalculateTriangleVel(TP_STRUCT const * const tp, TC_STRUCT * con */ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) { - double acc_scaled = tpGetScaledAccel(tp, tc); + double acc_scaled = tpGetScaledAccel(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); @@ -692,22 +739,25 @@ STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_S * initialize a blend arc. This function does not handle connecting the * segments together, however. */ -STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, TC_STRUCT const * const prev_line_tc, - TC_STRUCT* const blend_tc, double vel, double ini_maxvel, double acc) { - +STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, + TC_STRUCT const * const prev_tc, + TC_STRUCT* const blend_tc, + double vel, + double ini_maxvel, + double acc) { #ifdef TP_SHOW_BLENDS int canon_motion_type = EMC_MOTION_TYPE_ARC; #else - int canon_motion_type = prev_line_tc->canon_motion_type; + int canon_motion_type = prev_tc->canon_motion_type; #endif tcInit(blend_tc, TC_SPHERICAL, canon_motion_type, tp->cycleTime, - prev_line_tc->enables, - prev_line_tc->atspeed); + prev_tc->enables, + false); // NOTE: blend arc never needs the atspeed flag, since the previous line will have it (and cannot be consumed). // Copy over state data from TP tcSetupState(blend_tc, tp); @@ -719,7 +769,7 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, TC_STRUCT const * acc); // Skip syncdio setup since this blend extends the previous line - blend_tc->syncdio = prev_line_tc->syncdio; //enqueue the list of DIOs that need toggling + blend_tc->syncdio = prev_tc->syncdio; //enqueue the list of DIOs that need toggling // find "helix" length for target double length; @@ -756,6 +806,39 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) } +/** + * 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); + + // 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) +{ + if (sz < 1) { + return -1; + } + // Assumes at least one element + int max_idx = 0; + int idx; + for (idx = 0; idx < sz; ++idx) { + if (arr[idx] > arr[max_idx]) { + max_idx = idx; + } + } + return max_idx; +} /** * Compare performance of blend arc and equivalent tangent speed. @@ -764,36 +847,56 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) * mostly useful for some odd arc-to-arc cases where the blend arc becomes very * short (and therefore slow). */ -STATIC int tpCheckTangentPerformance(TP_STRUCT const * const tp, +STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) { - tcFinalizeLength(blend_tc); - if (blend_tc->maxvel < tc->kink_vel) { - tp_debug_print("segment maxvel %f is less than kink_vel %f, falling back to simple tangent\n", - blend_tc->maxvel, tc->kink_vel); - - // 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(tp, tc, kink_ratio); - tpAdjustAccelForTangent(tp, prev_tc, kink_ratio); - return TP_ERR_NO_ACTION; - } else { - tp_debug_print("segment maxvel %f is greater than kink_vel %f, using blend arc\n", - blend_tc->maxvel, tc->kink_vel); - // Since we get near-exact tangency with the blend arc, throw out the kink velocity since it's not relevant - tc->kink_vel = -1.0; + if (!tc || !prev_tc) { + return NO_BLEND; + } + + // Compute performance measures ("perf_xxx") for each method. This is + // basically the blend velocity. However, because parabolic blends require + // halving the acceleration of both blended segments, they in effect slow + // 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_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"); + tc->kink_vel = -1.0; + tcSetTermCond(prev_tc, TC_TERM_COND_PARABOLIC); + tcCheckLastParabolic(tc, prev_tc); + break; + case TANGENT_SEGMENTS_BLEND: // tangent + tp_debug_print("using parabolic blending\n"); + forceTangentSegments(prev_tc, tc); + break; + case ARC_BLEND: // arc blend + tp_debug_print("using blend arc\n"); + tc->kink_vel = -1.0; + break; + case NO_BLEND: + break; } - - return TP_ERR_OK; + return best_blend; } -STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) { tp_debug_print("-- Starting LineArc blend arc --\n"); @@ -913,7 +1016,6 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, //set the max velocity to v_plan, since we'll violate constraints otherwise. tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, param.v_plan, param.a_max); - blend_tc->target_vel = param.v_actual; int res_tangent = checkTangentAngle(&circ2_temp, &blend_tc->coords.arc.xyz, @@ -927,15 +1029,11 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, return TP_ERR_FAIL; } - if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) { + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { return TP_ERR_NO_ACTION; } tp_debug_print("Passed all tests, updating segments\n"); - - //Cleanup any mess from parabolic - tc->blend_prev = 0; - //TODO refactor to pass consume to connect function if (param.consume) { //Since we're consuming the previous segment, pop the last line off of the queue @@ -955,7 +1053,7 @@ STATIC int tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, } -STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) { tp_debug_print("-- Starting ArcLine blend arc --\n"); @@ -1074,7 +1172,6 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, //set the max velocity to v_plan, since we'll violate constraints otherwise. tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, param.v_plan, param.a_max); - blend_tc->target_vel = param.v_actual; int res_tangent = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, false); if (res_tangent) { @@ -1082,7 +1179,7 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, return TP_ERR_FAIL; } - if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) { + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { return TP_ERR_NO_ACTION; } @@ -1097,7 +1194,7 @@ STATIC int tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, return TP_ERR_OK; } -STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) { tp_debug_print("-- Starting ArcArc blend arc --\n"); @@ -1243,7 +1340,6 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, //set the max velocity to v_plan, since we'll violate constraints otherwise. tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, param.v_plan, param.a_max); - blend_tc->target_vel = param.v_actual; int res_tangent1 = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, false); int res_tangent2 = checkTangentAngle(&circ2_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, true); @@ -1252,7 +1348,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, return TP_ERR_FAIL; } - if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) { + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { return TP_ERR_NO_ACTION; } @@ -1269,7 +1365,7 @@ STATIC int tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, } -STATIC int tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, +STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) { @@ -1321,9 +1417,10 @@ STATIC int tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc //set the max velocity to v_plan, since we'll violate constraints otherwise. tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, param.v_plan, param.a_max); - blend_tc->target_vel = param.v_actual; - if (tpCheckTangentPerformance(tp, prev_tc, tc, blend_tc) == TP_ERR_NO_ACTION) { + tp_debug_print("blend_tc target_vel = %g\n", blend_tc->target_vel); + + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { return TP_ERR_NO_ACTION; } @@ -1401,6 +1498,8 @@ STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) { tc->syncdio.anychanged = 0; return TP_ERR_NO_ACTION; } + + } @@ -1464,7 +1563,7 @@ int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxv return retval; } -STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp, +STATIC blend_type_t tpCheckBlendArcType( TC_STRUCT const * const prev_tc, TC_STRUCT const * const tc) { @@ -1480,7 +1579,7 @@ STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp, } //If we have any rotary axis motion, then don't create a blend arc - if (tpRotaryMotionCheck(tp, tc) || tpRotaryMotionCheck(tp, prev_tc)) { + if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) { tp_debug_print("One of the segments has rotary motion, aborting blend arc\n"); return BLEND_NONE; } @@ -1517,7 +1616,7 @@ STATIC blend_type_t tpCheckBlendArcType(TP_STRUCT const * const tp, 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(tp, tc); + double acc_this = tpGetScaledAccel(tc); // Find the reachable velocity of tc, moving backwards in time double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); @@ -1525,7 +1624,8 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons double vf_limit_this = tc->maxvel; double vf_limit_prev = prev1_tc->maxvel; - if (prev1_tc->kink_vel >=0 ) { + if (prev1_tc->kink_vel >=0 && prev1_tc->term_cond == TC_TERM_COND_TANGENT) { + // Only care about kink_vel with tangent segments vf_limit_prev = fmin(vf_limit_prev, prev1_tc->kink_vel); } //Limit the PREVIOUS velocity by how much we can overshoot into @@ -1535,14 +1635,18 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons //If we've hit the requested velocity, then prev_tc is definitely a "peak" vs_back = vf_limit; prev1_tc->optimization_state = TC_OPTIM_AT_MAX; - tp_debug_print("found peak due to v_limit\n"); + tp_debug_print("found peak due to v_limit %f\n", vf_limit); } //Limit tc's target velocity to avoid creating "humps" in the velocity profile prev1_tc->finalvel = vs_back; - tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f, capped target = %f\n", - prev1_tc->finalvel, tc->finalvel, tc->target_vel); + //Reduce max velocity to match sample rate + double sample_maxvel = tc->target / (tp->cycleTime * TP_MIN_SEGMENT_CYCLES); + tc->maxvel = fmin(tc->maxvel, sample_maxvel); + + tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f\n", + prev1_tc->finalvel, tc->finalvel); return TP_ERR_OK; } @@ -1661,7 +1765,7 @@ STATIC double pmCartAbsMax(PmCartesian const * const v) return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); } -STATIC int tpAdjustAccelForTangent(TP_STRUCT const * const tp, +STATIC int tpAdjustAccelForTangent( TC_STRUCT * const tc, double normal_acc_scale) { @@ -1688,7 +1792,7 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, return TP_ERR_FAIL; } //If we have ABCUVW movement, then don't check for tangency - if (tpRotaryMotionCheck(tp, tc) || tpRotaryMotionCheck(tp, prev_tc)) { + if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) { tp_debug_print("found rotary axis motion\n"); return TP_ERR_FAIL; } @@ -1728,8 +1832,10 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, // Calculate instantaneous acceleration required for change in direction // from v1 to v2, assuming constant speed - double v_max1 = fmin(prev_tc->maxvel, prev_tc->reqvel * emcmotConfig->maxFeedScale); - double v_max2 = fmin(tc->maxvel, tc->reqvel * emcmotConfig->maxFeedScale); + double v_max1 = 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_max = fmin(v_max1, v_max2); tp_debug_print("tangent v_max = %f\n",v_max); @@ -1764,11 +1870,11 @@ STATIC int tpSetupTangent(TP_STRUCT const * const tp, const double kink_ratio = tpGetTangentKinkRatio(); if (acc_scale_max < kink_ratio) { - tp_debug_print(" Kink acceleration within %g, treating as tangent\n", 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; - tpAdjustAccelForTangent(tp, tc, acc_scale_max); - tpAdjustAccelForTangent(tp, prev_tc, acc_scale_max); + tpAdjustAccelForTangent(tc, acc_scale_max); + tpAdjustAccelForTangent(prev_tc, acc_scale_max); return TP_ERR_OK; } else { @@ -1818,7 +1924,6 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { tp_debug_print(" tpSetupTangent failed, aborting blend arc\n"); case TP_ERR_OK: return res_tan; - break; case TP_ERR_NO_ACTION: default: //Continue with creation @@ -1827,7 +1932,7 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { TC_STRUCT blend_tc = {0}; - blend_type_t type = tpCheckBlendArcType(tp, prev_tc, tc); + blend_type_t type = tpCheckBlendArcType(prev_tc, tc); int res_create; switch (type) { case BLEND_LINE_LINE: @@ -1842,6 +1947,7 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { case BLEND_ARC_ARC: 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; @@ -1850,12 +1956,14 @@ STATIC int tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { if (res_create == TP_ERR_OK) { //Need to do this here since the length changed + tcCheckLastParabolic(&blend_tc, prev_tc); tpAddSegmentToQueue(tp, &blend_tc, false); - } else { - return res_create; + } else if (res_create < 0){ + // If blend arc creation failed early on, catch it here and find the best blend + tpChooseBestBlend(tp, prev_tc, tc, NULL) ; } - return TP_ERR_OK; + return res_create; } //TODO final setup steps as separate functions @@ -1882,24 +1990,25 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double v enables, atspeed); - // Copy in motion parameters - tcSetupMotion(&tc, - vel, - ini_maxvel, - acc); - // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + // Copy in motion parameters + tcSetupMotion(&tc, + vel, + ini_maxvel, + acc); + // Setup line geometry pmLine9Init(&tc.coords.line, &tp->goalPos, &end); tc.target = pmLine9Target(&tc.coords.line); 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; } tc.nominal_length = tc.target; @@ -2024,30 +2133,29 @@ int tpAddCircle(TP_STRUCT * const tp, * If we are blending between tc and nexttc, then we need to figure out what a * safe blend velocity is based on the known trajectory parameters. This * function updates the TC_STRUCT data with a safe blend velocity. + * + * @note This function will compute the parabolic blend start / end velocities + * regardless of the current terminal condition (useful for planning). */ -STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp, - TC_STRUCT * const tc, TC_STRUCT * const nexttc) +STATIC int tpComputeBlendVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double target_vel_this, + double target_vel_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net) { /* Pre-checks for valid pointers */ - if (!nexttc || !tc) { + if (!nexttc || !tc || !v_blend_this || !v_blend_next ) { return TP_ERR_FAIL; } - if (tc->term_cond != TC_TERM_COND_PARABOLIC) { - return TP_ERR_NO_ACTION; - } - - double acc_this = tpGetScaledAccel(tp, tc); - double acc_next = tpGetScaledAccel(tp, nexttc); - - // cap the blend velocity at the current requested speed (factoring in feed override) - double target_vel_this; - double target_vel_next; - target_vel_this = tpGetRealTargetVel(tp, tc); - target_vel_next = tpGetRealTargetVel(tp, nexttc); + double acc_this = tpGetScaledAccel(tc); + double acc_next = tpGetScaledAccel(nexttc); - double v_reachable_this = fmin(tpCalculateTriangleVel(tp,tc), target_vel_this); - double v_reachable_next = fmin(tpCalculateTriangleVel(tp,nexttc), target_vel_next); + double v_reachable_this = fmin(tpCalculateTriangleVel(tc), target_vel_this); + double v_reachable_next = fmin(tpCalculateTriangleVel(nexttc), target_vel_next); /* Compute the maximum allowed blend time for each segment. * This corresponds to the minimum acceleration that will just barely reach @@ -2067,10 +2175,19 @@ STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp, double t_blend = fmin(t_max_reachable, t_max_blend); // Now, use this blend time to find the best acceleration / velocity for each segment - double v_blend_this = fmin(v_reachable_this, t_blend * acc_this); - double v_blend_next = fmin(v_reachable_next, t_blend * acc_next); + *v_blend_this = fmin(v_reachable_this, t_blend * acc_this); + *v_blend_next = fmin(v_reachable_next, t_blend * acc_next); double theta; + + PmCartesian v1, v2; + + tcGetEndAccelUnitVector(tc, &v1); + tcGetStartAccelUnitVector(nexttc, &v2); + findIntersectionAngle(&v1, &v2, &theta); + + double cos_theta = cos(theta); + if (tc->tolerance > 0) { /* see diagram blend.fig. T (blend tolerance) is given, theta * is calculated from dot(s1, s2) @@ -2087,25 +2204,45 @@ STATIC int tpComputeBlendVelocity(TP_STRUCT const * const tp, * equate the two expressions for d and solve for v */ double tblend_vel; - PmCartesian v1, v2; - - tcGetEndAccelUnitVector(tc, &v1); - tcGetStartAccelUnitVector(nexttc, &v2); - findIntersectionAngle(&v1, &v2, &theta); /* Minimum value of cos(theta) to prevent numerical instability */ const double min_cos_theta = cos(PM_PI / 2.0 - TP_MIN_ARC_ANGLE); - if (cos(theta) > min_cos_theta) { - tblend_vel = 2.0 * pmSqrt(acc_this * tc->tolerance / cos(theta)); - v_blend_this = fmin(v_blend_this, tblend_vel); - v_blend_next = fmin(v_blend_next, tblend_vel); + if (cos_theta > min_cos_theta) { + tblend_vel = 2.0 * pmSqrt(acc_this * tc->tolerance / cos_theta); + *v_blend_this = fmin(*v_blend_this, tblend_vel); + *v_blend_next = fmin(*v_blend_next, tblend_vel); } } + if (v_blend_net) { + /* + * Find net velocity in the direction tangent to the blend. + * When theta ~ 0, net velocity in tangent direction is very small. + * When the segments are nearly tangent (theta ~ pi/2), the blend + * velocity is almost entirely in the tangent direction. + */ + *v_blend_net = sin(theta) * (*v_blend_this + *v_blend_next) / 2.0; + } - tc->blend_vel = v_blend_this; - nexttc->blend_vel = v_blend_next; return TP_ERR_OK; } +STATIC double estimateParabolicBlendPerformance( + TP_STRUCT const *tp, + TC_STRUCT const *tc, + TC_STRUCT const *nexttc) +{ + double v_this = 0.0, v_next = 0.0; + + // Use maximum possible target velocity to get best-case performance + double target_vel_this = tpGetMaxTargetVel(tp, tc); + double target_vel_next = tpGetMaxTargetVel(tp, nexttc); + + double v_net = 0.0; + tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net); + + return v_net; +} + + /** * Calculate distance update from velocity and acceleration. @@ -2190,7 +2327,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(tp, tc); + double maxaccel = tpGetScaledAccel(tc); double discr_term1 = pmSq(tc_finalvel); double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time); @@ -2268,7 +2405,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(tp, tc); + double acc_max = tpGetScaledAccel(tc); // Output acceleration and velocity for position update *acc = saturate(acc_final, acc_max); @@ -2425,6 +2562,9 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + if (!nexttc) { + return; + } double save_vel = nexttc->target_vel; if (tpGetFeedScale(tp, nexttc) > TP_VEL_EPSILON) { @@ -2444,7 +2584,6 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, tpUpdateCycle(tp, nexttc, NULL); //Restore the original target velocity nexttc->target_vel = save_vel; - } @@ -2525,7 +2664,7 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, * Handle an abort command. * Based on the current motion state, handle the consequences of an abort command. */ -STATIC int tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, +STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { if(!tp->aborting) { @@ -2559,7 +2698,7 @@ STATIC int tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, * has not reached the requested speed, or the spindle index has not been * detected. */ -STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) +STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) { int s; // this is no longer the segment we were waiting_for_index for @@ -2604,17 +2743,15 @@ STATIC int tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) tp->spindle.revs = 0; } } - return TP_ERR_OK; } - /** * "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 int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { +STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { //Check if already active if (!tc || tc->active) { @@ -2703,7 +2840,7 @@ STATIC int tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { * Run velocity mode synchronization. * Update requested velocity to follow the spindle's velocity (scaled by feed rate). */ -STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT const * nexttc) { +STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * nexttc) { double speed = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleSpeedIn; double pos_error = fabs(speed) * tc->uu_per_rev; // Account for movement due to parabolic blending with next segment @@ -2711,6 +2848,12 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST pos_error -= nexttc->progress; } tc->target_vel = pos_error; + + if (nexttc && nexttc->synchronized) { + //If the next move is synchronized too, then match it's + //requested velocity to the current move + nexttc->target_vel = tc->target_vel; + } } @@ -2766,7 +2909,7 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, double errorvel; spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime; target_vel = spindle_vel * tc->uu_per_rev; - errorvel = pmSqrt(fabs(pos_error) * tpGetScaledAccel(tp,tc)); + errorvel = pmSqrt(fabs(pos_error) * tpGetScaledAccel(tc)); if(pos_error<0) { errorvel *= -1.0; } @@ -2883,6 +3026,7 @@ STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) { emcmotStatus->tcqlen = tcqLen(&tp->queue); // Set default value for requested speed emcmotStatus->requested_vel = 0.0; + emcmotStatus->current_vel = 0.0; return TP_ERR_OK; } @@ -2972,7 +3116,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(tp,tc); + double a_max = tpGetScaledAccel(tc); //If we exceed the maximum acceleration, then the dt estimate is too small. double a = a_f; @@ -3014,6 +3158,7 @@ STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, tcSetSplitCycle(tc, dt, v_f); } else { tc_debug_print(" dt = %f, not at end yet\n",dt); + return TP_ERR_NO_ACTION; } return TP_ERR_OK; } @@ -3109,7 +3254,18 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, /* Parabolic blending */ - tpComputeBlendVelocity(tp, tc, nexttc); + double v_this = 0.0, v_next = 0.0; + + // cap the blend velocity at the current requested speed (factoring in feed override) + 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 (nexttc && tcIsBlending(tc)) { tpDoParabolicBlending(tp, tc, nexttc); } else { @@ -3143,6 +3299,13 @@ 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 + //If we have a NULL pointer, then the queue must be empty, so we're done. if(!tc) { tpHandleEmptyQueue(tp); @@ -3151,11 +3314,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tc_debug_print("-------------------\n"); -#ifdef TC_DEBUG - //Hack debug output for timesteps - static double time_elapsed = 0; - time_elapsed+=tp->cycleTime; -#endif /* 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.*/ diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index 9985a17da..380b0cf50 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -133,4 +133,17 @@ typedef struct { } TP_STRUCT; + +/** + * Describes blend modes used in the trajectory planner. + * @note these values are used as array indices, so make sure valid options + * start at 0 and increase by one. + */ +typedef enum { + NO_BLEND = -1, + PARABOLIC_BLEND, + TANGENT_SEGMENTS_BLEND, + ARC_BLEND +} tc_blend_type_t; + #endif /* TP_TYPES_H */ -- 2.11.0