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.

Author: Luca Toniolo

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:

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:

src/emc/tp/sp_scurve.c

@@ -983,78 +983,105 @@ 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);
         v += velocity(t, a, -maxJ);
         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;
+}

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);
 

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);