From 308512352e4eea5c75841f904b1bbb93a937f2a2 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 23 Mar 2016 15:21:19 -0700 Subject: [PATCH] More planner cleanup --- src/plan/arc.h | 7 ++- src/plan/exec.c | 124 ++++++++++++++++++++++---------------------- src/plan/feedhold.c | 2 +- src/plan/planner.h | 46 ++++++---------- 4 files changed, 86 insertions(+), 93 deletions(-) diff --git a/src/plan/arc.h b/src/plan/arc.h index 272ddb7..2d05be3 100644 --- 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 2c68580..1bb6106 100644 --- 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 8016c18..266d053 100644 --- 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 c70413e..1114605 100644 --- 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; -- 2.27.0