From 04631677cc45d6caaa3aac1e6ef6d9758639f0ca Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 29 Aug 2021 17:37:29 +0200 Subject: [PATCH] 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 | 4 ++-- src/modules/motion.h | 32 +++++++++++++++----------- src/modules/pulse_gen.cpp | 13 +++++++++-- src/modules/pulse_gen.h | 5 ++-- tests/unit/logic/stubs/stub_motion.cpp | 2 +- 5 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 4f3ef10..edc6e79 100644 --- a/src/modules/motion.cpp +++ b/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); diff --git a/src/modules/motion.h b/src/modules/motion.h index cd78ce2..05d6f10 100644 --- a/src/modules/motion.h +++ b/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(pos, rate). /// @see PlanMoveTo, unitToSteps template - constexpr void PlanMoveTo(AxisUnit pos, AxisUnit feedrate) { - PlanMoveTo(A, pos.v, feedrate.v); + constexpr void PlanMoveTo(AxisUnit pos, + AxisUnit feed_rate, AxisUnit 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 constexpr void PlanMoveTo(config::Unit pos, - config::Unit feedrate) { + config::Unit feed_rate, config::Unit end_rate = { 0 }) { PlanMoveTo( unitToAxisUnit>(pos), - unitToAxisUnit>(feedrate)); + unitToAxisUnit>(feed_rate), + unitToAxisUnit>(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(pos, rate). /// @see PlanMove, unitToSteps template - constexpr void PlanMove(AxisUnit delta, AxisUnit feedrate) { - PlanMove(A, delta.v, feedrate.v); + constexpr void PlanMove(AxisUnit delta, + AxisUnit feed_rate, AxisUnit 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 constexpr void PlanMove(config::Unit delta, - config::Unit feedrate) { + config::Unit feed_rate, config::Unit end_rate = { 0 }) { PlanMove( unitToAxisUnit>(delta), - unitToAxisUnit>(feedrate)); + unitToAxisUnit>(feed_rate), + unitToAxisUnit>(end_rate)); } /// @returns head position of an axis (last enqueued position) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index a48a16f..0510316 100644 --- a/src/modules/pulse_gen.cpp +++ b/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; diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 1322b05..7012342 100644 --- a/src/modules/pulse_gen.h +++ b/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. diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index 4d9f506..0dacd75 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/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; }