From 8f5a2e346e32f4f7165e59cf6a4f00547243cd8f Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Tue, 20 Jan 2026 21:25:36 +0800 Subject: [PATCH 1/7] Fix S-curve lookahead and segment transitions for smooth deceleration Refactors finishWithSpeedDist() to handle both accel/decel cases properly, adds conservative velocity estimation for lookahead that accounts for worst-case acceleration state, and fixes segment handoff to inherit actual velocity/acceleration instead of assuming instant changes. --- configs/sim/axis/axis_9axis_scurve.ini | 1 - configs/sim/axis/axis_mm_scurve.ini | 1 - src/emc/tp/sp_scurve.c | 170 ++++++++++++++++++------- src/emc/tp/sp_scurve.h | 1 + src/emc/tp/tp.c | 73 ++++++++--- 5 files changed, 176 insertions(+), 70 deletions(-) diff --git a/configs/sim/axis/axis_9axis_scurve.ini b/configs/sim/axis/axis_9axis_scurve.ini index c91ab4a7165..a6abf8106d6 100644 --- a/configs/sim/axis/axis_9axis_scurve.ini +++ b/configs/sim/axis/axis_9axis_scurve.ini @@ -54,7 +54,6 @@ HALFILE = axis_9axis_scurve.hal # PLANNER_TYPE: Select the acceleration profile shape # 0 = Trapezoidal (traditional - constant acceleration) # 1 = S-curve (smoother - limited jerk/derivative of acceleration) -# 2 = S-curve (smoother - but sharp in corners - to be deprecated) # Default: 0 (trapezoidal) # # NOTE: This parameter can be changed at RUNTIME via HAL pin: diff --git a/configs/sim/axis/axis_mm_scurve.ini b/configs/sim/axis/axis_mm_scurve.ini index ca58a2f469f..035f650de52 100644 --- a/configs/sim/axis/axis_mm_scurve.ini +++ b/configs/sim/axis/axis_mm_scurve.ini @@ -54,7 +54,6 @@ HALFILE = axis_mm_scurve.hal # PLANNER_TYPE: Select the acceleration profile shape # 0 = Trapezoidal (traditional - constant acceleration) # 1 = S-curve (smoother - limited jerk/derivative of acceleration) -# 2 = S-curve (smoother - but sharp in corners - to be deprecated) # Default: 0 (trapezoidal) # # NOTE: This parameter can be changed at RUNTIME via HAL pin: diff --git a/src/emc/tp/sp_scurve.c b/src/emc/tp/sp_scurve.c index b7608b5c852..77a7f487dc0 100644 --- a/src/emc/tp/sp_scurve.c +++ b/src/emc/tp/sp_scurve.c @@ -983,50 +983,82 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/ } double finishWithSpeedDist(double v, double ve, double a, double maxA, double maxJ/*, int* phase*/) { - // Already stopped - //*phase = 0; - if (fabs(v) < 0.0001) return 0; - // Handle negative velocity + // Handle negative velocity: transform to positive domain if (v < 0) { - v = -v; - a = -a; - ve = -ve; + v = -v; + a = -a; + ve = -ve; + } + + // Velocity difference too small, no accel/decel needed + if (fabs(v - ve) < 0.0001 && fabs(a) < 0.0001) { + return 0; } double d = 0; - //if(ve > v) - // return 0; - if(ve > v){ - + // ========== Acceleration case (ve > v) ========== + if (ve > v) { + // Phase 1: If a < 0 (decelerating), first bring a to 0 + if (a < 0) { + double t = -a / maxJ; + d += sc_distance(t, v, a, maxJ); + v += velocity(t, a, maxJ); + a = 0; + } + + // Compute required max acceleration + // Amax^2 = (ve - v) * maxJ + 0.5 * a * a + double sqrt_arg = (ve - v) * maxJ + 0.5 * a * a; + if (sqrt_arg < 0) sqrt_arg = 0; + double maxAccel = sqrt(sqrt_arg); + if (maxAccel > maxA) maxAccel = maxA; + + // Phase 2: Increase a from current value to maxAccel + if (maxAccel > a) { + double t = (maxAccel - a) / maxJ; + d += sc_distance(t, v, a, maxJ); + v += velocity(t, a, maxJ); + a = maxAccel; + } + + // Compute velocity when entering decel phase + double deltaV = ve - 0.5 * a * a / maxJ; + + // Phase 3: Constant acceleration phase (if needed) + if (v < deltaV && a > 0.0001) { + double t = (deltaV - v) / a; + d += sc_distance(t, v, a, 0); + v += velocity(t, a, 0); + } + + // Phase 4: Decrease a from maxAccel to 0, velocity reaches ve + if (a > 0.0001) { + double t = a / maxJ; + d += sc_distance(t, v, a, -maxJ); + } + + return d; } - //double vs = v; - // Compute distance and velocity change to accel = 0 - if (0 < a) { - // Compute distance to decrease accel to zero - // distance(double t, double v, double a, double j) - // distance => v * t + 1/2 * a * t^2 + 1/6 * j * t^3 - // velocity => a * t + 1/2 * j * t^2 - double t = a / maxJ; - d += sc_distance(t, v, a, -maxJ); - v += velocity(t, a, -maxJ); - a = 0; - //if(*phase == 0) *phase = 3; + // ========== Deceleration case (v > ve) ========== + + // Phase 1: If a > 0 (accelerating), first bring a to 0 + if (a > 0) { + double t = a / maxJ; + d += sc_distance(t, v, a, -maxJ); + v += velocity(t, a, -maxJ); + a = 0; } - // Compute max deccel - // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 - // VT = V0 + A0 * T + J * T^2 /2 - // AT = A0 + J * T - // - // Amax^2 = (Vs - Ve) * maxJ + 0.5 * a * a - double maxDeccel = -sqrt((v - ve) * maxJ + 0.5 * a * a); + // Compute required max deceleration + // Amax^2 = (v - ve) * maxJ + 0.5 * a * a + double sqrt_arg = (v - ve) * maxJ + 0.5 * a * a; + if (sqrt_arg < 0) sqrt_arg = 0; + double maxDeccel = -sqrt(sqrt_arg); if (maxDeccel < -maxA) maxDeccel = -maxA; - //double maxDeccel = -fabs(maxA); - // Compute distance and velocity change to max deccel - // a * t + 1/2 * j * t^2 + // Phase 2: Decrease a from current value to maxDeccel if (maxDeccel < a) { double t = (a - maxDeccel) / maxJ; d += sc_distance(t, v, a, -maxJ); @@ -1034,27 +1066,22 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma a = maxDeccel; } - // Compute velocity change over remaining accel + // Compute velocity when entering decel end phase // VT = Ve + Amax^2 / (2 * J) double deltaV = ve + 0.5 * a * a / maxJ; - // Compute constant deccel period - // P = v * t + 1/2 * a * t^2 + 1/6 * j * t^3 - // VT = V0 + A0 * T + J * T^2 /2 - if (deltaV < v) { - // distance => v * t + 1/2 * a * t^2 + 1/6 * j * t^3 - // velocity => a * t + 1/2 * j * t^2 - double t = (v - deltaV) / -a; + // Phase 3: Constant deceleration phase (if needed) + if (deltaV < v && a < -0.0001) { + double t = (v - deltaV) / (-a); d += sc_distance(t, v, a, 0); v += velocity(t, a, 0); - //if(*phase == 0) *phase = 6; } - // Compute distance to ve vel - // distance => v * t + 1/2 * a * t^2 + 1/6 * j * t^3 - // A = J * T - d += sc_distance(-a / maxJ, v, a, maxJ); - //if(*phase == 0) *phase = 7; + // Phase 4: Increase a from maxDeccel to 0, velocity reaches ve + if (a < -0.0001) { + double t = (-a) / maxJ; + d += sc_distance(t, v, a, maxJ); + } return d; } @@ -1351,3 +1378,52 @@ double calcSCurveSpeedWithT(double amax, double jerk, double T) { // Use fma to optimize multiply-add operations, improving numerical stability return fma(jerk * T1, T1 + T2, 0.0); } + +/** + * Conservative S-curve reachable velocity calculation (for lookahead optimization) + * + * Unlike findSCurveVSpeedWithEndSpeed which assumes decel starts from a=0, + * this function considers worst case: decel starting from a=maxA state. + * + * This ensures lookahead-computed velocities can smoothly decel to target + * regardless of current acc state, avoiding acc discontinuities. + * + * @param distance Available decel distance + * @param Ve Target end velocity + * @param maxA Maximum acceleration + * @param maxJ Maximum jerk + * @param req_v [output] Computed conservative reachable velocity + * @return 1 success, -1 failure + */ +int findSCurveVSpeedConservative(double distance, double Ve, double maxA, double maxJ, double* req_v) { + if (distance <= 0 || maxA <= 0 || maxJ <= 0) { + *req_v = Ve; + return -1; + } + + // Extra distance needed to bring a from maxA to 0 (velocity still increasing during this) + // t = maxA / maxJ, v_increase = 0.5 * maxA^2 / maxJ + double v_increase = 0.5 * maxA * maxA / maxJ; + double d_extra = maxA * maxA / (2.0 * maxJ); + + // Effective decel distance = total - extra + double effective_distance = distance - d_extra; + + if (effective_distance <= 0) { + *req_v = Ve; + return -1; + } + + double vs_from_zero; + int res = findSCurveVSpeedWithEndSpeed(effective_distance, Ve, maxA, maxJ, &vs_from_zero); + + if (res != 1) { + *req_v = Ve; + return -1; + } + + // Conservative estimate: subtract velocity increase during acc ramp-down + *req_v = fmax(vs_from_zero - v_increase * 0.5, Ve); + + return 1; +} \ No newline at end of file diff --git a/src/emc/tp/sp_scurve.h b/src/emc/tp/sp_scurve.h index 73844f1ecb8..38cbb669a19 100644 --- a/src/emc/tp/sp_scurve.h +++ b/src/emc/tp/sp_scurve.h @@ -43,6 +43,7 @@ int getNext( simple_tp_t *tp, double Vs, double Ve, double period); double getNextPoint(simple_tp_t *tp, int n, double T, double* req_v, double* req_a); int findSCurveVSpeed(double distence,/* double maxV, */double maxA, double maxJ, double *req_v); int findSCurveVSpeedWithEndSpeed(double distence, double Ve, double maxA, double maxJ, double* req_v); +int findSCurveVSpeedConservative(double distance, double Ve, double maxA, double maxJ, double* req_v); double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2); double calcSCurveSpeedWithT(double amax, double jerk, double T); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index d326ef77f8a..c5d3c89d052 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1765,7 +1765,7 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons // Find the reachable velocity of tc, moving backwards in time double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); - if(GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2){ + if(GET_TRAJ_PLANNER_TYPE() == 1){ double vs_back2; if(findSCurveVSpeedWithEndSpeed(tc->target * 2.0 , tc->finalvel, acc_this, emcmotStatus->jerk, &vs_back2) == 1) vs_back = vs_back2; @@ -1890,7 +1890,7 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { 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. - if(GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2) + if(GET_TRAJ_PLANNER_TYPE() == 1) prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationSCurveInitialVel(tp,tc)); else prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationInitialVel(tp,tc)); @@ -2506,7 +2506,7 @@ STATIC double estimateParabolicBlendPerformance( double target_vel_next = tpGetMaxTargetVel(tp, nexttc); double v_net = 0.0; - if(GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2) + if(GET_TRAJ_PLANNER_TYPE() == 1) tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net); else tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net); @@ -2715,8 +2715,7 @@ STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double j { // If the resulting velocity is less than zero, than we're done. This // causes a small overshoot, but in practice it is very small. - //double v_next = vel_desired;//velocity(tc->cycle_time, acc, jerk); //tc->currentvel + acc * tc->cycle_time; double velocity(double t, double a, double j); - double v_next = vel_desired;//velocity(tc->cycle_time, acc, jerk); + double v_next = vel_desired; // update position in this tc using trapezoidal integration // Note that progress can be greater than the target after this step. @@ -2754,11 +2753,11 @@ STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double j tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); } } - tc->currentvel = v_next;// v_next; + tc->currentvel = v_next; tc->currentacc = acc; // Check if we can make the desired velocity - tc->on_final_decel = dec;//(fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc < 0.0); //dec && (acc < 0.0); + tc->on_final_decel = dec; return TP_ERR_OK; } @@ -2816,25 +2815,45 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_target_vel, maxaccel, maxjerk, &req_v, &req_a, &req_j); double dlen1 = finishWithSpeedDist(tc->currentvel, tc_finalvel, tc->currentacc, maxaccel, maxjerk); - double dlen2 = finishWithSpeedDist(saturate(req_v, tc_target_vel), tc_finalvel, saturate(req_a, maxaccel), maxaccel, maxjerk); + + // When velocity exceeds target, correct acceleration + // Since velocity is limited to target, acceleration should be 0 (maintain speed) or decel + double v_for_dlen2 = req_v; + double a_for_dlen2 = req_a; + if (req_v > tc_target_vel) { + v_for_dlen2 = tc_target_vel; + a_for_dlen2 = 0; + } + double dlen2 = finishWithSpeedDist(v_for_dlen2, tc_finalvel, a_for_dlen2, maxaccel, maxjerk); double moveL = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, req_j); - double error = fabs(dx) - dlen1; //误差 - *pos_error = error; + // margin = remaining distance - decel distance + // margin > 0: can continue accelerating + // margin <= 0: must decelerate + double margin = dx - dlen1; + *pos_error = margin; - if(tc->currentvel < 1e-6 && dx > TP_POS_EPSILON && dx < 1e-4){ - //nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_target_vel, maxaccel, maxjerk, &req_v, &req_a, &req_j); + // Handle special case: position overshoot or reached endpoint + if (dx <= TP_POS_EPSILON) { + nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_finalvel, maxaccel, maxjerk, &req_v, &req_a, &req_j); + res = 1; + } + else if(tc->currentvel < 1e-6 && dx > TP_POS_EPSILON && dx < 1e-4){ + // Very low velocity and close to target res = 1; }else{ - //fabs(fabs(pos_err) - decLen) <= 0.000001 || fabs(pos_err) - moveL * dir <= decLen2 || fabs(pos_err) <= decLen - if (fabs(error) <= 0.0001 || dx - moveL <= dlen2 || dx < dlen1){ + // Decel conditions: + // 1. margin <= 0: decel distance >= remaining distance + // 2. dx - moveL <= dlen2: if we continue accel, next cycle remaining <= decel dist + int need_decel = (margin <= TP_POS_EPSILON) || (dx - moveL <= dlen2); + + if (need_decel){ nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_finalvel, maxaccel, maxjerk, &req_v, &req_a, &req_j); }else{ if(tc_finalvel > TP_VEL_EPSILON || tc_target_vel > TP_VEL_EPSILON) res = 0; } } - //} maxnewvel = req_v; maxnewacc = req_a; @@ -2848,8 +2867,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ maxnewaccel = saturate(maxnewaccel, maxaccel); *acc = maxnewaccel; *vel_desired = maxnewvel; - *jerk =maxnewjerk;//(maxnewaccel - tc->currentacc) / dt; - //*vel_desired = tc->currentvel + velocity(dt, *acc, *jerk); + *jerk = maxnewjerk; return res; } @@ -3470,7 +3488,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, if(mode == NULL) planner_type = 0; - if(planner_type != 1 && planner_type != 2){ + if(planner_type != 1){ // If the slowdown is not too great, use velocity ramping instead of trapezoidal velocity // Also, don't ramp up for parabolic blends if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { @@ -3739,8 +3757,21 @@ STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, 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"); + // In S-curve mode, use actual current velocity instead of expected term_vel + // S-curve can't change velocity instantly, term_vel is just desired value + if (GET_TRAJ_PLANNER_TYPE() == 1) { + nexttc->currentvel = tc->currentvel; + // Inherit acceleration, but limit to nexttc's allowed range + // Important for line-to-arc transitions where arc has lower tangential accel + double maxacc_next = tcGetTangentialMaxAccel(nexttc); + nexttc->currentacc = saturate(tc->currentacc, maxacc_next); + tp_debug_print("Doing tangent split (S-curve): vel=%f, acc=%f (limited by %f)\n", + nexttc->currentvel, nexttc->currentacc, maxacc_next); + } else { + // Trapezoidal: can use term_vel (assumes instant velocity change) + nexttc->currentvel = tc->term_vel; + tp_debug_print("Doing tangent split (trapezoidal): vel=%f\n", nexttc->currentvel); + } break; case TC_TERM_COND_PARABOLIC: break; @@ -3799,7 +3830,7 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, double target_vel_this = tpGetRealTargetVel(tp, tc); double target_vel_next = tpGetRealTargetVel(tp, nexttc); - if(mode != -5 && (GET_TRAJ_PLANNER_TYPE() == 1 || GET_TRAJ_PLANNER_TYPE() == 2)) + if(mode != -5 && GET_TRAJ_PLANNER_TYPE() == 1) tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); else tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); From 1dfbca000a097a7e65a2f083bbdca55fab4d12d9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Mon, 19 Jan 2026 19:49:11 +0800 Subject: [PATCH 2/7] Fix jerk calculation units in getStraightJerk() MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Use cubic root for characteristic time (t = cbrt(d/j)) and cube of time for path jerk (jerk = d/t³) to match jerk-limited motion physics. --- src/emc/task/emccanon.cc | 47 +++++++++++++++++++++------------------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/src/emc/task/emccanon.cc b/src/emc/task/emccanon.cc index f8c6cfb69ed..0df62f0d5e7 100644 --- a/src/emc/task/emccanon.cc +++ b/src/emc/task/emccanon.cc @@ -704,13 +704,16 @@ static double getStraightJerk(double x, double y, double z, } // Pure linear move: + // For jerk-limited motion: d = (1/6)*j*t³, so t = cbrt(6*d/j) + // We use t = cbrt(d/j) as a characteristic time (omitting the constant factor, + // which cancels out when we compute path jerk = dtot / tmax³) if (canon.cartesian_move && !canon.angular_move) { - tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; - ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; - tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; - tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; - tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; - tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; + tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; + ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; + tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; + tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; + tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; + tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; out.tmax = MAX3(tx, ty ,tz); out.tmax = MAX4(tu, tv, tw, out.tmax); @@ -720,38 +723,38 @@ static double getStraightJerk(double x, double y, double z, out.dtot = sqrt(du * du + dv * dv + dw * dw); if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax); + out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); } } // Pure angular move: else if (!canon.cartesian_move && canon.angular_move) { - ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; - tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; - tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; + ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; + tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; + tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; out.tmax = MAX3(ta, tb, tc); out.dtot = sqrt(da * da + db * db + dc * dc); if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax); + out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); } } // Combination angular and linear move: else if (canon.cartesian_move && canon.angular_move) { - tx = dx? (dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; - ty = dy? (dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; - tz = dz? (dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; - ta = da? (da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; - tb = db? (db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; - tc = dc? (dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; - tu = du? (du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; - tv = dv? (dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; - tw = dw? (dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; + tx = dx? cbrt(dx / FROM_EXT_LEN(emcAxisGetMaxJerk(0))): 0.0; + ty = dy? cbrt(dy / FROM_EXT_LEN(emcAxisGetMaxJerk(1))): 0.0; + tz = dz? cbrt(dz / FROM_EXT_LEN(emcAxisGetMaxJerk(2))): 0.0; + ta = da? cbrt(da / FROM_EXT_ANG(emcAxisGetMaxJerk(3))): 0.0; + tb = db? cbrt(db / FROM_EXT_ANG(emcAxisGetMaxJerk(4))): 0.0; + tc = dc? cbrt(dc / FROM_EXT_ANG(emcAxisGetMaxJerk(5))): 0.0; + tu = du? cbrt(du / FROM_EXT_LEN(emcAxisGetMaxJerk(6))): 0.0; + tv = dv? cbrt(dv / FROM_EXT_LEN(emcAxisGetMaxJerk(7))): 0.0; + tw = dw? cbrt(dw / FROM_EXT_LEN(emcAxisGetMaxJerk(8))): 0.0; out.tmax = MAX9(tx, ty, tz, ta, tb, tc, tu, tv, tw); if(debug_velacc) - printf("getStraightJerk t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", + printf("getStraightJerk t tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", tx, ty, tz, ta, tb, tc, tu, tv, tw); if(dx || dy || dz) @@ -760,7 +763,7 @@ static double getStraightJerk(double x, double y, double z, out.dtot = sqrt(du * du + dv * dv + dw * dw); if (out.tmax > 0.0) { - out.jerk = out.dtot / (out.tmax * out.tmax); + out.jerk = out.dtot / (out.tmax * out.tmax * out.tmax); } } //if(debug_velacc) From 267ac38faf6a589266f3f922a4d34d45d039f8c4 Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Sat, 17 Jan 2026 13:25:22 +0800 Subject: [PATCH 3/7] small fix max jerk limit --- src/emc/tp/tp.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index c5d3c89d052..a8ad7d19cc4 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1767,7 +1767,9 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); if(GET_TRAJ_PLANNER_TYPE() == 1){ double vs_back2; - if(findSCurveVSpeedWithEndSpeed(tc->target * 2.0 , tc->finalvel, acc_this, emcmotStatus->jerk, &vs_back2) == 1) + // Use clamped jerk to match execution phase behavior + double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); + if(findSCurveVSpeedWithEndSpeed(tc->target * 2.0 , tc->finalvel, acc_this, maxjerk, &vs_back2) == 1) vs_back = vs_back2; } // Find the reachable velocity of prev1_tc, moving forwards in time From 1e2e75dd774fc9eac464de62669ace11fbd46132 Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Wed, 21 Jan 2026 17:21:41 +0800 Subject: [PATCH 4/7] Add validation for planner type and improve S-curve deceleration logic This commit: Validates PLANNER_TYPE in INI files and motion commands to only accept 0 (trapezoidal) or 1 (S-curve), defaulting to 0 for invalid values Improves S-curve lookahead calculation in tpComputeOptimalVelocity() to use proper S-curve kinematics instead of trapezoidal formulas Adds named constants for tpCalculateSCurveAccel() return values for better code clarity Removes incorrect position error correction logic in tcUpdateDistFromSCurveAccel() Clarifies deceleration condition comments and logic flow The changes fix edge cases in S-curve mode and make the planner type handling more robust. --- src/emc/ini/inihal.cc | 7 ++++- src/emc/ini/initraj.cc | 4 +++ src/emc/motion/command.c | 7 ++++- src/emc/tp/sp_scurve.h | 11 ++++++++ src/emc/tp/tp.c | 59 ++++++++++++++++++++++------------------ 5 files changed, 59 insertions(+), 29 deletions(-) diff --git a/src/emc/ini/inihal.cc b/src/emc/ini/inihal.cc index 93ebf6ceb9b..4825bb08bac 100644 --- a/src/emc/ini/inihal.cc +++ b/src/emc/ini/inihal.cc @@ -297,7 +297,12 @@ int check_ini_hal_items(int numjoints) if (CHANGED(traj_planner_type)) { if (debug) SHOW_CHANGE_INT(traj_planner_type) UPDATE(traj_planner_type); - if (0 != emcTrajPlannerType(NEW(traj_planner_type))) { + // Only 0 and 1 are supported, set to 0 if invalid + int planner_type = NEW(traj_planner_type); + if (planner_type != 0 && planner_type != 1) { + planner_type = 0; + } + if (0 != emcTrajPlannerType(planner_type)) { if (emc_debug & EMC_DEBUG_CONFIG) { rcs_print("check_ini_hal_items:bad return value from emcTrajPlannerType\n"); } diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index 76c5404f0e8..3cd2de52c2a 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -225,6 +225,10 @@ static int loadTraj(EmcIniFile *trajInifile) old_inihal_data.traj_default_jerk = jerk; planner_type = 0; // Default: 0 = trapezoidal, 1 = S-curve trajInifile->Find(&planner_type, "PLANNER_TYPE", "TRAJ"); + // Only 0 and 1 are supported, set to 0 if invalid + if (planner_type != 0 && planner_type != 1) { + planner_type = 0; + } if (0 != emcTrajPlannerType(planner_type)) { if (emc_debug & EMC_DEBUG_CONFIG) { rcs_print("bad return value from emcTrajPlannerType\n"); diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 9d4d30367e2..2d50b6db80f 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -1194,7 +1194,12 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) /* set the type of planner: 0 = trapezoidal, 1 = S-curve */ /* can do it at any time */ rtapi_print_msg(RTAPI_MSG_DBG, "SET_PLANNER_TYPE, type(%d)", emcmotCommand->planner_type); - emcmotStatus->planner_type = emcmotCommand->planner_type; + // Only 0 and 1 are supported, set to 0 if invalid + if (emcmotCommand->planner_type != 0 && emcmotCommand->planner_type != 1) { + emcmotStatus->planner_type = 0; + } else { + emcmotStatus->planner_type = emcmotCommand->planner_type; + } break; case EMCMOT_PAUSE: diff --git a/src/emc/tp/sp_scurve.h b/src/emc/tp/sp_scurve.h index 38cbb669a19..c12c3d87c6e 100644 --- a/src/emc/tp/sp_scurve.h +++ b/src/emc/tp/sp_scurve.h @@ -47,4 +47,15 @@ int findSCurveVSpeedConservative(double distance, double Ve, double maxA, double double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2); double calcSCurveSpeedWithT(double amax, double jerk, double T); +/** + * tpCalculateSCurveAccel return value definitions + * + * TP_SCURVE_ACCEL_ERROR - calculation failed (maxjerk invalid or less than/equal to 1) + * TP_SCURVE_ACCEL_ACCEL - acceleration or normal state (no deceleration needed) + * TP_SCURVE_ACCEL_DECEL - deceleration needed + */ +#define TP_SCURVE_ACCEL_ERROR -5 +#define TP_SCURVE_ACCEL_ACCEL 0 +#define TP_SCURVE_ACCEL_DECEL 1 + #endif \ No newline at end of file diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index a8ad7d19cc4..c68ea8dcf2a 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -1764,13 +1764,20 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons 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); + double vs_back; if(GET_TRAJ_PLANNER_TYPE() == 1){ - double vs_back2; - // Use clamped jerk to match execution phase behavior + // S-curve mode: use S-curve kinematics calculations + // Starting from finalvel, maximum starting speed achievable within distance tc->target + // During actual execution, tpCalculateSCurveAccel will adjust dynamically based on current state (including current acceleration) + // Use minimum of segment's max jerk and system max jerk to ensure limits are not exceeded double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); - if(findSCurveVSpeedWithEndSpeed(tc->target * 2.0 , tc->finalvel, acc_this, maxjerk, &vs_back2) == 1) - vs_back = vs_back2; + if(findSCurveVSpeedWithEndSpeed(tc->target, tc->finalvel, acc_this, maxjerk, &vs_back) != 1){ + // S-curve calculation failed, use conservative estimate (at least maintain finalvel) + vs_back = tc->finalvel; + } + } else { + // Trapezoidal acceleration/deceleration mode: use trapezoidal kinematics formula + vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); } // Find the reachable velocity of prev1_tc, moving forwards in time @@ -2713,7 +2720,7 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, /** * Calculate distance update from velocity and acceleration. */ -STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double jerk, double vel_desired, double perror, int reverse_run, int dec) +STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double jerk, double vel_desired, double perror __attribute__((unused)), int reverse_run, int dec) { // If the resulting velocity is less than zero, than we're done. This // causes a small overshoot, but in practice it is very small. @@ -2733,24 +2740,20 @@ STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double j tc->progress = tcGetTarget(tc,reverse_run); } } else { - if(dx < 1e-6){ + if(dx < TP_POS_EPSILON){ tc->progress = tcGetTarget(tc,reverse_run); }else{ // sc_distance(double t, double v, double a, double j); - double displacement = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, jerk); + double displacement = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, jerk); // Account for reverse run (flip sign if need be) double disp_sign = reverse_run ? -1 : 1; - if(perror > 0 && tc->last_move_length >= perror && displacement <= perror){ - tc->progress += (disp_sign * perror); - tc->last_move_length = perror; - //printf("TRY TO FIX ERROR: %.15f \n", perror); - return TP_ERR_OK; - } + // Update position using calculated displacement + // Note: perror is actually margin (dx - dlen1), not position error, + // so it should not be used for position correction tc->last_move_length = displacement; - tc->progress += (disp_sign * displacement); - + //Progress has to be within the allowable range tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); } @@ -2791,7 +2794,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ maxjerk = 1; //rtapi_print_msg(RTAPI_MSG_ERR, // "ERROR!!! maxjerk Is less than 1\n"); - return -5; + return TP_SCURVE_ACCEL_ERROR; } // Find maximum allowed velocity from feed and machine limits @@ -2810,9 +2813,9 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ *acc = tc->currentacc; *vel_desired = tc->currentvel; *jerk =0;//(maxnewaccel - tc->currentacc) / dt; - return 0; + return TP_SCURVE_ACCEL_ACCEL; } - int res = 1; + int res = TP_SCURVE_ACCEL_DECEL; nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_target_vel, maxaccel, maxjerk, &req_v, &req_a, &req_j); @@ -2837,23 +2840,24 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ // Handle special case: position overshoot or reached endpoint if (dx <= TP_POS_EPSILON) { + // Already reached or exceeded endpoint, must decelerate/stop nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_finalvel, maxaccel, maxjerk, &req_v, &req_a, &req_j); - res = 1; + res = TP_SCURVE_ACCEL_DECEL; } - else if(tc->currentvel < 1e-6 && dx > TP_POS_EPSILON && dx < 1e-4){ + else if(tc->currentvel < TP_VEL_EPSILON && dx > TP_POS_EPSILON && dx < 1e-4){ // Very low velocity and close to target - res = 1; + res = TP_SCURVE_ACCEL_DECEL; }else{ // Decel conditions: - // 1. margin <= 0: decel distance >= remaining distance - // 2. dx - moveL <= dlen2: if we continue accel, next cycle remaining <= decel dist + // Condition 1: margin <= 0 - decel distance already >= remaining distance, must decelerate + // Condition 2: dx - moveL <= dlen2 - if we continue accel, next cycle remaining <= decel dist int need_decel = (margin <= TP_POS_EPSILON) || (dx - moveL <= dlen2); if (need_decel){ nextSpeed(tc->currentvel, tc->currentacc, tc->cycle_time, tc_finalvel, maxaccel, maxjerk, &req_v, &req_a, &req_j); }else{ if(tc_finalvel > TP_VEL_EPSILON || tc_target_vel > TP_VEL_EPSILON) - res = 0; + res = TP_SCURVE_ACCEL_ACCEL; } } @@ -3510,7 +3514,8 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, if(*mode == 1){ tc->cycle_time = tp->cycleTime; int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1); - if(is_dec == -5){ //If the calculation fails, revert to T-shaped acceleration/deceleration. + + if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. *mode = -5; res_accel = 1; acc=0, vel_desired=0; @@ -3527,7 +3532,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, } }else{ int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0); - if(is_dec == -5){ //If the calculation fails, revert to T-shaped acceleration/deceleration. + if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. *mode = -5; res_accel = 1; acc=0, vel_desired=0; From c25592a6c3931508c0ad64e82a7373419177b16a Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Wed, 21 Jan 2026 11:23:53 +0800 Subject: [PATCH 5/7] sp_scurve header fix --- src/emc/tp/sp_scurve.h | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/src/emc/tp/sp_scurve.h b/src/emc/tp/sp_scurve.h index c12c3d87c6e..a4db73fdb14 100644 --- a/src/emc/tp/sp_scurve.h +++ b/src/emc/tp/sp_scurve.h @@ -1,13 +1,3 @@ -/* - * Author: 杨阳 - * Date: 2020-06-05 22:32:44 - * LastEditors: 杨阳 - * LastEditTime: 2020-07-25 10:47:58 - * Contact: mika-net@outlook.com - * Copyright (c) 2017-2020: 杨阳 - * FilePath: \pomelo\PROGRAM\linuxcnc\src\emc\tp\sp_scurve.h - * - */ /******************************************************************** * Description: sp_scurve.h * Discriminate-based trajectory planning @@ -15,6 +5,7 @@ * Derived from a work by 杨阳 * * Author: 杨阳 +* Contact: mika-net@outlook.com * License: GPL Version 2 * System: Linux * From c4f68287a1f2d3fff5c113b4288df942b1d61d8d Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Tue, 27 Jan 2026 12:28:44 +0800 Subject: [PATCH 6/7] Remove obsolete S-curve functions and improve displacement integration fixed bug #3720 and #3719 - Remove unused functions: getPhase, getTargetV, getNextPoint, getStoppingDist, findSCurveVSpeedConservative - Rename velocity() to delta_velocity() and acceleration() to delta_accel() for clarity - Replace analytical S-curve displacement with trapezoidal integration for stability - Add trapz_distance() helper function - Add currentjerk field to TC_STRUCT --- src/emc/tp/sp_scurve.c | 473 ++--------------------------------------- src/emc/tp/sp_scurve.h | 10 +- src/emc/tp/tc_types.h | 1 + src/emc/tp/tp.c | 101 ++++----- 4 files changed, 67 insertions(+), 518 deletions(-) diff --git a/src/emc/tp/sp_scurve.c b/src/emc/tp/sp_scurve.c index 77a7f487dc0..f2f1a7a3994 100644 --- a/src/emc/tp/sp_scurve.c +++ b/src/emc/tp/sp_scurve.c @@ -20,30 +20,6 @@ #include #endif -unsigned getPhase(double v, double a, double j) { - if (!v) return 0; - // Handle negative velocity - //double v = this->v; - //double a = this->a; - if (v < 0) { - v = -v; - a = -a; - } - - if (0 < a) { - if (0 < j) return 1; - if (!j) return 2; - return 3; - } - - if (!a) return 4; - if (j < 0) return 5; - if (!j) return 6; - - return 7; -} - - /* a cubic coefficient b quadratic coefficient @@ -184,300 +160,6 @@ int solve_cubic(double a, double b, double c, double d, double res[3], int* len) } -/* - Continuous form - PT = P0 + V0T + 1/2A0T2 + 1/6JT3 - VT = PT' = V0 + A0T + 1/2 JT2 - AT = VT' = PT'' = A0 + JT - - Discrete time form (let T be 1, then T^2 == 1, T^3 == 1) - PT = PT + VT + 1/2AT + 1/6J - VT = VT + AT + 1/2JT - AT = AT + JT - */ -int getTargetV(double distence, double v, double a, double period, double maxV, double maxA, double maxJ, double* req_v, double* req_a){ - if(distence == 0) return 0; - double T1 = maxA / maxJ; - double V1 = maxJ * T1 * T1 / 2; - int phase = 0; - //double tT = 0; - - if(V1 >= maxV / 2){ // Handle case where jerk is small, causing T2 and T6 segments to not exist - T1 = sqrt(maxV / maxJ); - } - - double S1 = maxJ * T1 * T1 * T1 / 6.0; - double S2 = S1; - double S3 = S2; - double V2 = V1; - double T2 = 0; - double T3 = 0; - double needTime = 0; - - if (distence < 0) { // Due to symmetry, only consider one case - distence = -distence; - v = -v; - a = -a; - } - - double calc_v = 0; - double calc_a = a; - - if(distence <= S1){ - double t1; - //rtai - t1 = pow(6.0 * distence / maxJ, 1.0/3.0); - //uspace - //t1 = cbrt(6.0 * distence / maxJ) ; - - - //if(t1 -period > 0) - // t1 = t1 - period; - - - calc_a = maxJ * t1; - calc_v = (calc_a * t1) / 2; - //tT = t1; - - phase = 1; - - //double p = calc_v * period; - //double cp = calc_v * t1 + maxJ * t1 * t1 * t1 / 6; - //double cpNext = calc_v * (t1 - period) + maxJ * (t1 - period) * (t1 - period) * (t1 - period) / 6; - //if(t1 - period > 0) - // calc_v = (distence - cpNext) / period; - needTime = t1; - //printf("S1 | D: %.10f | CP: %f | CNP: %f | P: %f | T: %f | J: %f | V: %f | calc_a: %f | RV: %f | t1: %f \n", distence, cp, cpNext, p, period, maxJ, calc_v, calc_a, v, t1); - } - - if(phase == 0){ - if(V1 < maxV / 2){ // T2 and T6 segments exist - T2 = (maxV / maxA) - T1; - S2 = S1 + V1 * T2 + maxJ * T1 * T2 * T2 / 2; - V2 = V1 + maxA * T2; - if(distence < S2){ - double t2; - double A = maxJ * T1 / 2; - double B = V1; - double C = S1 - distence; - double dt = sqrt(B * B - 4 * A * C); - t2 = (-B + dt) / (2 * A); - //printf("In S2 segment "); - //t2 = t2 - period; - //if(t2 < 0){ - //calc_v = V1; - // calc_a = maxJ * t2; - // calc_v = (calc_a * t2) / 2; - //}else{ - //Vt = Vs + J * T1^2 - 0.5 * J * (t - 2 * T1) ^2 - calc_a = maxA; - calc_v = V1 + maxA * t2; - //} - //tT = t2; - phase = 2; - - //double p = calc_v * period; - //printf("P: %f, Ov: %f Nv: %f Oa: %f, Na: %f ", p * 1000, v, calc_v, a, calc_a); - - //double p = calc_v * period; - //t2 = t2 - period; - // S2 = S1 + V1 * T2 + maxJ * T1 * T2 * T2 / 2; - //double cp = S1 + V1 * t2 + 0.5 * maxJ * T1 * t2 * t2; - //double cpNext = S1 + V1 * (t2 - period) + 0.5 * maxJ * T1 * (t2 - period) * (t2 - period); - needTime = T1 + t2; - //printf("S2 | D: %.10f | CP: %f | CNP: %f | P: %f | T: %f | J: %f | V: %f | calc_a: %f | RV: %f | t2: %f \n", distence, cp, cpNext, p, period, maxJ, calc_v, calc_a, v, T1 + t2); - } - - } - } - - if(phase == 0){ - S3 = S2 + V2 * T1 + S1 * 2; - T3 = T1; - if(distence <= S3){ // Solving cubic equation is expensive - double A = - maxJ / 6; - double B = maxJ * T1 / 2; - double C = V2; - double D = S2 - distence; - double t3 = 0;//solute(A,B,C,D,T2); - //double x1, x2, x3; - //int n; - //x1 = x2 = x3 = 0; - //n = solve_cubic(A, B, C, D, &x1, &x2, &x3); - //double temp; - double xo[10]; - int len; - int i = 0; - solve_cubic(A, B, C, D, xo, &len); - if(len > 0){ - t3 = xo[0]; - for (; i < len; i++) - { - if(i == 0)continue; - t3 = fmax(xo[i], t3); - } - } - //t3 = fmax(fmax(xo[0], x2), x3); - if(t3 < 0){ - t3 = 0.001; - } - //if(x1>x2) temp=x1,x1=x2,x2=temp; - //if(x2>x3) temp=x2,x2=x3,x3=temp; - //if(x1>x2) temp=x1,x1=x2,x2=temp; - //if(x1 > 0) t3 = x1; - //else if(x2 > 0) t3 = x2; - //else if(x3 > 0) t3 = x3; - //else t3 = 0.001; - //printf("S3 t3: %f n: %d x1: %f x2: %f x3: %f A: %f B: %f C: %f D: %f\n" , t3, n, x1, x2, x3, A, B, C, D); - //JT1 - J ( t - T1 ) - //t3 = t3 - period; - //if(t3 <= 0 ){ - // calc_a = maxA; - // calc_v = V1 + maxA * (T2 + t3); - // }else{ - calc_v = V2 + maxA * t3 - 0.5 * maxJ * t3 * t3; - calc_a = maxA - maxJ * t3; - //} - //tT = t3; - - //double p = calc_v * period; - //t3 = t3 - period; - //double cp = 0; - //if(t3 < 0){ - - //}else{ - //cp = S2 + calc_v * t3 + 0.5 * maxJ * T1 * t3* t3 - maxJ * t3 * t3 * t3 / 6; - //double cpNext = S2 + calc_v * (t3 - period) + 0.5 * maxJ * T1 * (t3 - period) * (t3 - period) - maxJ * (t3 - period) * (t3 - period) * (t3 - period) / 6; - //(cp - distence) / period; - //} - //double rp = - //printf("P: %f, Ov: %f Nv: %f Oa: %f, Na: %f ", p * 1000, v, calc_v, a, calc_a); - needTime = T1 + T2 + t3; - //printf("S3 | D: %.10f | CP: %f | CNP: %f | P: %f | T: %f | J: %f | T1: %f | V: %f | calc_a: %f | RV: %f | t3: %f \n", distence, cp, cpNext, p, period, maxJ, T1, calc_v, calc_a, v, T1 + T2 + t3); - phase = 3; - } - } - //printf("In constant velocity phase "); - // Need to enter S3 early by one period - - //*req_v = calc_v; - if(phase == 0) - phase = 4; - - // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 - // VT = V0 + A0 * T + J * T^2 /2 - // AT = A0 + J * T - // After discretization: - // PT = PT + VT + 0.5 * AT + J / 6 - // VT = VT + AT + 0.5 * J * T - // AT = AT + J * T - - if(phase == 4){// - if(v < maxV){ // Need to accelerate to target speed - double tt = fabs(a / maxJ); - double ve = v + (a - maxJ * period) * tt - maxJ * tt * tt / 2; - // a * t + 1/2 * j * t^2 - if(ve >= maxV ){ - phase = 5; - calc_a = a - maxJ * period; - } - else{ - phase = 7; - calc_a = a + maxJ * period; - } - - if(calc_a >= maxA){ - calc_a = maxA; - phase = 6; - } - //double p = 0; - // VT = V0 + A0 * T + J * T^2 /2 - // AT = A0 + J * T - if(phase == 7){ - //p = v * period + 0.5 * calc_a * period * period + maxJ * period * period* period / 6; - calc_v = v + calc_a * period + maxJ * period * period / 2; - } - else if(phase == 6){ - if(a != maxA){ - double J = (maxA - a) / period; - calc_v = v + maxA * period + J * period * period / 2; - }else - calc_v = v + calc_a * period; - } - else if(phase == 5){ // Need compensation; after discretization each segment is s = v * t, so distance may be more reliable - // V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 - //p = v * period + 0.5 * calc_a * period * period - maxJ * period * period* period / 6; - calc_v = v + calc_a * period - maxJ * period * period / 2; - } - - if(calc_v >= maxV){ - calc_v = maxV; - } - //printf("VE: %f, OV: %f NV: %f P: %f OA: %f, NA: %f ", ve, v, calc_v, p * 1000, a, calc_a); - }else if(v > maxV){// Need to decelerate to target speed; triangular acceleration algorithm used here; entered when velocity is modified - double tt = fabs(a) / maxJ; - double ve = v + a * tt + maxJ * tt * tt / 2; - if(ve <= maxV) { - phase = 8; - calc_a = a + maxJ * period; - } - else{ - phase = 9; - calc_a = a - maxJ * period; - } - - if(calc_a < -maxA){ - calc_a = -maxA; - } - - calc_v = v + calc_a * period - maxJ * period * period / 2; - if(calc_v < maxV){ - calc_v = maxV; - } - }else{ // Constant velocity motion, forced - phase = 4; - calc_v = maxV; - } - }else{ // This section is intended for optimizing the stopping segment, very important, may be used to smooth the curve - //if(a) - - if(needTime == 0){ // Constant velocity or acceleration phase - // Decelerate a few periods early - }else if(needTime > T1 + T2 && needTime <= T1 + T2 +T3){ // Handle S3 segment - - }else if(needTime > T1 && needTime <= T1 + T2){ // Handle S2 segment - - }else if(needTime >= 0 && needTime <= T1){ // Handle S1 segment - - } - - - } - - - if(calc_v > maxV) - calc_v = maxV; - *req_a = calc_a; - //*req_a = (calc_v - v) / period; - -#define MAX_A_OVERRIDE 1.5 - double ra = (calc_v - v) / period; - if(ra > maxA * MAX_A_OVERRIDE){ // This is not an ideal solution - *req_a = maxA* MAX_A_OVERRIDE; - calc_v = v + *req_a * period; - }else if(ra < -maxA * MAX_A_OVERRIDE){ - *req_a = -maxA * MAX_A_OVERRIDE; - calc_v = v + *req_a * period; - } -#undef MAX_A_OVERRIDE - - *req_v = calc_v; - //if(phase == 1 || phase == 2 || phase == 3) - // printf("S%d L: %.10f, ACC %f V: %f *req_v: %f v: %f S3: %f A: %f CA: %f\n", phase, distence , *req_a, calc_v, *req_v, v, S3, (calc_v - v) / period, ov); - return phase; -} - - // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 // VT = V0 + A0 * T + J * T^2 /2 // AT = A0 + J * T @@ -778,66 +460,6 @@ tp->prograss = 0; return 0; } -double getNextPoint(simple_tp_t *tp, int n, double T, double* req_v, double* req_a){ - double cal_v; - double cal_a; - int phase = 0; - if(n <= tp->n0){ // S0 segment - cal_v = tp->vc + tp->jm * T * T * n * (n + 1) / 2; - cal_a = n * tp->jm * T; - phase = 0; - }else if(tp->n1 != 0 && n <= tp->n0 + tp->n1){ // S1 segment - cal_v = tp->v1 + (n - tp->n0) * tp->a1 * T; - cal_a = tp->a1; - phase = 1; - }else if(n <= tp->n0 + tp->n1 + tp->n2){ // S2 segment - //Vm - ∑(k * J1 * Tp^2) - double x = tp->n0 + tp->n1 + tp->n2 - n; - cal_v = tp->v3 - fabs(tp->j2) * T * T * x * (x + 1) / 2; - cal_a = tp->a3 - x * fabs(tp->j2) * T; - phase = 2; - }else if(n <= tp->n0 + tp->n1 + tp->n2 + tp->n3){ // S3 segment - cal_v = tp->vm; - cal_a = 0; - phase = 3; - }else if(n <= tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4){ // S4 segment - double x = n - (tp->n0 + tp->n1 + tp->n2 + tp->n3) ; - cal_v = tp->vm - tp->jm * T * T * x * (x + 1) / 2; - cal_a = - x * tp->jm * T; - phase = 4; - }else if(tp->n5 != 0 &&n <= tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5){ - phase = 5; - double x = n - (tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4); - cal_v = tp->v5 + x * tp->a6 * T; - cal_a = tp->a6; - }else if(n <= tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6){ - phase = 6; - double x = tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6 - n; - cal_v = tp->v7 + fabs(tp->j4) * T * T * x * (x + 1) / 2.0; - cal_a = tp->a6 - x * fabs(tp->j4) * T; - }else{ - if( tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6 == n + 1 ){ - tp->curr_n++ ; - phase = 7; - cal_v = tp->ve; - cal_a = tp->a7; - } - return 0; - } - if(phase > 3 && tp->fix_verr == 0 && cal_v < tp->verr){ - tp->fix_verr = 1; // Insert additional residual velocity - cal_v = tp->verr; - //printf("ver: %f | acc: %f | n: %d | phase: %d | prograss: %.10f F\n", cal_v, cal_a, n, phase, tp->prograss); - }else{ - tp->curr_n++ ; - //printf("ver: %f | acc: %f | n: %d | phase: %d | prograss: %.10f TN: %d\n", cal_v, cal_a, n, phase, tp->prograss, tp->n0 + tp->n1 + tp->n2 + tp->n3 + tp->n4 + tp->n5 + tp->n6); - } - tp->prograss += cal_v * T; - *req_v = cal_v; - *req_a = cal_a; - return 0; -} - // PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 // VT = V0 + A0 * T + J * T^2 /2 @@ -895,15 +517,6 @@ double nextSpeed(double v, double a, double t, double targetV, double maxA, doub return v; } -double getStoppingDist(simple_tp_t *tp) { - double maxJ = tp->max_jerk ; - double v = tp->curr_vel; - double a = tp->curr_acc; - double maxA = tp->max_acc; - //double phase; - return stoppingDist(v, a, maxA, maxJ/*, &phase*/); -} - double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/) { // Already stopped //*phase = 0; @@ -924,7 +537,7 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/ // velocity => a * t + 1/2 * j * t^2 double t = a / maxJ; d += sc_distance(t, v, a, -maxJ); - v += velocity(t, a, -maxJ); + v += delta_velocity(t, a, -maxJ); a = 0; //if(*phase == 0) *phase = 3; } @@ -955,7 +568,7 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/ if (maxDeccel < a) { double t = (a - maxDeccel) / maxJ; d += sc_distance(t, v, a, -maxJ); - v += velocity(t, a, -maxJ); + v += delta_velocity(t, a, -maxJ); a = maxDeccel; } @@ -971,7 +584,7 @@ double stoppingDist(double v, double a, double maxA, double maxJ/*, int* phase*/ // velocity => a * t + 1/2 * j * t^2 double t = (v - deltaV) / -a; d += sc_distance(t, v, a, 0); - v += velocity(t, a, 0); + v += delta_velocity(t, a, 0); //if(*phase == 0) *phase = 6; } @@ -1003,7 +616,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma if (a < 0) { double t = -a / maxJ; d += sc_distance(t, v, a, maxJ); - v += velocity(t, a, maxJ); + v += delta_velocity(t, a, maxJ); a = 0; } @@ -1018,7 +631,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma if (maxAccel > a) { double t = (maxAccel - a) / maxJ; d += sc_distance(t, v, a, maxJ); - v += velocity(t, a, maxJ); + v += delta_velocity(t, a, maxJ); a = maxAccel; } @@ -1029,7 +642,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma if (v < deltaV && a > 0.0001) { double t = (deltaV - v) / a; d += sc_distance(t, v, a, 0); - v += velocity(t, a, 0); + v += delta_velocity(t, a, 0); } // Phase 4: Decrease a from maxAccel to 0, velocity reaches ve @@ -1047,7 +660,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma if (a > 0) { double t = a / maxJ; d += sc_distance(t, v, a, -maxJ); - v += velocity(t, a, -maxJ); + v += delta_velocity(t, a, -maxJ); a = 0; } @@ -1062,7 +675,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma if (maxDeccel < a) { double t = (a - maxDeccel) / maxJ; d += sc_distance(t, v, a, -maxJ); - v += velocity(t, a, -maxJ); + v += delta_velocity(t, a, -maxJ); a = maxDeccel; } @@ -1074,7 +687,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma if (deltaV < v && a < -0.0001) { double t = (v - deltaV) / (-a); d += sc_distance(t, v, a, 0); - v += velocity(t, a, 0); + v += delta_velocity(t, a, 0); } // Phase 4: Increase a from maxDeccel to 0, velocity reaches ve @@ -1093,7 +706,7 @@ double finishWithSpeedDist(double v, double ve, double a, double maxA, double ma double nextAccel(double t, double targetV, double v, double a, double maxA, double maxJ) { int increasing = v < targetV; - double deltaA = acceleration(t, maxJ); + double deltaA = delta_accel(t, maxJ); if (increasing && a < -deltaA) return a + deltaA; // negative accel, increasing speed @@ -1143,7 +756,7 @@ double nextAccel(double t, double targetV, double v, double a, double maxA, double nextAccel(double t, double targetV, double v, double a, double maxA, double maxJ) { double max_da, tiny_da, vel_err, acc_req; - max_da = acceleration(t, maxJ); + max_da = delta_accel(t, maxJ); tiny_da = max_da * t * 0.001; vel_err = targetV - v; if (vel_err > tiny_da){ @@ -1173,18 +786,27 @@ double nextAccel(double t, double targetV, double v, double a, double maxA, } double sc_distance(double t, double v, double a, double j) { - // v * t + 1/2 * a * t^2 + 1/6 * j * t^3 + // P = v*t + (1/2)*a*t² + (1/6)*j*t³ return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t)); } +/** + * Trapezoidal integration for displacement calculation + * Uses average jerk assumption for more stable integration + */ +double trapz_distance(double t, double v, double a, double j) { + // v * t + 1/2 * a * t^2 + 1/4 * j * t^3 + return t * (v + t * (0.5 * a + 0.25 * j * t)); +} -double velocity(double t, double a, double j) { + +double delta_velocity(double t, double a, double j) { // a * t + 1/2 * j * t^2 return t * (a + 0.5 * j * t); } -double acceleration(double t, double j) {return j * t;} +double delta_accel(double t, double j) {return j * t;} #include "tp_types.h" @@ -1377,53 +999,4 @@ double calcSCurveSpeedWithT(double amax, double jerk, double T) { // Use fma to optimize multiply-add operations, improving numerical stability return fma(jerk * T1, T1 + T2, 0.0); -} - -/** - * Conservative S-curve reachable velocity calculation (for lookahead optimization) - * - * Unlike findSCurveVSpeedWithEndSpeed which assumes decel starts from a=0, - * this function considers worst case: decel starting from a=maxA state. - * - * This ensures lookahead-computed velocities can smoothly decel to target - * regardless of current acc state, avoiding acc discontinuities. - * - * @param distance Available decel distance - * @param Ve Target end velocity - * @param maxA Maximum acceleration - * @param maxJ Maximum jerk - * @param req_v [output] Computed conservative reachable velocity - * @return 1 success, -1 failure - */ -int findSCurveVSpeedConservative(double distance, double Ve, double maxA, double maxJ, double* req_v) { - if (distance <= 0 || maxA <= 0 || maxJ <= 0) { - *req_v = Ve; - return -1; - } - - // Extra distance needed to bring a from maxA to 0 (velocity still increasing during this) - // t = maxA / maxJ, v_increase = 0.5 * maxA^2 / maxJ - double v_increase = 0.5 * maxA * maxA / maxJ; - double d_extra = maxA * maxA / (2.0 * maxJ); - - // Effective decel distance = total - extra - double effective_distance = distance - d_extra; - - if (effective_distance <= 0) { - *req_v = Ve; - return -1; - } - - double vs_from_zero; - int res = findSCurveVSpeedWithEndSpeed(effective_distance, Ve, maxA, maxJ, &vs_from_zero); - - if (res != 1) { - *req_v = Ve; - return -1; - } - - // Conservative estimate: subtract velocity increase during acc ramp-down - *req_v = fmax(vs_from_zero - v_increase * 0.5, Ve); - - return 1; } \ No newline at end of file diff --git a/src/emc/tp/sp_scurve.h b/src/emc/tp/sp_scurve.h index a4db73fdb14..20187db1b8c 100644 --- a/src/emc/tp/sp_scurve.h +++ b/src/emc/tp/sp_scurve.h @@ -21,20 +21,16 @@ double nextAccel(double t, double targetV, double v, double a, double maxA, double maxJ); double sc_distance(double t, double v, double a, double j); -double velocity(double t, double a, double j); -double acceleration(double t, double j); -unsigned getPhase(double v, double a, double j); +double trapz_distance(double t, double v, double a, double j); +double delta_velocity(double t, double a, double j); +double delta_accel(double t, double j); double nextSpeed(double v, double a, double t, double targetV, double maxA, double maxJ, double* req_v, double* req_a, double* req_j); -double getStoppingDist(simple_tp_t *tp) ; double stoppingDist(double v, double a, double maxA, double maxJ) ; double finishWithSpeedDist(double v, double ve, double a, double maxA, double maxJ) ; -int getTargetV(double distence, double v, double a, double period, double maxV, double maxA, double maxJ, double* req_v, double* req_a); int getNext( simple_tp_t *tp, double Vs, double Ve, double period); -double getNextPoint(simple_tp_t *tp, int n, double T, double* req_v, double* req_a); int findSCurveVSpeed(double distence,/* double maxV, */double maxA, double maxJ, double *req_v); int findSCurveVSpeedWithEndSpeed(double distence, double Ve, double maxA, double maxJ, double* req_v); -int findSCurveVSpeedConservative(double distance, double Ve, double maxA, double maxJ, double* req_v); double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2); double calcSCurveSpeedWithT(double amax, double jerk, double T); diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 492cf253b70..90c62041cd7 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -141,6 +141,7 @@ typedef struct { //S-curve jerk limiting double maxjerk; // max jerk for S-curve motion double currentacc; // current acceleration for S-curve planning + double currentjerk; // current jerk for S-curve planning double initialvel; // initial velocity when segment activated int accel_phase; // current phase of S-curve acceleration double elapsed_time; // time elapsed since segment activation diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index c68ea8dcf2a..5454152c7e3 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -353,29 +353,6 @@ STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, return fmin(tc->finalvel, v_target); } -/** - * Get acceleration for a tc based on the trajectory planner state. - */ -STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp __attribute__((unused)), - TC_STRUCT const * const tc) { - 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) { - a_scale *= 0.5; - } - else { - a_scale *= 8.0/15.0; - } - 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; -} - /** * Convert the 2-part spindle position and sign to a signed double. */ @@ -2481,7 +2458,7 @@ STATIC int tpComputeBlendSCurveVelocity( double tblend_vel; /* 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 && (maxjerk > 0 || maxjerk < 99999)) { + 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); @@ -2722,47 +2699,49 @@ STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, */ STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double jerk, double vel_desired, double perror __attribute__((unused)), int reverse_run, int dec) { - // If the resulting velocity is less than zero, than we're done. This - // causes a small overshoot, but in practice it is very small. double v_next = vel_desired; + double dx = tcGetDistanceToGo(tc, reverse_run); - // update position in this tc using trapezoidal integration - // Note that progress can be greater than the target after this step. - double dx = tcGetDistanceToGo(tc,reverse_run) ; if (v_next < 0.0) { v_next = 0.0; - //KLUDGE: the trapezoidal planner undershoots by half a cycle time, so - //forcing the endpoint here is necessary. However, velocity undershoot - //also occurs during pausing and stopping, which can happen far from - //the end. If we could "cruise" to the endpoint within a cycle at our - //current speed, then assume that we want to be at the end. - if (dx < (fabs(tc->currentvel) * tc->cycle_time)) { - tc->progress = tcGetTarget(tc,reverse_run); - } - } else { - if(dx < TP_POS_EPSILON){ - tc->progress = tcGetTarget(tc,reverse_run); - }else{ - // sc_distance(double t, double v, double a, double j); - double displacement = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, jerk); - // Account for reverse run (flip sign if need be) - double disp_sign = reverse_run ? -1 : 1; - - // Update position using calculated displacement - // Note: perror is actually margin (dx - dlen1), not position error, - // so it should not be used for position correction - tc->last_move_length = displacement; - tc->progress += (disp_sign * displacement); + } - //Progress has to be within the allowable range - tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); + if (dx < TP_POS_EPSILON) { + tc->progress = tcGetTarget(tc, reverse_run); + tc->last_move_length = dx; + if (dec) { + v_next = 0.0; + acc = 0.0; + jerk = 0.0; } } + else if (v_next < TP_VEL_EPSILON && tc->currentvel < TP_VEL_EPSILON && dx < 1e-4) { + tc->progress = tcGetTarget(tc, reverse_run); + tc->last_move_length = dx; + if (dec) { + v_next = 0.0; + acc = 0.0; + jerk = 0.0; + } + } + else { + // Use trapezoidal integration for displacement calculation + // displacement = (v_current + v_next) * dt / 2 + double displacement = (tc->currentvel + v_next) * tc->cycle_time / 2.0; + + double disp_sign = reverse_run ? -1 : 1; + + tc->last_move_length = displacement; + tc->progress += (disp_sign * displacement); + + // Progress has to be within the allowable range + tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); + } + tc->currentvel = v_next; tc->currentacc = acc; - - // Check if we can make the desired velocity - tc->on_final_decel = dec; + tc->currentjerk = jerk; + tc->on_final_decel = dec; return TP_ERR_OK; } @@ -2830,7 +2809,7 @@ int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_ a_for_dlen2 = 0; } double dlen2 = finishWithSpeedDist(v_for_dlen2, tc_finalvel, a_for_dlen2, maxaccel, maxjerk); - double moveL = sc_distance(tc->cycle_time, tc->currentvel, tc->currentacc, req_j); + double moveL = (tc->currentvel + req_v) * tc->cycle_time / 2.0; // margin = remaining distance - decel distance // margin > 0: can continue accelerating @@ -3511,12 +3490,12 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, }else{ double jerk; double perror; - if(*mode == 1){ + if(*mode == 1){ tc->cycle_time = tp->cycleTime; int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1); if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. - *mode = -5; + *mode = TP_SCURVE_ACCEL_ERROR; res_accel = 1; acc=0, vel_desired=0; if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { @@ -3533,7 +3512,7 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, }else{ int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0); if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. - *mode = -5; + *mode = TP_SCURVE_ACCEL_ERROR; res_accel = 1; acc=0, vel_desired=0; if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { @@ -3837,7 +3816,7 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, double target_vel_this = tpGetRealTargetVel(tp, tc); double target_vel_next = tpGetRealTargetVel(tp, nexttc); - if(mode != -5 && GET_TRAJ_PLANNER_TYPE() == 1) + if(mode != TP_SCURVE_ACCEL_ERROR && GET_TRAJ_PLANNER_TYPE() == 1) tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); else tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); From 0027f3cd27a29e426d7b7852b44800d2c3400386 Mon Sep 17 00:00:00 2001 From: Luca Toniolo Date: Tue, 27 Jan 2026 13:57:49 +0800 Subject: [PATCH 7/7] Unify arc jerk limiting for S-curve planner MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Rename tcUpdateCircleAccRatio() to tcUpdateArcLimits() and extend to handle both TC_CIRCULAR and TC_SPHERICAL arcs - Add comprehensive jerk-based velocity limiting for S-curve planner (planner_type 1) with three constraints: * Steady-state rotational jerk (v³/R²) with entry/exit transition budget * Normal jerk from tangential acceleration coupling (3·v·a_t/R) * Entry/exit transition jerk at arc boundaries - Apply jerk constraints to blend arcs in blendComputeParameters() - Remove obsolete pmCircleActualMaxVel() function and PmCircleLimits struct - Consolidate all arc limit calculations (acceleration and jerk) in unified tcUpdateArcLimits() - Add debug output for jerk limiting analysis This ensures consistent jerk limiting for both programmed arcs (G2/G3) and blend arcs at segment corners. --- src/emc/tp/blendmath.c | 72 +++++++++++------------- src/emc/tp/blendmath.h | 9 --- src/emc/tp/tc.c | 123 +++++++++++++++++++++++++++++++++++++---- src/emc/tp/tc.h | 2 +- src/emc/tp/tp.c | 3 + 5 files changed, 149 insertions(+), 60 deletions(-) diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index 06093a3bbac..3d7c6cdcee9 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -1156,7 +1156,7 @@ int blendComputeParameters(BlendParameters * const param) // Find maximum velocity allowed by accel and radius double v_normal; - + if(GET_TRAJ_PLANNER_TYPE() == 1){ v_normal = findSCurveVPeak(param->a_n_max, emcmotStatus->jerk, R_geom); }else{ @@ -1179,7 +1179,38 @@ int blendComputeParameters(BlendParameters * const param) double s_blend = t_blend * param->v_plan; double R_blend = fmin(s_blend / param->phi, R_geom); //Clamp by limiting radius - param->R_plan = fmax(pmSq(param->v_plan) / param->a_n_max, R_blend); + // Calculate minimum radius needed to keep jerk within limits at v_plan + // For circular motion: j = v³/R² (worst case at corner transitions) + // Therefore: R_min = v^(3/2) / sqrt(j_max) + double R_jerk_min = 0.0; + if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON) { + // R_min = v^(3/2) / sqrt(j) + double v_32 = pmSqrt(param->v_plan) * param->v_plan; // v^(3/2) + double j_sqrt = pmSqrt(emcmotStatus->jerk); + R_jerk_min = v_32 / j_sqrt; + + tp_debug_print("R_jerk_min = %f (for v=%f, j=%f)\n", + R_jerk_min, param->v_plan, emcmotStatus->jerk); + } + + // Calculate radius from acceleration constraint + double R_accel = pmSq(param->v_plan) / param->a_n_max; + + // Apply jerk constraint - use the larger of R_blend and R_jerk_min + double R_min = fmax(R_blend, R_jerk_min); + + // Final radius must satisfy both acceleration and minimum (blend/jerk) constraints + param->R_plan = fmax(R_accel, R_min); + + // Calculate arc length + param->s_arc = param->R_plan * param->phi; + + tp_debug_print("R_accel=%f, R_blend=%f, R_jerk_min=%f, R_plan=%f\n", + R_accel, R_blend, R_jerk_min, param->R_plan); + + // Note: Velocity jerk limiting for blend arcs is now handled uniformly + // in tcUpdateArcLimits() during segment finalization, along with TC_CIRCULAR arcs. + param->d_plan = param->R_plan / tan(param->theta); tp_debug_print("v_plan = %f\n", param->v_plan); @@ -1662,43 +1693,6 @@ double pmCartAbsMax(PmCartesian const * const v) } -PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, - double v_max, - double a_max) -{ - double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; - - 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 - 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 (a_n_vmax > a_n_max_cutoff) { - v_max_actual = v_max_cutoff; - } else { - acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); - } - - 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); - 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; -} /** @section spiralfuncs Functions to approximate spiral arc length */ diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index 8f2d699f5af..3d290ba1c1c 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -243,15 +243,6 @@ int blendPoints3Print(BlendPoints3 const * const points); double pmCartAbsMax(PmCartesian const * const v); -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, diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 84a334ba6e1..022c4b9b605 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -23,10 +23,18 @@ #include "tp_types.h" #include "spherical_arc.h" #include "motion_types.h" +#include "motion.h" //Debug output #include "tp_debug.h" +// For jerk-limited arc velocity (planner_type 1) +extern emcmot_status_t *emcmotStatus; + +#ifndef GET_TRAJ_PLANNER_TYPE +#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type) +#endif + double tcGetMaxTargetVel(TC_STRUCT const * const tc, double max_scale) @@ -748,18 +756,111 @@ double pmCircle9Target(PmCircle9 const * const circ9) return helical_length; } -int tcUpdateCircleAccRatio(TC_STRUCT * tc) +/** + * Apply acceleration and jerk limits to circular/spherical arc segments. + * + * For any arc (TC_CIRCULAR or TC_SPHERICAL), this function: + * 1. Limits velocity based on centripetal acceleration budget + * 2. For planner_type 1 (S-curve), applies three jerk constraints: + * - Steady-state rotational jerk: v³/R² + * - Normal jerk from tangential acceleration coupling: 3·v·a_t/R + * - Entry/exit transition jerk at arc boundaries + * 3. Calculates the tangential acceleration ratio for the arc + * + * This unified approach ensures consistent jerk limiting for both + * programmed arcs (G2/G3) and blend arcs at segment corners. + */ +int tcUpdateArcLimits(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; + double radius, angle; + + // Extract radius and angle based on motion type + switch (tc->motion_type) { + case TC_CIRCULAR: + radius = pmCircleEffectiveMinRadius(&tc->coords.circle.xyz); + angle = tc->coords.circle.xyz.angle; + break; + case TC_SPHERICAL: + radius = tc->coords.arc.xyz.radius; + angle = tc->coords.arc.xyz.angle; + break; + default: + return 1; // Not an arc, nothing to do + } + + if (radius < DOUBLE_FUZZ || angle < TP_ANGLE_EPSILON) { + return 1; // Degenerate arc } - // TODO handle blend arc here too? - return 1; //nothing to do, but not an error + + double a_max = tcGetOverallMaxAccel(tc); + double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + + // Find the acceleration necessary to reach the maximum velocity + double a_n_vmax = pmSq(tc->maxvel) / radius; + + // Find the maximum velocity that still obeys our desired normal/total acceleration ratio + double v_max_cutoff = pmSqrt(a_n_max_cutoff * radius); + + double v_max_actual = tc->maxvel; + double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + + if (a_n_vmax > a_n_max_cutoff) { + v_max_actual = v_max_cutoff; + } else { + acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); + } + + // Jerk-based velocity limiting for S-curve planner (planner_type 1) + if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON && + tc->cycle_time > TP_TIME_EPSILON) { + + double jerk = emcmotStatus->jerk; + double R_sq = pmSq(radius); + + // Constraint 1: Steady-state rotational jerk + entry/exit transitions + // The jerk budget is shared between steady-state (v³/R²) and transitions. + // Solving: v³ ≤ R² × j × φ / (2 + φ) + // The (2 + φ) term: 2 for two transitions, φ for steady-state budget + double v_max_jerk_steady = cbrt(R_sq * jerk * angle / (2.0 + angle)); + + // Constraint 2: Normal jerk from tangential acceleration coupling + // During S-curve ramps on arc: j_n = 3·v·a_t/R + // Using BLEND_ACC_RATIO_TANGENTIAL as max tangential accel ratio + double a_t_max = BLEND_ACC_RATIO_TANGENTIAL * a_max; + double v_max_jerk_tan = jerk * radius / (3.0 * a_t_max); + + // Constraint 3: Entry/exit transition jerk (centripetal accel ramp) + // At line-arc boundary, centripetal accel changes from 0 to v²/R + // j_entry = (v²/R) / cycle_time ≤ j_max + double v_max_jerk_entry = pmSqrt(jerk * radius * tc->cycle_time); + + double v_max_jerk = fmin(fmin(v_max_jerk_steady, v_max_jerk_tan), v_max_jerk_entry); + + tp_debug_print("tcUpdateArcLimits: type=%d R=%f phi=%f j=%f\n", + tc->motion_type, radius, angle, jerk); + tp_debug_print(" v_jerk: steady=%f tan=%f entry=%f => min=%f\n", + v_max_jerk_steady, v_max_jerk_tan, v_max_jerk_entry, v_max_jerk); + + if (v_max_jerk < v_max_actual) { + tp_debug_print(" Limiting v_max from %f to %f for jerk\n", + v_max_actual, v_max_jerk); + v_max_actual = v_max_jerk; + + // Recalculate acc_ratio_tan for jerk-limited velocity + double a_n_at_jerk_vel = pmSq(v_max_actual) / radius; + if (a_n_at_jerk_vel < a_max) { + acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_at_jerk_vel / a_max)); + } + } + } + + tc->maxvel = v_max_actual; + tc->acc_ratio_tan = acc_ratio_tan; + + tp_debug_print("tcUpdateArcLimits: final v_max=%f acc_ratio_tan=%f\n", + tc->maxvel, tc->acc_ratio_tan); + + return 0; } /** @@ -785,7 +886,7 @@ int tcFinalizeLength(TC_STRUCT * const tc) tcClampVelocityByLength(tc); - tcUpdateCircleAccRatio(tc); + tcUpdateArcLimits(tc); tc->finalized = 1; return TP_ERR_OK; diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 6e43217ef0f..3c1afa6dde2 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -105,7 +105,7 @@ int tcSetupMotion(TC_STRUCT * const tc, int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp); -int tcUpdateCircleAccRatio(TC_STRUCT * tc); +int tcUpdateArcLimits(TC_STRUCT * tc); int tcFinalizeLength(TC_STRUCT * const tc); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index 5454152c7e3..b9a15ba357f 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2258,6 +2258,9 @@ int tpAddCircle(TP_STRUCT * const tp, //Reduce max velocity to match sample rate tcClampVelocityByLength(&tc); + // Apply acceleration and jerk limits for circular motion + tcUpdateArcLimits(&tc); + TC_STRUCT *prev_tc; prev_tc = tcqLast(&tp->queue);