Fix long slow decel on feedhold. Fix div by zero in planning buffer

diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c
index 29d13fc0d9db1eedfb9dc499bc6b978a2785cb0d..f46e96c53ae397deaa5812e5d8fbcc5b1f5fb447 100644 (file)
--- 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 130959ed9f4ce89328bbae1f756da1057d6ec845..ac5149e7eeb52e966daa719fca514008a90ab336 100644 (file)
--- 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 d94de9ad1132e61ba5fdad2797d2c33ef7e45047..60c7476b879817b82baea67f88525e1328e58ceb 100644 (file)
--- 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 {