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

Breakout block normalization into a helper; Added handler for mp_get_decel_velocity() failure case; Added normalizer to feedhold block manipulation; Re-wrote the feedhold cases to be clearer; General cleanup.

Browse Source
This commit is contained in:
Alden Hart
2017-02-23 08:25:32 -05:00
parent 3bbda127c4
commit ea7586253f
4 changed files with 154 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
g2core/plan_exec.cpp
+149 -118
View File
@@ -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);
}