bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

297
Marlin/src/gcode/bedlevel/G26.cpp
View File

@@ -62,7 +62,7 @@
#define G26_ERR true
#if ENABLED(ARC_SUPPORT)
void plan_arc(const float (&cart)[XYZE], const float (&offset)[2], const uint8_t clockwise);
void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const uint8_t clockwise);
#endif
/**
@@ -142,7 +142,7 @@
// Private functions
static uint16_t circle_flags[16], horizontal_mesh_line_flags[16], vertical_mesh_line_flags[16];
static MeshFlags circle_flags, horizontal_mesh_line_flags, vertical_mesh_line_flags;
float g26_e_axis_feedrate = 0.025,
random_deviation = 0.0;
@@ -154,7 +154,7 @@ float g26_extrusion_multiplier,
g26_layer_height,
g26_prime_length;
float g26_x_pos = 0, g26_y_pos = 0;
xy_pos_t g26_pos; // = { 0, 0 }
int16_t g26_bed_temp,
g26_hotend_temp;
@@ -178,85 +178,85 @@ int8_t g26_prime_flag;
#endif
mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) {
mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) {
float closest = 99999.99;
mesh_index_pair return_val;
mesh_index_pair out_point;
return_val.x_index = return_val.y_index = -1;
out_point.pos = -1;
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (!is_bitmap_set(circle_flags, i, j)) {
const float mx = _GET_MESH_X(i), // We found a circle that needs to be printed
my = _GET_MESH_Y(j);
if (!circle_flags.marked(i, j)) {
// We found a circle that needs to be printed
const xy_pos_t m = { _GET_MESH_X(i), _GET_MESH_Y(j) };
// Get the distance to this intersection
float f = HYPOT(X - mx, Y - my);
float f = (pos - m).magnitude();
// It is possible that we are being called with the values
// to let us find the closest circle to the start position.
// But if this is not the case, add a small weighting to the
// distance calculation to help it choose a better place to continue.
f += HYPOT(g26_x_pos - mx, g26_y_pos - my) / 15.0;
f += (g26_pos - m).magnitude() / 15.0f;
// Add in the specified amount of Random Noise to our search
if (random_deviation > 1.0)
f += random(0.0, random_deviation);
// Add the specified amount of Random Noise to our search
if (random_deviation > 1.0) f += random(0.0, random_deviation);
if (f < closest) {
closest = f; // We found a closer location that is still
return_val.x_index = i; // un-printed --- save the data for it
return_val.y_index = j;
return_val.distance = closest;
closest = f; // Found a closer un-printed location
out_point.pos.set(i, j); // Save its data
out_point.distance = closest;
}
}
}
}
bitmap_set(circle_flags, return_val.x_index, return_val.y_index); // Mark this location as done.
return return_val;
circle_flags.mark(out_point); // Mark this location as done.
return out_point;
}
void move_to(const float &rx, const float &ry, const float &z, const float &e_delta) {
static float last_z = -999.99;
bool has_xy_component = (rx != current_position[X_AXIS] || ry != current_position[Y_AXIS]); // Check if X or Y is involved in the movement.
const xy_pos_t dest = { rx, ry };
const bool has_xy_component = dest != current_position; // Check if X or Y is involved in the movement.
destination = current_position;
if (z != last_z) {
last_z = z;
last_z = destination.z = z;
const feedRate_t feed_value = planner.settings.max_feedrate_mm_s[Z_AXIS] * 0.5f; // Use half of the Z_AXIS max feed rate
destination[X_AXIS] = current_position[X_AXIS];
destination[Y_AXIS] = current_position[Y_AXIS];
destination[Z_AXIS] = z; // We know the last_z!=z or we wouldn't be in this block of code.
destination[E_AXIS] = current_position[E_AXIS];
prepare_internal_move_to_destination(feed_value);
set_destination_from_current();
destination = current_position;
}
// If X or Y is involved do a 'normal' move. Otherwise retract/recover/hop.
destination = dest;
destination.e += e_delta;
const feedRate_t feed_value = has_xy_component ? feedRate_t(G26_XY_FEEDRATE) : planner.settings.max_feedrate_mm_s[E_AXIS] * 0.666f;
destination[X_AXIS] = rx;
destination[Y_AXIS] = ry;
destination[E_AXIS] += e_delta;
prepare_internal_move_to_destination(feed_value);
set_destination_from_current();
destination = current_position;
}
FORCE_INLINE void move_to(const float (&where)[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); }
FORCE_INLINE void move_to(const xyz_pos_t &where, const float &de) { move_to(where.x, where.y, where.z, de); }
void retract_filament(const float (&where)[XYZE]) {
void retract_filament(const xyz_pos_t &where) {
if (!g26_retracted) { // Only retract if we are not already retracted!
g26_retracted = true;
move_to(where, -1.0 * g26_retraction_multiplier);
move_to(where, -1.0f * g26_retraction_multiplier);
}
}
void recover_filament(const float (&where)[XYZE]) {
// TODO: Parameterize the Z lift with a define
void retract_lift_move(const xyz_pos_t &s) {
retract_filament(destination);
move_to(current_position.x, current_position.y, current_position.z + 0.5f, 0.0); // Z lift to minimize scraping
move_to(s.x, s.y, s.z + 0.5f, 0.0); // Get to the starting point with no extrusion while lifted
}
void recover_filament(const xyz_pos_t &where) {
if (g26_retracted) { // Only un-retract if we are retracted.
move_to(where, 1.2 * g26_retraction_multiplier);
move_to(where, 1.2f * g26_retraction_multiplier);
g26_retracted = false;
}
}
@@ -276,41 +276,34 @@ void recover_filament(const float (&where)[XYZE]) {
* segment of a 'circle'. The time this requires is very short and is easily saved by the other
* cases where the optimization comes into play.
*/
void print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) {
const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment
dy_s = current_position[Y_AXIS] - sy,
dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2
// to save computation time
dx_e = current_position[X_AXIS] - ex, // find our distance from the end of the actual line segment
dy_e = current_position[Y_AXIS] - ey,
dist_end = HYPOT2(dx_e, dy_e),
void print_line_from_here_to_there(const xyz_pos_t &s, const xyz_pos_t &e) {
line_length = HYPOT(ex - sx, ey - sy);
// Distances to the start / end of the line
xy_float_t svec = current_position - s, evec = current_position - e;
const float dist_start = HYPOT2(svec.x, svec.y),
dist_end = HYPOT2(evec.x, evec.y),
line_length = HYPOT(e.x - s.x, e.y - s.y);
// If the end point of the line is closer to the nozzle, flip the direction,
// moving from the end to the start. On very small lines the optimization isn't worth it.
if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length))
return print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
return print_line_from_here_to_there(e, s);
// Decide whether to retract & bump
// Decide whether to retract & lift
if (dist_start > 2.0) retract_lift_move(s);
if (dist_start > 2.0) {
retract_filament(destination);
//todo: parameterize the bump height with a define
move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + 0.500, 0.0); // Z bump to minimize scraping
move_to(sx, sy, sz + 0.500, 0.0); // Get to the starting point with no extrusion while bumped
}
move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion / un-Z bump
move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift
const float e_pos_delta = line_length * g26_e_axis_feedrate * g26_extrusion_multiplier;
recover_filament(destination);
move_to(ex, ey, ez, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion
move_to(e, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion
}
inline bool look_for_lines_to_connect() {
float sx, sy, ex, ey;
xyz_pos_t s, e;
s.z = e.z = g26_layer_height;
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
@@ -319,43 +312,43 @@ inline bool look_for_lines_to_connect() {
if (user_canceled()) return true;
#endif
if (i < GRID_MAX_POINTS_X) { // Can't connect to anything to the right than GRID_MAX_POINTS_X.
// Already a half circle at the edge of the bed.
if (i < GRID_MAX_POINTS_X) { // Can't connect to anything farther to the right than GRID_MAX_POINTS_X.
// Already a half circle at the edge of the bed.
if (is_bitmap_set(circle_flags, i, j) && is_bitmap_set(circle_flags, i + 1, j)) { // check if we can do a line to the left
if (!is_bitmap_set(horizontal_mesh_line_flags, i, j)) {
if (circle_flags.marked(i, j) && circle_flags.marked(i + 1, j)) { // Test whether a leftward line can be done
if (!horizontal_mesh_line_flags.marked(i, j)) {
// Two circles need a horizontal line to connect them
sx = _GET_MESH_X( i ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // right edge
ex = _GET_MESH_X(i + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // left edge
s.x = _GET_MESH_X( i ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // right edge
e.x = _GET_MESH_X(i + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // left edge
LIMIT(sx, X_MIN_POS + 1, X_MAX_POS - 1);
sy = ey = constrain(_GET_MESH_Y(j), Y_MIN_POS + 1, Y_MAX_POS - 1);
LIMIT(ex, X_MIN_POS + 1, X_MAX_POS - 1);
LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1);
s.y = e.y = constrain(_GET_MESH_Y(j), Y_MIN_POS + 1, Y_MAX_POS - 1);
LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1);
if (position_is_reachable(sx, sy) && position_is_reachable(ex, ey))
print_line_from_here_to_there(sx, sy, g26_layer_height, ex, ey, g26_layer_height);
if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
print_line_from_here_to_there(s, e);
bitmap_set(horizontal_mesh_line_flags, i, j); // Mark done, even if skipped
horizontal_mesh_line_flags.mark(i, j); // Mark done, even if skipped
}
}
if (j < GRID_MAX_POINTS_Y) { // Can't connect to anything further back than GRID_MAX_POINTS_Y.
// Already a half circle at the edge of the bed.
if (is_bitmap_set(circle_flags, i, j) && is_bitmap_set(circle_flags, i, j + 1)) { // check if we can do a line straight down
if (!is_bitmap_set( vertical_mesh_line_flags, i, j)) {
if (circle_flags.marked(i, j) && circle_flags.marked(i, j + 1)) { // Test whether a downward line can be done
if (!vertical_mesh_line_flags.marked(i, j)) {
// Two circles that need a vertical line to connect them
sy = _GET_MESH_Y( j ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // top edge
ey = _GET_MESH_Y(j + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // bottom edge
s.y = _GET_MESH_Y( j ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // top edge
e.y = _GET_MESH_Y(j + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // bottom edge
sx = ex = constrain(_GET_MESH_X(i), X_MIN_POS + 1, X_MAX_POS - 1);
LIMIT(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
LIMIT(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
s.x = e.x = constrain(_GET_MESH_X(i), X_MIN_POS + 1, X_MAX_POS - 1);
LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (position_is_reachable(sx, sy) && position_is_reachable(ex, ey))
print_line_from_here_to_there(sx, sy, g26_layer_height, ex, ey, g26_layer_height);
if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y))
print_line_from_here_to_there(s, e);
bitmap_set(vertical_mesh_line_flags, i, j); // Mark done, even if skipped
vertical_mesh_line_flags.mark(i, j); // Mark done, even if skipped
}
}
}
@@ -436,19 +429,19 @@ inline bool prime_nozzle() {
ui.set_status_P(PSTR(MSG_G26_MANUAL_PRIME), 99);
ui.chirp();
set_destination_from_current();
destination = current_position;
recover_filament(destination); // Make sure G26 doesn't think the filament is retracted().
while (!ui.button_pressed()) {
ui.chirp();
destination[E_AXIS] += 0.25;
destination.e += 0.25;
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
Total_Prime += 0.25;
if (Total_Prime >= EXTRUDE_MAXLENGTH) return G26_ERR;
#endif
prepare_internal_move_to_destination(fr_slow_e);
set_destination_from_current();
destination = current_position;
planner.synchronize(); // Without this synchronize, the purge is more consistent,
// but because the planner has a buffer, we won't be able
// to stop as quickly. So we put up with the less smooth
@@ -468,10 +461,10 @@ inline bool prime_nozzle() {
ui.set_status_P(PSTR(MSG_G26_FIXED_LENGTH), 99);
ui.quick_feedback();
#endif
set_destination_from_current();
destination[E_AXIS] += g26_prime_length;
destination = current_position;
destination.e += g26_prime_length;
prepare_internal_move_to_destination(fr_slow_e);
set_destination_from_current();
destination.e -= g26_prime_length;
retract_filament(destination);
}
@@ -630,9 +623,9 @@ void GcodeSuite::G26() {
return;
}
g26_x_pos = parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position[X_AXIS];
g26_y_pos = parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position[Y_AXIS];
if (!position_is_reachable(g26_x_pos, g26_y_pos)) {
g26_pos.set(parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x,
parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position.y);
if (!position_is_reachable(g26_pos.x, g26_pos.y)) {
SERIAL_ECHOLNPGM("?Specified X,Y coordinate out of bounds.");
return;
}
@@ -642,9 +635,9 @@ void GcodeSuite::G26() {
*/
set_bed_leveling_enabled(!parser.seen('D'));
if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) {
if (current_position.z < Z_CLEARANCE_BETWEEN_PROBES) {
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
set_current_from_destination();
current_position = destination;
}
#if DISABLED(NO_VOLUMETRICS)
@@ -655,7 +648,7 @@ void GcodeSuite::G26() {
if (turn_on_heaters() != G26_OK) goto LEAVE;
current_position[E_AXIS] = 0.0;
current_position.e = 0.0;
sync_plan_position_e();
if (g26_prime_flag && prime_nozzle() != G26_OK) goto LEAVE;
@@ -670,13 +663,13 @@ void GcodeSuite::G26() {
* It's "Show Time" !!!
*/
ZERO(circle_flags);
ZERO(horizontal_mesh_line_flags);
ZERO(vertical_mesh_line_flags);
circle_flags.reset();
horizontal_mesh_line_flags.reset();
vertical_mesh_line_flags.reset();
// Move nozzle to the specified height for the first layer
set_destination_from_current();
destination[Z_AXIS] = g26_layer_height;
destination = current_position;
destination.z = g26_layer_height;
move_to(destination, 0.0);
move_to(destination, g26_ooze_amount);
@@ -706,71 +699,68 @@ void GcodeSuite::G26() {
mesh_index_pair location;
do {
location = g26_continue_with_closest
? find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS])
: find_closest_circle_to_print(g26_x_pos, g26_y_pos); // Find the closest Mesh Intersection to where we are now.
// Find the nearest confluence
location = find_closest_circle_to_print(g26_continue_with_closest ? xy_pos_t(current_position) : g26_pos);
if (location.x_index >= 0 && location.y_index >= 0) {
const float circle_x = _GET_MESH_X(location.x_index),
circle_y = _GET_MESH_Y(location.y_index);
if (location.valid()) {
const xy_pos_t circle = _GET_MESH_POS(location.pos);
// If this mesh location is outside the printable_radius, skip it.
if (!position_is_reachable(circle_x, circle_y)) continue;
if (!position_is_reachable(circle)) continue;
// Determine where to start and end the circle,
// which is always drawn counter-clockwise.
const uint8_t xi = location.x_index, yi = location.y_index;
const bool f = yi == 0, r = xi >= GRID_MAX_POINTS_X - 1, b = yi >= GRID_MAX_POINTS_Y - 1;
const xy_int8_t st = location;
const bool f = st.y == 0,
r = st.x >= GRID_MAX_POINTS_X - 1,
b = st.y >= GRID_MAX_POINTS_Y - 1;
#if ENABLED(ARC_SUPPORT)
#define ARC_LENGTH(quarters) (INTERSECTION_CIRCLE_RADIUS * M_PI * (quarters) / 2)
#define INTERSECTION_CIRCLE_DIAM ((INTERSECTION_CIRCLE_RADIUS) * 2)
float sx = circle_x + INTERSECTION_CIRCLE_RADIUS, // default to full circle
ex = circle_x + INTERSECTION_CIRCLE_RADIUS,
sy = circle_y, ey = circle_y,
arc_length = ARC_LENGTH(4);
xy_float_t e = { circle.x + INTERSECTION_CIRCLE_RADIUS, circle.y };
xyz_float_t s = e;
// Figure out where to start and end the arc - we always print counterclockwise
if (xi == 0) { // left edge
if (!f) { sx = circle_x; sy -= INTERSECTION_CIRCLE_RADIUS; }
if (!b) { ex = circle_x; ey += INTERSECTION_CIRCLE_RADIUS; }
float arc_length = ARC_LENGTH(4);
if (st.x == 0) { // left edge
if (!f) { s.x = circle.x; s.y -= INTERSECTION_CIRCLE_RADIUS; }
if (!b) { e.x = circle.x; e.y += INTERSECTION_CIRCLE_RADIUS; }
arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2);
}
else if (r) { // right edge
sx = b ? circle_x - (INTERSECTION_CIRCLE_RADIUS) : circle_x;
ex = f ? circle_x - (INTERSECTION_CIRCLE_RADIUS) : circle_x;
sy = b ? circle_y : circle_y + INTERSECTION_CIRCLE_RADIUS;
ey = f ? circle_y : circle_y - (INTERSECTION_CIRCLE_RADIUS);
if (b) s.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y);
else s.set(circle.x, circle.y + INTERSECTION_CIRCLE_RADIUS);
if (f) e.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y);
else e.set(circle.x, circle.y - (INTERSECTION_CIRCLE_RADIUS));
arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2);
}
else if (f) {
ex -= INTERSECTION_CIRCLE_DIAM;
e.x -= INTERSECTION_CIRCLE_DIAM;
arc_length = ARC_LENGTH(2);
}
else if (b) {
sx -= INTERSECTION_CIRCLE_DIAM;
s.x -= INTERSECTION_CIRCLE_DIAM;
arc_length = ARC_LENGTH(2);
}
const float arc_offset[2] = { circle_x - sx, circle_y - sy },
dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual circle
dy_s = current_position[Y_AXIS] - sy,
dist_start = HYPOT2(dx_s, dy_s),
endpoint[XYZE] = {
ex, ey,
g26_layer_height,
current_position[E_AXIS] + (arc_length * g26_e_axis_feedrate * g26_extrusion_multiplier)
};
const ab_float_t arc_offset = circle - s;
const xy_float_t dist = current_position - s; // Distance from the start of the actual circle
const float dist_start = HYPOT2(dist.x, dist.y);
const xyze_pos_t endpoint = {
e.x, e.y, g26_layer_height,
current_position.e + (arc_length * g26_e_axis_feedrate * g26_extrusion_multiplier)
};
if (dist_start > 2.0) {
retract_filament(destination);
//todo: parameterize the bump height with a define
move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + 0.500, 0.0); // Z bump to minimize scraping
move_to(sx, sy, g26_layer_height + 0.500, 0.0); // Get to the starting point with no extrusion while bumped
s.z = g26_layer_height + 0.5f;
retract_lift_move(s);
}
move_to(sx, sy, g26_layer_height, 0.0); // Get to the starting point with no extrusion / un-Z bump
s.z = g26_layer_height;
move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift
recover_filament(destination);
@@ -778,7 +768,7 @@ void GcodeSuite::G26() {
feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
plan_arc(endpoint, arc_offset, false); // Draw a counter-clockwise arc
feedrate_mm_s = old_feedrate;
set_destination_from_current();
destination = current_position;
#if HAS_LCD_MENU
if (user_canceled()) goto LEAVE; // Check if the user wants to stop the Mesh Validation
@@ -787,7 +777,7 @@ void GcodeSuite::G26() {
#else // !ARC_SUPPORT
int8_t start_ind = -2, end_ind = 9; // Assume a full circle (from 5:00 to 5:00)
if (xi == 0) { // Left edge? Just right half.
if (st.x == 0) { // Left edge? Just right half.
start_ind = f ? 0 : -3; // 03:00 to 12:00 for front-left
end_ind = b ? 0 : 2; // 06:00 to 03:00 for back-left
}
@@ -810,23 +800,21 @@ void GcodeSuite::G26() {
if (user_canceled()) goto LEAVE; // Check if the user wants to stop the Mesh Validation
#endif
float rx = circle_x + _COS(ind), // For speed, these are now a lookup table entry
ry = circle_y + _SIN(ind),
xe = circle_x + _COS(ind + 1),
ye = circle_y + _SIN(ind + 1);
xy_float_t p = { circle.x + _COS(ind ), circle.y + _SIN(ind ), g26_layer_height },
q = { circle.x + _COS(ind + 1), circle.y + _SIN(ind + 1), g26_layer_height };
#if IS_KINEMATIC
// Check to make sure this segment is entirely on the bed, skip if not.
if (!position_is_reachable(rx, ry) || !position_is_reachable(xe, ye)) continue;
#else // not, we need to skip
LIMIT(rx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
LIMIT(ry, Y_MIN_POS + 1, Y_MAX_POS - 1);
LIMIT(xe, X_MIN_POS + 1, X_MAX_POS - 1);
LIMIT(ye, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (!position_is_reachable(p) || !position_is_reachable(q)) continue;
#else
LIMIT(p.x, X_MIN_POS + 1, X_MAX_POS - 1); // Prevent hitting the endstops
LIMIT(p.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
LIMIT(q.x, X_MIN_POS + 1, X_MAX_POS - 1);
LIMIT(q.y, Y_MIN_POS + 1, Y_MAX_POS - 1);
#endif
print_line_from_here_to_there(rx, ry, g26_layer_height, xe, ye, g26_layer_height);
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
print_line_from_here_to_there(p, q);
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
}
#endif // !ARC_SUPPORT
@@ -836,19 +824,18 @@ void GcodeSuite::G26() {
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
} while (--g26_repeats && location.x_index >= 0 && location.y_index >= 0);
} while (--g26_repeats && location.valid());
LEAVE:
ui.set_status_P(PSTR(MSG_G26_LEAVING), -1);
retract_filament(destination);
destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
destination.z = Z_CLEARANCE_BETWEEN_PROBES;
move_to(destination, 0); // Raise the nozzle
destination[X_AXIS] = g26_x_pos; // Move back to the starting position
destination[Y_AXIS] = g26_y_pos;
//destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is
destination.set(g26_pos.x, g26_pos.y); // Move back to the starting position
//destination.z = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is
move_to(destination, 0); // Move back to the starting position

11
Marlin/src/gcode/bedlevel/G42.cpp
View File

@@ -27,6 +27,7 @@
#include "../gcode.h"
#include "../../Marlin.h" // for IsRunning()
#include "../../module/motion.h"
#include "../../module/probe.h" // for probe_offset
#include "../../feature/bedlevel/bedlevel.h"
/**
@@ -44,15 +45,15 @@ void GcodeSuite::G42() {
return;
}
set_destination_from_current();
destination = current_position;
if (hasI) destination[X_AXIS] = _GET_MESH_X(ix);
if (hasJ) destination[Y_AXIS] = _GET_MESH_Y(iy);
if (hasI) destination.x = _GET_MESH_X(ix);
if (hasJ) destination.y = _GET_MESH_Y(iy);
#if HAS_BED_PROBE
if (parser.boolval('P')) {
if (hasI) destination[X_AXIS] -= probe_offset[X_AXIS];
if (hasJ) destination[Y_AXIS] -= probe_offset[Y_AXIS];
if (hasI) destination.x -= probe_offset.x;
if (hasJ) destination.y -= probe_offset.y;
}
#endif

11
Marlin/src/gcode/bedlevel/M420.cpp
View File

@@ -66,10 +66,9 @@ void GcodeSuite::M420() {
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
const float x_min = probe_min_x(), x_max = probe_max_x(),
y_min = probe_min_y(), y_max = probe_max_y();
bilinear_start[X_AXIS] = x_min;
bilinear_start[Y_AXIS] = y_min;
bilinear_grid_spacing[X_AXIS] = (x_max - x_min) / (GRID_MAX_POINTS_X - 1);
bilinear_grid_spacing[Y_AXIS] = (y_max - y_min) / (GRID_MAX_POINTS_Y - 1);
bilinear_start.set(x_min, y_min);
bilinear_grid_spacing.set((x_max - x_min) / (GRID_MAX_POINTS_X - 1),
(y_max - y_min) / (GRID_MAX_POINTS_Y - 1));
#endif
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
@@ -91,7 +90,7 @@ void GcodeSuite::M420() {
// (Don't disable for just M420 or M420 V)
if (seen_S && !to_enable) set_bed_leveling_enabled(false);
const float oldpos[] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
xyz_pos_t oldpos = current_position;
#if ENABLED(AUTO_BED_LEVELING_UBL)
@@ -251,7 +250,7 @@ void GcodeSuite::M420() {
#endif
// Report change in position
if (memcmp(oldpos, current_position, sizeof(oldpos)))
if (oldpos != current_position)
report_current_position();
}

316
Marlin/src/gcode/bedlevel/abl/G29.cpp
View File

@@ -61,15 +61,15 @@
#if ABL_GRID
#if ENABLED(PROBE_Y_FIRST)
#define PR_OUTER_VAR xCount
#define PR_OUTER_END abl_grid_points_x
#define PR_INNER_VAR yCount
#define PR_INNER_END abl_grid_points_y
#define PR_OUTER_VAR meshCount.x
#define PR_OUTER_END abl_grid_points.x
#define PR_INNER_VAR meshCount.y
#define PR_INNER_END abl_grid_points.y
#else
#define PR_OUTER_VAR yCount
#define PR_OUTER_END abl_grid_points_y
#define PR_INNER_VAR xCount
#define PR_INNER_END abl_grid_points_x
#define PR_OUTER_VAR meshCount.y
#define PR_OUTER_END abl_grid_points.y
#define PR_INNER_VAR meshCount.x
#define PR_INNER_END abl_grid_points.x
#endif
#endif
@@ -210,7 +210,8 @@ G29_TYPE GcodeSuite::G29() {
#endif
ABL_VAR int verbose_level;
ABL_VAR float xProbe, yProbe, measured_z;
ABL_VAR xy_pos_t probePos;
ABL_VAR float measured_z;
ABL_VAR bool dryrun, abl_should_enable;
#if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR)
@@ -224,20 +225,17 @@ G29_TYPE GcodeSuite::G29() {
#if ABL_GRID
#if ENABLED(PROBE_MANUALLY)
ABL_VAR uint8_t PR_OUTER_VAR;
ABL_VAR int8_t PR_INNER_VAR;
ABL_VAR xy_int8_t meshCount;
#endif
ABL_VAR int left_probe_bed_position, right_probe_bed_position, front_probe_bed_position, back_probe_bed_position;
ABL_VAR float xGridSpacing = 0, yGridSpacing = 0;
ABL_VAR xy_int_t probe_position_lf, probe_position_rb;
ABL_VAR xy_float_t gridSpacing = { 0, 0 };
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
ABL_VAR uint8_t abl_grid_points_x = GRID_MAX_POINTS_X,
abl_grid_points_y = GRID_MAX_POINTS_Y;
ABL_VAR bool do_topography_map;
ABL_VAR xy_uint8_t abl_grid_points;
#else // Bilinear
uint8_t constexpr abl_grid_points_x = GRID_MAX_POINTS_X,
abl_grid_points_y = GRID_MAX_POINTS_Y;
constexpr xy_uint8_t abl_grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y };
#endif
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
@@ -269,15 +267,15 @@ G29_TYPE GcodeSuite::G29() {
const float x_min = probe_min_x(), x_max = probe_max_x(), y_min = probe_min_y(), y_max = probe_max_y();
ABL_VAR vector_3 points[3] = {
#if ENABLED(HAS_FIXED_3POINT)
vector_3(PROBE_PT_1_X, PROBE_PT_1_Y, 0),
vector_3(PROBE_PT_2_X, PROBE_PT_2_Y, 0),
vector_3(PROBE_PT_3_X, PROBE_PT_3_Y, 0)
#else
vector_3(x_min, y_min, 0),
vector_3(x_max, y_min, 0),
vector_3((x_max - x_min) / 2, y_max, 0)
#endif
#if ENABLED(HAS_FIXED_3POINT)
{ PROBE_PT_1_X, PROBE_PT_1_Y, 0 },
{ PROBE_PT_2_X, PROBE_PT_2_Y, 0 },
{ PROBE_PT_3_X, PROBE_PT_3_Y, 0 }
#else
{ x_min, y_min, 0 },
{ x_max, y_min, 0 },
{ (x_max - x_min) / 2, y_max, 0 }
#endif
};
#endif // AUTO_BED_LEVELING_3POINT
@@ -311,7 +309,7 @@ G29_TYPE GcodeSuite::G29() {
G29_RETURN(false);
}
const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position[Z_AXIS];
const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position.z;
if (!WITHIN(rz, -10, 10)) {
SERIAL_ERROR_MSG("Bad Z value");
G29_RETURN(false);
@@ -323,8 +321,8 @@ G29_TYPE GcodeSuite::G29() {
if (!isnan(rx) && !isnan(ry)) {
// Get nearest i / j from rx / ry
i = (rx - bilinear_start[X_AXIS] + 0.5 * xGridSpacing) / xGridSpacing;
j = (ry - bilinear_start[Y_AXIS] + 0.5 * yGridSpacing) / yGridSpacing;
i = (rx - bilinear_start.x + 0.5 * gridSpacing.x) / gridSpacing.x;
j = (ry - bilinear_start.y + 0.5 * gridSpacing.y) / gridSpacing.y;
LIMIT(i, 0, GRID_MAX_POINTS_X - 1);
LIMIT(j, 0, GRID_MAX_POINTS_Y - 1);
}
@@ -373,20 +371,22 @@ G29_TYPE GcodeSuite::G29() {
// X and Y specify points in each direction, overriding the default
// These values may be saved with the completed mesh
abl_grid_points_x = parser.intval('X', GRID_MAX_POINTS_X);
abl_grid_points_y = parser.intval('Y', GRID_MAX_POINTS_Y);
if (parser.seenval('P')) abl_grid_points_x = abl_grid_points_y = parser.value_int();
abl_grid_points.set(
parser.byteval('X', GRID_MAX_POINTS_X),
parser.byteval('Y', GRID_MAX_POINTS_Y)
);
if (parser.seenval('P')) abl_grid_points.x = abl_grid_points.y = parser.value_int();
if (!WITHIN(abl_grid_points_x, 2, GRID_MAX_POINTS_X)) {
if (!WITHIN(abl_grid_points.x, 2, GRID_MAX_POINTS_X)) {
SERIAL_ECHOLNPGM("?Probe points (X) implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ").");
G29_RETURN(false);
}
if (!WITHIN(abl_grid_points_y, 2, GRID_MAX_POINTS_Y)) {
if (!WITHIN(abl_grid_points.y, 2, GRID_MAX_POINTS_Y)) {
SERIAL_ECHOLNPGM("?Probe points (Y) implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ").");
G29_RETURN(false);
}
abl_points = abl_grid_points_x * abl_grid_points_y;
abl_points = abl_grid_points.x * abl_grid_points.y;
mean = 0;
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
@@ -404,27 +404,35 @@ G29_TYPE GcodeSuite::G29() {
if (parser.seen('H')) {
const int16_t size = (int16_t)parser.value_linear_units();
left_probe_bed_position = _MAX(X_CENTER - size / 2, x_min);
right_probe_bed_position = _MIN(left_probe_bed_position + size, x_max);
front_probe_bed_position = _MAX(Y_CENTER - size / 2, y_min);
back_probe_bed_position = _MIN(front_probe_bed_position + size, y_max);
probe_position_lf.set(
_MAX(X_CENTER - size / 2, x_min),
_MAX(Y_CENTER - size / 2, y_min)
);
probe_position_rb.set(
_MIN(probe_position_lf.x + size, x_max),
_MIN(probe_position_lf.y + size, y_max)
);
}
else {
left_probe_bed_position = parser.seenval('L') ? (int)RAW_X_POSITION(parser.value_linear_units()) : _MAX(X_CENTER - X_BED_SIZE / 2, x_min);
right_probe_bed_position = parser.seenval('R') ? (int)RAW_X_POSITION(parser.value_linear_units()) : _MIN(left_probe_bed_position + X_BED_SIZE, x_max);
front_probe_bed_position = parser.seenval('F') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : _MAX(Y_CENTER - Y_BED_SIZE / 2, y_min);
back_probe_bed_position = parser.seenval('B') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : _MIN(front_probe_bed_position + Y_BED_SIZE, y_max);
probe_position_lf.set(
parser.seenval('L') ? (int)RAW_X_POSITION(parser.value_linear_units()) : _MAX(X_CENTER - (X_BED_SIZE) / 2, x_min),
parser.seenval('F') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : _MAX(Y_CENTER - (Y_BED_SIZE) / 2, y_min)
);
probe_position_rb.set(
parser.seenval('R') ? (int)RAW_X_POSITION(parser.value_linear_units()) : _MIN(probe_position_lf.x + X_BED_SIZE, x_max),
parser.seenval('B') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : _MIN(probe_position_lf.y + Y_BED_SIZE, y_max)
);
}
if (
#if IS_SCARA || ENABLED(DELTA)
!position_is_reachable_by_probe(left_probe_bed_position, 0)
|| !position_is_reachable_by_probe(right_probe_bed_position, 0)
|| !position_is_reachable_by_probe(0, front_probe_bed_position)
|| !position_is_reachable_by_probe(0, back_probe_bed_position)
!position_is_reachable_by_probe(probe_position_lf.x, 0)
|| !position_is_reachable_by_probe(probe_position_rb.x, 0)
|| !position_is_reachable_by_probe(0, probe_position_lf.y)
|| !position_is_reachable_by_probe(0, probe_position_rb.y)
#else
!position_is_reachable_by_probe(left_probe_bed_position, front_probe_bed_position)
|| !position_is_reachable_by_probe(right_probe_bed_position, back_probe_bed_position)
!position_is_reachable_by_probe(probe_position_lf)
|| !position_is_reachable_by_probe(probe_position_rb)
#endif
) {
SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds.");
@@ -432,8 +440,8 @@ G29_TYPE GcodeSuite::G29() {
}
// probe at the points of a lattice grid
xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (abl_grid_points_x - 1);
yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (abl_grid_points_y - 1);
gridSpacing.set((probe_position_rb.x - probe_position_lf.x) / (abl_grid_points.x - 1),
(probe_position_rb.y - probe_position_lf.y) / (abl_grid_points.y - 1));
#endif // ABL_GRID
@@ -464,19 +472,13 @@ G29_TYPE GcodeSuite::G29() {
#if ENABLED(PROBE_MANUALLY)
if (!no_action)
#endif
if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
|| yGridSpacing != bilinear_grid_spacing[Y_AXIS]
|| left_probe_bed_position != bilinear_start[X_AXIS]
|| front_probe_bed_position != bilinear_start[Y_AXIS]
) {
if (gridSpacing != bilinear_grid_spacing || probe_position_lf != bilinear_start) {
// Reset grid to 0.0 or "not probed". (Also disables ABL)
reset_bed_level();
// Initialize a grid with the given dimensions
bilinear_grid_spacing[X_AXIS] = xGridSpacing;
bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
bilinear_start[X_AXIS] = left_probe_bed_position;
bilinear_start[Y_AXIS] = front_probe_bed_position;
bilinear_grid_spacing = gridSpacing.asInt();
bilinear_start = probe_position_lf;
// Can't re-enable (on error) until the new grid is written
abl_should_enable = false;
@@ -546,17 +548,17 @@ G29_TYPE GcodeSuite::G29() {
// For G29 after adjusting Z.
// Save the previous Z before going to the next point
measured_z = current_position[Z_AXIS];
measured_z = current_position.z;
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
mean += measured_z;
eqnBVector[index] = measured_z;
eqnAMatrix[index + 0 * abl_points] = xProbe;
eqnAMatrix[index + 1 * abl_points] = yProbe;
eqnAMatrix[index + 0 * abl_points] = probePos.x;
eqnAMatrix[index + 1 * abl_points] = probePos.y;
eqnAMatrix[index + 2 * abl_points] = 1;
incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);
incremental_LSF(&lsf_results, probePos, measured_z);
#elif ENABLED(AUTO_BED_LEVELING_3POINT)
@@ -564,12 +566,13 @@ G29_TYPE GcodeSuite::G29() {
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
z_values[xCount][yCount] = measured_z + zoffset;
const float newz = measured_z + zoffset;
z_values[meshCount.x][meshCount.y] = newz;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(xCount, yCount, z_values[xCount][yCount]);
ExtUI::onMeshUpdate(meshCount, newz);
#endif
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Save X", xCount, " Y", yCount, " Z", measured_z + zoffset);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Save X", meshCount.x, " Y", meshCount.y, " Z", measured_z + zoffset);
#endif
}
@@ -583,7 +586,7 @@ G29_TYPE GcodeSuite::G29() {
// Skip any unreachable points
while (abl_probe_index < abl_points) {
// Set xCount, yCount based on abl_probe_index, with zig-zag
// Set meshCount.x, meshCount.y based on abl_probe_index, with zig-zag
PR_OUTER_VAR = abl_probe_index / PR_INNER_END;
PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END);
@@ -592,24 +595,23 @@ G29_TYPE GcodeSuite::G29() {
if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR;
const float xBase = xCount * xGridSpacing + left_probe_bed_position,
yBase = yCount * yGridSpacing + front_probe_bed_position;
const xy_pos_t base = probe_position_lf.asFloat() + gridSpacing * meshCount.asFloat();
xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));
probePos.set(FLOOR(base.x + (base.x < 0 ? 0 : 0.5)),
FLOOR(base.y + (base.y < 0 ? 0 : 0.5)));
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
indexIntoAB[xCount][yCount] = abl_probe_index;
indexIntoAB[meshCount.x][meshCount.y] = abl_probe_index;
#endif
// Keep looping till a reachable point is found
if (position_is_reachable(xProbe, yProbe)) break;
if (position_is_reachable(probePos)) break;
++abl_probe_index;
}
// Is there a next point to move to?
if (abl_probe_index < abl_points) {
_manual_goto_xy(xProbe, yProbe); // Can be used here too!
_manual_goto_xy(probePos); // Can be used here too!
#if HAS_SOFTWARE_ENDSTOPS
// Disable software endstops to allow manual adjustment
// If G29 is not completed, they will not be re-enabled
@@ -633,9 +635,8 @@ G29_TYPE GcodeSuite::G29() {
// Probe at 3 arbitrary points
if (abl_probe_index < abl_points) {
xProbe = points[abl_probe_index].x;
yProbe = points[abl_probe_index].y;
_manual_goto_xy(xProbe, yProbe);
probePos = points[abl_probe_index];
_manual_goto_xy(probePos);
#if HAS_SOFTWARE_ENDSTOPS
// Disable software endstops to allow manual adjustment
// If G29 is not completed, they will not be re-enabled
@@ -654,11 +655,7 @@ G29_TYPE GcodeSuite::G29() {
if (!dryrun) {
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
if (planeNormal.z < 0) {
planeNormal.x *= -1;
planeNormal.y *= -1;
planeNormal.z *= -1;
}
if (planeNormal.z < 0) planeNormal *= -1;
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
// Can't re-enable (on error) until the new grid is written
@@ -681,8 +678,11 @@ G29_TYPE GcodeSuite::G29() {
measured_z = 0;
xy_int8_t meshCount;
// Outer loop is X with PROBE_Y_FIRST enabled
// Outer loop is Y with PROBE_Y_FIRST disabled
for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
int8_t inStart, inStop, inInc;
@@ -703,21 +703,21 @@ G29_TYPE GcodeSuite::G29() {
uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_END) + 1;
// Inner loop is Y with PROBE_Y_FIRST enabled
for (int8_t PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) {
// Inner loop is X with PROBE_Y_FIRST disabled
for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) {
const float xBase = left_probe_bed_position + xGridSpacing * xCount,
yBase = front_probe_bed_position + yGridSpacing * yCount;
const xy_pos_t base = probe_position_lf.asFloat() + gridSpacing * meshCount.asFloat();
xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));
probePos.set(FLOOR(base.x + (base.x < 0 ? 0 : 0.5)),
FLOOR(base.y + (base.y < 0 ? 0 : 0.5)));
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
indexIntoAB[xCount][yCount] = ++abl_probe_index; // 0...
indexIntoAB[meshCount.x][meshCount.y] = ++abl_probe_index; // 0...
#endif
#if IS_KINEMATIC
// Avoid probing outside the round or hexagonal area
if (!position_is_reachable_by_probe(xProbe, yProbe)) continue;
if (!position_is_reachable_by_probe(probePos)) continue;
#endif
if (verbose_level) SERIAL_ECHOLNPAIR("Probing mesh point ", int(pt_index), "/", int(GRID_MAX_POINTS), ".");
@@ -725,7 +725,7 @@ G29_TYPE GcodeSuite::G29() {
ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), PSTR(MSG_PROBING_MESH), int(pt_index), int(GRID_MAX_POINTS));
#endif
measured_z = faux ? 0.001 * random(-100, 101) : probe_at_point(xProbe, yProbe, raise_after, verbose_level);
measured_z = faux ? 0.001 * random(-100, 101) : probe_at_point(probePos, raise_after, verbose_level);
if (isnan(measured_z)) {
set_bed_leveling_enabled(abl_should_enable);
@@ -736,17 +736,17 @@ G29_TYPE GcodeSuite::G29() {
mean += measured_z;
eqnBVector[abl_probe_index] = measured_z;
eqnAMatrix[abl_probe_index + 0 * abl_points] = xProbe;
eqnAMatrix[abl_probe_index + 1 * abl_points] = yProbe;
eqnAMatrix[abl_probe_index + 0 * abl_points] = probePos.x;
eqnAMatrix[abl_probe_index + 1 * abl_points] = probePos.y;
eqnAMatrix[abl_probe_index + 2 * abl_points] = 1;
incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);
incremental_LSF(&lsf_results, probePos, measured_z);
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
z_values[xCount][yCount] = measured_z + zoffset;
z_values[meshCount.x][meshCount.y] = measured_z + zoffset;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(xCount, yCount, z_values[xCount][yCount]);
ExtUI::onMeshUpdate(meshCount.x, meshCount.y, z_values[meshCount.x][meshCount.y]);
#endif
#endif
@@ -768,9 +768,8 @@ G29_TYPE GcodeSuite::G29() {
#endif
// Retain the last probe position
xProbe = points[i].x;
yProbe = points[i].y;
measured_z = faux ? 0.001 * random(-100, 101) : probe_at_point(xProbe, yProbe, raise_after, verbose_level);
probePos = points[i];
measured_z = faux ? 0.001 * random(-100, 101) : probe_at_point(probePos, raise_after, verbose_level);
if (isnan(measured_z)) {
set_bed_leveling_enabled(abl_should_enable);
break;
@@ -845,19 +844,19 @@ G29_TYPE GcodeSuite::G29() {
* plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
* so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
*/
float plane_equation_coefficients[3];
struct { float a, b, d; } plane_equation_coefficients;
finish_incremental_LSF(&lsf_results);
plane_equation_coefficients[0] = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below
plane_equation_coefficients[1] = -lsf_results.B; // but that is not yet tested.
plane_equation_coefficients[2] = -lsf_results.D;
plane_equation_coefficients.a = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below
plane_equation_coefficients.b = -lsf_results.B; // but that is not yet tested.
plane_equation_coefficients.d = -lsf_results.D;
mean /= abl_points;
if (verbose_level) {
SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients[0], 8);
SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients[1], 8);
SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients[2], 8);
SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients.a, 8);
SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients.b, 8);
SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients.d, 8);
if (verbose_level > 2)
SERIAL_ECHOPAIR_F("\nMean of sampled points: ", mean, 8);
SERIAL_EOL();
@@ -866,13 +865,34 @@ G29_TYPE GcodeSuite::G29() {
// Create the matrix but don't correct the position yet
if (!dryrun)
planner.bed_level_matrix = matrix_3x3::create_look_at(
vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) // We can eliminate the '-' here and up above
vector_3(-plane_equation_coefficients.a, -plane_equation_coefficients.b, 1) // We can eliminate the '-' here and up above
);
// Show the Topography map if enabled
if (do_topography_map) {
SERIAL_ECHOLNPGM("\nBed Height Topography:\n"
float min_diff = 999;
auto print_topo_map = [&](PGM_P const title, const bool get_min) {
serialprintPGM(title);
for (int8_t yy = abl_grid_points.y - 1; yy >= 0; yy--) {
for (uint8_t xx = 0; xx < abl_grid_points.x; xx++) {
const int ind = indexIntoAB[xx][yy];
xyz_float_t tmp = { eqnAMatrix[ind + 0 * abl_points],
eqnAMatrix[ind + 1 * abl_points], 0 };
apply_rotation_xyz(planner.bed_level_matrix, tmp);
if (get_min) NOMORE(min_diff, eqnBVector[ind] - tmp.z);
const float subval = get_min ? mean : tmp.z + min_diff,
diff = eqnBVector[ind] - subval;
SERIAL_CHAR(' '); if (diff >= 0.0) SERIAL_CHAR('+'); // Include + for column alignment
SERIAL_ECHO_F(diff, 5);
} // xx
SERIAL_EOL();
} // yy
SERIAL_EOL();
};
print_topo_map(PSTR("\nBed Height Topography:\n"
" +--- BACK --+\n"
" | |\n"
" L | (+) | R\n"
@@ -882,56 +902,10 @@ G29_TYPE GcodeSuite::G29() {
" | (-) | T\n"
" | |\n"
" O-- FRONT --+\n"
" (0,0)");
" (0,0)\n"), true);
if (verbose_level > 3)
print_topo_map(PSTR("\nCorrected Bed Height vs. Bed Topology:\n"), false);
float min_diff = 999;
for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
int ind = indexIntoAB[xx][yy];
float diff = eqnBVector[ind] - mean,
x_tmp = eqnAMatrix[ind + 0 * abl_points],
y_tmp = eqnAMatrix[ind + 1 * abl_points],
z_tmp = 0;
apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
NOMORE(min_diff, eqnBVector[ind] - z_tmp);
if (diff >= 0.0)
SERIAL_ECHOPGM(" +"); // Include + for column alignment
else
SERIAL_CHAR(' ');
SERIAL_ECHO_F(diff, 5);
} // xx
SERIAL_EOL();
} // yy
SERIAL_EOL();
if (verbose_level > 3) {
SERIAL_ECHOLNPGM("\nCorrected Bed Height vs. Bed Topology:");
for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
int ind = indexIntoAB[xx][yy];
float x_tmp = eqnAMatrix[ind + 0 * abl_points],
y_tmp = eqnAMatrix[ind + 1 * abl_points],
z_tmp = 0;
apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
float diff = eqnBVector[ind] - z_tmp - min_diff;
if (diff >= 0.0)
SERIAL_ECHOPGM(" +");
// Include + for column alignment
else
SERIAL_CHAR(' ');
SERIAL_ECHO_F(diff, 5);
} // xx
SERIAL_EOL();
} // yy
SERIAL_EOL();
}
} //do_topography_map
#endif // AUTO_BED_LEVELING_LINEAR
@@ -950,24 +924,20 @@ G29_TYPE GcodeSuite::G29() {
if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
float converted[XYZ];
COPY(converted, current_position);
planner.leveling_active = true;
planner.unapply_leveling(converted); // use conversion machinery
planner.leveling_active = false;
xyze_pos_t converted = current_position;
planner.force_unapply_leveling(converted); // use conversion machinery
// Use the last measured distance to the bed, if possible
if ( NEAR(current_position[X_AXIS], xProbe - probe_offset[X_AXIS])
&& NEAR(current_position[Y_AXIS], yProbe - probe_offset[Y_AXIS])
if ( NEAR(current_position.x, probePos.x - probe_offset.x)
&& NEAR(current_position.y, probePos.y - probe_offset.y)
) {
const float simple_z = current_position[Z_AXIS] - measured_z;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probed Z", simple_z, " Matrix Z", converted[Z_AXIS], " Discrepancy ", simple_z - converted[Z_AXIS]);
converted[Z_AXIS] = simple_z;
const float simple_z = current_position.z - measured_z;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probed Z", simple_z, " Matrix Z", converted.z, " Discrepancy ", simple_z - converted.z);
converted.z = simple_z;
}
// The rotated XY and corrected Z are now current_position
COPY(current_position, converted);
current_position = converted;
if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
}
@@ -975,13 +945,13 @@ G29_TYPE GcodeSuite::G29() {
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
if (!dryrun) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("G29 uncorrected Z:", current_position[Z_AXIS]);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("G29 uncorrected Z:", current_position.z);
// Unapply the offset because it is going to be immediately applied
// and cause compensation movement in Z
current_position[Z_AXIS] -= bilinear_z_offset(current_position);
current_position.z -= bilinear_z_offset(current_position);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(" corrected Z:", current_position[Z_AXIS]);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(" corrected Z:", current_position.z);
}
#endif // ABL_PLANAR

8
Marlin/src/gcode/bedlevel/mbl/G29.cpp
View File

@@ -110,7 +110,7 @@ void GcodeSuite::G29() {
}
else {
// Save Z for the previous mesh position
mbl.set_zigzag_z(mbl_probe_index - 1, current_position[Z_AXIS]);
mbl.set_zigzag_z(mbl_probe_index - 1, current_position.z);
#if HAS_SOFTWARE_ENDSTOPS
soft_endstops_enabled = saved_soft_endstops_state;
#endif
@@ -124,11 +124,11 @@ void GcodeSuite::G29() {
#endif
mbl.zigzag(mbl_probe_index++, ix, iy);
_manual_goto_xy(mbl.index_to_xpos[ix], mbl.index_to_ypos[iy]);
_manual_goto_xy({ mbl.index_to_xpos[ix], mbl.index_to_ypos[iy] });
}
else {
// One last "return to the bed" (as originally coded) at completion
current_position[Z_AXIS] = MANUAL_PROBE_HEIGHT;
current_position.z = MANUAL_PROBE_HEIGHT;
line_to_current_position();
planner.synchronize();
@@ -142,7 +142,7 @@ void GcodeSuite::G29() {
set_bed_leveling_enabled(true);
#if ENABLED(MESH_G28_REST_ORIGIN)
current_position[Z_AXIS] = 0;
current_position.z = 0;
line_to_current_position(homing_feedrate(Z_AXIS));
planner.synchronize();
#endif

19
Marlin/src/gcode/bedlevel/ubl/M421.cpp
View File

@@ -46,28 +46,25 @@
* M421 C Q<offset>
*/
void GcodeSuite::M421() {
int8_t ix = parser.intval('I', -1), iy = parser.intval('J', -1);
const bool hasI = ix >= 0,
hasJ = iy >= 0,
xy_int8_t ij = { int8_t(parser.intval('I', -1)), int8_t(parser.intval('J', -1)) };
const bool hasI = ij.x >= 0,
hasJ = ij.y >= 0,
hasC = parser.seen('C'),
hasN = parser.seen('N'),
hasZ = parser.seen('Z'),
hasQ = !hasZ && parser.seen('Q');
if (hasC) {
const mesh_index_pair location = ubl.find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, nullptr);
ix = location.x_index;
iy = location.y_index;
}
if (hasC) ij = ubl.find_closest_mesh_point_of_type(REAL, current_position);
if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ || hasN))
SERIAL_ERROR_MSG(MSG_ERR_M421_PARAMETERS);
else if (!WITHIN(ix, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1))
else if (!WITHIN(ij.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(ij.y, 0, GRID_MAX_POINTS_Y - 1))
SERIAL_ERROR_MSG(MSG_ERR_MESH_XY);
else {
ubl.z_values[ix][iy] = hasN ? NAN : parser.value_linear_units() + (hasQ ? ubl.z_values[ix][iy] : 0);
float &zval = ubl.z_values[ij.x][ij.y];
zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0);
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(ix, iy, ubl.z_values[ix][iy]);
ExtUI::onMeshUpdate(ij.x, ij.y, zval);
#endif
}
}