Motion/PulseGen: implement move chaining and end-speed control

Allow to chain moves by adding one extra parameter to the PlanMove[to]
functions: ending speed.

A move will always be accelerated from the last speed towards end ending
speed. The following:

  PlanMove(100._mm, 50._mm_s, 50._mm_s);
  PlanMove(200._mm, 100._mm_s);

Will first move the axis 100mm, accelerating towards 50mm/s, then
accelerate again to 100mm/s. The move will for then decelerate towards a
full stop after reaching 300mm in total.

Acceleration can be changed for each segment, so that a custom
acceleration curve can be created:

  SetAcceleration(10._mm_s2);
  PlanMove(100._mm, 50._mm_s, 50._mm_s);
  SetAcceleration(100._mm_s2);
  PlanMove(100._mm, 50._mm_s, 50._mm_s);

The ending speed might not always be reached, depending on the current
acceleration settings. The new function "Rate()" will return the ending
feedrate of the last move, if necessary.

AbortPlannedMoves accepts a new "halt" parameter to control how moves
will be chanined when interrupting the current move. By default
(halt=true) the move is completely interrupted.

When halt=false is requested, a subsequent move will be chained starting
at the currently aborted velocity. This allows to chain moves in reponse
to events, for example to accelerate the pulley without stopping as soon
as the FINDA is triggered, it's sufficient to interrupt the current move
followed by a new one:

  PlanMove(maximum_loading_lenght, slow_feedrate);
  ... wait for PINDA trigger ...
  AbortPlannedMoves(true);
  PlanMove(bowden_lenght, fast_feedrate);

will seamlessy continue loading and transition to the fast feedrate.

Jerk control has been simplified. It now handles only the maximal
velocity change of the last segment, which doesn't require reverse
planning.

src/modules/motion.cpp

@@ -43,8 +43,8 @@ void Motion::StallGuardReset(Axis axis) {
 void Motion::Home(Axis axis, bool direction) {
 }
 
-void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
-    if (axisData[axis].ctrl.PlanMoveTo(pos, feedrate)) {
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) {
+    if (axisData[axis].ctrl.PlanMoveTo(pos, feed_rate, end_rate)) {
         // move was queued, prepare the axis
         if (!axisData[axis].enabled)
             SetEnabled(axis, true);

src/modules/motion.h

@@ -105,15 +105,17 @@ public:
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param axis axis affected
     /// @param pos target position
-    /// @param feedrate maximum feedrate
-    void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate);
+    /// @param feed_rate maximum feedrate
+    /// @param end_rate endding feedrate (may not be reached!)
+    void PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate = 0);
 
     /// Enqueue a single axis move using PlanMoveTo, but using AxisUnit. The Axis needs to
     /// be supplied as the first template argument: PlanMoveTo<axis>(pos, rate).
     /// @see PlanMoveTo, unitToSteps
     template <Axis A>
-    constexpr void PlanMoveTo(AxisUnit<pos_t, A, Lenght> pos, AxisUnit<steps_t, A, Speed> feedrate) {
-        PlanMoveTo(A, pos.v, feedrate.v);
+    constexpr void PlanMoveTo(AxisUnit<pos_t, A, Lenght> pos,
+        AxisUnit<steps_t, A, Speed> feed_rate, AxisUnit<steps_t, A, Speed> end_rate = { 0 }) {
+        PlanMoveTo(A, pos.v, feed_rate.v, end_rate.v);
     }
 
     /// Enqueue a single axis move using PlanMoveTo, but using physical units. The Axis
@@ -121,27 +123,30 @@ public:
     /// @see PlanMoveTo, unitToSteps
     template <Axis A, config::UnitBase B>
     constexpr void PlanMoveTo(config::Unit<long double, B, Lenght> pos,
-        config::Unit<long double, B, Speed> feedrate) {
+        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
         PlanMoveTo<A>(
             unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(pos),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
     }
 
     /// Enqueue a single axis move in steps starting and ending at zero speed with maximum
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param axis axis affected
     /// @param delta relative to current position
-    /// @param feedrate maximum feedrate
-    void PlanMove(Axis axis, pos_t delta, steps_t feedrate) {
-        PlanMoveTo(axis, Position(axis) + delta, feedrate);
+    /// @param feed_rate maximum feedrate
+    /// @param end_rate endding feedrate (may not be reached!)
+    void PlanMove(Axis axis, pos_t delta, steps_t feed_rate, steps_t end_rate = 0) {
+        PlanMoveTo(axis, Position(axis) + delta, feed_rate, end_rate);
     }
 
     /// Enqueue a single axis move using PlanMove, but using AxisUnit. The Axis needs to
     /// be supplied as the first template argument: PlanMove<axis>(pos, rate).
     /// @see PlanMove, unitToSteps
     template <Axis A>
-    constexpr void PlanMove(AxisUnit<pos_t, A, Lenght> delta, AxisUnit<steps_t, A, Speed> feedrate) {
-        PlanMove(A, delta.v, feedrate.v);
+    constexpr void PlanMove(AxisUnit<pos_t, A, Lenght> delta,
+        AxisUnit<steps_t, A, Speed> feed_rate, AxisUnit<steps_t, A, Speed> end_rate = { 0 }) {
+        PlanMove(A, delta.v, feed_rate.v, end_rate.v);
     }
 
     /// Enqueue a single axis move using PlanMove, but using physical units. The Axis needs to
@@ -149,10 +154,11 @@ public:
     /// @see PlanMove, unitToSteps
     template <Axis A, config::UnitBase B>
     constexpr void PlanMove(config::Unit<long double, B, Lenght> delta,
-        config::Unit<long double, B, Speed> feedrate) {
+        config::Unit<long double, B, Speed> feed_rate, config::Unit<long double, B, Speed> end_rate = { 0 }) {
         PlanMove<A>(
             unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(delta),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feed_rate),
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(end_rate));
     }
 
     /// @returns head position of an axis (last enqueued position)

src/modules/pulse_gen.cpp

@@ -66,8 +66,10 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e
                 accelerate_steps += acceleration_x2;
             accelerate_steps /= acceleration_x4;
             accelerate_steps += (block->steps >> 1);
-            if (accelerate_steps > block->steps)
+            if (accelerate_steps > block->steps) {
                 accelerate_steps = block->steps;
+                final_rate = sqrt(acceleration_x2 * accelerate_steps + initial_rate_sqr);
+            }
         } else {
             decelerate_steps = initial_rate_sqr - final_rate_sqr;
             if (block->steps & 1)
@@ -86,7 +88,7 @@ void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t e
     block->final_rate = final_rate;
 }
 
-bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) {
+bool PulseGen::PlanMoveTo(pos_t target, rate_t feed_rate, rate_t end_rate) {
     // Prepare to set up new block
     if (block_index.full())
         return false;
@@ -107,6 +109,13 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) {
     block->acceleration = acceleration;
     block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / config::stepTimerFrequencyDivider));
 
+    // Simplified forward jerk: do not handle reversals
+    steps_t entry_speed;
+    if (feed_rate >= last_rate)
+        entry_speed = last_rate + min(feed_rate - last_rate, max_jerk);
+    else
+        entry_speed = last_rate - min(last_rate - feed_rate, max_jerk);
+
     // Perform the trapezoid calculations
     CalculateTrapezoid(block, entry_speed, end_rate);
     last_rate = block->final_rate;

src/modules/pulse_gen.h

@@ -39,9 +39,10 @@ public:
     /// Enqueue a single move in steps starting and ending at zero speed with maximum
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param pos target position
-    /// @param feedrate maximum feedrate
+    /// @param feed_rate maximum feedrate
+    /// @param end_rate endding feedrate (may not be reached!)
     /// @returns true if the move has been enqueued
-    bool PlanMoveTo(pos_t pos, steps_t feedrate);
+    bool PlanMoveTo(pos_t pos, rate_t feed_rate, rate_t end_rate = 0);
 
     /// Stop whatever moves are being done
     /// @param halt When true, also abruptly stop axis movement.

tests/unit/logic/stubs/stub_motion.cpp

@@ -32,7 +32,7 @@ void Motion::StallGuardReset(Axis axis) {
     axes[axis].stallGuard = false;
 }
 
-void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) {
     axes[axis].targetPos = pos;
 }