static stat_t _homing_axis_start(int8_t axis)
{
// get the first or next axis
if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
return (_set_homing_func(_homing_finalize_exit));
} else if (axis == -2) { // -2 is error
cm_set_units_mode(hm.saved_units_mode);
cm_set_distance_mode(hm.saved_distance_mode);
cm.cycle_state = CYCLE_OFF;
cm_cycle_end();
return (_homing_error_exit(-2));
}
}
// trap gross mis-configurations
if ((fp_ZERO(cm.a[axis].search_velocity)) || (fp_ZERO(cm.a[axis].latch_velocity))) {
return (_homing_error_exit(axis));
}
if ((cm.a[axis].travel_max <= 0) || (cm.a[axis].latch_backoff <= 0)) {
return (_homing_error_exit(axis));
}
// determine the switch setup and that config is OK
hm.min_mode = get_switch_mode(MIN_SWITCH(axis));
hm.max_mode = get_switch_mode(MAX_SWITCH(axis));
if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) {// one or the other must be homing
return (_homing_error_exit(axis)); // axis cannot be homed
}
hm.axis = axis; // persist the axis
hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive
hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive
// setup parameters homing to the minimum switch
if (hm.min_mode & SW_HOMING_BIT) {
hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch
hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch
hm.search_travel = -cm.a[axis].travel_max; // search travels in negative direction
hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction
hm.zero_backoff = cm.a[axis].zero_backoff;
// setup parameters for positive travel (homing to the maximum switch)
} else {
hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch
hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch
hm.search_travel = cm.a[axis].travel_max; // search travels in positive direction
hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction
hm.zero_backoff = -cm.a[axis].zero_backoff;
}
// if homing is disabled for the axis then skip to the next axis
uint8_t sw_mode = get_switch_mode(hm.homing_switch);
if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) {
return (_set_homing_func(_homing_axis_start));
}
// disable the limit switch parameter if there is no limit switch
if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) { hm.limit_switch = -1;}
hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value
return (_set_homing_func(_homing_axis_clear)); // start the clear
}
static stat_t _homing_axis_search(int8_t axis) // start the search
{
ritorno(_verify_position(axis));
cm.a[axis].jerk_max = cm.a[axis].jerk_homing; // use the homing jerk for search onward
_homing_axis_move(axis, hm.search_travel, hm.search_velocity);
return (_set_homing_func(_homing_axis_latch));
}
// Handle an initial switch closure by backing off switches
// NOTE: Relies on independent switches per axis (not shared)
static stat_t _homing_axis_clear(int8_t axis) // first clear move
{
//+++++ int8_t homing = read_switch(hm.homing_switch);
//+++++ int8_t limit = read_switch(hm.limit_switch);
int8_t homing = SW_OPEN; //+++++
int8_t limit = SW_OPEN; //+++++
if ((homing == SW_OPEN) && (limit != SW_CLOSED)) {
return (_set_homing_func(_homing_axis_search)); // OK to start the search
}
if (homing == SW_CLOSED) {
_homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_backoff_home));// will backoff homing switch some more
}
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_backoff_limit)); // will backoff limit switch some more
}
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;
}
static stat_t _homing_axis_set_zero(int8_t axis) // set zero and finish up
{
if (hm.set_coordinates != false) { // do not set axis if in G28.4 cycle
cm_set_axis_origin(axis, 0);
mp_set_runtime_position(axis, 0);
} else {
// cm_set_axis_origin(axis, cm_get_runtime_work_position(axis));
cm_set_axis_origin(axis, cm_get_work_position(RUNTIME, axis));
}
cm.a[axis].jerk_max = hm.saved_jerk; // restore the max jerk value
cm.homed[axis] = true;
return (_set_homing_func(_homing_axis_start));
}
static stat_t _homing_axis_set_zero(int8_t axis) // set zero and finish up
{
if (hm.set_coordinates != false) { // do not set axis if in G28.4 cycle
cm_set_position(axis, 0);
cm.homed[axis] = true;
} else {
cm_set_position(axis, cm_get_work_position(RUNTIME, axis));
}
cm.a[axis].jerk_max = hm.saved_jerk; // restore the max jerk value
#ifdef __NEW_SWITCHES
switch_t *s = &sw.s[hm.homing_switch_axis][hm.homing_switch_position];
s->on_trailing = hm.switch_saved_on_trailing;
_restore_switch_settings(&sw.s[hm.homing_switch_axis][hm.homing_switch_position]);
#endif
return (_set_homing_func(_homing_axis_start));
}
// 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_latch(int8_t axis) // latch to switch open
{
/* // Removed this code section because if a NO homing switch opens before the
// search deceleration is complete the switch will (correctly) be open.
// Therefore the homing cycle should continue -- ASH (build 445.01)
//
// Still need to figure out how to tell the difference between a rapid switch opening
// condition (above) and a user- initiated feedhold during a homing operation.
// verify assumption that we arrived here because of homing switch closure
// rather than user-initiated feedhold or other disruption
#ifndef __NEW_SWITCHES
if (read_switch(hm.homing_switch) != SW_CLOSED)
return (_set_homing_func(_homing_abort));
#else
if (read_switch(hm.homing_switch_axis, hm.homing_switch_position) != SW_CLOSED)
return (_set_homing_func(_homing_abort));
#endif
*/
_homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
return (_set_homing_func(_homing_axis_zero_backoff));
}
static stat_t _homing_axis_zero_backoff(int8_t axis) // backoff to zero position
{
_homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_set_zero));
}
static stat_t _homing_axis_latch(int8_t axis) // latch to switch open
{
_homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
return (_set_homing_func(_homing_axis_zero_backoff));
}
static stat_t _homing_axis_backoff_limit(int8_t axis) // back off cleared limit switch
{
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
return (_set_homing_func(_homing_axis_search));
}
static stat_t _homing_axis_start(int8_t axis)
{
// get the first or next axis
if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
cm.homing_state = HOMING_HOMED;
return (_set_homing_func(_homing_finalize_exit));
} else if (axis == -2) { // -2 is error
return (_homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS));
}
}
// clear the homed flag for axis so we'll be able to move w/o triggering soft limits
cm.homed[axis] = false;
// trap axis mis-configurations
if (fp_ZERO(cm.a[axis].search_velocity)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY));
if (fp_ZERO(cm.a[axis].latch_velocity)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY));
if (cm.a[axis].latch_backoff < 0) return (_homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF));
// calculate and test travel distance
float travel_distance = fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) + cm.a[axis].latch_backoff;
if (fp_ZERO(travel_distance)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL));
// determine the switch setup and that config is OK
#ifndef __NEW_SWITCHES
hm.min_mode = get_switch_mode(MIN_SWITCH(axis));
hm.max_mode = get_switch_mode(MAX_SWITCH(axis));
#else
hm.min_mode = get_switch_mode(axis, SW_MIN);
hm.max_mode = get_switch_mode(axis, SW_MAX);
#endif
if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) { // one or the other must be homing
return (_homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION)); // axis cannot be homed
}
hm.axis = axis; // persist the axis
hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive
hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive
// setup parameters homing to the minimum switch
if (hm.min_mode & SW_HOMING_BIT) {
#ifndef __NEW_SWITCHES
hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch
hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch
#else
hm.homing_switch_axis = axis;
hm.homing_switch_position = SW_MIN; // the min is the homing switch
hm.limit_switch_axis = axis;
hm.limit_switch_position = SW_MAX; // the max would be the limit switch
#endif
hm.search_travel = -travel_distance; // search travels in negative direction
hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction
hm.zero_backoff = cm.a[axis].zero_backoff;
// setup parameters for positive travel (homing to the maximum switch)
} else {
#ifndef __NEW_SWITCHES
hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch
hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch
#else
hm.homing_switch_axis = axis;
hm.homing_switch_position = SW_MAX; // the max is the homing switch
hm.limit_switch_axis = axis;
hm.limit_switch_position = SW_MIN; // the min would be the limit switch
#endif
hm.search_travel = travel_distance; // search travels in positive direction
hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction
hm.zero_backoff = -cm.a[axis].zero_backoff;
}
// if homing is disabled for the axis then skip to the next axis
#ifndef __NEW_SWITCHES
uint8_t sw_mode = get_switch_mode(hm.homing_switch);
if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) {
return (_set_homing_func(_homing_axis_start));
}
// disable the limit switch parameter if there is no limit switch
if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) hm.limit_switch = -1;
#else
// switch_t *s = &sw.s[hm.homing_switch_axis][hm.homing_switch_position];
// _bind_switch_settings(s);
_bind_switch_settings(&sw.s[hm.homing_switch_axis][hm.homing_switch_position]);
uint8_t sw_mode = get_switch_mode(hm.homing_switch_axis, hm.homing_switch_position);
if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT)) {
return (_set_homing_func(_homing_axis_start));
}
// disable the limit switch parameter if there is no limit switch
if (get_switch_mode(hm.limit_switch_axis, hm.limit_switch_position) == SW_MODE_DISABLED) {
hm.limit_switch_axis = -1;
}
#endif
hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value
return (_set_homing_func(_homing_axis_clear)); // start the clear
}
