From 1ffe3f349f6721188db23aa8def2ec2a74eaaf29 Mon Sep 17 00:00:00 2001
From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Wed, 21 Sep 2016 13:13:12 -0700
Subject: [PATCH] Fixed occasional step correction induced stutter.

---
 src/motor.c        | 19 +++++++++++++++----
 src/plan/exec.c    |  9 ++++-----
 src/plan/planner.c |  2 +-
 src/plan/runtime.c | 26 ++++++++++----------------
 src/util.c         |  2 +-
 5 files changed, 31 insertions(+), 27 deletions(-)

diff --git a/src/motor.c b/src/motor.c
index a39be7d..61a64cd 100644
--- a/src/motor.c
+++ b/src/motor.c
@@ -82,6 +82,7 @@ typedef struct {
   bool wasEnabled;
   int32_t error;
   bool last_negative;            // Last step sign
+  bool reading;
 
   // Move prep
   uint8_t timer_clock;           // clock divisor setting or zero for off
@@ -233,15 +234,25 @@ int32_t motor_get_encoder(int motor) {return motors[motor].encoder;}
 
 
 void motor_set_encoder(int motor, float encoder) {
+  if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR);
+
   motor_t *m = &motors[motor];
-  cli();
   m->encoder = m->position = m->commanded = round(encoder);
   m->error = 0;
-  sei();
 }
 
 
-int32_t motor_get_error(int motor) {return motors[motor].error;}
+int32_t motor_get_error(int motor) {
+  motor_t *m = &motors[motor];
+
+  m->reading = true;
+  int32_t error = motors[motor].error;
+  m->reading = false;
+
+  return error;
+}
+
+
 int32_t motor_get_position(int motor) {return motors[motor].position;}
 
 
@@ -381,7 +392,7 @@ void motor_load_move(int motor) {
   m->last_negative = m->negative;
 
   // Compute error
-  m->error = m->commanded - m->encoder;
+  if (!m->reading) m->error = m->commanded - m->encoder;
   m->commanded = m->position;
 }
 
diff --git a/src/plan/exec.c b/src/plan/exec.c
index 887aab4..a4c75ce 100644
--- a/src/plan/exec.c
+++ b/src/plan/exec.c
@@ -58,7 +58,6 @@ typedef struct {
   float cruise_velocity;
   float exit_velocity;
 
-  float segments;           // number of segments in line
   uint32_t segment_count;   // count of running segments
   float segment_velocity;   // computed velocity for segment
   float segment_time;       // actual time increment per segment
@@ -105,13 +104,13 @@ static stat_t _exec_aline_section(float length, float vin, float vout) {
 
     // len / avg. velocity
     float move_time = 2 * length / (vin + vout);
-    ex.segments = ceil(move_time / NOM_SEGMENT_TIME);
-    ex.segment_time = move_time / ex.segments;
-    ex.segment_count = (uint32_t)ex.segments;
+    float segments = ceil(move_time / NOM_SEGMENT_TIME);
+    ex.segment_time = move_time / segments;
+    ex.segment_count = round(segments);
 
     if (vin == vout) ex.segment_velocity = vin;
     else ex.segment_velocity =
-           mp_init_forward_dif(ex.fdif, vin, vout, ex.segments);
+           mp_init_forward_dif(ex.fdif, vin, vout, segments);
 
     if (ex.segment_time < MIN_SEGMENT_TIME)
       return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
diff --git a/src/plan/planner.c b/src/plan/planner.c
index d10b677..e83c63e 100644
--- a/src/plan/planner.c
+++ b/src/plan/planner.c
@@ -676,7 +676,7 @@ 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) {
-  return fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf));
+  return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf));
 }
 
 
diff --git a/src/plan/runtime.c b/src/plan/runtime.c
index 333f367..01de326 100644
--- a/src/plan/runtime.c
+++ b/src/plan/runtime.c
@@ -64,8 +64,7 @@ typedef struct {
   distance_mode_t distance_mode;
   distance_mode_t arc_distance_mode;
 
-  float previousP[MOTORS];
-  float previousI[MOTORS];
+  float previous_error[MOTORS];
 } mp_runtime_t;
 
 static mp_runtime_t rt = {0};
@@ -175,38 +174,33 @@ stat_t mp_runtime_move_to_target(float target[], float time) {
       motor[error] = 0;
     }
 
-    float P = error[motor] - rt.previousI[motor];
-    float I = error[motor];
-    float D = P - rt.previousP[motor];
+    error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]);
+    rt.previous_error[motor] = error[motor];
 
-    rt.previousP[motor] = P;
-    rt.previousI[motor] = I;
-
-    error[motor] = 0.5 * P + 0.5 * I + 0.15 * D;
-
-#if 0
     if (error[motor] < -MAX_STEP_CORRECTION)
       error[motor] = -MAX_STEP_CORRECTION;
     else if (MAX_STEP_CORRECTION < error[motor])
       error[motor] = MAX_STEP_CORRECTION;
-#endif
 
     old_length_sqr += square(travel[motor]);
     new_length_sqr += square(travel[motor] - error[motor]);
   }
 
   bool use_error = false;
-  if (!fp_ZERO(old_length_sqr)) {
+  if (!fp_ZERO(new_length_sqr)) {
     float new_time = time * invsqrt(old_length_sqr / new_length_sqr);
 
-    if (EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) {
+    if (!isnan(new_time) && !isinf(new_time) &&
+        EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) {
       time = new_time;
       use_error = true;
     }
   }
 
-  if (!use_error)
-    for (int motor = 0; motor < MOTORS; motor++) error[motor] = 0;
+  if (!use_error) memset(error, 0, sizeof(error));
+
+#else
+  const int32_t error[MOTORS] = {0};
 #endif
 
   // Call the stepper prep function
diff --git a/src/util.c b/src/util.c
index e5e8341..f30feb6 100644
--- a/src/util.c
+++ b/src/util.c
@@ -50,5 +50,5 @@ float invsqrt(float number) {
   y = y * (threehalfs - x2 * y * y);   // 1st iteration
   y = y * (threehalfs - x2 * y * y);   // 2nd iteration, this can be removed
 
-  return y;
+  return y; //isnan(y) ? 1.0 / sqrt(number) : y;
 }
-- 
2.27.0