diff --git a/g2core/g2core.cppproj b/g2core/g2core.cppproj
index eef20ce4..bb1cf7a0 100644
--- a/g2core/g2core.cppproj
+++ b/g2core/g2core.cppproj
@@ -68,12 +68,12 @@
- 2000000
+ 10000000
SWD
com.atmel.avrdbg.tool.atmelice
- J41800036434
+ J41800030015
Atmel-ICE
True
@@ -100,9 +100,9 @@
True
true
- J41800036434
+ J41800030015
0x284E0A60
- 2000000
+ 10000000
diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp
index ec5f69c3..73173b45 100644
--- a/g2core/plan_exec.cpp
+++ b/g2core/plan_exec.cpp
@@ -43,7 +43,8 @@ static stat_t _exec_aline_head(mpBuf_t *bf); // passing bf because body might ne
static stat_t _exec_aline_body(mpBuf_t *bf); // passing bf so that body can extend itself if the exit velocity rises.
static stat_t _exec_aline_tail(mpBuf_t *bf);
static stat_t _exec_aline_segment(void);
-static stat_t _exec_feedhold_processing(mpBuf_t *bf);
+static void _exec_aline_normalize_block(mpBlockRuntimeBuf_t *b);
+static stat_t _exec_aline_feedhold_processing(mpBuf_t *bf);
static void _init_forward_diffs(float v_0, float v_1);
@@ -394,8 +395,6 @@ stat_t mp_exec_move()
stat_t mp_exec_aline(mpBuf_t *bf)
{
- stat_t status;
-
if (bf->block_state == BLOCK_INACTIVE) {
return (STAT_NOOP);
}
@@ -404,16 +403,15 @@ stat_t mp_exec_aline(mpBuf_t *bf)
if (mr->block_state == BLOCK_INACTIVE) {
// ASSERTIONS
-
// Zero length moves (and other too-short moves) should have already been removed earlier
// But let's still alert the condition should it ever occur
debug_trap_if_zero(bf->length, "mp_exec_aline() zero length move");
- // Equalities that must be true for this to work:
- // entry velocity <= cruise velocity &&
- // exit velocity <= cruise velocity
+ // These equalities in the assertions must be true for this to work:
+ // entry_velocity <= cruise_velocity
+ // exit_velocity <= cruise_velocity
//
- // Even if the move is head or tail only, cruise velocity needs to be valid.
+ // NB: Even if the move is head or tail only, cruise velocity needs to be valid.
// This is because a "head" is *always* entry->cruise, and a "tail" is *always* cruise->exit,
// even if there are no other sections in the move. (This is a significant time savings.)
debug_trap_if_true((mr->entry_velocity > mr->r->cruise_velocity),
@@ -431,61 +429,11 @@ stat_t mp_exec_aline(mpBuf_t *bf)
// !!! THIS IS THE ONLY PLACE WHERE mr->r AND mr->p ARE ALLOWED TO BE CHANGED !!!
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// Swap P and R blocks
- mr->previous_exit_velocity = mr->r->exit_velocity; // +++++ DIAGNOSTIC
mr->r = mr->p; // we are now going to run the planning block
mr->p = mr->p->nx; // re-use the old running block as the new planning block
-
-
+
// Check to make sure no sections are less than MIN_SEGMENT_TIME & adjust if necessary
- if ((mr->r->head_length > 0) && (mr->r->head_time < MIN_SEGMENT_TIME)) {
- // Compute the new body time. head_time !== body_time
- mr->r->body_length += mr->r->head_length;
- mr->r->body_time = mr->r->body_length / mr->r->cruise_velocity;
- mr->r->head_length = 0;
- mr->r->head_time = 0;
- }
- if ((mr->r->tail_length > 0) && (mr->r->tail_time < MIN_SEGMENT_TIME)) {
- // Compute the new body time. tail_time !== body_time
- mr->r->body_length += mr->r->tail_length;
- mr->r->body_time = mr->r->body_length / mr->r->cruise_velocity;
- mr->r->tail_length = 0;
- mr->r->tail_time = 0;
- }
-
- // At this point, we've already possibly merged head and/or tail into the body.
- // If the body is still too "short" (brief) we *might* be able to add it to a head or tail.
- // If there's still a head or a tail, we will add the body to whichever there is, maybe both.
- // We saved it for last since it's the most expensive.
- if ((mr->r->body_length > 0) && (mr->r->body_time < MIN_SEGMENT_TIME)) {
-
- // We'll add the time to either the head or the tail or split it
- if (mr->r->tail_length > 0) {
- if (mr->r->head_length > 0) { // Split the body to the head and tail
- mr->r->head_length += mr->r->body_length * 0.5;
- mr->r->tail_length += mr->r->body_length * 0.5; // let the compiler optimize out one of these *
- mr->r->head_time = (2.0 * mr->r->head_length) / (mr->entry_velocity + mr->r->cruise_velocity);
- mr->r->tail_time = (2.0 * mr->r->tail_length) / (mr->r->cruise_velocity + mr->r->exit_velocity);
- mr->r->body_length = 0;
- mr->r->body_time = 0;
- } else { // Put it all in the tail
- mr->r->tail_length += mr->r->body_length;
- mr->r->tail_time = (2.0 * mr->r->tail_length) / (mr->r->cruise_velocity + mr->r->exit_velocity);
- mr->r->body_length = 0;
- mr->r->body_time = 0;
- }
- }
- else if (mr->r->head_length > 0) { // Put it all in the head
- mr->r->head_length += mr->r->body_length;
- mr->r->head_time = (2.0 * mr->r->head_length) / (mr->entry_velocity + mr->r->cruise_velocity);
- mr->r->body_length = 0;
- mr->r->body_time = 0;
- }
- else { // Uh oh! We have a move that's all body, and is still too short!!
- debug_trap("mp_exec_aline() - found a move that is too short");
- cs.exec_aline_assertion_failure = true;
- return (STAT_EXEC_ALINE_ASSERTION_FAILURE);
- }
- }
+ _exec_aline_normalize_block(mr->r);
// transfer move parameters from planner buffer to the runtime
copy_vector(mr->unit, bf->unit);
@@ -514,11 +462,12 @@ stat_t mp_exec_aline(mpBuf_t *bf)
// Feedhold Processing - We need to handle the following cases (listed in rough sequence order):
if (cm->motion_state == MOTION_HOLD) {
- if (cm->hold_state >= FEEDHOLD_ACTIONS_START) { // FEEDHOLD_ACTIONS_START, FEEDHOLD_ACTIONS_WAIT or FEEDHOLD HOLD
+ // if FEEDHOLD_ACTIONS_START, FEEDHOLD_ACTIONS_WAIT, FEEDHOLD HOLD or FEEDHOLD_P2_EXIT
+ if (cm->hold_state >= FEEDHOLD_ACTIONS_START) { // handles _exec_aline_feedhold_processing case (7)
return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. No more movement
}
- if (_exec_feedhold_processing(bf) == STAT_OK) { // anything but STAT_OK will continue processing the exec
- return (STAT_OK);
+ if (_exec_aline_feedhold_processing(bf) == STAT_OK) {
+ return (STAT_OK); // STAT_OK terminates aline execution for this move
}
}
@@ -527,7 +476,7 @@ stat_t mp_exec_aline(mpBuf_t *bf)
// NB: from this point on the contents of the bf buffer do not affect execution
//**** main dispatcher to process segments ***
- status = STAT_OK;
+ stat_t status = STAT_OK;
if (mr->section == SECTION_HEAD) { status = _exec_aline_head(bf); }
else if (mr->section == SECTION_BODY) { status = _exec_aline_body(bf); }
else if (mr->section == SECTION_TAIL) { status = _exec_aline_tail(bf); }
@@ -547,7 +496,8 @@ stat_t mp_exec_aline(mpBuf_t *bf)
bf->plannable = false;
}
- // Feedhold Case (5, continued): Look for the end of the deceleration to go into HOLD state
+ // Feedhold Case (3b): Look for the end of the deceleration to transition HOLD states
+ // This code sets states used by _exec_feedhold_processing() helper.
if (cm->hold_state == FEEDHOLD_DECEL_TO_ZERO) {
if ((status == STAT_OK) || (status == STAT_NOOP)) {
cm->hold_state = FEEDHOLD_DECEL_COMPLETE;
@@ -742,9 +692,9 @@ static void _init_forward_diffs(const float v_0, const float v_1)
mr->forward_diff_5 = const1*Ah_5 + 5.0*Bh_4 + const2*Ch_3;
mr->forward_diff_4 = const3*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_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;
// Calculate the initial velocity by calculating V(h/2)
const float half_h = h * 0.5; // h/2
@@ -784,7 +734,7 @@ static stat_t _exec_aline_head(mpBuf_t *bf)
}
if (mr->segment_time < MIN_SEGMENT_TIME) {
debug_trap("mr->segment_time < MIN_SEGMENT_TIME (head)");
- return(STAT_OK); // exit without advancing position, say we're done
+ return (STAT_OK); // exit without advancing position, say we're done
}
mr->section = SECTION_HEAD; // +++++ Redundant???
mr->section_state = SECTION_RUNNING;
@@ -794,7 +744,7 @@ static stat_t _exec_aline_head(mpBuf_t *bf)
if (_exec_aline_segment() == STAT_OK) { // set up for second half
if ((fp_ZERO(mr->r->body_length)) && (fp_ZERO(mr->r->tail_length))) {
- return(STAT_OK); // ends the move
+ return (STAT_OK); // ends the move
}
mr->section = SECTION_BODY; // advance to body
mr->section_state = SECTION_NEW;
@@ -805,7 +755,7 @@ static stat_t _exec_aline_head(mpBuf_t *bf)
mr->forward_diff_3 += mr->forward_diff_2;
mr->forward_diff_2 += mr->forward_diff_1;
}
- return(STAT_EAGAIN);
+ return (STAT_EAGAIN);
}
/*********************************************************************************************
@@ -828,7 +778,7 @@ static stat_t _exec_aline_body(mpBuf_t *bf)
mr->segment_count = (uint32_t)mr->segments;
if (mr->segment_time < MIN_SEGMENT_TIME) {
debug_trap("mr->segment_time < MIN_SEGMENT_TIME (body)");
- return(STAT_OK); // exit without advancing position, say we're done
+ return (STAT_OK); // exit without advancing position, say we're done
}
mr->section = SECTION_BODY; // +++++ Redundant???
@@ -836,12 +786,12 @@ static stat_t _exec_aline_body(mpBuf_t *bf)
}
if (_exec_aline_segment() == STAT_OK) { // OK means this section is done
if (fp_ZERO(mr->r->tail_length)) {
- return(STAT_OK); // ends the move
+ return (STAT_OK); // ends the move
}
mr->section = SECTION_TAIL; // advance to tail
mr->section_state = SECTION_NEW;
}
- return(STAT_EAGAIN);
+ return (STAT_EAGAIN);
}
/*********************************************************************************************
@@ -869,7 +819,7 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
}
if (mr->segment_time < MIN_SEGMENT_TIME) {
debug_trap("mr->segment_time < MIN_SEGMENT_TIME (tail)");
- return(STAT_OK); // exit without advancing position, say we're done
+ return (STAT_OK); // exit without advancing position, say we're done
}
mr->section = SECTION_TAIL; // +++++ Redundant???
mr->section_state = SECTION_RUNNING;
@@ -878,7 +828,7 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
}
if (_exec_aline_segment() == STAT_OK) {
- return(STAT_OK); // STAT_OK completes the move
+ return (STAT_OK); // STAT_OK completes the move
}
else if (!first_pass) {
mr->forward_diff_5 += mr->forward_diff_4;
@@ -886,7 +836,7 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
mr->forward_diff_3 += mr->forward_diff_2;
mr->forward_diff_2 += mr->forward_diff_1;
}
- return(STAT_EAGAIN);
+ return (STAT_EAGAIN);
}
/*********************************************************************************************
@@ -966,37 +916,103 @@ static stat_t _exec_aline_segment()
return (STAT_EAGAIN); // this section still has more segments to run
}
-
/*********************************************************************************************
- * _exec_feedhold_processing() -
+ * _exec_aline_normalize_block() - re-organize block to eliminate minimum time segments
*
- * Feedhold Processing - We need to handle the following cases (listed in rough sequence order):
- * (1) - We have a block midway through normal execution and a new feedhold request
- * (1a) - The deceleration will fit in the length remaining in the running block (mr)
- * (1b) - 1a, except the remaining length would be zero or EPSILON2 close to zero (unlikely)
- * (1c) - The deceleration will not fit in the running block
- * (2) - We have a new block and a new feedhold request that arrived at EXACTLY the same time (unlikely, but handled)
- * (3) - We are in the middle of a block
- * (3a) - The block is currently accelerating (we wait for the body to start)
- * (3b) - The block is in a body (or has not yet started the head) - start deceleration
- * (3c) - The block is currently in the tail (we wait until the end of the block)
- * (4) - We have decelerated a block to some velocity > zero (needs continuation in next block)
- * (5) - We have decelerated a block to zero velocity
- * (6) - We have finished all the runtime work now we have to wait for the steppers to stop
- * (6a) - It's a homing or probing feedhold - ditch the remaining buffer & go directly to OFF
- * (6b) - It's a p2 feedhold - ditch the remaining buffer & signal we want a p2 queue flush
- * (6c) - It's a normal feedhold - signal we want the entry action
- * (7) - The steppers have stopped. No motion should occur. Allows hold actions to complete
- * (8) - We are removing the hold state and there is queued motion (handled outside this routine)
- * (9) - We are removing the hold state and there is no queued motion (also handled outside this routine)
- *
- * Returning STAT_OK stops move execution
- * Returning STAT_EAGAIN continues execution of mp_exec_aline()
+ * Check to make sure no sections are less than MIN_SEGMENT_TIME & adjust if necessary
*/
-static stat_t _exec_feedhold_processing(mpBuf_t *bf)
+static void _exec_aline_normalize_block(mpBlockRuntimeBuf_t *b)
{
- // Case (6) - Wait for the steppers to stop
+
+ if ((b->head_length > 0) && (b->head_time < MIN_SEGMENT_TIME)) {
+ // Compute the new body time. head_time !== body_time
+ b->body_length += b->head_length;
+ b->body_time = b->body_length / b->cruise_velocity;
+ b->head_length = 0;
+ b->head_time = 0;
+ }
+ if ((b->tail_length > 0) && (b->tail_time < MIN_SEGMENT_TIME)) {
+ // Compute the new body time. tail_time !== body_time
+ b->body_length += b->tail_length;
+ b->body_time = b->body_length / b->cruise_velocity;
+ b->tail_length = 0;
+ b->tail_time = 0;
+ }
+
+ // At this point, we've already possibly merged head and/or tail into the body.
+ // If the body is still too "short" (brief) we *might* be able to add it to a head or tail.
+ // If there's still a head or a tail, we will add the body to whichever there is, maybe both.
+ // We saved it for last since it's the most expensive.
+ if ((b->body_length > 0) && (b->body_time < MIN_SEGMENT_TIME)) {
+
+ // We'll add the time to either the head or the tail or split it
+ if (b->tail_length > 0) {
+ if (b->head_length > 0) { // Split the body to the head and tail
+ b->head_length += b->body_length * 0.5;
+ b->tail_length += b->body_length * 0.5; // let the compiler optimize out one of these *
+ b->head_time = (2.0 * b->head_length) / (mr->entry_velocity + b->cruise_velocity);
+ b->tail_time = (2.0 * b->tail_length) / (b->cruise_velocity + b->exit_velocity);
+ b->body_length = 0;
+ b->body_time = 0;
+ } else { // Put it all in the tail
+ b->tail_length += b->body_length;
+ b->tail_time = (2.0 * b->tail_length) / (b->cruise_velocity + b->exit_velocity);
+ b->body_length = 0;
+ b->body_time = 0;
+ }
+ }
+ else if (b->head_length > 0) { // Put it all in the head
+ b->head_length += b->body_length;
+ b->head_time = (2.0 * b->head_length) / (mr->entry_velocity + b->cruise_velocity);
+ b->body_length = 0;
+ b->body_time = 0;
+ }
+ else { // Uh oh! We have a move that's all body, and is still too short!!
+ debug_trap("_exec_aline_normalize_block() - found a move that is too short");
+ }
+ }
+}
+
+/*********************************************************************************************
+ * _exec_aline_feedhold_processing() - feedhold helper for mp_exec_aline()
+ *
+ * This function performs the bulk of the feedhold state machine processing from within
+ * mp_exec_aline(). There is also a little chunk labeled "Feedhold Case (3-continued)".
+ * Feedhold processing mostly manages the deceleration phase into the hold, and sets
+ * state variables used in cycle_feedhold.cpp
+ *
+ * Returning STAT_OK ends the move. (i.e. returns STAT_OK from mp_exec_aline())
+ * Returning STAT_EAGAIN allows mp_exec_aline() to continue execution
+ *
+ * Feedhold Processing - We need to handle the following cases (listed in rough sequence order):
+ * (1) - Feedhold arrives while we are in the middle executing of a block
+ * (1a) - The block is currently accelerating - wait for the end of acceleration
+ * (1b) - The block is in a body - start deceleration
+ * (1b1) - The deceleration fits into the current block
+ * (1b2) - The deceleration does not fit and needs to continue in the next block
+ * (1c) - The block is in a head, but has not started execution yet - start deceleration
+ * (1c1) - The deceleration fits into the current block
+ * (1c2) - The deceleration does not fit and needs to continue in the next block
+ * (1d) - The block is currently in the tail - wait until the end of the block
+ * (1e) - We have a new block and a new feedhold request that arrived at EXACTLY the same time
+ * (unlikely, but handled as 1c).
+ * (2) - The block has decelerated to some velocity > zero, so needs continuation into next block
+ * (3) - The block has decelerated to zero velocity
+ * (3a) - The end of deceleration is detected (inline in mp_exec_aline())
+ * (3b) - The end of deceleration is signeled and transitioned
+ * (4) - We have finished all the runtime work now we have to wait for the motors to stop
+ * (4a) - It's a homing or probing feedhold - ditch the remaining buffer & go directly to OFF
+ * (4b) - It's a p2 feedhold - ditch the remaining buffer & signal we want a p2 queue flush
+ * (4c) - It's a normal feedhold - signal we want the p2 entry actions to execute
+ * (5) - The steppers have stopped. No motion should occur. Allows hold actions to complete
+ * (6) - Removing the hold state and there is queued motion - see cycle_feedhold.cpp
+ * (7) - Removing the hold state and there is no queued motion - see cycle_feedhold.cpp
+ */
+
+static stat_t _exec_aline_feedhold_processing(mpBuf_t *bf)
+{
+ // Case (4) - Wait for the steppers to stop
if (cm->hold_state == FEEDHOLD_MOTORS_STOPPING) {
if (mp_runtime_is_idle()) { // wait for steppers to actually finish
// finalize position and velocity
@@ -1019,7 +1035,7 @@ static stat_t _exec_feedhold_processing(mpBuf_t *bf)
return (STAT_OK); // hold here. No more movement
}
- // Case (5) - Decelerated to zero. See also Feedhold Case (5, continued), toward end of mp_exec_aline()
+ // Case (3b) - Decelerated to zero. See also Feedhold Case (3a) in mp_exec_aline()
// Update the run buffer then force a replan of the whole planner queue. Replans from zero velocity
if (cm->hold_state == FEEDHOLD_DECEL_COMPLETE) {
mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move
@@ -1031,15 +1047,15 @@ static stat_t _exec_feedhold_processing(mpBuf_t *bf)
mp_free_run_buffer();
}
mp_replan_queue(mp_get_r()); // make it replan all the blocks
- return (STAT_OK);
+ return (STAT_OK); // stop mp_exec_aline() from further execution
}
- // Cases (3), (1a, 1b, 1c), Case (2), Case (4)
+ // Cases (1x), Case (2)
// Build a tail-only move from here. Decelerate as fast as possible in the space available.
if ((cm->hold_state == FEEDHOLD_SYNC) ||
((cm->hold_state == FEEDHOLD_DECEL_CONTINUE) && (mr->block_state == BLOCK_INITIAL_ACTION))) {
- // Case (3c) - Already decelerating (in a tail), continue the deceleration.
+ // Case (1d) - Already decelerating (in a tail), continue the deceleration.
if (mr->section == SECTION_TAIL) { // if already in a tail don't decelerate. You already are
if (mr->r->exit_velocity < EPSILON2) { // allow near-zero velocities to be treated as zero
cm->hold_state = FEEDHOLD_DECEL_TO_ZERO;
@@ -1049,19 +1065,18 @@ static stat_t _exec_feedhold_processing(mpBuf_t *bf)
return (STAT_EAGAIN);
}
- // Case (3a) - Currently accelerating (in a head), skip and waited for body or tail
+ // Case (1a) - Currently accelerating (in a head), skip and waited for body or tail
// This is true because to do otherwise the jerk would not have returned to zero.
// Small exception, if we *just started* the head, then we're not actually accelerating yet.
if ((mr->section == SECTION_HEAD) && (mr->section_state != SECTION_NEW)) {
return (STAT_EAGAIN);
}
- // Case (3b) - Block is in a body, or about to start a new head. Turn it into a new tail.
- // In the new head case plan deceleration move (tail) starting at the at the entry velocity
+ // Case (1b, 1c) - Block is in a body or about to start a new head. Turn it into a new tail.
+ // In the new_head case plan deceleration move (tail) starting at the at the entry velocity
mr->section = SECTION_TAIL;
mr->section_state = SECTION_NEW;
mr->entry_velocity = mr->segment_velocity;
-// mr->entry_velocity = mr->previous_exit_velocity; // ++++ COMPARISON
mr->r->cruise_velocity = mr->entry_velocity; // cruise velocity must be set even if there's no body
mr->r->tail_length = mp_get_target_length(0, mr->r->cruise_velocity, bf); // braking length
mr->r->head_length = 0;
@@ -1069,25 +1084,41 @@ static stat_t _exec_feedhold_processing(mpBuf_t *bf)
mr->r->head_time = 0;
mr->r->body_time = 0;
- // (1a, 1b) The deceleration distance either fits in the available length (1a) or fits
- // exactly or close enough (to EPSILON2) (1b). Case 1b happens when the tail in the move
- // was already planned to zero. This is also case (2). EPSILON2 deals with floating point
- // rounding errors that can mis-classify this case.
+ // The deceleration distance either fits in the available length or fits exactly or close
+ // enough (to EPSILON2) (1e). Case 1e happens frequently when the tail in the move was
+ // already planned to zero. EPSILON2 deals with floating point rounding errors that can
+ // mis-classify this case. EPSILON2 is 0.0001, which is 0.1 microns in length.
float available_length = get_axis_vector_length(mr->target, mr->position);
- if ((available_length + EPSILON2 - mr->r->tail_length) > 0) { // it will fit
+ // Cases (1b1, 1c1) deceleration will fit in the block
+ if ((available_length + EPSILON2 - mr->r->tail_length) > 0) {
cm->hold_state = FEEDHOLD_DECEL_TO_ZERO;
mr->r->exit_velocity = 0;
mr->r->tail_time = mr->r->tail_length*2 / (mr->r->exit_velocity + mr->r->cruise_velocity);
bf->block_time = mr->r->tail_time;
}
+ // Cases (1b2, 1c2) deceleration will not fit in the block
else {
cm->hold_state = FEEDHOLD_DECEL_CONTINUE;
mr->r->tail_length = available_length;
mr->r->exit_velocity = mp_get_decel_velocity(mr->r->cruise_velocity, mr->r->tail_length, bf);
- mr->r->tail_time = mr->r->tail_length*2 / (mr->r->exit_velocity + mr->r->cruise_velocity);
- bf->block_time = mr->r->tail_time;
+ if (mr->r->exit_velocity >= 0) {
+ mr->r->tail_time = mr->r->tail_length*2 / (mr->r->exit_velocity + mr->r->cruise_velocity);
+ bf->block_time = mr->r->tail_time;
+ }
+ // The following branch is rarely if ever taken. It's possible for the deceleration calculation
+ // to return an error if the length is too short and other conditions exist. In that case
+ // make the block into a cruise (body) and push the deceleration to the next block.
+ else {
+ mr->section = SECTION_BODY;
+ mr->r->exit_velocity = mr->r->cruise_velocity; // both should be @ mr->segment_velocity
+ mr->r->body_length = available_length;
+ mr->r->body_time = mr->r->body_length / mr->r->cruise_velocity;
+ mr->r->tail_length = 0;
+ mr->r->tail_time = 0;
+ }
}
+ _exec_aline_normalize_block(mr->r);
}
return (STAT_EAGAIN);
}
diff --git a/g2core/plan_zoid.cpp b/g2core/plan_zoid.cpp
index d85c6641..fc90178d 100644
--- a/g2core/plan_zoid.cpp
+++ b/g2core/plan_zoid.cpp
@@ -448,10 +448,9 @@ float mp_get_decel_velocity(const float v_0, const float L, const mpBuf_t* bf)
const float recip_l_t = (2 * sqrt_delta_v_0) / ((v_0 - v_1x3) * q_recip_2_sqrt_j);
v_1 = v_1 - (l_t * recip_l_t);
- // hack for cases where there is no solution because the length is so short
+ // In some extreme cases there is no solution because the length is too short
if (v_1 > v_0) {
return (-1.0); // cannot decelerate. Return an error
-// return (v_0); // cannot decelerate. Return entry velocity
}
}
return v_1;
diff --git a/g2core/planner.h b/g2core/planner.h
index 6711cfcf..606c2785 100644
--- a/g2core/planner.h
+++ b/g2core/planner.h
@@ -467,7 +467,6 @@ typedef struct mpPlannerRuntime { // persistent runtime variables
mpBlockRuntimeBuf_t bf[2]; // buffer holding the two blocks
float entry_velocity; // entry values for the currently running block
- float previous_exit_velocity; // +++++ DIAGNOSTIC
float segments; // number of segments in line (also used by arc generation)
uint32_t segment_count; // count of running segments