stat_t mp_plan_hold_callback()
{
if (cm.hold_state != FEEDHOLD_PLAN)
return (STAT_NOOP); // not planning a feedhold
mpBuf_t *bp; // working buffer pointer
if ((bp = mp_get_run_buffer()) == NULL)
return (STAT_NOOP); // Oops! nothing's running
uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx
float mr_available_length; // available length left in mr buffer for deceleration
float braking_velocity; // velocity left to shed to brake to zero
float braking_length; // distance required to brake to zero from braking_velocity
// examine and process mr buffer
mr_available_length = get_axis_vector_length(mr.target, mr.position);
/* mr_available_length =
(sqrt(square(mr.endpoint[AXIS_X] - mr.position[AXIS_X]) +
square(mr.endpoint[AXIS_Y] - mr.position[AXIS_Y]) +
square(mr.endpoint[AXIS_Z] - mr.position[AXIS_Z]) +
square(mr.endpoint[AXIS_A] - mr.position[AXIS_A]) +
square(mr.endpoint[AXIS_B] - mr.position[AXIS_B]) +
square(mr.endpoint[AXIS_C] - mr.position[AXIS_C])));
*/
// compute next_segment velocity
// braking_velocity = mr.segment_velocity;
// if (mr.section != SECTION_BODY) { braking_velocity += mr.forward_diff_1;}
braking_velocity = _compute_next_segment_velocity();
braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here
// Hack to prevent Case 2 moves for perfect-fit decels. Happens in 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 work.
if ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) {
braking_length = mr_available_length;
}
// Case 1: deceleration fits entirely into the length remaining in mr buffer
if (braking_length <= mr_available_length) {
// set mr to a tail to perform the deceleration
mr.exit_velocity = 0;
mr.tail_length = braking_length;
mr.cruise_velocity = braking_velocity;
mr.section = SECTION_TAIL;
mr.section_state = SECTION_NEW;
// re-use bp+0 to be the hold point and to run the remaining block length
bp->length = mr_available_length - braking_length;
bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
bp->entry_vmax = 0; // set bp+0 as hold point
bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer
_reset_replannable_list(); // make it replan all the blocks
_plan_block_list(mp_get_last_buffer(), &mr_flag);
cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
return (STAT_OK);
}
// 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.tail_length = mr_available_length;
mr.cruise_velocity = braking_velocity;
mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp);
// Find the point where deceleration reaches zero. This could span multiple buffers.
braking_velocity = mr.exit_velocity; // adjust braking velocity downward
bp->move_state = MOVE_NEW; // tell _exec to re-use buffer
for (uint8_t i=0; i<PLANNER_BUFFER_POOL_SIZE; i++) {// a safety to avoid wraparound
mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0 (and onward...)
if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers
bp = mp_get_next_buffer(bp); // point to next buffer
continue;
}
bp->entry_vmax = braking_velocity; // velocity we need to shed
braking_length = mp_get_target_length(braking_velocity, 0, bp);
if (braking_length > bp->length) { // decel does not fit in bp buffer
bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp);
braking_velocity = bp->exit_vmax; // braking velocity for next buffer
bp = mp_get_next_buffer(bp); // point to next buffer
continue;
}
break;
}
// Deceleration now fits in the current bp buffer
// Plan the first buffer of the pair as the decel, the second as the accel
bp->length = braking_length;
bp->exit_vmax = 0;
bp = mp_get_next_buffer(bp); // point to the acceleration buffer
bp->entry_vmax = 0;
bp->length -= braking_length; // the buffers were identical (and hence their lengths)
bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
bp->exit_vmax = bp->delta_vmax;
_reset_replannable_list(); // make it replan all the blocks
_plan_block_list(mp_get_last_buffer(), &mr_flag);
cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
return (STAT_OK);
}
void mp_calculate_trapezoid(mpBuf_t *bf)
{
//********************************************
//********************************************
//** RULE #1 of mp_calculate_trapezoid() **
//** DON'T CHANGE bf->length **
//********************************************
//********************************************
// F case: Block is too short - run time < minimum segment time
// Force block into a single segment body with limited velocities
// Accept the entry velocity, limit the cruise, and go for the best exit velocity
// you can get given the delta_vmax (maximum velocity slew) supportable.
bf->naiive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// We are violating the jerk value but since it's a single segment move we don't use it.
return;
}
// B" case: Block is short, but fits into a single body segment
if (bf->naiive_move_time <= NOM_SEGMENT_TIME) {
bf->entry_velocity = bf->pv->exit_velocity;
if (fp_NOT_ZERO(bf->entry_velocity)) {
bf->cruise_velocity = bf->entry_velocity;
bf->exit_velocity = bf->entry_velocity;
} else {
bf->cruise_velocity = bf->delta_vmax / 2;
bf->exit_velocity = bf->delta_vmax;
}
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// We are violating the jerk value but since it's a single segment move we don't use it.
return;
}
// B case: Velocities all match (or close enough)
// This occurs frequently in normal gcode files with lots of short lines
// This case is not really necessary, but saves lots of processing time
if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) &&
((bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) {
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
return;
}
// Head-only and tail-only short-line cases
// H" and T" degraded-fit cases
// H' and T' requested-fit cases where the body residual is less than MIN_BODY_LENGTH
bf->body_length = 0;
float minimum_length = mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf);
if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { // head-only & tail-only cases
if (bf->entry_velocity > bf->exit_velocity) { // tail-only cases (short decelerations)
if (bf->length < minimum_length) { // T" (degraded case)
bf->entry_velocity = mp_get_target_velocity(bf->exit_velocity, bf->length, bf);
}
bf->cruise_velocity = bf->entry_velocity;
bf->tail_length = bf->length;
bf->head_length = 0;
return;
}
if (bf->entry_velocity < bf->exit_velocity) { // head-only cases (short accelerations)
if (bf->length < minimum_length) { // H" (degraded case)
bf->exit_velocity = mp_get_target_velocity(bf->entry_velocity, bf->length, bf);
}
bf->cruise_velocity = bf->exit_velocity;
bf->head_length = bf->length;
bf->tail_length = 0;
return;
}
}
// Set head and tail lengths for evaluating the next cases
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
// Rate-limited HT and HT' cases
if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited
// Symmetric rate-limited case (HT)
if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) {
bf->head_length = bf->length/2;
bf->tail_length = bf->head_length;
bf->cruise_velocity = min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf));
if (bf->head_length < MIN_HEAD_LENGTH) {
// Convert this to a body-only move
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// Average the entry speed and computed best cruise-speed
bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2;
bf->entry_velocity = bf->cruise_velocity;
bf->exit_velocity = bf->cruise_velocity;
}
return;
}
// Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often
// iteration trap: uint8_t i=0;
// iteration trap: if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));}
float computed_velocity = bf->cruise_vmax;
do {
bf->cruise_velocity = computed_velocity; // initialize from previous iteration
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
if (bf->head_length > bf->tail_length) {
bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity = mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf);
} else {
bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity = mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf);
}
// insert iteration trap here if needed
} while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT);
// set velocity and clean up any parts that are too short
bf->cruise_velocity = computed_velocity;
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = bf->length - bf->head_length;
if (bf->head_length < MIN_HEAD_LENGTH) {
bf->tail_length = bf->length; // adjust the move to be all tail...
bf->head_length = 0;
}
if (bf->tail_length < MIN_TAIL_LENGTH) {
bf->head_length = bf->length; //...or all head
bf->tail_length = 0;
}
return;
}
// Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases
bf->body_length = bf->length - bf->head_length - bf->tail_length;
// If a non-zero body is < minimum length distribute it to the head and/or tail
// This will generate small (acceptable) velocity errors in runtime execution
// but preserve correct distance, which is more important.
if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) {
if (fp_NOT_ZERO(bf->head_length)) {
if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT
bf->head_length += bf->body_length/2;
bf->tail_length += bf->body_length/2;
} else { // HB reduces to H
bf->head_length += bf->body_length;
}
} else { // BT reduces to T
bf->tail_length += bf->body_length;
}
bf->body_length = 0;
// If the body is a standalone make the cruise velocity match the entry velocity
// This removes a potential velocity discontinuity at the expense of top speed
} else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) {
bf->cruise_velocity = bf->entry_velocity;
}
}
