static stat_t _probing_finish()
{
int8_t probe = gpio_read_input(pb.probe_input);
cm.probe_state = (probe==true) ? PROBE_SUCCEEDED : PROBE_FAILED;
for (uint8_t axis=0; axis<AXES; axis++ ) {
// if we got here because of a feed hold we need to keep the model position correct
cm_set_position(axis, cm_get_work_position(RUNTIME, axis));
// store the probe results
cm.probe_results[axis] = cm_get_absolute_position(ACTIVE_MODEL, axis);
}
// If probe was successful the 'e' word == 1, otherwise e == 0 to signal an error
printf_P(PSTR("{\"prb\":{\"e\":%i"), (int)cm.probe_state);
if (fp_TRUE(pb.flags[AXIS_X])) printf_P(PSTR(",\"x\":%0.3f"), cm.probe_results[AXIS_X]);
if (fp_TRUE(pb.flags[AXIS_Y])) printf_P(PSTR(",\"y\":%0.3f"), cm.probe_results[AXIS_Y]);
if (fp_TRUE(pb.flags[AXIS_Z])) printf_P(PSTR(",\"z\":%0.3f"), cm.probe_results[AXIS_Z]);
if (fp_TRUE(pb.flags[AXIS_A])) printf_P(PSTR(",\"a\":%0.3f"), cm.probe_results[AXIS_A]);
if (fp_TRUE(pb.flags[AXIS_B])) printf_P(PSTR(",\"b\":%0.3f"), cm.probe_results[AXIS_B]);
if (fp_TRUE(pb.flags[AXIS_C])) printf_P(PSTR(",\"c\":%0.3f"), cm.probe_results[AXIS_C]);
printf_P(PSTR("}}\n"));
return (_set_pb_func(_probing_finalize_exit));
}
static stat_t _verify_position(int8_t axis)
{
// abort if we aren't in the expected position (e.g. due to a user-initiated feedhold)
if (fp_NE(cm_get_absolute_position(MODEL, axis), hm.target_position)) {
_set_homing_func(_homing_abort);
return STAT_EAGAIN;
}
return STAT_OK;
}
// Handle an initial switch closure by backing off the closed switch
// NOTE: Relies on independent switches per axis (not shared)
static stat_t _homing_axis_clear(int8_t axis) // first clear move
{
#ifndef __NEW_SWITCHES
if (read_switch(hm.homing_switch) == SW_CLOSED) {
_homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
} else if (read_switch(hm.limit_switch) == SW_CLOSED) {
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
} else { // no move needed, so target position is same as current position
hm.target_position = cm_get_absolute_position(MODEL, axis);
}
#else
if (read_switch(hm.homing_switch_axis, hm.homing_switch_position) == SW_CLOSED) {
_homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
} else if (read_switch(hm.limit_switch_axis, hm.limit_switch_position) == SW_CLOSED) {
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
} else { // no move needed, so target position is same as current position
hm.target_position = cm_get_absolute_position(MODEL, axis);
}
#endif
return (_set_homing_func(_homing_axis_search));
}
static stat_t _homing_axis_move(int8_t axis, float target, float velocity)
{
float vect[] = {0,0,0,0,0,0};
float flags[] = {false, false, false, false, false, false};
vect[axis] = target;
flags[axis] = true;
cm_set_feed_rate(velocity);
mp_flush_planner(); // don't use cm_request_queue_flush() here
cm_request_cycle_start();
hm.target_position = cm_get_absolute_position(MODEL, axis) + target;
ritorno(cm_straight_feed(vect, flags));
return (STAT_EAGAIN);
}
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
}
static stat_t _probing_finish()
{
int8_t probe = gpio_read_input(pb.probe_input);
cm.probe_state = (probe==true) ? PROBE_SUCCEEDED : PROBE_FAILED;
// store the probe results
for (uint8_t axis=0; axis<AXES; axis++ ) {
cm.probe_results[axis] = cm_get_absolute_position(ACTIVE_MODEL, axis);
}
// If probe was successful the 'e' word == 1, otherwise e == 0 to signal an error
printf_P(PSTR("{\"prb\":{\"e\":%i"), (int)cm.probe_state);
if (pb.flags[AXIS_X]) { printf_P(PSTR(",\"x\":%0.3f"), cm.probe_results[AXIS_X]); }
if (pb.flags[AXIS_Y]) { printf_P(PSTR(",\"y\":%0.3f"), cm.probe_results[AXIS_Y]); }
if (pb.flags[AXIS_Z]) { printf_P(PSTR(",\"z\":%0.3f"), cm.probe_results[AXIS_Z]); }
if (pb.flags[AXIS_A]) { printf_P(PSTR(",\"a\":%0.3f"), cm.probe_results[AXIS_A]); }
if (pb.flags[AXIS_B]) { printf_P(PSTR(",\"b\":%0.3f"), cm.probe_results[AXIS_B]); }
if (pb.flags[AXIS_C]) { printf_P(PSTR(",\"c\":%0.3f"), cm.probe_results[AXIS_C]); }
printf_P(PSTR("}}\n"));
return (_set_pb_func(_probing_finalize_exit));
}
