From ad106ca7f39dfe438a68434df7cdb41614447f29 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sat, 18 Feb 2017 10:21:13 -0500 Subject: [PATCH] Changes to mp_forward_plan() and _plan_commands() to properly report cases where commands are planned multiple times, and return correct status code. Also required changes to Feedhold Case (5, continued) in mp_exec_aline(); Change IN_DEBUGGER cases; Changes to MIN_SEGMENT_MS and related definitions --- g2core/g2core.cppproj | 4 +-- g2core/plan_exec.cpp | 62 ++++++++++++++++++++++++++----------------- g2core/plan_zoid.cpp | 4 +-- g2core/planner.cpp | 4 +-- g2core/planner.h | 9 ++++--- g2core/util.h | 2 +- 6 files changed, 48 insertions(+), 37 deletions(-) diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj index 57a1fc79..a4704359 100644 --- a/g2core/g2core.cppproj +++ b/g2core/g2core.cppproj @@ -73,7 +73,7 @@ SWD com.atmel.avrdbg.tool.atmelice - J41800019454 + J41800036434 Atmel-ICE True @@ -100,7 +100,7 @@ True true - J41800019454 + J41800036434 0x284E0A60 10000000 diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 2cc0a55e..49ba19c9 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -149,13 +149,14 @@ static void _init_forward_diffs(float v_0, float v_1); * (Note: all COMMAND(s) in j. should be in PLANNED state) */ -static mpBuf_t *_plan_commands(mpBuf_t *bf) // plan or skip commands; return bf past last command +static mpBuf_t *_plan_commands(mpBuf_t *bf, bool &planned) // plan or skip commands; return bf past last command { + planned = false; + // must test for buffer state first as the buffer is only "safe" once it's >= PREPPED while ((bf->buffer_state >= MP_BUFFER_PREPPED) && (bf->block_type >= BLOCK_TYPE_COMMAND)) { - if (bf->buffer_state != MP_BUFFER_PLANNED) { // skip already planned buffers - bf->buffer_state = MP_BUFFER_PLANNED; // "planning" is just setting the state (for now) - } + bf->buffer_state = MP_BUFFER_PLANNED; // "planning" is just setting the state (for now) + planned = true; bf = bf->nx; } return (bf); @@ -179,7 +180,7 @@ static stat_t _plan_move(mpBuf_t *bf, float entry_velocity) // diagnostic traps -#if IN_DEBUGGER == 1 +#ifdef IN_DEBUGGER if (block->exit_velocity > block->cruise_velocity) { __asm__("BKPT"); // exit > cruise after calculate_block } @@ -216,9 +217,12 @@ stat_t mp_forward_plan() } // bf points to command; start cases 1f, 1g, 1h, 1i, 1j, 1k, 2c, 2d, 2e, 2h, 2i, 2j + bool planned_something = false; + if (bf->block_type != BLOCK_TYPE_ALINE) { // meaning it's a COMMAND - bf = _plan_commands(bf); // plan commands or skip past already planned commands - // bf now points to the first non-command buffer past the command(s) + bf = _plan_commands(bf, planned_something); // plan commands or skip past already planned commands + + // Note: bf now points to the first non-command buffer past the command(s) if ((bf->block_type == BLOCK_TYPE_ALINE) && (bf->buffer_state > MP_BUFFER_PREPPED )) { // case 1i entry_velocity = mr->r->exit_velocity; // set entry_velocity for Note 1a } @@ -228,12 +232,11 @@ stat_t mp_forward_plan() // process move if (bf->block_type == BLOCK_TYPE_ALINE) { // do cases 1a - 1e; finish cases 1f - 1k if (bf->buffer_state == MP_BUFFER_PREPPED) {// do 1a; finish 1f, 1j, 2d, 2i - return (_plan_move(bf, entry_velocity)); - } else { - return (STAT_NOOP); // do 1b, 1c, 1d, 1e; finish 1g, 1h, 1j, 1k, 2e, 2j + _plan_move(bf, entry_velocity); + planned_something = true; } } - return (STAT_OK); // report that we planned something... + return (planned_something ? STAT_OK : STAT_NOOP); } /************************************************************************* @@ -265,7 +268,7 @@ stat_t mp_exec_move() // first-time operations if (bf->buffer_state != MP_BUFFER_RUNNING) { if ((bf->buffer_state < MP_BUFFER_PREPPED) && (cm->motion_state == MOTION_RUN)) { -#if IN_DEBUGGER == 1 +#ifdef IN_DEBUGGER __asm__("BKPT"); // mp_exec_move() buffer is not prepped #endif // IMPORTANT: We can't rpt_exception from here! @@ -280,7 +283,7 @@ stat_t mp_exec_move() if (bf->buffer_state == MP_BUFFER_PREPPED) { if (cm->motion_state == MOTION_RUN) { -#if IN_DEBUGGER == 1 +#ifdef IN_DEBUGGER // __asm__("BKPT"); // we are running but don't have a block planned #endif } @@ -558,7 +561,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) return (STAT_OK); // hold here. No more movement } - // Case (5) - Decelerated to zero + // Case (5) - Decelerated to zero. See also Feedhold Case (5, continued), toward end of mp_exec_aline() // Update the run buffer then force a replan of the whole planner queue. Replans from 0 velocity if (cm->hold_state == FEEDHOLD_DECEL_END) { mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move @@ -640,26 +643,35 @@ stat_t mp_exec_aline(mpBuf_t *bf) else if (mr->section == SECTION_TAIL) { status = _exec_aline_tail(bf);} else { return(cm_panic(STAT_INTERNAL_ERROR, "exec_aline()"));} // never supposed to get here - // We can't use the if/else block above, since the head may call body, and body call tail, so we wait till after - if ((mr->section == SECTION_TAIL) // Once we're in the tail, we can't plan the block anymore - || ((mr->section == SECTION_BODY) && (mr->segment_count < 3))) { // or are too close to the end of the body - + // Conditionally set the move to be unplannable. We can't use the if/else block above, + // since the head may call a body or a tail, and a body call tail, so we wait till after. + // + // Conditions are: + // - Allow 3 segments: 1 segment isn't enough, because there's one running as we execute, + // so it has to be the next one. There's a slight possibility we'll miss that, since we + // didn't necessarily start at the beginning, so three. + // - If it's a head/tail move and we've started the head we can't replan it anyway as + // the head can't be interrupted, and the tail is already as sharp as it can be (or there'd be a body) + // - ...so if you are in a body mark the body unplannable if we are too close to its end. + if ((mr->section == SECTION_TAIL) || ((mr->section == SECTION_BODY) && (mr->segment_count < 3))) { bf->plannable = false; } - // Feedhold Case (5): Look for the end of the deceleration to go into HOLD state - if ((cm->hold_state == FEEDHOLD_DECEL_TO_ZERO) && (status == STAT_OK)) { - cm->hold_state = FEEDHOLD_DECEL_END; - bf->block_state = BLOCK_INITIAL_ACTION; // reset bf so it can restart the rest of the move + // Feedhold Case (5, continued): Look for the end of the deceleration to go into HOLD state + if (cm->hold_state == FEEDHOLD_DECEL_TO_ZERO) { + if ((status == STAT_OK) || (status == STAT_NOOP)) { + cm->hold_state = FEEDHOLD_DECEL_END; + bf->block_state = BLOCK_INITIAL_ACTION; // reset bf so it can restart the rest of the move + } } // There are 4 things that can happen here depending on return conditions: - // status bf->block_state Description - // ----------- -------------- ---------------------------------------- + // status bf->block_state Description + // ----------- -------------- ---------------------------------------- // STAT_EAGAIN mr buffer has more segments to run // STAT_OK BLOCK_ACTIVE mr and bf buffers are done // STAT_OK BLOCK_INITIAL_ACTION mr done; bf must be run again (it's been reused) - // There is no fourth thing. Nobody expects the Spanish Inquisition + // STAT_NOOP treated as a STAT_OK if (status == STAT_EAGAIN) { sr_request_status_report(SR_REQUEST_TIMED); // continue reporting mr buffer diff --git a/g2core/plan_zoid.cpp b/g2core/plan_zoid.cpp index 943b4216..7deecd09 100644 --- a/g2core/plan_zoid.cpp +++ b/g2core/plan_zoid.cpp @@ -34,7 +34,7 @@ // DIAGNOSTICS -//#if IN_DEBUGGER < 1 +//#ifndef IN_DEBUGGER #define LOG_RETURN(msg) // LOG_RETURN with no action (production) //#else //#include "xio.h" @@ -50,7 +50,7 @@ //#define LOG_RETURN(msg) { _logger(msg, bf); } //#endif -#if IN_DEBUGGER < 1 +#ifndef IN_DEBUGGER #define TRAP_ZERO(t,m) #else #define TRAP_ZERO(t, m) \ diff --git a/g2core/planner.cpp b/g2core/planner.cpp index f3dce327..42ecc046 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -821,12 +821,10 @@ mpBuf_t * mp_get_run_buffer() // Clearing and advancing must be done atomically as other interrupts may be using the run buffer bool mp_free_run_buffer() // EMPTY current run buffer & advance to the next { - mpPlannerQueue_t *q = &(mp->q); - + mpPlannerQueue_t *q = &(mp->q); mpBuf_t *r_now = q->r; // save this pointer is to avoid a race condition when clearing the buffer _audit_buffers(); // DIAGNOSTIC audit for buffer chain integrity (only runs in DEBUG mode) - q->r = q->r->nx; // advance to next run buffer first, _clear_buffer(r_now); // ... then clear out the old buffer (& set MP_BUFFER_EMPTY) q->buffers_available++; diff --git a/g2core/planner.h b/g2core/planner.h index ea447fb9..8602140f 100644 --- a/g2core/planner.h +++ b/g2core/planner.h @@ -250,9 +250,12 @@ typedef enum { #define JUNCTION_INTEGRATION_MIN (0.05) // JT minimum allowable setting #define JUNCTION_INTEGRATION_MAX (5.00) // JT maximum allowable setting +#ifndef MIN_SEGMENT_MS // boards can override this value in hardware.h #define MIN_SEGMENT_MS ((float)0.75) // minimum segment milliseconds -#define NOM_SEGMENT_MS ((float)1.5) // nominal segment ms (at LEAST MIN_SEGMENT_MS * 2) -#define MIN_BLOCK_MS ((float)1.5) // minimum block (whole move) milliseconds +#endif +#define NOM_SEGMENT_MS ((float)MIN_SEGMENT_MS * 2) // nominal segment ms (at LEAST MIN_SEGMENT_MS * 2) +#define MIN_BLOCK_MS ((float)MIN_SEGMENT_MS * 2) // minimum block (whole move) milliseconds + #define BLOCK_TIMEOUT_MS ((float)30.0) // MS before deciding there are no new blocks arriving #define PHAT_CITY_MS ((float)100.0) // if you have at least this much time in the planner @@ -395,7 +398,6 @@ typedef struct mpBuffer { plannable_length = 0; meet_iterations = 0; #endif - buffer_state = MP_BUFFER_EMPTY; block_type = BLOCK_TYPE_NULL; block_state = BLOCK_INACTIVE; @@ -405,7 +407,6 @@ typedef struct mpBuffer { unit[i] = 0; axis_flags[i] = 0; } - plannable = false; length = 0.0; block_time = 0.0; diff --git a/g2core/util.h b/g2core/util.h index d8907f5b..75471031 100644 --- a/g2core/util.h +++ b/g2core/util.h @@ -59,7 +59,7 @@ inline void _debug_trap(const char *reason) { // We might be able to put a print here, but it MIGHT interrupt other output // and might be deep in an ISR, so we had better just _NOP() and hope for the best. __NOP(); -#if IN_DEBUGGER == 1 +#ifdef IN_DEBUGGER __asm__("BKPT"); #endif }