diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index a2523f22e8a..82c449937b6 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -64,21 +64,29 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, return fmin(v_max_target, tc->maxvel); } -double tcGetOverallMaxAccel(const TC_STRUCT *tc) +static double tcOverallMaxAccelScaled(const TC_STRUCT *tc, int apply_parabolic) { // 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) { + // so that the sum of the two does not exceed the maximum. This is only + // physically required while both segments accelerate simultaneously in the + // blend overlap; callers that need the geometric corner limit pass + // apply_parabolic != 0, the per-cycle motion cap gates it on the overlap. + if (apply_parabolic && (tc->blend_prev || TC_TERM_COND_PARABOLIC == tc->term_cond)) { a_scale *= 0.5; } return tc->maxaccel * a_scale; } +double tcGetOverallMaxAccel(const TC_STRUCT *tc) +{ + return tcOverallMaxAccelScaled(tc, 1); +} + /** * Get acceleration for a tc based on the trajectory planner state. */ @@ -96,6 +104,28 @@ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) return a_scale; } +/** + * Per-cycle tangential acceleration limit. + * + * Same as tcGetTangentialMaxAccel, except the parabolic-blend 1/2 reduction is + * only applied when the segment is actually overlapping a neighbor in an active + * parabolic blend this cycle (in_overlap != 0). Away from that overlap (a lone + * segment, the accel-from-rest of the first segment, or the decel-to-stop of the + * last segment) the full path acceleration is used. The path acceleration is + * derived to respect every joint's limit for the segment direction, so it is safe + * whenever only one segment is moving; the 1/2 split is only needed while two + * blended segments accelerate at once and their per-axis contributions add. + */ +double tcGetCycleMaxAccel(TC_STRUCT const * const tc, int in_overlap) +{ + double a_scale = tcOverallMaxAccelScaled(tc, in_overlap); + + if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { + a_scale *= tc->acc_ratio_tan; + } + return a_scale; +} + int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction) { diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index d9d581b1285..5558a55e280 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -28,6 +28,7 @@ double tcGetMaxTargetVel(TC_STRUCT const * const tc, double tcGetOverallMaxAccel(TC_STRUCT const * tc); double tcGetTangentialMaxAccel(TC_STRUCT const * const tc); +double tcGetCycleMaxAccel(TC_STRUCT const * const tc, int in_overlap); 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 e5667b3f29c..09b46e40544 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -130,10 +130,10 @@ STATIC double estimateParabolicBlendPerformance( 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); +STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int in_overlap); STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode); + TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode, int in_overlap); STATIC int tpRunOptimization(TP_STRUCT * const tp); @@ -2640,7 +2640,7 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const * non-zero velocity at the instant the target is reached. */ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, - double * const acc, double * const vel_desired) + double * const acc, double * const vel_desired, int in_overlap) { tc_debug_print("using trapezoidal acceleration\n"); @@ -2658,7 +2658,7 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t /* Calculations for desired velocity based on trapezoidal profile */ double dx = tcGetDistanceToGo(tc, tp->reverse_run); - double maxaccel = tcGetTangentialMaxAccel(tc); + double maxaccel = tcGetCycleMaxAccel(tc, in_overlap); double discr_term1 = pmSq(tc_finalvel); double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time); @@ -2703,7 +2703,8 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, double * const acc, - double * const vel_desired) + double * const vel_desired, + int in_overlap) { tc_debug_print("using ramped acceleration\n"); // displacement remaining in this segment @@ -2736,7 +2737,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 = tcGetTangentialMaxAccel(tc); + double acc_max = tcGetCycleMaxAccel(tc, in_overlap); // Output acceleration and velocity for position update *acc = saturate(acc_final, acc_max); @@ -2802,7 +2803,8 @@ STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double j * non-zero velocity at the instant the target is reached. */ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, - double * const acc, double * const jerk, double * const vel_desired, double * const pos_error, int blend, double * const req_pos) + double * const acc, double * const jerk, double * const vel_desired, double * const pos_error, int blend, double * const req_pos, + int in_overlap) { tc_debug_print("using s-curve acceleration with Ruckig\n"); @@ -2819,7 +2821,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); double dx = tcGetDistanceToGo(tc, tp->reverse_run); - double maxaccel = tcGetTangentialMaxAccel(tc); + double maxaccel = tcGetCycleMaxAccel(tc, in_overlap); *pos_error = 0; if(!blend && tc->cycle_time < TP_TIME_EPSILON){ @@ -3277,7 +3279,9 @@ STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, if(is_abort) mode = 0; else mode = 1; - tpUpdateCycle(tp, nexttc, NULL, &mode); + // Secondary segment of an active parabolic blend: it overlaps the primary + // this cycle, so the 1/2 accel split applies. + tpUpdateCycle(tp, nexttc, NULL, &mode, 1); //Restore the original target velocity nexttc->target_vel = save_vel; } @@ -3692,7 +3696,7 @@ 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, int* mode) { + TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode, int in_overlap) { //placeholders for position for this update EmcPose before; @@ -3717,13 +3721,13 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, // Also, don't ramp up for parabolic blends if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { if(planner_type == 0) - res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired); + res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap); } // Check the return in case the ramp calculation failed, fall back to trapezoidal if (res_accel != TP_ERR_OK) { if(planner_type == 0) - tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); + tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap); } tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); @@ -3735,17 +3739,17 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, double req_pos = -1.0; // -1.0 means not provided tc->cycle_time = tp->cycleTime; - int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1, &req_pos); + int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1, &req_pos, in_overlap); if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. *mode = TP_SCURVE_ACCEL_ERROR; res_accel = 1; acc=0, vel_desired=0; if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { - res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired); + res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap); } // Check the return in case the ramp calculation failed, fall back to trapezoidal if (res_accel != TP_ERR_OK) { - tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); + tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap); } tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); }else{ @@ -3755,17 +3759,17 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, double jerk; double perror; double req_pos = -1.0; // -1.0 means not provided - int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0, &req_pos); + int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0, &req_pos, in_overlap); if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. *mode = TP_SCURVE_ACCEL_ERROR; res_accel = 1; acc=0, vel_desired=0; if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { - res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired); + res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap); } // Check the return in case the ramp calculation failed, fall back to trapezoidal if (res_accel != TP_ERR_OK) { - tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); + tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired, in_overlap); } tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); }else{ @@ -3777,7 +3781,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, } //Check if we're near the end of the cycle and set appropriate changes - tpCheckEndCondition(tp, tc, nexttc); + tpCheckEndCondition(tp, tc, nexttc, in_overlap); EmcPose displacement; @@ -3843,7 +3847,7 @@ STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, * 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) { +STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int in_overlap) { //Assume no split time unless we find otherwise tc->cycle_time = tp->cycleTime; @@ -3901,7 +3905,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 = tcGetTangentialMaxAccel(tc); + double a_max = tcGetCycleMaxAccel(tc, in_overlap); //If we exceed the maximum acceleration, then the dt estimate is too small. double a = a_f; @@ -4024,7 +4028,9 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT *next2tc = tcqItem(&tp->queue, queue_dir_step*2); int mode = 0; - tpUpdateCycle(tp, nexttc, next2tc, &mode); + // Tangent hand-off after a split cycle: no simultaneous parabolic blend, so + // the next segment runs at full acceleration. + tpUpdateCycle(tp, nexttc, next2tc, &mode, 0); // Update status for the split portion // FIXME redundant tangent check, refactor to switch @@ -4053,7 +4059,12 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, tc->cycle_time = tp->cycleTime; int mode = 0; - tpUpdateCycle(tp, tc, nexttc, &mode); + // The 1/2 parabolic-blend accel reduction is only needed while this segment + // actually overlaps a neighbor in an active blend. blending_next latches once + // the blend into nexttc has begun; away from that (lone segment, accel from + // rest, decel to a final stop) the segment gets its full path acceleration. + int in_overlap = (nexttc != NULL) && tc->blending_next; + tpUpdateCycle(tp, tc, nexttc, &mode, in_overlap); /* Parabolic blending */ diff --git a/tests/motion/g0/checkresult b/tests/motion/g0/checkresult index 1306d8060f8..99aa19186fc 100755 --- a/tests/motion/g0/checkresult +++ b/tests/motion/g0/checkresult @@ -40,8 +40,10 @@ max_acceleration_ips2 = float(Popen( ).communicate()[0]) # max acceleration in inches per second per servo-thread cycle -# the factor of half at the end is because emc2 reserves half the acceleration for blending -max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second) * 0.5 +# A move that never blends (like this lone G0) runs at the full machine +# acceleration; the 1/2 parabolic-blend reservation only applies during an +# actual blend overlap. +max_acceleration_ipspc = (max_acceleration_ips2 / cycles_per_second) # max velocity in inches per second max_velocity_ips = float(Popen(