Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-09-21 18:56:31 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Alden Hart
2017-02-18 10:21:13 -05:00
parent cfd1990255
commit ad106ca7f3
6 changed files with 48 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/g2core.cppproj
+2 -2
View File
@@ -73,7 +73,7 @@
<InterfaceName>SWD</InterfaceName>
</ToolOptions>
<ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
<ToolNumber>J41800019454</ToolNumber>
<ToolNumber>J41800036434</ToolNumber>
<ToolName>Atmel-ICE</ToolName>
</com_atmel_avrdbg_tool_atmelice>
<UseGdb>True</UseGdb>
@@ -100,7 +100,7 @@
<HWProgramCounterSampling>True</HWProgramCounterSampling>
</PercepioTrace>
<preserveEEPROM>true</preserveEEPROM>
<avrtoolserialnumber>J41800019454</avrtoolserialnumber>
<avrtoolserialnumber>J41800036434</avrtoolserialnumber>
<avrdeviceexpectedsignature>0x284E0A60</avrdeviceexpectedsignature>
<avrtoolinterfaceclock>10000000</avrtoolinterfaceclock>
<custom>
g2core/plan_exec.cpp
+37 -25
View File
@@ -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 <don't care> 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 <don't care> treated as a STAT_OK
if (status == STAT_EAGAIN) {
sr_request_status_report(SR_REQUEST_TIMED); // continue reporting mr buffer
g2core/plan_zoid.cpp
+2 -2
View File
@@ -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) \
g2core/planner.cpp
+1 -3
View File
@@ -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++;
g2core/planner.h
+5 -4
View File
@@ -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;
g2core/util.h
+1 -1
View File
@@ -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
}