/*
* mp_end_hold() - end a feedhold
*/
stat_t mp_end_hold()
{
if (cm.hold_state == FEEDHOLD_END_HOLD) {
cm.hold_state = FEEDHOLD_OFF;
mpBuf_t *bf;
if ((bf = mp_get_run_buffer()) == NULL) { // NULL means nothing's running
cm_set_motion_state(MOTION_STOP);
return (STAT_NOOP);
}
cm.motion_state = MOTION_RUN;
st_request_exec_move(); // restart the steppers
}
return (STAT_OK);
}
mpBuf_t * mp_get_last_buffer(void)
{
mpBuf_t *bf = mp_get_run_buffer();
mpBuf_t *bp = bf;
if (bf == NULL) return(NULL);
do {
if ((bp->nx->move_state == MOVE_OFF) || (bp->nx == bf)) {
return (bp);
}
} while ((bp = mp_get_next_buffer(bp)) != bf);
return (bp);
}
stat_t mp_exec_move()
{
mpBuf_t *bf;
if ((bf = mp_get_run_buffer()) == NULL) return (STAT_NOOP); // NULL means nothing's running
// Manage cycle and motion state transitions.
// Cycle auto-start for lines only.
if (bf->move_type == MOVE_TYPE_ALINE) {
if (cm.cycle_state == CYCLE_OFF) cm_cycle_start();
if (cm.motion_state == MOTION_STOP) cm.motion_state = MOTION_RUN;
}
// run the move callback in the planner buffer
if (bf->bf_func != NULL) {
return (bf->bf_func(bf));
}
return (STAT_INTERNAL_ERROR); // never supposed to get here
}
/*
#define axis_length bf->body_length
#define axis_velocity bf->cruise_velocity
#define axis_tail bf->tail_length
#define longest_tail bf->head_length
*/
stat_t mp_aline(GCodeState_t *gm_in)
{
mpBuf_t *bf; // current move pointer
float exact_stop = 0; // preset this value OFF
float junction_velocity;
uint8_t mr_flag = false;
// compute some reusable terms
float axis_length[AXES];
float axis_square[AXES];
float length_square = 0;
for (uint8_t axis=0; axis<AXES; axis++) {
axis_length[axis] = gm_in->target[axis] - mm.position[axis];
axis_square[axis] = square(axis_length[axis]);
length_square += axis_square[axis];
}
float length = sqrt(length_square);
if (fp_ZERO(length)) {
// sr_request_status_report();
return (STAT_OK);
}
// If _calc_move_times() says the move will take less than the minimum move time
// get a more accurate time estimate based on starting velocity and acceleration.
// The time of the move is determined by its initial velocity (Vi) and how much
// acceleration will be occur. For this we need to look at the exit velocity of
// the previous block. There are 3 possible cases:
// (1) There is no previous block. Vi = 0
// (2) Previous block is optimally planned. Vi = previous block's exit_velocity
// (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity
_calc_move_times(gm_in, axis_length, axis_square); // set move time and minimum time in the state
if (gm_in->move_time < MIN_BLOCK_TIME) {
float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move
float entry_velocity = 0; // pre-set as if no previous block
if ((bf = mp_get_run_buffer()) != NULL) {
if (bf->replannable == true) { // not optimally planned
entry_velocity = bf->entry_velocity + bf->delta_vmax;
} else { // optimally planned
entry_velocity = bf->exit_velocity;
}
}
float move_time = (2 * length) / (2*entry_velocity + delta_velocity);// compute execution time for this move
if (move_time < MIN_BLOCK_TIME)
return (STAT_MINIMUM_TIME_MOVE);
}
// get a cleared buffer and setup move variables
if ((bf = mp_get_write_buffer()) == NULL)
return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // never supposed to fail
bf->bf_func = mp_exec_aline; // register the callback to the exec function
bf->length = length;
memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer
// Compute the unit vector and find the right jerk to use (combined operations)
// To determine the jerk value to use for the block we want to find the axis for which
// the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which
// the time to decelerate from the target velocity to zero would be the longest.
//
// We can determine the "longest" deceleration in terms of time or distance.
//
// The math for time-to-decelerate T from speed S to speed E with constant
// jerk J is:
//
// T = 2*sqrt((S-E)/J[n])
//
// Since E is always zero in this calculation, we can simplify:
// T = 2*sqrt(S/J[n])
//
// The math for distance-to-decelerate l from speed S to speed E with constant
// jerk J is:
//
// l = (S+E)*sqrt((S-E)/J)
//
// Since E is always zero in this calculation, we can simplify:
// l = S*sqrt(S/J)
//
// The final value we want is to know which one is *longest*, compared to the others,
// so we don't need the actual value. This means that the scale of the value doesn't
// matter, so for T we can remove the "2 *" and For L we can remove the "S*".
// This leaves both to be simply "sqrt(S/J)". Since we don't need the scale,
// it doesn't matter what speed we're coming from, so S can be replaced with 1.
//
// However, we *do* need to compensate for the fact that each axis contributes
// differently to the move, so we will scale each contribution C[n] by the
// proportion of the axis movement length D[n] to the total length of the move L.
// Using that, we construct the following, with these definitions:
//
// J[n] = max_jerk for axis n
// D[n] = distance traveled for this move for axis n
// L = total length of the move in all axes
// C[n] = "axis contribution" of axis n
//
// For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L)
//
// Keeping in mind that we only need a rank compared to the other axes, we can further
// optimize the calculations::
//
// Square the expression to remove the square root:
// C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.)
//
// Re-arranged to optimize divides for pre-calculated values, we create a value
// M that we compute once:
// M = (1/(L^2))
// Then we use it in the calculations for every axis:
// C[n] = (1/J[n]) * D[n]^2 * M
//
// Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further.
//
// Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value
// of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into
// its constituent axes for execution the jerk for that axis will be at it's maximum value.
float C; // contribution term. C = T * a
float maxC = 0;
float recip_L2 = 1/length_square;
for (uint8_t axis=0; axis<AXES; axis++) {
if (fabs(axis_length[axis]) > 0) { // You cannot use the fp_XXX comparisons here!
bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero)
C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's positive
if (C > maxC) {
maxC = C;
bf->jerk_axis = axis; // also needed for junction vmax calculation
}
}
}
// set up and pre-compute the jerk terms needed for this round of planning
bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale the jerk
if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta
mm.jerk = bf->jerk; // used before this point next time around
mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning
mm.cbrt_jerk = cbrt(bf->jerk);
}
bf->recip_jerk = mm.recip_jerk;
bf->cbrt_jerk = mm.cbrt_jerk;
// finish up the current block variables
if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed
bf->replannable = true;
exact_stop = 8675309; // an arbitrarily large floating point number
}
bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested
junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit);
bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop);
bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf);
bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop);
bf->braking_velocity = bf->delta_vmax;
// Note: these next lines must remain in exact order. Position must update before committing the buffer.
_plan_block_list(bf, &mr_flag); // replan block list
copy_vector(mm.position, bf->gm.target); // set the planner position
mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update)
return (STAT_OK);
}
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);
}
mpBuf_t * mp_get_first_buffer(void)
{
return(mp_get_run_buffer()); // returns buffer or NULL if nothing's running
}
