// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock
// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
// circumvent the processes for executing motions in normal operation.
// NOTE: Only the abort realtime command can interrupt this process.
// TODO: Move limit pin-specific calls to a general function for portability.
void limits_go_home(uint8_t cycle_mask)
{
if (sys.abort) { return; } // Block if system reset has been issued.
// Initialize
uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);
uint8_t step_pin[N_AXIS];
float target[N_AXIS];
float max_travel = 0.0;
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
// Initialize step pin masks
step_pin[idx] = get_step_pin_mask(idx);
#ifdef COREXY
if ((idx==A_MOTOR)||(idx==B_MOTOR)) { step_pin[idx] = (get_step_pin_mask(X_AXIS)|get_step_pin_mask(Y_AXIS)); }
#endif
if (bit_istrue(cycle_mask,bit(idx))) {
// Set target based on max_travel setting. Ensure homing switches engaged with search scalar.
// NOTE: settings.max_travel[] is stored as a negative value.
max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]);
}
}
// Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches.
bool approach = true;
float homing_rate = settings.homing_seek_rate;
uint8_t limit_state, axislock, n_active_axis;
do {
system_convert_array_steps_to_mpos(target,sys.position);
// Initialize and declare variables needed for homing routine.
axislock = 0;
n_active_axis = 0;
for (idx=0; idx<N_AXIS; idx++) {
// Set target location for active axes and setup computation for homing rate.
if (bit_istrue(cycle_mask,bit(idx))) {
n_active_axis++;
sys.position[idx] = 0;
// Set target direction based on cycle mask and homing cycle approach state.
// NOTE: This happens to compile smaller than any other implementation tried.
if (bit_istrue(settings.homing_dir_mask,bit(idx))) {
if (approach) { target[idx] = -max_travel; }
else { target[idx] = max_travel; }
} else {
if (approach) { target[idx] = max_travel; }
else { target[idx] = -max_travel; }
}
// Apply axislock to the step port pins active in this cycle.
axislock |= step_pin[idx];
}
}
homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.
sys.homing_axis_lock = axislock;
plan_sync_position(); // Sync planner position to current machine position.
// Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, homing_rate, false, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion.
#else
plan_buffer_line(target, homing_rate, false, false); // Bypass mc_line(). Directly plan homing motion.
#endif
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
st_wake_up(); // Initiate motion
do {
if (approach) {
// Check limit state. Lock out cycle axes when they change.
limit_state = limits_get_state();
for (idx=0; idx<N_AXIS; idx++) {
if (axislock & step_pin[idx]) {
if (limit_state & (1 << idx)) { axislock &= ~(step_pin[idx]); }
}
}
sys.homing_axis_lock = axislock;
}
st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
// Exit routines: No time to run protocol_execute_realtime() in this loop.
if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) {
// Homing failure: Limit switches are still engaged after pull-off motion
if ( (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET)) || // Safety door or reset issued
(!approach && (limits_get_state() & cycle_mask)) || // Limit switch still engaged after pull-off motion
( approach && (sys_rt_exec_state & EXEC_CYCLE_STOP)) ) { // Limit switch not found during approach.
mc_reset(); // Stop motors, if they are running.
protocol_execute_realtime();
return;
} else {
// Pull-off motion complete. Disable CYCLE_STOP from executing.
system_clear_exec_state_flag(EXEC_CYCLE_STOP);
break;
}
}
} while (STEP_MASK & axislock);
st_reset(); // Immediately force kill steppers and reset step segment buffer.
plan_reset(); // Reset planner buffer to zero planner current position and to clear previous motions.
delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.
// Reverse direction and reset homing rate for locate cycle(s).
approach = !approach;
// After first cycle, homing enters locating phase. Shorten search to pull-off distance.
if (approach) {
max_travel = settings.homing_pulloff*HOMING_AXIS_LOCATE_SCALAR;
homing_rate = settings.homing_feed_rate;
} else {
max_travel = settings.homing_pulloff;
homing_rate = settings.homing_seek_rate;
}
} while (n_cycle-- > 0);
// The active cycle axes should now be homed and machine limits have been located. By
// default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches
// can be on either side of an axes, check and set axes machine zero appropriately. Also,
// set up pull-off maneuver from axes limit switches that have been homed. This provides
// some initial clearance off the switches and should also help prevent them from falsely
// triggering when hard limits are enabled or when more than one axes shares a limit pin.
#ifdef COREXY
int32_t off_axis_position = 0;
#endif
int32_t set_axis_position;
// Set machine positions for homed limit switches. Don't update non-homed axes.
for (idx=0; idx<N_AXIS; idx++) {
// NOTE: settings.max_travel[] is stored as a negative value.
if (cycle_mask & bit(idx)) {
#ifdef HOMING_FORCE_SET_ORIGIN
set_axis_position = 0;
#else
if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {
set_axis_position = lround((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]);
} else {
set_axis_position = lround(-settings.homing_pulloff*settings.steps_per_mm[idx]);
}
#endif
#ifdef COREXY
if (idx==X_AXIS) {
off_axis_position = (sys.position[B_MOTOR] - sys.position[A_MOTOR])/2;
sys.position[A_MOTOR] = set_axis_position - off_axis_position;
sys.position[B_MOTOR] = set_axis_position + off_axis_position;
} else if (idx==Y_AXIS) {
off_axis_position = (sys.position[A_MOTOR] + sys.position[B_MOTOR])/2;
sys.position[A_MOTOR] = off_axis_position - set_axis_position;
sys.position[B_MOTOR] = off_axis_position + set_axis_position;
} else {
sys.position[idx] = set_axis_position;
}
#else
sys.position[idx] = set_axis_position;
#endif
}
}
plan_sync_position(); // Sync planner position to homed machine position.
// sys.state = STATE_HOMING; // Ensure system state set as homing before returning.
}
// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock
// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
// circumvent the processes for executing motions in normal operation.
// NOTE: Only the abort runtime command can interrupt this process.
void limits_go_home(uint8_t cycle_mask)
{
if (sys.abort) { return; } // Block if system reset has been issued.
// Initialize homing in search mode to quickly engage the specified cycle_mask limit switches.
bool approach = true;
float homing_rate = settings.homing_seek_rate;
uint8_t invert_pin, idx;
uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1); ///***5
float target[N_AXIS];
uint8_t limit_pin[N_AXIS], step_pin[N_AXIS];
float max_travel = 0.0;
for (idx=0; idx<N_AXIS; idx++) {
// Initialize limit and step pin masks
limit_pin[idx] = get_limit_pin_mask(idx); ///////****get the Pin of limit min x, y ,z , Limit OK
step_pin[idx] = get_step_pin_mask(idx); ///////****get the Pin of limit min x, y ,z now I let it 0, 1, 2
// Determine travel distance to the furthest homing switch based on user max travel settings.
// NOTE: settings.max_travel[] is stored as a negative value.
if (max_travel > settings.max_travel[idx]) { max_travel = settings.max_travel[idx]; }
}
max_travel *= -HOMING_AXIS_SEARCH_SCALAR; // Ensure homing switches engaged by over-estimating max travel.
plan_reset(); // Reset planner buffer to zero planner current position and to clear previous motions.
do {
// Initialize invert_pin boolean based on approach and invert pin user setting.
if (bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { invert_pin = approach; }
else { invert_pin = !approach; }
// Initialize and declare variables needed for homing routine.
uint8_t n_active_axis = 0;
uint8_t axislock = 0;
for (idx=0; idx<N_AXIS; idx++) {
// Set target location for active axes and setup computation for homing rate.
if (bit_istrue(cycle_mask,bit(idx))) {
n_active_axis++;
if (!approach) { target[idx] = -max_travel; }
else { target[idx] = max_travel; }
} else {
target[idx] = 0.0;
}
// Set target direction based on cycle mask
if (bit_istrue(settings.homing_dir_mask,bit(idx))) { target[idx] = -target[idx]; }
// Apply axislock to the step port pins active in this cycle.
if (bit_istrue(cycle_mask,bit(idx))) { axislock |= step_pin[idx]; }
}
homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.
sys.homing_axis_lock = axislock;
// Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
uint8_t limit_state;
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, homing_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion.
#else
plan_buffer_line(target, homing_rate, false); // Bypass mc_line(). Directly plan homing motion.
#endif
st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
st_wake_up(); // Initiate motion
do {
// Check limit state. Lock out cycle axes when they change.
/////////////////########################################***********
#if MotherBoard==3 /////**MB==3
#ifdef limit_int_style
limit_state = LIMIT_PIN;
if (invert_pin) { limit_state ^= LIMIT_MASK; }
#else
//************Get Limit Pin State. PiBot get limit/endstop mask same with the step mask.
#if LIMIT_MAX_OPEN
if(isXMinEndstopHit()) { limit_state^= MASK(X_LIMIT_BIT);} ////***read X_endstop then write into limite_state
if(isYMinEndstopHit()) { limit_state^= MASK(Y_LIMIT_BIT);}
if(isZMinEndstopHit()) { limit_state^= MASK(Z_LIMIT_BIT);} ////*****add MAX limit
if(isXMaxEndstopHit()) { limit_state^= MASK(X_LIMIT_MAX_BIT);} ////***read X_endstop then write into limite_state
if(isYMaxEndstopHit()) { limit_state^= MASK(Y_LIMIT_MAX_BIT);}
if(isZMaxEndstopHit()) { limit_state^= MASK(Z_LIMIT_MAX_BIT);} ////*****add MAX limit
#else
if(isXMinEndstopHit()) { limit_state^= MASK(X_MASK);} ////***read X_endstop then write into limite_state
if(isYMinEndstopHit()) { limit_state^= MASK(Y_MASK);}
if(isZMinEndstopHit()) { limit_state^= MASK(Z_MASK);} ////*****add MAX limit
#endif
#endif
#else /////**MB!=3 ////////#################################################
limit_state = LIMIT_PIN;
if (invert_pin) { limit_state ^= LIMIT_MASK; }
#endif /////**MB==3
/////////////////////////////#####################################
for (idx=0; idx<N_AXIS; idx++) {
if (axislock & step_pin[idx]) {
if (limit_state & limit_pin[idx]) { axislock &= ~(step_pin[idx]); }
}
}
sys.homing_axis_lock = axislock;
st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
// Check only for user reset. No time to run protocol_execute_runtime() in this loop.
if (sys.execute & EXEC_RESET) { protocol_execute_runtime(); return; }
} while (STEP_MASK & axislock);
st_reset(); // Immediately force kill steppers and reset step segment buffer.
plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared.
delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.
// Reverse direction and reset homing rate for locate cycle(s).
homing_rate = settings.homing_feed_rate;
approach = !approach;
} while (n_cycle-- > 0);
// The active cycle axes should now be homed and machine limits have been located. By
// default, grbl defines machine space as all negative, as do most CNCs. Since limit switches
// can be on either side of an axes, check and set axes machine zero appropriately. Also,
// set up pull-off maneuver from axes limit switches that have been homed. This provides
// some initial clearance off the switches and should also help prevent them from falsely
// triggering when hard limits are enabled or when more than one axes shares a limit pin.
for (idx=0; idx<N_AXIS; idx++) {
// Set up pull off targets and machine positions for limit switches homed in the negative
// direction, rather than the traditional positive. Leave non-homed positions as zero and
// do not move them.
// NOTE: settings.max_travel[] is stored as a negative value.
if (cycle_mask & bit(idx)) {
#ifdef HOMING_FORCE_SET_ORIGIN
sys.position[idx] = 0; // Set axis homed location as axis origin
target[idx] = settings.homing_pulloff;
if ( bit_isfalse(settings.homing_dir_mask,bit(idx)) ) { target[idx] = -target[idx]; }
#else
if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {
target[idx] = settings.homing_pulloff+settings.max_travel[idx];
sys.position[idx] = lround(settings.max_travel[idx]*settings.steps_per_mm[idx]);
} else {
target[idx] = -settings.homing_pulloff;
sys.position[idx] = 0;
}
#endif
} else { // Non-active cycle axis. Set target to not move during pull-off.
target[idx] = (float)sys.position[idx]/settings.steps_per_mm[idx];
}
}
plan_sync_position(); // Sync planner position to current machine position for pull-off move.
#ifdef USE_LINE_NUMBERS
plan_buffer_line(target, settings.homing_seek_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan motion.
#else
plan_buffer_line(target, settings.homing_seek_rate, false); // Bypass mc_line(). Directly plan motion.
#endif
// Initiate pull-off using main motion control routines.
// TODO : Clean up state routines so that this motion still shows homing state.
sys.state = STATE_QUEUED;
bit_true_atomic(sys.execute, EXEC_CYCLE_START);
protocol_execute_runtime();
protocol_buffer_synchronize(); // Complete pull-off motion.
// Set system state to homing before returning.
sys.state = STATE_HOMING;
}
