More planner cleanup

diff --git a/src/plan/arc.h b/src/plan/arc.h
index 272ddb7fb8f88931f2c5d5958167c9b70fa97989..2d05be380c9bb5763dabae59e85e10fbd1837629 100644 (file)
--- a/src/plan/arc.h
+++ b/src/plan/arc.h
@@ -28,11 +28,16 @@
 
 #pragma once
 
-#include "planner.h"
+#include "util.h"
+#include "status.h"
+
 
 #define ARC_SEGMENT_LENGTH      0.1 // mm
 #define MIN_ARC_RADIUS          0.1
+
+#define MIN_ARC_SEGMENT_USEC    10000.0 // minimum arc segment time
 #define MIN_ARC_SEGMENT_TIME    (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
 
+
 stat_t cm_arc_callback();
 void cm_abort_arc();

diff --git a/src/plan/exec.c b/src/plan/exec.c
index 2c68580a6a32ae0165d7971ef86600e4298070a4..1bb6106a004be2bb08ff644c2823e2fb843c4989 100644 (file)
--- a/src/plan/exec.c
+++ b/src/plan/exec.c
@@ -524,18 +524,18 @@ static void _init_forward_diffs(float Vi, float Vt) {
   float Bh_4 = B * h * h * h * h;
   float Ch_3 = C * h * h * h;
 
-  mr.forward_diff_5 = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3;
-  mr.forward_diff_4 = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3;
-  mr.forward_diff_3 = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3;
-  mr.forward_diff_2 = 300.0 * Ah_5 + 24.0 * Bh_4;
-  mr.forward_diff_1 = 120.0 * Ah_5;
+  mr.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3;
+  mr.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3;
+  mr.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3;
+  mr.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4;
+  mr.forward_diff[0] = 120.0 * Ah_5;
 
 #ifdef __KAHAN
-  mr.forward_diff_5_c = 0;
-  mr.forward_diff_4_c = 0;
-  mr.forward_diff_3_c = 0;
-  mr.forward_diff_2_c = 0;
-  mr.forward_diff_1_c = 0;
+  mr.forward_diff_c[4] = 0;
+  mr.forward_diff_c[3] = 0;
+  mr.forward_diff_c[2] = 0;
+  mr.forward_diff_c[1] = 0;
+  mr.forward_diff_c[0] = 0;
 #endif
 
   // Calculate the initial velocity by calculating V(h/2)
@@ -589,12 +589,12 @@ static stat_t _exec_aline_head() {
   // Second half (convex part of accel curve)
   if (mr.section_state == SECTION_2nd_HALF) {
 #ifndef __KAHAN
-    mr.segment_velocity += mr.forward_diff_5;
+    mr.segment_velocity += mr.forward_diff[4];
 #else
     // Use Kahan summation algorithm to mitigate floating-point errors for above
-    float y = mr.forward_diff_5 - mr.forward_diff_5_c;
+    float y = mr.forward_diff[4] - mr.forward_diff_c[4];
     float v = mr.segment_velocity + y;
-    mr.forward_diff_5_c = (v - mr.segment_velocity) - y;
+    mr.forward_diff_c[4] = (v - mr.segment_velocity) - y;
     mr.segment_velocity = v;
 #endif
 
@@ -607,30 +607,30 @@ static stat_t _exec_aline_head() {
 
     } else {
 #ifndef __KAHAN
-      mr.forward_diff_5 += mr.forward_diff_4;
-      mr.forward_diff_4 += mr.forward_diff_3;
-      mr.forward_diff_3 += mr.forward_diff_2;
-      mr.forward_diff_2 += mr.forward_diff_1;
+      mr.forward_diff[4] += mr.forward_diff[3];
+      mr.forward_diff[3] += mr.forward_diff[2];
+      mr.forward_diff[2] += mr.forward_diff[1];
+      mr.forward_diff[1] += mr.forward_diff[0];
 #else
-      y = mr.forward_diff_4 - mr.forward_diff_4_c;
-      v = mr.forward_diff_5 + y;
-      mr.forward_diff_4_c = (v - mr.forward_diff_5) - y;
-      mr.forward_diff_5 = v;
-
-      y = mr.forward_diff_3 - mr.forward_diff_3_c;
-      v = mr.forward_diff_4 + y;
-      mr.forward_diff_3_c = (v - mr.forward_diff_4) - y;
-      mr.forward_diff_4 = v;
-
-      y = mr.forward_diff_2 - mr.forward_diff_2_c;
-      v = mr.forward_diff_3 + y;
-      mr.forward_diff_2_c = (v - mr.forward_diff_3) - y;
-      mr.forward_diff_3 = v;
-
-      y = mr.forward_diff_1 - mr.forward_diff_1_c;
-      v = mr.forward_diff_2 + y;
-      mr.forward_diff_1_c = (v - mr.forward_diff_2) - y;
-      mr.forward_diff_2 = v;
+      y = mr.forward_diff[3] - mr.forward_diff_c[3];
+      v = mr.forward_diff[4] + y;
+      mr.forward_diff_c[3] = (v - mr.forward_diff[4]) - y;
+      mr.forward_diff[4] = v;
+
+      y = mr.forward_diff[2] - mr.forward_diff_c[2];
+      v = mr.forward_diff[3] + y;
+      mr.forward_diff_c[2] = (v - mr.forward_diff[3]) - y;
+      mr.forward_diff[3] = v;
+
+      y = mr.forward_diff[1] - mr.forward_diff_c[1];
+      v = mr.forward_diff[2] + y;
+      mr.forward_diff_c[1] = (v - mr.forward_diff[2]) - y;
+      mr.forward_diff[2] = v;
+
+      y = mr.forward_diff[0] - mr.forward_diff_c[0];
+      v = mr.forward_diff[1] + y;
+      mr.forward_diff_c[0] = (v - mr.forward_diff[1]) - y;
+      mr.forward_diff[1] = v;
 #endif
     }
   }
@@ -679,42 +679,42 @@ static stat_t _exec_aline_tail() {
   // Second half - concave part (period 5)
   if (mr.section_state == SECTION_2nd_HALF) {
 #ifndef __KAHAN
-    mr.segment_velocity += mr.forward_diff_5;
+    mr.segment_velocity += mr.forward_diff[4];
 #else
     // Use Kahan summation algorithm to mitigate floating-point errors for above
-    float y = mr.forward_diff_5 - mr.forward_diff_5_c;
+    float y = mr.forward_diff[4] - mr.forward_diff_c[4];
     float v = mr.segment_velocity + y;
-    mr.forward_diff_5_c = (v - mr.segment_velocity) - y;
+    mr.forward_diff_c[4] = (v - mr.segment_velocity) - y;
     mr.segment_velocity = v;
 #endif
 
     if (_exec_aline_segment() == STAT_OK) return STAT_OK; // set up for body
     else {
 #ifndef __KAHAN
-      mr.forward_diff_5 += mr.forward_diff_4;
-      mr.forward_diff_4 += mr.forward_diff_3;
-      mr.forward_diff_3 += mr.forward_diff_2;
-      mr.forward_diff_2 += mr.forward_diff_1;
+      mr.forward_diff[4] += mr.forward_diff[3];
+      mr.forward_diff[3] += mr.forward_diff[2];
+      mr.forward_diff[2] += mr.forward_diff[1];
+      mr.forward_diff[1] += mr.forward_diff[0];
 #else
-      y = mr.forward_diff_4 - mr.forward_diff_4_c;
-      v = mr.forward_diff_5 + y;
-      mr.forward_diff_4_c = (v - mr.forward_diff_5) - y;
-      mr.forward_diff_5 = v;
-
-      y = mr.forward_diff_3 - mr.forward_diff_3_c;
-      v = mr.forward_diff_4 + y;
-      mr.forward_diff_3_c = (v - mr.forward_diff_4) - y;
-      mr.forward_diff_4 = v;
-
-      y = mr.forward_diff_2 - mr.forward_diff_2_c;
-      v = mr.forward_diff_3 + y;
-      mr.forward_diff_2_c = (v - mr.forward_diff_3) - y;
-      mr.forward_diff_3 = v;
-
-      y = mr.forward_diff_1 - mr.forward_diff_1_c;
-      v = mr.forward_diff_2 + y;
-      mr.forward_diff_1_c = (v - mr.forward_diff_2) - y;
-      mr.forward_diff_2 = v;
+      y = mr.forward_diff[3] - mr.forward_diff_c[3];
+      v = mr.forward_diff[4] + y;
+      mr.forward_diff_c[3] = (v - mr.forward_diff[4]) - y;
+      mr.forward_diff[4] = v;
+
+      y = mr.forward_diff[2] - mr.forward_diff_c[2];
+      v = mr.forward_diff[3] + y;
+      mr.forward_diff_c[2] = (v - mr.forward_diff[3]) - y;
+      mr.forward_diff[3] = v;
+
+      y = mr.forward_diff[1] - mr.forward_diff_c[1];
+      v = mr.forward_diff[2] + y;
+      mr.forward_diff_c[1] = (v - mr.forward_diff[2]) - y;
+      mr.forward_diff[2] = v;
+
+      y = mr.forward_diff[0] - mr.forward_diff_c[0];
+      v = mr.forward_diff[1] + y;
+      mr.forward_diff_c[0] = (v - mr.forward_diff[1]) - y;
+      mr.forward_diff[1] = v;
 #endif
     }
   }

diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c
index 8016c18a49d656a9ea2c53d114e9f72940363301..266d05310dd50c9cd8b8131eb5bd0ec1997326ba 100644 (file)
--- a/src/plan/feedhold.c
+++ b/src/plan/feedhold.c
@@ -114,7 +114,7 @@ static float _compute_next_segment_velocity() {
 #ifdef __JERK_EXEC
   return mr.segment_velocity; // an approximation
 #else
-  return mr.segment_velocity + mr.forward_diff_5;
+  return mr.segment_velocity + mr.forward_diff[4];
 #endif
 }
 

diff --git a/src/plan/planner.h b/src/plan/planner.h
index c70413e2e6bba330677eabe68e9c7e2cc8c64b94..1114605ba5283c5b6289b34308aeb55fb2bd93cf 100644 (file)
--- a/src/plan/planner.h
+++ b/src/plan/planner.h
@@ -32,37 +32,32 @@
 
 #include "canonical_machine.h" // used for GCodeState_t
 #include "util.h"
-#include "config.h"
 
-typedef enum {
-  SECTION_HEAD,           // acceleration
-  SECTION_BODY,           // cruise
-  SECTION_TAIL,           // deceleration
-  SECTIONS                // section count
-} moveSection_t;
 
 // Most of these factors are the result of a lot of tweaking.
 // Change with caution.
 #define JERK_MULTIPLIER         1000000.0
-/// precision jerk must match to be considered same
-#define JERK_MATCH_PRECISION    1000.0
+#define JERK_MATCH_PRECISION    1000.0 // jerk precision to be considered same
 
-#define NOM_SEGMENT_USEC        5000.0 // nominal segment time
-/// minimum segment time / minimum move time
-#define MIN_SEGMENT_USEC        2500.0
-#define MIN_ARC_SEGMENT_USEC    10000.0 // minimum arc segment time
+#define NOM_SEGMENT_USEC        5000.0  // nominal segment time
+#define MIN_SEGMENT_USEC        2500.0  // minimum segment time
 
 #define NOM_SEGMENT_TIME        (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
 #define MIN_SEGMENT_TIME        (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-/// minimum time a move can be is one segment
-#define MIN_TIME_MOVE           MIN_SEGMENT_TIME
-/// factor for minimum size Gcode block to process
-#define MIN_BLOCK_TIME          MIN_SEGMENT_TIME
+#define MIN_BLOCK_TIME          MIN_SEGMENT_TIME // minimum size Gcode block
 
 #define MIN_SEGMENT_TIME_PLUS_MARGIN \
   ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE)
 
 
+typedef enum {
+  SECTION_HEAD,           // acceleration
+  SECTION_BODY,           // cruise
+  SECTION_TAIL,           // deceleration
+  SECTIONS                // section count
+} moveSection_t;
+
+
 typedef struct mpMoveRuntimeSingleton { // persistent runtime variables
   uint8_t move_state;    // state of the overall move
   uint8_t section;       // what section is the move in?
@@ -111,20 +106,13 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables
   float accel_time;
   float segment_accel_time;
   float elapsed_accel_time;
-#else // values used exclusively by forward differencing acceleration
-  float forward_diff_1;             // forward difference level 1
-  float forward_diff_2;             // forward difference level 2
-  float forward_diff_3;             // forward difference level 3
-  float forward_diff_4;             // forward difference level 4
-  float forward_diff_5;             // forward difference level 5
+
+#else // __JERK_EXEC - used exclusively by forward differencing acceleration
+  float forward_diff[5];             // forward difference levels
 #ifdef __KAHAN
-  float forward_diff_1_c;           // level 1 floating-point compensation
-  float forward_diff_2_c;           // level 2 floating-point compensation
-  float forward_diff_3_c;           // level 3 floating-point compensation
-  float forward_diff_4_c;           // level 4 floating-point compensation
-  float forward_diff_5_c;           // level 5 floating-point compensation
-#endif
+  float forward_diff_c[5];           // levels floating-point compensation
 #endif
+#endif // __JERK_EXEC
 
   GCodeState_t gm;                  // gcode model state currently executing
 } mpMoveRuntimeSingleton_t;