From 56e769cf6e30f2d5406b6b64575afae20a0863e2 Mon Sep 17 00:00:00 2001
From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Mon, 5 Jun 2017 15:58:31 -0700
Subject: [PATCH] Fix long slow decel on feedhold.  Fix div by zero in planning
 buffer

---
 avr/src/plan/exec.c    |  6 +++++-
 avr/src/plan/planner.c | 14 +++++++++-----
 avr/src/plan/planner.h |  1 +
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c
index 29d13fc..f46e96c 100644
--- a/avr/src/plan/exec.c
+++ b/avr/src/plan/exec.c
@@ -237,12 +237,16 @@ static void _plan_hold() {
     bf->entry_vmax = 0;
     bf->state = BUFFER_RESTART; // Restart the buffer when done
 
-  } else {
+  } else if (HOLD_VELOCITY_TOLERANCE < braking_velocity) {
     // Case 2: deceleration exceeds length remaining in buffer
     // Replan to minimum (but non-zero) exit velocity
     ex.tail_length = available_length;
     ex.exit_velocity =
       braking_velocity - mp_get_target_velocity(0, available_length, bf);
+
+  } else { // Were're close enough
+    ex.tail_length = available_length;
+    ex.exit_velocity = 0;
   }
 }
 
diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c
index 130959e..ac5149e 100644
--- a/avr/src/plan/planner.c
+++ b/avr/src/plan/planner.c
@@ -377,12 +377,13 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) {
         computed_velocity =
           mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf);
 
-      } else {
+      } else if (bf->head_length + bf->tail_length) {
         bf->tail_length =
           (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length;
         computed_velocity =
           mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf);
-      }
+
+      } else break;
 
     } while ((fabs(bf->cruise_velocity - computed_velocity) /
               computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT);
@@ -656,7 +657,8 @@ void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) {
  *     PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
  */
 float mp_get_target_length(float Vi, float Vf, float jerk) {
-  ASSERT(0 <= Vi && 0 <= Vf);
+  ASSERT(0 <= Vi);
+  ASSERT(0 <= Vf);
   ASSERT(isfinite(jerk));
   return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf));
 }
@@ -729,8 +731,10 @@ float mp_get_target_length(float Vi, float Vf, float jerk) {
  *   J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2
  */
 float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) {
-  ASSERT(0 <= Vi && 0 <= L);
-  ASSERT(isfinite(bf->cbrt_jerk) && isfinite(bf->jerk));
+  ASSERT(0 <= Vi);
+  ASSERT(0 <= L);
+  ASSERT(isfinite(bf->jerk));
+  ASSERT(isfinite(bf->cbrt_jerk));
 
   if (!L) return Vi;
 
diff --git a/avr/src/plan/planner.h b/avr/src/plan/planner.h
index d94de9a..60c7476 100644
--- a/avr/src/plan/planner.h
+++ b/avr/src/plan/planner.h
@@ -55,6 +55,7 @@
  * short to process correctly.
  */
 #define HOLD_DECELERATION_TOLERANCE 1 // In mm
+#define HOLD_VELOCITY_TOLERANCE 60 // In mm/min
 
 
 typedef enum {
-- 
2.27.0