andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #1740 from thinkyhead/fixup_homing 

Apply leveling for DELTA
pull/1/head
Scott Lahteine 10 years ago
parent
346c07cc1a 41d258697a
commit
baa6787393
2 changed files with 572 additions and 514 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 270
      Marlin/Marlin_main.cpp
  2. 98
      Marlin/stepper.cpp

270
Marlin/Marlin_main.cpp
View File

@ -79,7 +79,7 @@
// G4 - Dwell S<seconds> or P<milliseconds>
// G10 - retract filament according to settings of M207
// 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.
// G30 - Single Z Probe, probes bed at current XY location.
// G31 - Dock sled (Z_PROBE_SLED only)
@ -476,8 +476,6 @@ bool enquecommand(const char *cmd)
return true;
}
void setup_killpin()
{
#if defined(KILL_PIN) && KILL_PIN > -1
@ -965,10 +963,10 @@ static void axis_is_at_home(int axis) {
return;
}
else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE) {
current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
min_pos[X_AXIS] = base_min_pos(X_AXIS) + home_offset[X_AXIS];
max_pos[X_AXIS] = min(base_max_pos(X_AXIS) + home_offset[X_AXIS],
max(extruder_offset[1][X_AXIS], X2_MAX_POS) - duplicate_extruder_x_offset);
float xoff = home_offset[X_AXIS];
current_position[X_AXIS] = base_home_pos(X_AXIS) + xoff;
min_pos[X_AXIS] = base_min_pos(X_AXIS) + xoff;
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;
}
}
@ -1022,16 +1020,27 @@ 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() {
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 AUTO_BED_LEVELING_GRID
#ifndef DELTA
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");
@ -1050,9 +1059,10 @@ inline void sync_plan_position() {
sync_plan_position();
}
#endif
#else // not AUTO_BED_LEVELING_GRID
#endif // !DELTA
#else // !AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
@ -1079,9 +1089,10 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float
sync_plan_position();
}
#endif // AUTO_BED_LEVELING_GRID
#endif // !AUTO_BED_LEVELING_GRID
static void run_z_probe() {
#ifdef DELTA
float start_z = current_position[Z_AXIS];
@ -1101,14 +1112,14 @@ static void run_z_probe() {
calculate_delta(current_position);
plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
#else
#else // !DELTA
plan_bed_level_matrix.set_to_identity();
feedrate = homing_feedrate[Z_AXIS];
// move down until you find the bed
float zPosition = -10;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
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.
@ -1117,21 +1128,20 @@ static void run_z_probe() {
// move up the retract distance
zPosition += home_retract_mm(Z_AXIS);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
line_to_z(zPosition);
st_synchronize();
endstops_hit_on_purpose();
// move back down slowly to find bed
if (homing_bump_divisor[Z_AXIS] >= 1) {
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 then 1");
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 1");
}
zPosition -= home_retract_mm(Z_AXIS) * 2;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
line_to_z(zPosition);
st_synchronize();
endstops_hit_on_purpose();
@ -1139,7 +1149,7 @@ static void run_z_probe() {
// 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
#endif // !DELTA
}
static void do_blocking_move_to(float x, float y, float z) {
@ -1160,14 +1170,14 @@ static void do_blocking_move_to(float x, float y, float z) {
feedrate = homing_feedrate[Z_AXIS];
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();
feedrate = xy_travel_speed;
current_position[X_AXIS] = x;
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();
#endif
@ -1180,7 +1190,6 @@ static void setup_for_endstop_move() {
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
previous_millis_cmd = millis();
enable_endstops(true);
}
@ -1188,16 +1197,17 @@ static void clean_up_after_endstop_move() {
#ifdef ENDSTOPS_ONLY_FOR_HOMING
enable_endstops(false);
#endif
feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply;
previous_millis_cmd = millis();
}
static void engage_z_probe() {
// Engage Z Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
if (servo_endstops[Z_AXIS] > -1) {
// Engage Z Servo endstop if enabled
if (servo_endstops[Z_AXIS] >= 0) {
#if SERVO_LEVELING
servos[servo_endstops[Z_AXIS]].attach(0);
#endif
@ -1207,7 +1217,9 @@ static void engage_z_probe() {
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
@ -1229,40 +1241,45 @@ static void engage_z_probe() {
st_synchronize();
bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
if (z_min_endstop)
{
if (!Stopped)
{
if (z_min_endstop) {
if (!Stopped) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Z-Probe failed to engage!");
LCD_ALERTMESSAGEPGM("Err: ZPROBE");
}
Stop();
}
#endif
#endif // Z_PROBE_ALLEN_KEY
}
static void retract_z_probe() {
// Retract Z Servo endstop if enabled
static void retract_z_probe(const float z_after=Z_RAISE_AFTER_PROBING) {
#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);
// Retract Z Servo endstop if enabled
if (servo_endstops[Z_AXIS] >= 0) {
if (z_after > 0) {
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_after);
st_synchronize();
#endif
}
#if SERVO_LEVELING
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;
@ -1293,16 +1310,15 @@ static void retract_z_probe() {
st_synchronize();
bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
if (!z_min_endstop)
{
if (!Stopped)
{
if (!z_min_endstop) {
if (!Stopped) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Z-Probe failed to retract!");
LCD_ALERTMESSAGEPGM("Err: ZPROBE");
}
Stop();
}
#endif
}
@ -1314,7 +1330,7 @@ enum ProbeAction {
ProbeEngageAndRetract = (ProbeEngage | ProbeRetract)
};
/// Probe bed height at position (x,y), returns the measured z value
// 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);
@ -1328,7 +1344,7 @@ static float probe_pt(float x, float y, float z_before, ProbeAction retract_acti
float measured_z = current_position[Z_AXIS];
#if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
if (retract_action & ProbeRetract) retract_z_probe();
if (retract_action & ProbeRetract) retract_z_probe(z_before);
#endif
if (verbose_level > 2) {
@ -1345,6 +1361,11 @@ static float probe_pt(float x, float y, float z_before, ProbeAction retract_acti
}
#ifdef DELTA
/**
* All DELTA leveling in the Marlin uses NONLINEAR_BED_LEVELING
*/
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.
@ -1408,18 +1429,19 @@ static void homeaxis(int axis) {
if (axis == X_AXIS ? HOMEAXIS_DO(X) :
axis == Y_AXIS ? HOMEAXIS_DO(Y) :
axis==Z_AXIS ? HOMEAXIS_DO(Z) :
0) {
int axis_home_dir = home_dir(axis);
axis == Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
int axis_home_dir;
#ifdef DUAL_X_CARRIAGE
if (axis == X_AXIS)
axis_home_dir = x_home_dir(active_extruder);
if (axis == X_AXIS) axis_home_dir = x_home_dir(active_extruder);
#else
axis_home_dir = home_dir(axis);
#endif
current_position[axis] = 0;
sync_plan_position();
#ifndef Z_PROBE_SLED
// Engage Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
@ -1428,39 +1450,40 @@ static void homeaxis(int axis) {
engage_z_probe();
}
else
#endif
if (servo_endstops[axis] > -1) {
#endif // SERVO_LEVELING
if (servo_endstops[axis] > -1)
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
}
#endif
#endif // SERVO_ENDSTOPS
#endif // Z_PROBE_SLED
#ifdef Z_DUAL_ENDSTOPS
if (axis == Z_AXIS) In_Homing_Process(true);
#endif
destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
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();
current_position[axis] = 0;
sync_plan_position();
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();
destination[axis] = 2 * home_retract_mm(axis) * axis_home_dir;
if (homing_bump_divisor[axis] >= 1)
{
feedrate = homing_feedrate[axis] / homing_bump_divisor[axis];
}
else
{
else {
feedrate = homing_feedrate[axis] / 10;
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less then 1");
SERIAL_ECHOLN("Warning: The Homing Bump Feedrate Divisor cannot be less than 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();
#ifdef Z_DUAL_ENDSTOPS
if (axis==Z_AXIS)
@ -1475,7 +1498,7 @@ static void homeaxis(int axis) {
destination[axis] = fabs(z_endstop_adj);
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();
Lock_z_motor(false);
Lock_z2_motor(false);
@ -1488,7 +1511,7 @@ static void homeaxis(int axis) {
if (endstop_adj[axis] * axis_home_dir < 0) {
sync_plan_position();
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();
}
#endif
@ -1724,17 +1747,16 @@ inline void gcode_G4() {
*/
inline void gcode_G28() {
#ifdef ENABLE_AUTO_BED_LEVELING
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#ifdef DELTA
reset_bed_level();
#else
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#endif
#endif
#if defined(MESH_BED_LEVELING)
uint8_t mbl_was_active = mbl.active;
mbl.active = 0;
#endif // MESH_BED_LEVELING
#endif
saved_feedrate = feedrate;
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;
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();
endstops_hit_on_purpose();
@ -1805,7 +1827,7 @@ inline void gcode_G28() {
} else {
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();
axis_is_at_home(X_AXIS);
@ -1813,7 +1835,7 @@ inline void gcode_G28() {
sync_plan_position();
destination[X_AXIS] = current_position[X_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;
st_synchronize();
endstops_hit_on_purpose();
@ -1880,7 +1902,7 @@ inline void gcode_G28() {
#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
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();
#endif
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[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
feedrate = XY_TRAVEL_SPEED / 60;
feedrate = XY_TRAVEL_SPEED;
current_position[Z_AXIS] = 0;
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();
current_position[X_AXIS] = destination[X_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]);
destination[Z_AXIS] = -Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS); // Set destination away from bed
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();
HOMEAXIS(Z);
}
@ -1971,7 +1993,7 @@ inline void gcode_G28() {
destination[Z_AXIS] = current_position[Z_AXIS];
destination[E_AXIS] = current_position[E_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();
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
sync_plan_position();
@ -1985,6 +2007,19 @@ inline void gcode_G28() {
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
/**
@ -1999,6 +2034,10 @@ inline void gcode_G28() {
*
*/
inline void gcode_G29() {
// Prevent leveling without first homing in X and Y
if (!can_run_bed_leveling()) return;
static int probe_point = -1;
int state = 0;
if (code_seen('S') || code_seen('s')) {
@ -2115,13 +2154,8 @@ inline void gcode_G28() {
*/
inline void gcode_G29() {
// Prevent user from running a G29 without first homing in X and Y
if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
return;
}
// Prevent leveling without first homing in X and Y
if (!can_run_bed_leveling()) return;
int verbose_level = 1;
@ -2203,16 +2237,15 @@ inline void gcode_G28() {
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
reset_bed_level();
#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();
//corrected_position.debug("position before G29");
plan_bed_level_matrix.set_to_identity();
vector_3 uncorrected_position = plan_get_position();
//uncorrected_position.debug("position during G29");
current_position[X_AXIS] = uncorrected_position.x;
@ -2220,7 +2253,7 @@ inline void gcode_G28() {
current_position[Z_AXIS] = uncorrected_position.z;
sync_plan_position();
#endif
#endif // !DELTA
}
setup_for_endstop_move();
@ -2281,13 +2314,12 @@ inline void gcode_G28() {
// raise extruder
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
// Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
if (distance_from_center > DELTA_PROBABLE_RADIUS)
continue;
if (distance_from_center > DELTA_PROBABLE_RADIUS) continue;
#endif //DELTA
// Enhanced G29 - Do not retract servo between probes
@ -2315,6 +2347,11 @@ inline void gcode_G28() {
#endif
probePointCounter++;
manage_heater();
manage_inactivity();
lcd_update();
} //xProbe
} //yProbe
@ -2401,16 +2438,14 @@ inline void gcode_G28() {
if (verbose_level > 0)
plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
if (!dryrun) {
// Correct the Z height difference from z-probe position and hotend tip position.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// When the bed is uneven, this height must be corrected.
if (!dryrun)
{
float x_tmp, y_tmp, z_tmp, real_z;
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)
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
z_tmp = current_position[Z_AXIS];
float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
z_tmp = current_position[Z_AXIS],
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)
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.
@ -4685,18 +4720,14 @@ void process_commands() {
gcode_G28();
break;
#if defined(MESH_BED_LEVELING)
case 29: // G29 Handle mesh based leveling
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
gcode_G29();
break;
#endif
#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
case 30: // G30 Single Z Probe
@ -5392,9 +5423,7 @@ void prepare_move()
#ifdef SCARA //for now same as delta-code
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]) +
@ -5403,16 +5432,17 @@ 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;
}
calculate_delta(destination);
//SERIAL_ECHOPGM("destination[X_AXIS]="); SERIAL_ECHOLN(destination[X_AXIS]);
//SERIAL_ECHOPGM("destination[Y_AXIS]="); SERIAL_ECHOLN(destination[Y_AXIS]);
@ -5425,29 +5455,33 @@ for (int s = 1; s <= steps; s++) {
destination[E_AXIS], feedrate*feedmultiply/60/100.0,
active_extruder);
}
#endif // SCARA
#ifdef DELTA
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]) +
sq(difference[Z_AXIS]));
if (cartesian_mm < 0.000001) { cartesian_mm = abs(difference[E_AXIS]); }
if (cartesian_mm < 0.000001) { return; }
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(delta_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;
}
for (int8_t i = 0; i < NUM_AXIS; i++) 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);
@ -5499,13 +5533,13 @@ for (int s = 1; s <= steps; s++) {
#if ! (defined DELTA || defined SCARA)
// 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])) {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
line_to_destination();
} else {
#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;
#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 // !(DELTA || SCARA)

98
Marlin/stepper.cpp
View File

@ -511,69 +511,93 @@ ISR(TIMER1_COMPA_vect) {
}
if (TEST(out_bits, Z_AXIS)) { // -direction
Z_APPLY_DIR(INVERT_Z_DIR,0);
count_direction[Z_AXIS] = -1;
if (check_endstops)
{
#if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
#ifndef Z_DUAL_ENDSTOPS
UPDATE_ENDSTOP(z, Z, min, MIN);
#else
bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
#if defined(Z2_MIN_PIN) && Z2_MIN_PIN > -1
bool z2_min_endstop=(READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING);
if (check_endstops) {
#if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
#ifdef Z_DUAL_ENDSTOPS
bool z_min_endstop = READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING,
z2_min_endstop =
#if defined(Z2_MIN_PIN) && Z2_MIN_PIN >= 0
READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING
#else
bool z2_min_endstop=z_min_endstop;
z_min_endstop
#endif
if(((z_min_endstop && old_z_min_endstop) || (z2_min_endstop && old_z2_min_endstop)) && (current_block->steps[Z_AXIS] > 0))
{
;
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];
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;
}
}
old_z_min_endstop = z_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
Z_APPLY_DIR(!INVERT_Z_DIR,0);
count_direction[Z_AXIS] = 1;
if (check_endstops) {
#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);
#ifdef Z_DUAL_ENDSTOPS
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
bool z2_max_endstop=z_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))
{
;
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_endstop && old_z_max_endstop) SERIAL_ECHOLN("z_max_endstop = true");
// if (z2_max_endstop && old_z2_max_endstop) SERIAL_ECHOLN("z2_max_endstop = 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_endstop && old_z_max_endstop)&&(z2_max_endstop && old_z2_max_endstop))) //if not performing home or if both endstops were trigged during homing...
{
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;
}
}
old_z_max_endstop = z_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
if (TEST(out_bits, E_AXIS)) { // -direction

Write Preview
Loading…
Cancel
Save