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; } }