forksand
/
marlin-lulzbot-laser
1
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1740 from thinkyhead/fixup_homing

Browse Source 
Apply leveling for DELTA
master
Scott Lahteine 10 years ago
parent 346c07cc1a 41d258697a
commit baa6787393
2 changed files with 572 additions and 514 deletions
Show Stats Download Patch File Download Diff File

934
Marlin/Marlin_main.cpp
Unescape View File

@ -79,7 +79,7 @@
// G4 - Dwell S<seconds> or P<milliseconds> // G4 - Dwell S<seconds> or P<milliseconds>
// G10 - retract filament according to settings of M207 // G10 - retract filament according to settings of M207
// G11 - retract recover filament according to settings of M208 // G11 - retract recover filament according to settings of M208
// G28 - Home all Axis // G28 - Home one or more axes
// G29 - Detailed Z-Probe, probes the bed at 3 or more points. Will fail if you haven't homed yet. // G29 - Detailed Z-Probe, probes the bed at 3 or more points. Will fail if you haven't homed yet.
// G30 - Single Z Probe, probes bed at current XY location. // G30 - Single Z Probe, probes bed at current XY location.
// G31 - Dock sled (Z_PROBE_SLED only) // G31 - Dock sled (Z_PROBE_SLED only)
@ -476,8 +476,6 @@ bool enquecommand(const char *cmd)
return true; return true;
} }
void setup_killpin() void setup_killpin()
{ {
#if defined(KILL_PIN) && KILL_PIN > -1 #if defined(KILL_PIN) && KILL_PIN > -1
@ -931,7 +929,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
static float x_home_pos(int extruder) { static float x_home_pos(int extruder) {
if (extruder == 0) if (extruder == 0)
return base_home_pos(X_AXIS) + home_offset[X_AXIS]; return base_home_pos(X_AXIS) + home_offset[X_AXIS];
else else
// In dual carriage mode the extruder offset provides an override of the // In dual carriage mode the extruder offset provides an override of the
// second X-carriage offset when homed - otherwise X2_HOME_POS is used. // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
@ -960,15 +958,15 @@ static void axis_is_at_home(int axis) {
if (axis == X_AXIS) { if (axis == X_AXIS) {
if (active_extruder != 0) { if (active_extruder != 0) {
current_position[X_AXIS] = x_home_pos(active_extruder); current_position[X_AXIS] = x_home_pos(active_extruder);
min_pos[X_AXIS] = X2_MIN_POS; min_pos[X_AXIS] = X2_MIN_POS;
max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS); max_pos[X_AXIS] = max(extruder_offset[1][X_AXIS], X2_MAX_POS);
return; return;
} }
else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) { else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS]; float xoff = home_offset[X_AXIS];
min_pos[X_AXIS] = base_min_pos(X_AXIS) + home_offset[X_AXIS]; current_position[X_AXIS] = base_home_pos(X_AXIS) + xoff;
max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + home_offset[X_AXIS], min_pos[X_AXIS] = base_min_pos(X_AXIS) + xoff;
max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset); max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + xoff, max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
return; return;
} }
} }
@ -1022,445 +1020,470 @@ static void axis_is_at_home(int axis) {
} }
/** /**
* Shorthand to tell the planner our current position (in mm). * Some planner shorthand inline functions
*/ */
inline void line_to_current_position() {
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
}
inline void line_to_z(float zPosition) {
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
}
inline void line_to_destination() {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
}
inline void sync_plan_position() { inline void sync_plan_position() {
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
} }
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
#ifndef DELTA #ifdef AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
planeNormal.debug("planeNormal");
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
//bedLevel.debug("bedLevel");
//plan_bed_level_matrix.debug("bed level before");
//vector_3 uncorrected_position = plan_get_position_mm();
//uncorrected_position.debug("position before");
vector_3 corrected_position = plan_get_position();
//corrected_position.debug("position after");
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
sync_plan_position(); #ifndef DELTA
}
#endif
#else // not AUTO_BED_LEVELING_GRID static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
planeNormal.debug("planeNormal");
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
//bedLevel.debug("bedLevel");
static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) { //plan_bed_level_matrix.debug("bed level before");
//vector_3 uncorrected_position = plan_get_position_mm();
//uncorrected_position.debug("position before");
plan_bed_level_matrix.set_to_identity(); vector_3 corrected_position = plan_get_position();
//corrected_position.debug("position after");
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1); sync_plan_position();
vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2); }
vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
if (planeNormal.z < 0) { #endif // !DELTA
planeNormal.x = -planeNormal.x;
planeNormal.y = -planeNormal.y;
planeNormal.z = -planeNormal.z;
}
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); #else // !AUTO_BED_LEVELING_GRID
vector_3 corrected_position = plan_get_position(); static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
sync_plan_position(); plan_bed_level_matrix.set_to_identity();
}
#endif // AUTO_BED_LEVELING_GRID vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
static void run_z_probe() { if (planeNormal.z < 0) {
#ifdef DELTA planeNormal.x = -planeNormal.x;
planeNormal.y = -planeNormal.y;
planeNormal.z = -planeNormal.z;
}
float start_z = current_position[Z_AXIS]; plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
long start_steps = st_get_position(Z_AXIS);
// move down slowly until you find the bed vector_3 corrected_position = plan_get_position();
feedrate = homing_feedrate[Z_AXIS] / 4; current_position[X_AXIS] = corrected_position.x;
destination[Z_AXIS] = -10; current_position[Y_AXIS] = corrected_position.y;
prepare_move_raw(); current_position[Z_AXIS] = zprobe_zoffset; // was: corrected_position.z
st_synchronize();
endstops_hit_on_purpose();
// we have to let the planner know where we are right now as it is not where we said to go. sync_plan_position();
long stop_steps = st_get_position(Z_AXIS); }
float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
current_position[Z_AXIS] = mm;
calculate_delta(current_position);
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#else #endif // !AUTO_BED_LEVELING_GRID
plan_bed_level_matrix.set_to_identity(); static void run_z_probe() {
feedrate = homing_feedrate[Z_AXIS];
// move down until you find the bed #ifdef DELTA
float zPosition = -10;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
// we have to let the planner know where we are right now as it is not where we said to go. float start_z = current_position[Z_AXIS];
zPosition = st_get_position_mm(Z_AXIS); long start_steps = st_get_position(Z_AXIS);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
// move up the retract distance // move down slowly until you find the bed
zPosition += home_retract_mm(Z_AXIS); feedrate = homing_feedrate[Z_AXIS] / 4;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder); destination[Z_AXIS] = -10;
st_synchronize(); prepare_move_raw();
endstops_hit_on_purpose(); st_synchronize();
endstops_hit_on_purpose();
// move back down slowly to find bed // we have to let the planner know where we are right now as it is not where we said to go.
if (homing_bump_divisor[Z_AXIS] >= 1) { long stop_steps = st_get_position(Z_AXIS);
feedrate = homing_feedrate[Z_AXIS]/homing_bump_divisor[Z_AXIS]; float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
} current_position[Z_AXIS] = mm;
else { calculate_delta(current_position);
feedrate = homing_feedrate[Z_AXIS]/10; plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
}
zPosition -= home_retract_mm(Z_AXIS) * 2; #else // !DELTA
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
endstops_hit_on_purpose();
current_position[Z_AXIS] = st_get_position_mm(Z_AXIS); plan_bed_level_matrix.set_to_identity();
// make sure the planner knows where we are as it may be a bit different than we last said to move to feedrate = homing_feedrate[Z_AXIS];
sync_plan_position();
#endif // move down until you find the bed
} float zPosition = -10;
line_to_z(zPosition);
st_synchronize();
// we have to let the planner know where we are right now as it is not where we said to go.
zPosition = st_get_position_mm(Z_AXIS);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
// move up the retract distance
zPosition += home_retract_mm(Z_AXIS);
line_to_z(zPosition);
st_synchronize();
endstops_hit_on_purpose();
// move back down slowly to find bed
if (homing_bump_divisor[Z_AXIS] >= 1)
feedrate = homing_feedrate[Z_AXIS] / homing_bump_divisor[Z_AXIS];
else {
feedrate = homing_feedrate[Z_AXIS] / 10;
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
}
static void do_blocking_move_to(float x, float y, float z) { zPosition -= home_retract_mm(Z_AXIS) * 2;
line_to_z(zPosition);
st_synchronize();
endstops_hit_on_purpose();
current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
// make sure the planner knows where we are as it may be a bit different than we last said to move to
sync_plan_position();
#endif // !DELTA
}
static void do_blocking_move_to(float x, float y, float z) {
float oldFeedRate = feedrate; float oldFeedRate = feedrate;
#ifdef DELTA #ifdef DELTA
feedrate = XY_TRAVEL_SPEED; feedrate = XY_TRAVEL_SPEED;
destination[X_AXIS] = x; destination[X_AXIS] = x;
destination[Y_AXIS] = y; destination[Y_AXIS] = y;
destination[Z_AXIS] = z; destination[Z_AXIS] = z;
prepare_move_raw(); prepare_move_raw();
st_synchronize(); st_synchronize();
#else #else
feedrate = homing_feedrate[Z_AXIS]; feedrate = homing_feedrate[Z_AXIS];
current_position[Z_AXIS] = z; current_position[Z_AXIS] = z;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder); line_to_current_position();
st_synchronize(); st_synchronize();
feedrate = xy_travel_speed; feedrate = xy_travel_speed;
current_position[X_AXIS] = x; current_position[X_AXIS] = x;
current_position[Y_AXIS] = y; current_position[Y_AXIS] = y;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder); line_to_current_position();
st_synchronize(); st_synchronize();
#endif #endif
feedrate = oldFeedRate; feedrate = oldFeedRate;
} }
static void setup_for_endstop_move() { static void setup_for_endstop_move() {
saved_feedrate = feedrate; saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply; saved_feedmultiply = feedmultiply;
feedmultiply = 100; feedmultiply = 100;
previous_millis_cmd = millis(); previous_millis_cmd = millis();
enable_endstops(true); enable_endstops(true);
} }
static void clean_up_after_endstop_move() {
#ifdef ENDSTOPS_ONLY_FOR_HOMING
enable_endstops(false);
#endif
static void clean_up_after_endstop_move() {
#ifdef ENDSTOPS_ONLY_FOR_HOMING
enable_endstops(false);
#endif
feedrate = saved_feedrate; feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply; feedmultiply = saved_feedmultiply;
previous_millis_cmd = millis(); previous_millis_cmd = millis();
} }
static void engage_z_probe() { static void engage_z_probe() {
// Engage Z Servo endstop if enabled
#ifdef SERVO_ENDSTOPS #ifdef SERVO_ENDSTOPS
if (servo_endstops[Z_AXIS] > -1) {
#if SERVO_LEVELING
servos[servo_endstops[Z_AXIS]].attach(0);
#endif
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
#if SERVO_LEVELING
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_endstops[Z_AXIS]].detach();
#endif
}
#elif defined(Z_PROBE_ALLEN_KEY)
feedrate = homing_feedrate[X_AXIS];
// Move to the start position to initiate deployment // Engage Z Servo endstop if enabled
destination[X_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_X; if (servo_endstops[Z_AXIS] >= 0) {
destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Y; #if SERVO_LEVELING
destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Z; servos[servo_endstops[Z_AXIS]].attach(0);
prepare_move_raw(); #endif
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
#if SERVO_LEVELING
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_endstops[Z_AXIS]].detach();
#endif
}
// Home X to touch the belt #elif defined(Z_PROBE_ALLEN_KEY)
feedrate = homing_feedrate[X_AXIS]/10;
destination[X_AXIS] = 0;
prepare_move_raw();
// Home Y for safety feedrate = homing_feedrate[X_AXIS];
feedrate = homing_feedrate[X_AXIS]/2;
destination[Y_AXIS] = 0;
prepare_move_raw();
st_synchronize(); // Move to the start position to initiate deployment
destination[X_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_X;
destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Y;
destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Z;
prepare_move_raw();
bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); // Home X to touch the belt
if (z_min_endstop) feedrate = homing_feedrate[X_AXIS]/10;
{ destination[X_AXIS] = 0;
if (!Stopped) prepare_move_raw();
{
SERIAL_ERROR_START; // Home Y for safety
SERIAL_ERRORLNPGM("Z-Probe failed to engage!"); feedrate = homing_feedrate[X_AXIS]/2;
LCD_ALERTMESSAGEPGM("Err: ZPROBE"); destination[Y_AXIS] = 0;
prepare_move_raw();
st_synchronize();
bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
if (z_min_endstop) {
if (!Stopped) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
LCD_ALERTMESSAGEPGM("Err: ZPROBE");
} }
Stop(); Stop();
} }
#endif
} #endif // Z_PROBE_ALLEN_KEY
static void retract_z_probe() { }
// Retract Z Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
if (servo_endstops[Z_AXIS] > -1)
{
#if Z_RAISE_AFTER_PROBING > 0
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
st_synchronize();
#endif
#if SERVO_LEVELING static void retract_z_probe(const float z_after=Z_RAISE_AFTER_PROBING) {
servos[servo_endstops[Z_AXIS]].attach(0);
#endif
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
#if SERVO_LEVELING
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_endstops[Z_AXIS]].detach();
#endif
}
#elif defined(Z_PROBE_ALLEN_KEY)
// Move up for safety
feedrate = homing_feedrate[X_AXIS];
destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
prepare_move_raw();
// Move to the start position to initiate retraction
destination[X_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_X;
destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Y;
destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Z;
prepare_move_raw();
// Move the nozzle down to push the probe into retracted position
feedrate = homing_feedrate[Z_AXIS]/10;
destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_RETRACT_DEPTH;
prepare_move_raw();
// Move up for safety
feedrate = homing_feedrate[Z_AXIS]/2;
destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_RETRACT_DEPTH * 2;
prepare_move_raw();
// Home XY for safety
feedrate = homing_feedrate[X_AXIS]/2;
destination[X_AXIS] = 0;
destination[Y_AXIS] = 0;
prepare_move_raw();
st_synchronize(); #ifdef SERVO_ENDSTOPS
bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING); // Retract Z Servo endstop if enabled
if (!z_min_endstop) if (servo_endstops[Z_AXIS] >= 0) {
{
if (!Stopped) if (z_after > 0) {
{ do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_after);
SERIAL_ERROR_START; st_synchronize();
SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
LCD_ALERTMESSAGEPGM("Err: ZPROBE");
} }
Stop();
}
#endif
} #if SERVO_LEVELING
servos[servo_endstops[Z_AXIS]].attach(0);
#endif
enum ProbeAction { servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
ProbeStay = 0,
ProbeEngage = BIT(0), #if SERVO_LEVELING
ProbeRetract = BIT(1), delay(PROBE_SERVO_DEACTIVATION_DELAY);
ProbeEngageAndRetract = (ProbeEngage | ProbeRetract) servos[servo_endstops[Z_AXIS]].detach();
}; #endif
}
/// Probe bed height at position (x,y), returns the measured z value
static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
// move to right place
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
#if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
if (retract_action & ProbeEngage) engage_z_probe();
#endif
run_z_probe(); #elif defined(Z_PROBE_ALLEN_KEY)
float measured_z = current_position[Z_AXIS];
#if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY) // Move up for safety
if (retract_action & ProbeRetract) retract_z_probe(); feedrate = homing_feedrate[X_AXIS];
#endif destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
prepare_move_raw();
if (verbose_level > 2) { // Move to the start position to initiate retraction
SERIAL_PROTOCOLPGM(MSG_BED); destination[X_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_X;
SERIAL_PROTOCOLPGM(" X: "); destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Y;
SERIAL_PROTOCOL_F(x, 3); destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_RETRACT_Z;
SERIAL_PROTOCOLPGM(" Y: "); prepare_move_raw();
SERIAL_PROTOCOL_F(y, 3);
SERIAL_PROTOCOLPGM(" Z: "); // Move the nozzle down to push the probe into retracted position
SERIAL_PROTOCOL_F(measured_z, 3); feedrate = homing_feedrate[Z_AXIS]/10;
SERIAL_EOL; destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_RETRACT_DEPTH;
} prepare_move_raw();
return measured_z;
} // Move up for safety
feedrate = homing_feedrate[Z_AXIS]/2;
destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_RETRACT_DEPTH * 2;
prepare_move_raw();
// Home XY for safety
feedrate = homing_feedrate[X_AXIS]/2;
destination[X_AXIS] = 0;
destination[Y_AXIS] = 0;
prepare_move_raw();
st_synchronize();
bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
if (!z_min_endstop) {
if (!Stopped) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
LCD_ALERTMESSAGEPGM("Err: ZPROBE");
}
Stop();
}
#endif
#ifdef DELTA
static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
if (bed_level[x][y] != 0.0) {
return; // Don't overwrite good values.
}
float a = 2*bed_level[x+xdir][y] - bed_level[x+xdir*2][y]; // Left to right.
float b = 2*bed_level[x][y+ydir] - bed_level[x][y+ydir*2]; // Front to back.
float c = 2*bed_level[x+xdir][y+ydir] - bed_level[x+xdir*2][y+ydir*2]; // Diagonal.
float median = c; // Median is robust (ignores outliers).
if (a < b) {
if (b < c) median = b;
if (c < a) median = a;
} else { // b <= a
if (c < b) median = b;
if (a < c) median = a;
} }
bed_level[x][y] = median;
}
// Fill in the unprobed points (corners of circular print surface) enum ProbeAction {
// using linear extrapolation, away from the center. ProbeStay = 0,
static void extrapolate_unprobed_bed_level() { ProbeEngage = BIT(0),
int half = (AUTO_BED_LEVELING_GRID_POINTS-1)/2; ProbeRetract = BIT(1),
for (int y = 0; y <= half; y++) { ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
for (int x = 0; x <= half; x++) { };
if (x + y < 3) continue;
extrapolate_one_point(half-x, half-y, x>1?+1:0, y>1?+1:0); // Probe bed height at position (x,y), returns the measured z value
extrapolate_one_point(half+x, half-y, x>1?-1:0, y>1?+1:0); static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageAndRetract, int verbose_level=1) {
extrapolate_one_point(half-x, half+y, x>1?+1:0, y>1?-1:0); // move to right place
extrapolate_one_point(half+x, half+y, x>1?-1:0, y>1?-1:0); do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
#if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
if (retract_action & ProbeEngage) engage_z_probe();
#endif
run_z_probe();
float measured_z = current_position[Z_AXIS];
#if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
if (retract_action & ProbeRetract) retract_z_probe(z_before);
#endif
if (verbose_level > 2) {
SERIAL_PROTOCOLPGM(MSG_BED);
SERIAL_PROTOCOLPGM(" X: ");
SERIAL_PROTOCOL_F(x, 3);
SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL_F(y, 3);
SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL_F(measured_z, 3);
SERIAL_EOL;
} }
return measured_z;
} }
}
// Print calibration results for plotting or manual frame adjustment. #ifdef DELTA
static void print_bed_level() {
for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) { /**
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) { * All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
SERIAL_PROTOCOL_F(bed_level[x][y], 2); */
SERIAL_PROTOCOLPGM(" ");
static void extrapolate_one_point(int x, int y, int xdir, int ydir) {
if (bed_level[x][y] != 0.0) {
return; // Don't overwrite good values.
}
float a = 2*bed_level[x+xdir][y] - bed_level[x+xdir*2][y]; // Left to right.
float b = 2*bed_level[x][y+ydir] - bed_level[x][y+ydir*2]; // Front to back.
float c = 2*bed_level[x+xdir][y+ydir] - bed_level[x+xdir*2][y+ydir*2]; // Diagonal.
float median = c; // Median is robust (ignores outliers).
if (a < b) {
if (b < c) median = b;
if (c < a) median = a;
} else { // b <= a
if (c < b) median = b;
if (a < c) median = a;
}
bed_level[x][y] = median;
} }
SERIAL_ECHOLN("");
}
}
// Reset calibration results to zero. // Fill in the unprobed points (corners of circular print surface)
void reset_bed_level() { // using linear extrapolation, away from the center.
for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) { static void extrapolate_unprobed_bed_level() {
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) { int half = (AUTO_BED_LEVELING_GRID_POINTS-1)/2;
bed_level[x][y] = 0.0; for (int y = 0; y <= half; y++) {
for (int x = 0; x <= half; x++) {
if (x + y < 3) continue;
extrapolate_one_point(half-x, half-y, x>1?+1:0, y>1?+1:0);
extrapolate_one_point(half+x, half-y, x>1?-1:0, y>1?+1:0);
extrapolate_one_point(half-x, half+y, x>1?+1:0, y>1?-1:0);
extrapolate_one_point(half+x, half+y, x>1?-1:0, y>1?-1:0);
}
}
} }
}
}
#endif // DELTA // Print calibration results for plotting or manual frame adjustment.
static void print_bed_level() {
for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
SERIAL_PROTOCOL_F(bed_level[x][y], 2);
SERIAL_PROTOCOLPGM(" ");
}
SERIAL_ECHOLN("");
}
}
// Reset calibration results to zero.
void reset_bed_level() {
for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
bed_level[x][y] = 0.0;
}
}
}
#endif // DELTA
#endif // ENABLE_AUTO_BED_LEVELING #endif // ENABLE_AUTO_BED_LEVELING
static void homeaxis(int axis) { static void homeaxis(int axis) {
#define HOMEAXIS_DO(LETTER) \ #define HOMEAXIS_DO(LETTER) \
((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1)) ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
if (axis==X_AXIS ? HOMEAXIS_DO(X) : if (axis == X_AXIS ? HOMEAXIS_DO(X) :
axis==Y_AXIS ? HOMEAXIS_DO(Y) : axis == Y_AXIS ? HOMEAXIS_DO(Y) :
axis==Z_AXIS ? HOMEAXIS_DO(Z) : axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
0) {
int axis_home_dir = home_dir(axis); int axis_home_dir;
#ifdef DUAL_X_CARRIAGE
if (axis == X_AXIS) #ifdef DUAL_X_CARRIAGE
axis_home_dir = x_home_dir(active_extruder); if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder);
#endif #else
axis_home_dir = home_dir(axis);
#endif
current_position[axis] = 0; current_position[axis] = 0;
sync_plan_position(); sync_plan_position();
#ifndef Z_PROBE_SLED
// Engage Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
#if SERVO_LEVELING
if (axis == Z_AXIS) {
engage_z_probe();
}
else
#endif // SERVO_LEVELING
if (servo_endstops[axis] > -1)
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
#endif // SERVO_ENDSTOPS
#endif // Z_PROBE_SLED
#ifndef Z_PROBE_SLED
// Engage Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
#if SERVO_LEVELING
if (axis==Z_AXIS) {
engage_z_probe();
}
else
#endif
if (servo_endstops[axis] > -1) {
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
}
#endif
#endif // Z_PROBE_SLED
#ifdef Z_DUAL_ENDSTOPS #ifdef Z_DUAL_ENDSTOPS
if (axis==Z_AXIS) In_Homing_Process(true); if (axis == Z_AXIS) In_Homing_Process(true);
#endif #endif
destination[axis] = 1.5 * max_length(axis) * axis_home_dir; destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
feedrate = homing_feedrate[axis]; feedrate = homing_feedrate[axis];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
current_position[axis] = 0; current_position[axis] = 0;
sync_plan_position(); sync_plan_position();
destination[axis] = -home_retract_mm(axis) * axis_home_dir; destination[axis] = -home_retract_mm(axis) * axis_home_dir;
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
destination[axis] = 2*home_retract_mm(axis) * axis_home_dir; destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
if (homing_bump_divisor[axis] >= 1) if (homing_bump_divisor[axis] >= 1)
{ feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
feedrate = homing_feedrate[axis]/homing_bump_divisor[axis]; else {
} feedrate = homing_feedrate[axis] / 10;
else SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
{
feedrate = homing_feedrate[axis]/10;
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
} }
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
#ifdef Z_DUAL_ENDSTOPS #ifdef Z_DUAL_ENDSTOPS
if (axis==Z_AXIS) if (axis==Z_AXIS)
@ -1475,7 +1498,7 @@ static void homeaxis(int axis) {
destination[axis] = fabs(z_endstop_adj); destination[axis] = fabs(z_endstop_adj);
if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true); if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);
} }
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
Lock_z_motor(false); Lock_z_motor(false);
Lock_z2_motor(false); Lock_z2_motor(false);
@ -1488,7 +1511,7 @@ static void homeaxis(int axis) {
if (endstop_adj[axis] * axis_home_dir < 0) { if (endstop_adj[axis] * axis_home_dir < 0) {
sync_plan_position(); sync_plan_position();
destination[axis] = endstop_adj[axis]; destination[axis] = endstop_adj[axis];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
} }
#endif #endif
@ -1533,7 +1556,7 @@ void refresh_cmd_timeout(void)
} }
plan_set_e_position(current_position[E_AXIS]); plan_set_e_position(current_position[E_AXIS]);
float oldFeedrate = feedrate; float oldFeedrate = feedrate;
feedrate=retract_feedrate*60; feedrate = retract_feedrate * 60;
retracted[active_extruder]=true; retracted[active_extruder]=true;
prepare_move(); prepare_move();
if(retract_zlift > 0.01) { if(retract_zlift > 0.01) {
@ -1569,8 +1592,8 @@ void refresh_cmd_timeout(void)
} }
plan_set_e_position(current_position[E_AXIS]); plan_set_e_position(current_position[E_AXIS]);
float oldFeedrate = feedrate; float oldFeedrate = feedrate;
feedrate=retract_recover_feedrate*60; feedrate = retract_recover_feedrate * 60;
retracted[active_extruder]=false; retracted[active_extruder] = false;
prepare_move(); prepare_move();
feedrate = oldFeedrate; feedrate = oldFeedrate;
} }
@ -1724,17 +1747,16 @@ inline void gcode_G4() {
*/ */
inline void gcode_G28() { inline void gcode_G28() {
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#ifdef DELTA #ifdef DELTA
reset_bed_level(); reset_bed_level();
#else
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#endif #endif
#endif #endif
#if defined(MESH_BED_LEVELING) #if defined(MESH_BED_LEVELING)
uint8_t mbl_was_active = mbl.active; uint8_t mbl_was_active = mbl.active;
mbl.active = 0; mbl.active = 0;
#endif // MESH_BED_LEVELING #endif
saved_feedrate = feedrate; saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply; saved_feedmultiply = feedmultiply;
@ -1757,7 +1779,7 @@ inline void gcode_G28() {
for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = 3 * Z_MAX_LENGTH; for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = 3 * Z_MAX_LENGTH;
feedrate = 1.732 * homing_feedrate[X_AXIS]; feedrate = 1.732 * homing_feedrate[X_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose();
@ -1805,7 +1827,7 @@ inline void gcode_G28() {
} else { } else {
feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1); feedrate *= sqrt(pow(max_length(X_AXIS) / max_length(Y_AXIS), 2) + 1);
} }
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
axis_is_at_home(X_AXIS); axis_is_at_home(X_AXIS);
@ -1813,7 +1835,7 @@ inline void gcode_G28() {
sync_plan_position(); sync_plan_position();
destination[X_AXIS] = current_position[X_AXIS]; destination[X_AXIS] = current_position[X_AXIS];
destination[Y_AXIS] = current_position[Y_AXIS]; destination[Y_AXIS] = current_position[Y_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
feedrate = 0.0; feedrate = 0.0;
st_synchronize(); st_synchronize();
endstops_hit_on_purpose(); endstops_hit_on_purpose();
@ -1880,7 +1902,7 @@ inline void gcode_G28() {
#if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0 #if defined(Z_RAISE_BEFORE_HOMING) && Z_RAISE_BEFORE_HOMING > 0
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = max_feedrate[Z_AXIS]; feedrate = max_feedrate[Z_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
#endif #endif
HOMEAXIS(Z); HOMEAXIS(Z);
@ -1892,11 +1914,11 @@ inline void gcode_G28() {
destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER); destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER); destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = XY_TRAVEL_SPEED / 60; feedrate = XY_TRAVEL_SPEED;
current_position[Z_AXIS] = 0; current_position[Z_AXIS] = 0;
sync_plan_position(); sync_plan_position();
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
current_position[X_AXIS] = destination[X_AXIS]; current_position[X_AXIS] = destination[X_AXIS];
current_position[Y_AXIS] = destination[Y_AXIS]; current_position[Y_AXIS] = destination[Y_AXIS];
@ -1918,7 +1940,7 @@ inline void gcode_G28() {
plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(cpx, cpy, current_position[Z_AXIS], current_position[E_AXIS]);
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
feedrate = max_feedrate[Z_AXIS]; feedrate = max_feedrate[Z_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
HOMEAXIS(Z); HOMEAXIS(Z);
} }
@ -1971,7 +1993,7 @@ inline void gcode_G28() {
destination[Z_AXIS] = current_position[Z_AXIS]; destination[Z_AXIS] = current_position[Z_AXIS];
destination[E_AXIS] = current_position[E_AXIS]; destination[E_AXIS] = current_position[E_AXIS];
feedrate = homing_feedrate[X_AXIS]; feedrate = homing_feedrate[X_AXIS];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder); line_to_destination();
st_synchronize(); st_synchronize();
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
sync_plan_position(); sync_plan_position();
@ -1985,6 +2007,19 @@ inline void gcode_G28() {
endstops_hit_on_purpose(); endstops_hit_on_purpose();
} }
#if defined(MESH_BED_LEVELING) || defined(ENABLE_AUTO_BED_LEVELING)
// Check for known positions in X and Y
inline bool can_run_bed_leveling() {
if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) return true;
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
return false;
}
#endif // MESH_BED_LEVELING || ENABLE_AUTO_BED_LEVELING
#ifdef MESH_BED_LEVELING #ifdef MESH_BED_LEVELING
/** /**
@ -1999,6 +2034,10 @@ inline void gcode_G28() {
* *
*/ */
inline void gcode_G29() { inline void gcode_G29() {
// Prevent leveling without first homing in X and Y
if (!can_run_bed_leveling()) return;
static int probe_point = -1; static int probe_point = -1;
int state = 0; int state = 0;
if (code_seen('S') || code_seen('s')) { if (code_seen('S') || code_seen('s')) {
@ -2115,13 +2154,8 @@ inline void gcode_G28() {
*/ */
inline void gcode_G29() { inline void gcode_G29() {
// Prevent user from running a G29 without first homing in X and Y // Prevent leveling without first homing in X and Y
if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) { if (!can_run_bed_leveling()) return;
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
return;
}
int verbose_level = 1; int verbose_level = 1;
@ -2203,16 +2237,15 @@ inline void gcode_G28() {
st_synchronize(); st_synchronize();
if (!dryrun) if (!dryrun) {
{ // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
plan_bed_level_matrix.set_to_identity();
#ifdef DELTA #ifdef DELTA
reset_bed_level(); reset_bed_level();
#else //!DELTA #else //!DELTA
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
//vector_3 corrected_position = plan_get_position_mm(); //vector_3 corrected_position = plan_get_position_mm();
//corrected_position.debug("position before G29"); //corrected_position.debug("position before G29");
plan_bed_level_matrix.set_to_identity();
vector_3 uncorrected_position = plan_get_position(); vector_3 uncorrected_position = plan_get_position();
//uncorrected_position.debug("position during G29"); //uncorrected_position.debug("position during G29");
current_position[X_AXIS] = uncorrected_position.x; current_position[X_AXIS] = uncorrected_position.x;
@ -2220,7 +2253,7 @@ inline void gcode_G28() {
current_position[Z_AXIS] = uncorrected_position.z; current_position[Z_AXIS] = uncorrected_position.z;
sync_plan_position(); sync_plan_position();
#endif #endif // !DELTA
} }
setup_for_endstop_move(); setup_for_endstop_move();
@ -2281,13 +2314,12 @@ inline void gcode_G28() {
// raise extruder // raise extruder
float measured_z, float measured_z,
z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS; z_before = Z_RAISE_BETWEEN_PROBINGS + (probePointCounter ? current_position[Z_AXIS] : 0);
#ifdef DELTA #ifdef DELTA
// Avoid probing the corners (outside the round or hexagon print surface) on a delta printer. // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe); float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
if (distance_from_center > DELTA_PROBABLE_RADIUS) if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
continue;
#endif //DELTA #endif //DELTA
// Enhanced G29 - Do not retract servo between probes // Enhanced G29 - Do not retract servo between probes
@ -2315,6 +2347,11 @@ inline void gcode_G28() {
#endif #endif
probePointCounter++; probePointCounter++;
manage_heater();
manage_inactivity();
lcd_update();
} //xProbe } //xProbe
} //yProbe } //yProbe
@ -2401,16 +2438,14 @@ inline void gcode_G28() {
if (verbose_level > 0) if (verbose_level > 0)
plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:"); plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
// Correct the Z height difference from z-probe position and hotend tip position. if (!dryrun) {
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend. // Correct the Z height difference from z-probe position and hotend tip position.
// When the bed is uneven, this height must be corrected. // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
if (!dryrun) // When the bed is uneven, this height must be corrected.
{ float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
float x_tmp, y_tmp, z_tmp, real_z; y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane) z_tmp = current_position[Z_AXIS],
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER; real_z = (float)st_get_position(Z_AXIS) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
z_tmp = current_position[Z_AXIS];
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner. current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
@ -4685,18 +4720,14 @@ void process_commands() {
gcode_G28(); gcode_G28();
break; break;
#if defined(MESH_BED_LEVELING) #if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
case 29: // G29 Handle mesh based leveling case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
gcode_G29(); gcode_G29();
break; break;
#endif #endif
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
gcode_G29();
break;
#ifndef Z_PROBE_SLED #ifndef Z_PROBE_SLED
case 30: // G30 Single Z Probe case 30: // G30 Single Z Probe
@ -5391,69 +5422,72 @@ void prepare_move()
#ifdef SCARA //for now same as delta-code #ifdef SCARA //for now same as delta-code
float difference[NUM_AXIS]; float difference[NUM_AXIS];
for (int8_t i=0; i < NUM_AXIS; i++) { for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
difference[i] = destination[i] - current_position[i];
} float cartesian_mm = sqrt( sq(difference[X_AXIS]) +
sq(difference[Y_AXIS]) +
sq(difference[Z_AXIS]));
if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
if (cartesian_mm < 0.000001) { return; }
float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
int steps = max(1, int(scara_segments_per_second * seconds));
//SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
//SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
//SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
for (int s = 1; s <= steps; s++) {
float fraction = float(s) / float(steps);
for(int8_t i = 0; i < NUM_AXIS; i++) {
destination[i] = current_position[i] + difference[i] * fraction;
}
float cartesian_mm = sqrt( sq(difference[X_AXIS]) + calculate_delta(destination);
sq(difference[Y_AXIS]) + //SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
sq(difference[Z_AXIS])); //SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); } //SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
if (cartesian_mm < 0.000001) { return; } //SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
float seconds = 6000 * cartesian_mm / feedrate / feedmultiply; //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
int steps = max(1, int(scara_segments_per_second * seconds)); //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
//SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
//SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
//SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
for (int s = 1; s <= steps; s++) {
float fraction = float(s) / float(steps);
for(int8_t i=0; i < NUM_AXIS; i++) {
destination[i] = current_position[i] + difference[i] * fraction;
}
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
destination[E_AXIS], feedrate*feedmultiply/60/100.0,
active_extruder);
}
calculate_delta(destination); #endif // SCARA
//SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
//SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
//SERIAL_ECHOPGM("destination[Z_AXIS]="); SERIAL_ECHOLN(destination[Z_AXIS]);
//SERIAL_ECHOPGM("delta[X_AXIS]="); SERIAL_ECHOLN(delta[X_AXIS]);
//SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
//SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], #ifdef DELTA
destination[E_AXIS], feedrate*feedmultiply/60/100.0,
active_extruder);
}
#endif // SCARA
#ifdef DELTA float difference[NUM_AXIS];
float difference[NUM_AXIS]; for (int8_t i=0; i < NUM_AXIS; i++) difference[i] = destination[i] - current_position[i];
for (int8_t i=0; i < NUM_AXIS; i++) {
difference[i] = destination[i] - current_position[i]; float cartesian_mm = sqrt(sq(difference[X_AXIS]) +
} sq(difference[Y_AXIS]) +
float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Z_AXIS]));
sq(difference[Y_AXIS]) + if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
sq(difference[Z_AXIS])); if (cartesian_mm < 0.000001) return;
if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); } float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
if (cartesian_mm < 0.000001) { return; } int steps = max(1, int(delta_segments_per_second * seconds));
float seconds = 6000 * cartesian_mm / feedrate / feedmultiply;
int steps = max(1, int(delta_segments_per_second * seconds)); // SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm);
// SERIAL_ECHOPGM("mm="); SERIAL_ECHO(cartesian_mm); // SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds);
// SERIAL_ECHOPGM(" seconds="); SERIAL_ECHO(seconds); // SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
// SERIAL_ECHOPGM(" steps="); SERIAL_ECHOLN(steps);
for (int s = 1; s <= steps; s++) { for (int s = 1; s <= steps; s++) {
float fraction = float(s) / float(steps); float fraction = float(s) / float(steps);
for(int8_t i=0; i < NUM_AXIS; i++) { for (int8_t i = 0; i < NUM_AXIS; i++) destination[i] = current_position[i] + difference[i] * fraction;
destination[i] = current_position[i] + difference[i] * fraction; calculate_delta(destination);
#ifdef ENABLE_AUTO_BED_LEVELING
adjust_delta(destination);
#endif
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
destination[E_AXIS], feedrate*feedmultiply/60/100.0,
active_extruder);
} }
calculate_delta(destination);
plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
destination[E_AXIS], feedrate*feedmultiply/60/100.0,
active_extruder);
}
#endif // DELTA #endif // DELTA
#ifdef DUAL_X_CARRIAGE #ifdef DUAL_X_CARRIAGE
if (active_extruder_parked) if (active_extruder_parked)
@ -5499,13 +5533,13 @@ for (int s = 1; s <= steps; s++) {
#if ! (defined DELTA || defined SCARA) #if ! (defined DELTA || defined SCARA)
// Do not use feedmultiply for E or Z only moves // Do not use feedmultiply for E or Z only moves
if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) { if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); line_to_destination();
} else { } else {
#if defined(MESH_BED_LEVELING) #if defined(MESH_BED_LEVELING)
mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder); mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
return; return;
#else #else
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder); plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], (feedrate/60)*(feedmultiply/100.0), active_extruder);
#endif // MESH_BED_LEVELING #endif // MESH_BED_LEVELING
} }
#endif // !(DELTA || SCARA) #endif // !(DELTA || SCARA)

116
Marlin/stepper.cpp
Unescape View File

@ -511,83 +511,107 @@ ISR(TIMER1_COMPA_vect) {
} }
if (TEST(out_bits, Z_AXIS)) { // -direction if (TEST(out_bits, Z_AXIS)) { // -direction
Z_APPLY_DIR(INVERT_Z_DIR,0); Z_APPLY_DIR(INVERT_Z_DIR,0);
count_direction[Z_AXIS] = -1; count_direction[Z_AXIS] = -1;
if (check_endstops)
{ if (check_endstops) {
#if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
#ifndef Z_DUAL_ENDSTOPS #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
UPDATE_ENDSTOP(z, Z, min, MIN);
#else #ifdef Z_DUAL_ENDSTOPS
bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
#if defined(Z2_MIN_PIN) && Z2_MIN_PIN > -1 bool z_min_endstop = READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING,
bool z2_min_endstop=(READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING); z2_min_endstop =
#else #if defined(Z2_MIN_PIN) && Z2_MIN_PIN >= 0
bool z2_min_endstop=z_min_endstop; READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
#endif #else
if(((z_min_endstop && old_z_min_endstop) || (z2_min_endstop && old_z2_min_endstop)) && (current_block->steps[Z_AXIS] > 0)) z_min_endstop
{ #endif
;
bool z_min_both = z_min_endstop && old_z_min_endstop,
z2_min_both = z2_min_endstop && old_z2_min_endstop;
if ((z_min_both || z2_min_both) && current_block->steps[Z_AXIS] > 0) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true; endstop_z_hit = true;
if (!(performing_homing) || ((performing_homing)&&(z_min_endstop && old_z_min_endstop)&&(z2_min_endstop && old_z2_min_endstop))) //if not performing home or if both endstops were trigged during homing... if (!performing_homing || (performing_homing && z_min_both && z2_min_both)) //if not performing home or if both endstops were trigged during homing...
{
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
}
} }
old_z_min_endstop = z_min_endstop; old_z_min_endstop = z_min_endstop;
old_z2_min_endstop = z2_min_endstop; old_z2_min_endstop = z2_min_endstop;
#endif
#endif #else // !Z_DUAL_ENDSTOPS
}
UPDATE_ENDSTOP(z, Z, min, MIN);
#endif // !Z_DUAL_ENDSTOPS
#endif // Z_MIN_PIN
} // check_endstops
} }
else { // +direction else { // +direction
Z_APPLY_DIR(!INVERT_Z_DIR,0); Z_APPLY_DIR(!INVERT_Z_DIR,0);
count_direction[Z_AXIS] = 1; count_direction[Z_AXIS] = 1;
if (check_endstops) { if (check_endstops) {
#if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0 #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
#ifndef Z_DUAL_ENDSTOPS
UPDATE_ENDSTOP(z, Z, max, MAX);
#else
bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
#if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
bool z2_max_endstop=(READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING);
#else
bool z2_max_endstop=z_max_endstop;
#endif
if(((z_max_endstop && old_z_max_endstop) || (z2_max_endstop && old_z2_max_endstop)) && (current_block->steps[Z_AXIS] > 0))
{
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
// if (z_max_endstop && old_z_max_endstop) SERIAL_ECHOLN("z_max_endstop = true"); #ifdef Z_DUAL_ENDSTOPS
// if (z2_max_endstop && old_z2_max_endstop) SERIAL_ECHOLN("z2_max_endstop = true");
bool z_max_endstop = READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING,
z2_max_endstop =
#if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING
#else
z_max_endstop
#endif
;
if (!(performing_homing) || ((performing_homing)&&(z_max_endstop && old_z_max_endstop)&&(z2_max_endstop && old_z2_max_endstop))) //if not performing home or if both endstops were trigged during homing... bool z_max_both = z_max_endstop && old_z_max_endstop,
{ z2_max_both = z2_max_endstop && old_z2_max_endstop;
if ((z_max_both || z2_max_both) && current_block->steps[Z_AXIS] > 0) {
endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit = true;
// if (z_max_both) SERIAL_ECHOLN("z_max_endstop = true");
// if (z2_max_both) SERIAL_ECHOLN("z2_max_endstop = true");
if (!performing_homing || (performing_homing && z_max_both && z2_max_both)) //if not performing home or if both endstops were trigged during homing...
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
}
} }
old_z_max_endstop = z_max_endstop; old_z_max_endstop = z_max_endstop;
old_z2_max_endstop = z2_max_endstop; old_z2_max_endstop = z2_max_endstop;
#endif
#endif #else // !Z_DUAL_ENDSTOPS
}
} UPDATE_ENDSTOP(z, Z, max, MAX);
#endif // !Z_DUAL_ENDSTOPS
#endif // Z_MAX_PIN
} // check_endstops
} // +direction
#ifndef ADVANCE #ifndef ADVANCE
if (TEST(out_bits, E_AXIS)) { // -direction if (TEST(out_bits, E_AXIS)) { // -direction
REV_E_DIR(); REV_E_DIR();
count_direction[E_AXIS]=-1; count_direction[E_AXIS] = -1;
} }
else { // +direction else { // +direction
NORM_E_DIR(); NORM_E_DIR();
count_direction[E_AXIS]=1; count_direction[E_AXIS] = 1;
} }
#endif //!ADVANCE #endif //!ADVANCE
// Take multiple steps per interrupt (For high speed moves) // Take multiple steps per interrupt (For high speed moves)
for (int8_t i=0; i < step_loops; i++) { for (int8_t i = 0; i < step_loops; i++) {
#ifndef AT90USB #ifndef AT90USB
MSerial.checkRx(); // Check for serial chars. MSerial.checkRx(); // Check for serial chars.
#endif #endif

Write Preview
Loading…
Cancel
Save