static uint8_t _probing_init()
{
float start_position[AXES];
// so optimistic... ;)
// NOTE: it is *not* an error condition for the probe not to trigger.
// it is an error for the limit or homing switches to fire, or for some other configuration error.
cm.probe_state = PROBE_FAILED;
cm.machine_state = MACHINE_CYCLE;
cm.cycle_state = CYCLE_PROBE;
// save relevant non-axis parameters from Gcode model
pb.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL);
pb.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL);
// set working values
cm_set_distance_mode(ABSOLUTE_MODE);
cm_set_coord_system(ABSOLUTE_COORDS); // probing is done in machine coordinates
// initialize the axes - save the jerk settings & switch to the jerk_homing settings
for( uint8_t axis=0; axis<AXES; axis++ ) {
pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value
cm_set_axis_jerk(axis, cm.a[axis].jerk_high); // use the high-speed jerk for probe
start_position[axis] = cm_get_absolute_position(ACTIVE_MODEL, axis);
}
// error if the probe target is too close to the current position
if (get_axis_vector_length(start_position, pb.target) < MINIMUM_PROBE_TRAVEL) {
_probing_error_exit(-2);
}
// error if the probe target requires a move along the A/B/C axes
for ( uint8_t axis=AXIS_A; axis<AXES; axis++ ) {
// if (fp_NE(start_position[axis], pb.target[axis])) { // old style
if (fp_TRUE(pb.flags[axis])) {
// if (pb.flags[axis]) { // will reduce to this once flags are booleans
_probing_error_exit(axis);
}
}
// initialize the probe switch
pb.probe_input = 5; // TODO -- for now we hard code it to zmin
gpio_set_probing_mode(pb.probe_input, true);
// turn off spindle and start the move
cm_spindle_optional_pause(true); // pause the spindle if it's on
return (_set_pb_func(_probing_start)); // start the probe move
}
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);
}
