From 1b876cb6b258a994453d23f1d916635224d3655c Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 30 Aug 2016 02:31:11 -0700 Subject: [PATCH] More code cleanup, fixed program stop --- MoveLifecycle.md | 13 ++-- src/machine.c | 13 ++-- src/plan/buffer.h | 7 +- src/plan/exec.c | 185 ++++++++++++++------------------------------ src/plan/feedhold.c | 13 ++-- src/plan/planner.c | 4 +- src/plan/planner.h | 6 +- src/util.h | 30 +++---- 8 files changed, 100 insertions(+), 171 deletions(-) diff --git a/MoveLifecycle.md b/MoveLifecycle.md index 58015ec..f9bdef1 100644 --- a/MoveLifecycle.md +++ b/MoveLifecycle.md @@ -70,14 +70,11 @@ later for position correction which adjust position for rounding errors. ### Move execution After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``, ``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. Each -of these functions are called repeatedly until the section finishes. They -advance through states ``SECTION_NEW`` which initializes the section, -``SECTION_1st_HALF`` which executes the first half of the S-curve and -``SECTION_2nd_HALF``. If any sections have zero length they are skipped and -execution is passed immediately to the next section. During each section -forward differencing is used to map the trapezoid computed during the planning -stage to a fifth-degree Bezier polynomial S-curve. The curve is used to find -the next target position. +of these functions are called repeatedly until the section finishes. If any +sections have zero length they are skipped and execution is passed immediately +to the next section. During each section forward differencing is used to map +the trapezoid computed during the planning stage to a fifth-degree Bezier +polynomial S-curve. The curve is used to find the next target position. ``_exec_aline_segment()`` is called for each non-zero section to convert the computed target position to target steps by calling ``mp_kinematics()``. The diff --git a/src/machine.c b/src/machine.c index 8b86bf5..c84bf68 100644 --- a/src/machine.c +++ b/src/machine.c @@ -75,18 +75,16 @@ #include "switch.h" #include "hardware.h" #include "util.h" -#include "usart.h" // for serial queue flush #include "estop.h" #include "report.h" #include "homing.h" #include "plan/planner.h" -#include "plan/buffer.h" -#include "plan/feedhold.h" #include "plan/dwell.h" #include "plan/command.h" #include "plan/arc.h" #include "plan/line.h" +#include "plan/state.h" #include #include @@ -1106,10 +1104,15 @@ static void _exec_program_end(float *value, float *flag) { } +static void _exec_program_stop(float *value, float *flag) { + mp_set_hold_state(FEEDHOLD_HOLD); +} + + /// M0 Queue a program stop void mach_program_stop() { - // TODO actually stop program - // Need to queue a feedhold event in the planner buffer + float value[AXES] = {0}; + mp_queue_command(_exec_program_stop, value, value); } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index b8bbbe3..5198fbe 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -40,18 +40,13 @@ typedef enum { // bf->move_type values MOVE_TYPE_COMMAND, // general command } moveType_t; + typedef enum { MOVE_OFF, // move inactive (MUST BE ZERO) MOVE_NEW, // initial value MOVE_RUN, // general run state (for non-acceleration moves) } moveState_t; -typedef enum { - SECTION_OFF, // section inactive - SECTION_NEW, // uninitialized section - SECTION_1st_HALF, // first half of S curve - SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) -} sectionState_t; // All the enums that equal zero must be zero. Don't change this typedef enum { // bf->buffer_state values diff --git a/src/plan/exec.c b/src/plan/exec.c index a0a559f..ba9f401 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -33,7 +33,6 @@ #include "motor.h" #include "util.h" #include "report.h" -#include "estop.h" #include "state.h" #include "config.h" @@ -42,7 +41,7 @@ #include -/* Segment runner helper +/*** Segment runner * * Notes on step error correction: * @@ -70,8 +69,7 @@ static stat_t _exec_aline_segment() { // head, body or tail end. Otherwise, if not at a section waypoint // compute target from segment time and velocity. Don't do waypoint // correction if you are going into a hold. - if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && - mp_get_state() == STATE_RUNNING) + if (!--mr.segment_count && !mr.section_new && mp_get_state() == STATE_RUNNING) copy_vector(mr.ms.target, mr.waypoint[mr.section]); else { @@ -116,7 +114,7 @@ static stat_t _exec_aline_segment() { } -/* Forward difference math explained: +/*** Forward difference math explained: * * We are using a quintic (fifth-degree) Bezier polynomial for the * velocity curve. This gives us a "linear pop" velocity curve; @@ -186,7 +184,7 @@ static stat_t _exec_aline_segment() { * http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 * for an example of how to calculate F_0 - F_5 for a cubic bezier * curve. Since this is a quintic bezier curve, we need to extend - * the formulas somewhat. I'll not go into the long-winded + * the formulas somewhat. I'll not go into the long-winded * step-by-step here, but it gives the resulting formulas: * * a = A, b = B, c = C, d = D, e = E, f = F @@ -222,7 +220,7 @@ static stat_t _exec_aline_segment() { * Normally, we could then assign t = 0, use the A-F values from * above, and get out initial F_* values. However, for the sake of * "averaging" the velocity of each segment, we actually want to have - * the initial V be be at t = h/2 and iterate I-1 times. So, the + * the initial V be be at t = h/2 and iterate I-1 times. So, the * resulting F_* values are (steps not shown): * * F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh @@ -253,54 +251,42 @@ static void _init_forward_diffs(float Vi, float Vt) { mr.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4; mr.forward_diff[0] = 120.0 * Ah_5; - // Calculate the initial velocity by calculating V(h/2) + // Calculate the initial velocity by calculating V(h / 2) float half_h = h / 2.0; float half_Ch_3 = C * half_h * half_h * half_h; float half_Bh_4 = B * half_h * half_h * half_h * half_h; float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h; + mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; } -/// helper for deceleration section -static stat_t _exec_aline_tail() { - if (mr.section_state == SECTION_NEW) { // Initialization - if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move +/// Common code for head and tail sections +static stat_t _exec_aline_ends(float length, float entry_velocity, + float exit_velocity) { + if (mr.section_new) { + if (fp_ZERO(length)) return STAT_OK; // end the move - // len/avg. velocity - mr.ms.move_time = - 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); - // # of segments for the section + // len / avg. velocity + mr.ms.move_time = 2 * length / (entry_velocity + exit_velocity); mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); - // time to advance for each segment mr.segment_time = mr.ms.move_time / mr.segments; - _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); mr.segment_count = (uint32_t)mr.segments; + _init_forward_diffs(entry_velocity, exit_velocity); + if (mr.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position - mr.section = SECTION_TAIL; - mr.section_state = SECTION_1st_HALF; - } - - // For forward differencing we should have only one segment in - // SECTION_1st_HALF. However, if it returns STAT_OK, then there was only - // one segment in this section. - // First half - convex part (period 4) - if (mr.section_state == SECTION_1st_HALF) { + // Do first segment if (_exec_aline_segment() == STAT_OK) { - // Indicate that we completed section 2 - mr.section_state = SECTION_2nd_HALF; + mr.section_new = false; return STAT_OK; + } - } else mr.section_state = SECTION_2nd_HALF; - - return STAT_EAGAIN; - } + mr.section_new = false; - // Second half - concave part (period 5) - if (mr.section_state == SECTION_2nd_HALF) { + } else { mr.segment_velocity += mr.forward_diff[4]; if (_exec_aline_segment() == STAT_OK) return STAT_OK; @@ -316,13 +302,19 @@ static stat_t _exec_aline_tail() { } -/// Helper for cruise section +/// Callback for tail section +static stat_t _exec_aline_tail() { + mr.section = SECTION_TAIL; + return _exec_aline_ends(mr.tail_length, mr.cruise_velocity, mr.exit_velocity); +} + + +/// Callback for cruise section static stat_t _exec_aline_body() { - if (mr.section_state == SECTION_NEW) { + if (mr.section_new) { if (fp_ZERO(mr.body_length)) { mr.section = SECTION_TAIL; - - return _exec_aline_tail(); // skip ahead to tail periods + return _exec_aline_tail(); // skip to tail } mr.ms.move_time = mr.body_length / mr.cruise_velocity; @@ -335,87 +327,35 @@ static stat_t _exec_aline_body() { return STAT_MINIMUM_TIME_MOVE; // exit without advancing position mr.section = SECTION_BODY; - - // Use SECTION_2nd_HALF so last segment detection works - mr.section_state = SECTION_2nd_HALF; + mr.section_new = false; } - if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3) - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move - - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - } + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + mr.section = SECTION_TAIL; + mr.section_new = true; + } return STAT_EAGAIN; } -/// Helper for acceleration section +/// Callback for head section static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) - if (fp_ZERO(mr.head_length)) { - mr.section = SECTION_BODY; - - return _exec_aline_body(); // skip ahead to the body generator - } - - // Time for entire accel region - mr.ms.move_time = - 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); - // # of segments for the section - mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); - mr.segment_time = mr.ms.move_time / mr.segments; - _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); - mr.segment_count = (uint32_t)mr.segments; - - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - - mr.section = SECTION_HEAD; - // Note: Set to SECTION_1st_HALF for one segment - mr.section_state = SECTION_1st_HALF; - } - - // For forward differencing we should have one only segment in - // SECTION_1st_HALF. However, if it returns STAT_OK, then there was only - // one segment in this section. - // First half (concave part of accel curve) - if (mr.section_state == SECTION_1st_HALF) { - if (_exec_aline_segment() == STAT_OK) { - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - - } else mr.section_state = SECTION_2nd_HALF; - - return STAT_EAGAIN; - } - - // Second half (convex part of accel curve) - if (mr.section_state == SECTION_2nd_HALF) { - mr.segment_velocity += mr.forward_diff[4]; - - if (_exec_aline_segment() == STAT_OK) { - if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length)) - return STAT_OK; // ends the move - - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; + mr.section = SECTION_HEAD; + stat_t status = + _exec_aline_ends(mr.head_length, mr.entry_velocity, mr.cruise_velocity); - } else { - 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]; - } + if (status == STAT_OK) { + mr.section = SECTION_BODY; + if (mr.section_new) return _exec_aline_body(); + mr.section_new = true; } - return STAT_EAGAIN; + return status; } -/// Initializes a new planner buffer +/// Initializes move runtime with a new planner buffer static stat_t _exec_aline_init(mpBuf_t *bf) { // Stop here if holding if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP; @@ -427,19 +367,19 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { // Remove zero length lines. Short lines have already been removed. if (fp_ZERO(bf->length)) { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; + mr.active = false; // reset mr buffer bf->nx->replannable = false; // prevent overplanning (Note 2) mp_free_run_buffer(); // free buffer return STAT_NOOP; } - // Initialize the move runtime + // Initialize move runtime bf->move_state = MOVE_RUN; - mr.move_state = MOVE_RUN; + mr.active = true; mr.section = SECTION_HEAD; - mr.section_state = SECTION_NEW; + mr.section_new = true; + mr.jerk = bf->jerk; mr.head_length = bf->head_length; mr.body_length = bf->body_length; @@ -449,7 +389,7 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { mr.exit_velocity = bf->exit_velocity; copy_vector(mr.unit, bf->unit); - copy_vector(mr.final_target, bf->ms.target); // save move final target + copy_vector(mr.final_target, bf->ms.target); // Generate waypoints for position correction at section ends for (int axis = 0; axis < AXES; axis++) { @@ -531,13 +471,6 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { * * from _NEW to _RUN on first call (sub_state set to _OFF) * from _RUN to _OFF on final call or just remain _OFF - * - * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, - * _TAIL. Within each section state may be: - * - * _NEW - trigger initialization - * _RUN1 - run the first part - * _RUN2 - run the second part */ stat_t mp_exec_aline(mpBuf_t *bf) { if (bf->move_state == MOVE_OFF) return STAT_NOOP; @@ -545,13 +478,13 @@ stat_t mp_exec_aline(mpBuf_t *bf) { stat_t status = STAT_OK; // Start a new move - if (mr.move_state == MOVE_OFF) { + if (!mr.active) { status = _exec_aline_init(bf); if (status != STAT_OK) return status; } - // Main segment processing dispatch. From this point on the contents of the - // bf buffer do not affect execution. + // Main segment dispatch. From this point on the contents of the bf buffer + // do not affect execution. switch (mr.section) { case SECTION_HEAD: status = _exec_aline_head(); break; case SECTION_BODY: status = _exec_aline_body(); break; @@ -570,10 +503,10 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // STAT_OK MOVE_NEW mr done; bf must be run again // (it's been reused) if (status != STAT_EAGAIN) { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; + mr.active = false; // reset mr buffer bf->nx->replannable = false; // prevent overplanning (Note 2) - + // Note, feedhold.c may change bf->move_state to reuse this buffer so it + // can plan the deceleration. if (bf->move_state == MOVE_RUN) mp_free_run_buffer(); } @@ -583,7 +516,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { /// Dequeues buffer and executes move callback stat_t mp_exec_move() { - if (estop_triggered()) return STAT_MACHINE_ALARMED; + if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED; mpBuf_t *bf = mp_get_run_buffer(); if (!bf) return STAT_NOOP; // nothing running diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 89e1367..73ae7b1 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -38,6 +38,7 @@ #include #include + /* Feedhold is executed as hold state transitions executed inside * _exec_aline() and main loop callbacks to mp_plan_hold_callback() * @@ -106,7 +107,7 @@ static float _compute_next_segment_velocity() { } -/// replan block list to execute hold +/// Replan block list to execute hold void mp_plan_hold_callback() { if (mp_get_hold_state() != FEEDHOLD_PLAN) return; // not planning a feedhold @@ -123,11 +124,11 @@ void mp_plan_hold_callback() { float braking_length = mp_get_target_length(braking_velocity, 0, bp); // Hack to prevent Case 2 moves for perfect-fit decels. Happens in - // homing situations. The real fix: The braking velocity cannot + // homing situations. The real fix: The braking velocity cannot // simply be the mr.segment_velocity as this is the velocity of the // last segment, not the one that's going to be executed next. The // braking_velocity needs to be the velocity of the next segment - // that has not yet been computed. In the mean time, this hack will + // that has not yet been computed. In the mean time, this hack will // work. if (mr_available_length < braking_length && fp_ZERO(bp->exit_velocity)) braking_length = mr_available_length; @@ -139,7 +140,7 @@ void mp_plan_hold_callback() { mr.tail_length = braking_length; mr.cruise_velocity = braking_velocity; mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; + mr.section_new = true; // re-use bp+0 to be the hold point and to run the remaining block length bp->length = mr_available_length - braking_length; @@ -157,7 +158,7 @@ void mp_plan_hold_callback() { // Case 2: deceleration exceeds length remaining in mr buffer // First, replan mr to minimum (but non-zero) exit velocity mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; + mr.section_new = true; mr.tail_length = mr_available_length; mr.cruise_velocity = braking_velocity; mr.exit_velocity = @@ -204,5 +205,5 @@ void mp_plan_hold_callback() { _reset_replannable_list(); // replan all the blocks mp_plan_block_list(mp_get_last_buffer(), true); - mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit + mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate } diff --git a/src/plan/planner.c b/src/plan/planner.c index b6ffa51..29a8057 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -171,7 +171,7 @@ float mp_get_runtime_work_position(uint8_t axis) { /// FALSE you know the queue is empty and the motors have stopped. uint8_t mp_get_runtime_busy() { if (mp_get_state() == STATE_ESTOPPED) return false; - return st_runtime_isbusy() || mr.move_state == MOVE_RUN; + return st_runtime_isbusy() || mr.active; } @@ -594,7 +594,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { mpBuf_t *bp = bf; - // Backward planning pass. Find first block and update the braking velocities. + // Backward planning pass. Find first block and update braking velocities. // At the end *bp points to the buffer before the first block. while ((bp = mp_get_prev_buffer(bp)) != bf) { if (!bp->replannable) break; diff --git a/src/plan/planner.h b/src/plan/planner.h index 329fdb9..1d76946 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -75,9 +75,9 @@ typedef enum { typedef struct mpMoveRuntimeSingleton { // persistent runtime variables - uint8_t move_state; // state of the overall move - uint8_t section; // what section is the move in? - uint8_t section_state; // state within a move section + bool active; // True if a move is running + moveSection_t section; // what section is the move in? + bool section_new; // true if it's a new section /// unit vector for axis scaling & planning float unit[AXES]; diff --git a/src/util.h b/src/util.h index c92205a..332642b 100644 --- a/src/util.h +++ b/src/util.h @@ -68,33 +68,33 @@ uint16_t compute_checksum(char const *string, const uint16_t length); // side-effect safe forms of min and max #ifndef max -#define max(a,b) \ +#define max(a, b) \ ({ __typeof__ (a) termA = (a); \ __typeof__ (b) termB = (b); \ - termA>termB ? termA:termB; }) + termA > termB ? termA : termB;}) #endif #ifndef min -#define min(a,b) \ +#define min(a, b) \ ({ __typeof__ (a) term1 = (a); \ __typeof__ (b) term2 = (b); \ - term1 EPSILON) +#define fp_NE(a, b) (fabs(a - b) > EPSILON) #endif #ifndef fp_ZERO #define fp_ZERO(a) (fabs(a) < EPSILON) @@ -112,14 +112,14 @@ uint16_t compute_checksum(char const *string, const uint16_t length); #endif // Constants -#define MAX_LONG (2147483647) -#define MAX_ULONG (4294967295) -#define MM_PER_INCH (25.4) +#define MAX_LONG 2147483647 +#define MAX_ULONG 4294967295 +#define MM_PER_INCH 25.4 #define INCHES_PER_MM (1 / 25.4) -#define MICROSECONDS_PER_MINUTE ((float)60000000) -#define uSec(a) ((float)(a * MICROSECONDS_PER_MINUTE)) +#define MICROSECONDS_PER_MINUTE 60000000.0 +#define uSec(a) ((a) * MICROSECONDS_PER_MINUTE) -#define RADIAN (57.2957795) +#define RADIAN 57.2957795 #ifndef M_SQRT3 -#define M_SQRT3 (1.73205080756888) +#define M_SQRT3 1.73205080756888 #endif -- 2.27.0