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
}
}

50
Marlin/src/gcode/calibrate/G28.cpp
View File

@ -59,7 +59,7 @@
static void quick_home_xy() {
// Pretend the current position is 0,0
current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
current_position.set(0.0, 0.0);
sync_plan_position();
const int x_axis_home_dir =
@ -95,7 +95,7 @@
endstops.validate_homing_move();
current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
current_position.set(0.0, 0.0);
#if ENABLED(SENSORLESS_HOMING)
tmc_disable_stallguard(stepperX, stealth_states.x);
@ -128,17 +128,15 @@
/**
* Move the Z probe (or just the nozzle) to the safe homing point
* (Z is already at the right height)
*/
destination[X_AXIS] = Z_SAFE_HOMING_X_POINT;
destination[Y_AXIS] = Z_SAFE_HOMING_Y_POINT;
destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
destination.set(safe_homing_xy, current_position.z);
#if HOMING_Z_WITH_PROBE
destination[X_AXIS] -= probe_offset[X_AXIS];
destination[Y_AXIS] -= probe_offset[Y_AXIS];
destination -= probe_offset;
#endif
if (position_is_reachable(destination[X_AXIS], destination[Y_AXIS])) {
if (position_is_reachable(destination)) {
if (DEBUGGING(LEVELING)) DEBUG_POS("home_z_safely", destination);
@ -151,7 +149,7 @@
safe_delay(500); // Short delay needed to settle
#endif
do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
do_blocking_move_to_xy(destination);
homeaxis(Z_AXIS);
}
else {
@ -232,16 +230,14 @@ void GcodeSuite::G28(const bool always_home_all) {
#endif
#if ENABLED(IMPROVE_HOMING_RELIABILITY)
slow_homing_t slow_homing { 0 };
slow_homing.acceleration.x = planner.settings.max_acceleration_mm_per_s2[X_AXIS];
slow_homing.acceleration.y = planner.settings.max_acceleration_mm_per_s2[Y_AXIS];
slow_homing_t slow_homing{0};
slow_homing.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS];
planner.settings.max_acceleration_mm_per_s2[Y_AXIS]);
planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
#if HAS_CLASSIC_JERK
slow_homing.jerk.x = planner.max_jerk[X_AXIS];
slow_homing.jerk.y = planner.max_jerk[Y_AXIS];
planner.max_jerk[X_AXIS] = 0;
planner.max_jerk[Y_AXIS] = 0;
slow_homing.jerk_xy = planner.max_jerk;
planner.max_jerk.set(0, 0);
#endif
planner.reset_acceleration_rates();
@ -274,7 +270,7 @@ void GcodeSuite::G28(const bool always_home_all) {
home_all = always_home_all || (homeX == homeY && homeX == homeZ),
doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
set_destination_from_current();
destination = current_position;
#if Z_HOME_DIR > 0 // If homing away from BED do Z first
@ -291,10 +287,10 @@ void GcodeSuite::G28(const bool always_home_all) {
if (z_homing_height && (doX || doY)) {
// Raise Z before homing any other axes and z is not already high enough (never lower z)
destination[Z_AXIS] = z_homing_height;
if (destination[Z_AXIS] > current_position[Z_AXIS]) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) to ", destination[Z_AXIS]);
do_blocking_move_to_z(destination[Z_AXIS]);
destination.z = z_homing_height;
if (destination.z > current_position.z) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) to ", destination.z);
do_blocking_move_to_z(destination.z);
}
}
@ -329,14 +325,14 @@ void GcodeSuite::G28(const bool always_home_all) {
homeaxis(X_AXIS);
// Remember this extruder's position for later tool change
inactive_extruder_x_pos = current_position[X_AXIS];
inactive_extruder_x_pos = current_position.x;
// Home the 1st (left) extruder
active_extruder = 0;
homeaxis(X_AXIS);
// Consider the active extruder to be parked
COPY(raised_parked_position, current_position);
raised_parked_position = current_position;
delayed_move_time = 0;
active_extruder_parked = true;
@ -390,14 +386,14 @@ void GcodeSuite::G28(const bool always_home_all) {
homeaxis(X_AXIS);
// Remember this extruder's position for later tool change
inactive_extruder_x_pos = current_position[X_AXIS];
inactive_extruder_x_pos = current_position.x;
// Home the 1st (left) extruder
active_extruder = 0;
homeaxis(X_AXIS);
// Consider the active extruder to be parked
COPY(raised_parked_position, current_position);
raised_parked_position = current_position;
delayed_move_time = 0;
active_extruder_parked = true;
extruder_duplication_enabled = IDEX_saved_duplication_state;
@ -441,10 +437,8 @@ void GcodeSuite::G28(const bool always_home_all) {
planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x;
planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y;
#if HAS_CLASSIC_JERK
planner.max_jerk[X_AXIS] = slow_homing.jerk.x;
planner.max_jerk[Y_AXIS] = slow_homing.jerk.y;
planner.max_jerk = slow_homing.jerk_xy;
#endif
planner.reset_acceleration_rates();
#endif

162
Marlin/src/gcode/calibrate/G33.cpp
View File

@ -70,7 +70,7 @@ enum CalEnum : char { // the 7 main calibration points -
#define AC_CLEANUP() ac_cleanup()
#endif
float lcd_probe_pt(const float &rx, const float &ry);
float lcd_probe_pt(const xy_pos_t &xy);
void ac_home() {
endstops.enable(true);
@ -122,9 +122,9 @@ void print_signed_float(PGM_P const prefix, const float &f) {
static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
SERIAL_ECHOPAIR(".Height:", delta_height);
if (end_stops) {
print_signed_float(PSTR("Ex"), delta_endstop_adj[A_AXIS]);
print_signed_float(PSTR("Ey"), delta_endstop_adj[B_AXIS]);
print_signed_float(PSTR("Ez"), delta_endstop_adj[C_AXIS]);
print_signed_float(PSTR("Ex"), delta_endstop_adj.a);
print_signed_float(PSTR("Ey"), delta_endstop_adj.b);
print_signed_float(PSTR("Ez"), delta_endstop_adj.c);
}
if (end_stops && tower_angles) {
SERIAL_ECHOPAIR(" Radius:", delta_radius);
@ -133,9 +133,9 @@ static void print_calibration_settings(const bool end_stops, const bool tower_an
SERIAL_ECHO_SP(13);
}
if (tower_angles) {
print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]);
print_signed_float(PSTR("Ty"), delta_tower_angle_trim[B_AXIS]);
print_signed_float(PSTR("Tz"), delta_tower_angle_trim[C_AXIS]);
print_signed_float(PSTR("Tx"), delta_tower_angle_trim.a);
print_signed_float(PSTR("Ty"), delta_tower_angle_trim.b);
print_signed_float(PSTR("Tz"), delta_tower_angle_trim.c);
}
if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
SERIAL_ECHOPAIR(" Radius:", delta_radius);
@ -188,12 +188,12 @@ static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool
/**
* - Probe a point
*/
static float calibration_probe(const float &nx, const float &ny, const bool stow) {
static float calibration_probe(const xy_pos_t &xy, const bool stow) {
#if HAS_BED_PROBE
return probe_at_point(nx, ny, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
return probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
#else
UNUSED(stow);
return lcd_probe_pt(nx, ny);
return lcd_probe_pt(xy);
#endif
}
@ -223,7 +223,8 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
if (!_0p_calibration) {
if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
z_pt[CEN] += calibration_probe(0, 0, stow_after_each);
const xy_pos_t center{0};
z_pt[CEN] += calibration_probe(center, stow_after_each);
if (isnan(z_pt[CEN])) return false;
}
@ -233,7 +234,8 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
I_LOOP_CAL_PT(rad, start, steps) {
const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
r = delta_calibration_radius * 0.1;
z_pt[CEN] += calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
const xy_pos_t vec = { cos(a), sin(a) };
z_pt[CEN] += calibration_probe(vec * r, stow_after_each);
if (isnan(z_pt[CEN])) return false;
}
z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
@ -257,7 +259,8 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
r = delta_calibration_radius * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
interpol = FMOD(rad, 1);
const float z_temp = calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
const xy_pos_t vec = { cos(a), sin(a) };
const float z_temp = calibration_probe(vec * r, stow_after_each);
if (isnan(z_temp)) return false;
// split probe point to neighbouring calibration points
z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
@ -281,80 +284,69 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi
* - formulae for approximative forward kinematics in the end-stop displacement matrix
* - definition of the matrix scaling parameters
*/
static void reverse_kinematics_probe_points(float z_pt[NPP + 1], float mm_at_pt_axis[NPP + 1][ABC]) {
float pos[XYZ] = { 0.0 };
static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) {
xyz_pos_t pos{0};
LOOP_CAL_ALL(rad) {
const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
r = (rad == CEN ? 0.0f : delta_calibration_radius);
pos[X_AXIS] = cos(a) * r;
pos[Y_AXIS] = sin(a) * r;
pos[Z_AXIS] = z_pt[rad];
pos.set(cos(a) * r, sin(a) * r, z_pt[rad]);
inverse_kinematics(pos);
LOOP_XYZ(axis) mm_at_pt_axis[rad][axis] = delta[axis];
mm_at_pt_axis[rad] = delta;
}
}
static void forward_kinematics_probe_points(float mm_at_pt_axis[NPP + 1][ABC], float z_pt[NPP + 1]) {
static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) {
const float r_quot = delta_calibration_radius / delta_radius;
#define ZPP(N,I,A) ((1 / 3.0f + r_quot * (N) / 3.0f ) * mm_at_pt_axis[I][A])
#define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A)
#define Z00(I, A) ZPP( 0, I, A)
#define Zp1(I, A) ZPP(+1, I, A)
#define Zm1(I, A) ZPP(-1, I, A)
#define Zp2(I, A) ZPP(+2, I, A)
#define Zm2(I, A) ZPP(-2, I, A)
z_pt[CEN] = Z00(CEN, A_AXIS) + Z00(CEN, B_AXIS) + Z00(CEN, C_AXIS);
z_pt[__A] = Zp2(__A, A_AXIS) + Zm1(__A, B_AXIS) + Zm1(__A, C_AXIS);
z_pt[__B] = Zm1(__B, A_AXIS) + Zp2(__B, B_AXIS) + Zm1(__B, C_AXIS);
z_pt[__C] = Zm1(__C, A_AXIS) + Zm1(__C, B_AXIS) + Zp2(__C, C_AXIS);
z_pt[_BC] = Zm2(_BC, A_AXIS) + Zp1(_BC, B_AXIS) + Zp1(_BC, C_AXIS);
z_pt[_CA] = Zp1(_CA, A_AXIS) + Zm2(_CA, B_AXIS) + Zp1(_CA, C_AXIS);
z_pt[_AB] = Zp1(_AB, A_AXIS) + Zp1(_AB, B_AXIS) + Zm2(_AB, C_AXIS);
z_pt[CEN] = Z00(CEN, a) + Z00(CEN, b) + Z00(CEN, c);
z_pt[__A] = Zp2(__A, a) + Zm1(__A, b) + Zm1(__A, c);
z_pt[__B] = Zm1(__B, a) + Zp2(__B, b) + Zm1(__B, c);
z_pt[__C] = Zm1(__C, a) + Zm1(__C, b) + Zp2(__C, c);
z_pt[_BC] = Zm2(_BC, a) + Zp1(_BC, b) + Zp1(_BC, c);
z_pt[_CA] = Zp1(_CA, a) + Zm2(_CA, b) + Zp1(_CA, c);
z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c);
}
static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], float delta_e[ABC], float delta_r, float delta_t[ABC]) {
static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
const float z_center = z_pt[CEN];
float diff_mm_at_pt_axis[NPP + 1][ABC],
new_mm_at_pt_axis[NPP + 1][ABC];
abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1];
reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
delta_radius += delta_r;
LOOP_XYZ(axis) delta_tower_angle_trim[axis] += delta_t[axis];
delta_tower_angle_trim += delta_t;
recalc_delta_settings();
reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
LOOP_XYZ(axis) LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad][axis] -= new_mm_at_pt_axis[rad][axis] + delta_e[axis];
LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e;
forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
z_pt[CEN] = z_center;
delta_radius -= delta_r;
LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= delta_t[axis];
delta_tower_angle_trim -= delta_t;
recalc_delta_settings();
}
static float auto_tune_h() {
const float r_quot = delta_calibration_radius / delta_radius;
float h_fac = 0.0f;
h_fac = r_quot / (2.0f / 3.0f);
h_fac = 1.0f / h_fac; // (2/3)/CR
return h_fac;
return RECIPROCAL(r_quot / (2.0f / 3.0f)); // (2/3)/CR
}
static float auto_tune_r() {
const float diff = 0.01f;
float r_fac = 0.0f,
z_pt[NPP + 1] = { 0.0f },
delta_e[ABC] = { 0.0f },
delta_r = { 0.0f },
delta_t[ABC] = { 0.0f };
constexpr float diff = 0.01f, delta_r = diff;
float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
delta_r = diff;
calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f;
r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
@ -362,14 +354,11 @@ static float auto_tune_r() {
}
static float auto_tune_a() {
const float diff = 0.01f;
float a_fac = 0.0f,
z_pt[NPP + 1] = { 0.0f },
delta_e[ABC] = { 0.0f },
delta_r = { 0.0f },
delta_t[ABC] = { 0.0f };
constexpr float diff = 0.01f, delta_r = 0.0f;
float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
ZERO(delta_t);
delta_t.reset();
LOOP_XYZ(axis) {
delta_t[axis] = diff;
calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
@ -453,21 +442,11 @@ void GcodeSuite::G33() {
zero_std_dev = (verbose_level ? 999.0f : 0.0f), // 0.0 in dry-run mode : forced end
zero_std_dev_min = zero_std_dev,
zero_std_dev_old = zero_std_dev,
h_factor,
r_factor,
a_factor,
e_old[ABC] = {
delta_endstop_adj[A_AXIS],
delta_endstop_adj[B_AXIS],
delta_endstop_adj[C_AXIS]
},
h_factor, r_factor, a_factor,
r_old = delta_radius,
h_old = delta_height,
a_old[ABC] = {
delta_tower_angle_trim[A_AXIS],
delta_tower_angle_trim[B_AXIS],
delta_tower_angle_trim[C_AXIS]
};
h_old = delta_height;
abc_pos_t e_old = delta_endstop_adj, a_old = delta_tower_angle_trim;
SERIAL_ECHOLNPGM("G33 Auto Calibrate");
@ -520,15 +499,14 @@ void GcodeSuite::G33() {
if (zero_std_dev < zero_std_dev_min) {
// set roll-back point
COPY(e_old, delta_endstop_adj);
e_old = delta_endstop_adj;
r_old = delta_radius;
h_old = delta_height;
COPY(a_old, delta_tower_angle_trim);
a_old = delta_tower_angle_trim;
}
float e_delta[ABC] = { 0.0f },
r_delta = 0.0f,
t_delta[ABC] = { 0.0f };
abc_float_t e_delta = { 0.0f }, t_delta = { 0.0f };
float r_delta = 0.0f;
/**
* convergence matrices:
@ -563,42 +541,42 @@ void GcodeSuite::G33() {
case 2:
if (towers_set) { // see 4 point calibration (towers) matrix
e_delta[A_AXIS] = (+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN);
e_delta[B_AXIS] = (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN);
e_delta[C_AXIS] = (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN);
r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
e_delta.set((+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN),
(-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN),
(-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN));
r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
}
else { // see 4 point calibration (opposites) matrix
e_delta[A_AXIS] = (-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
e_delta[B_AXIS] = (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
e_delta[C_AXIS] = (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN);
r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
e_delta.set((-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN),
(+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN),
(+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN));
r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
}
break;
default: // see 7 point calibration (towers & opposites) matrix
e_delta[A_AXIS] = (+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
e_delta[B_AXIS] = (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
e_delta[C_AXIS] = (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN);
r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
e_delta.set((+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN),
(-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN),
(-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN));
r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
t_delta[A_AXIS] = (+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor;
t_delta[B_AXIS] = (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor;
t_delta[C_AXIS] = (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor;
t_delta.set((+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor,
(+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor,
(-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor);
}
break;
}
LOOP_XYZ(axis) delta_endstop_adj[axis] += e_delta[axis];
delta_endstop_adj += e_delta;
delta_radius += r_delta;
LOOP_XYZ(axis) delta_tower_angle_trim[axis] += t_delta[axis];
delta_tower_angle_trim += t_delta;
}
else if (zero_std_dev >= test_precision) {
// roll back
COPY(delta_endstop_adj, e_old);
delta_endstop_adj = e_old;
delta_radius = r_old;
delta_height = h_old;
COPY(delta_tower_angle_trim, a_old);
delta_tower_angle_trim = a_old;
}
if (verbose_level != 0) { // !dry run
@ -611,7 +589,7 @@ void GcodeSuite::G33() {
}
// adjust delta_height and endstops by the max amount
const float z_temp = _MAX(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c);
delta_height -= z_temp;
LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
}

41
Marlin/src/gcode/calibrate/G34_M422.cpp
View File

@ -45,8 +45,17 @@
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../../core/debug_out.h"
float z_auto_align_xpos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_X,
z_auto_align_ypos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_Y;
// Sanity-check
constexpr xy_pos_t sanity_arr_z_align[] = Z_STEPPER_ALIGN_XY;
static_assert(COUNT(sanity_arr_z_align) == Z_STEPPER_COUNT,
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
"Z_STEPPER_ALIGN_XY requires three {X,Y} entries (Z, Z2, and Z3)."
#else
"Z_STEPPER_ALIGN_XY requires two {X,Y} entries (Z and Z2)."
#endif
);
xy_pos_t z_auto_align_pos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_XY;
inline void set_all_z_lock(const bool lock) {
stepper.set_z_lock(lock);
@ -123,11 +132,11 @@ void GcodeSuite::G34() {
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * (
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
SQRT(_MAX(HYPOT2(z_auto_align_xpos[0] - z_auto_align_ypos[0], z_auto_align_xpos[1] - z_auto_align_ypos[1]),
HYPOT2(z_auto_align_xpos[1] - z_auto_align_ypos[1], z_auto_align_xpos[2] - z_auto_align_ypos[2]),
HYPOT2(z_auto_align_xpos[2] - z_auto_align_ypos[2], z_auto_align_xpos[0] - z_auto_align_ypos[0])))
SQRT(_MAX(HYPOT2(z_auto_align_pos[0].x - z_auto_align_pos[0].y, z_auto_align_pos[1].x - z_auto_align_pos[1].y),
HYPOT2(z_auto_align_pos[1].x - z_auto_align_pos[1].y, z_auto_align_pos[2].x - z_auto_align_pos[2].y),
HYPOT2(z_auto_align_pos[2].x - z_auto_align_pos[2].y, z_auto_align_pos[0].x - z_auto_align_pos[0].y)))
#else
HYPOT(z_auto_align_xpos[0] - z_auto_align_ypos[0], z_auto_align_xpos[1] - z_auto_align_ypos[1])
HYPOT(z_auto_align_pos[0].x - z_auto_align_pos[0].y, z_auto_align_pos[1].x - z_auto_align_pos[1].y)
#endif
);
@ -135,7 +144,7 @@ void GcodeSuite::G34() {
if (!all_axes_known()) home_all_axes();
// Move the Z coordinate realm towards the positive - dirty trick
current_position[Z_AXIS] -= z_probe * 0.5;
current_position.z -= z_probe * 0.5f;
float last_z_align_move[Z_STEPPER_COUNT] = ARRAY_N(Z_STEPPER_COUNT, 10000.0f, 10000.0f, 10000.0f),
z_measured[Z_STEPPER_COUNT] = { 0 },
@ -162,7 +171,7 @@ void GcodeSuite::G34() {
if (iteration == 0 || izstepper > 0) do_blocking_move_to_z(z_probe);
// Probe a Z height for each stepper.
const float z_probed_height = probe_at_point(z_auto_align_xpos[zstepper], z_auto_align_ypos[zstepper], raise_after, 0, true);
const float z_probed_height = probe_at_point(z_auto_align_pos[zstepper], raise_after, 0, true);
if (isnan(z_probed_height)) {
SERIAL_ECHOLNPGM("Probing failed.");
err_break = true;
@ -240,7 +249,7 @@ void GcodeSuite::G34() {
}
// Do a move to correct part of the misalignment for the current stepper
do_blocking_move_to_z(amplification * z_align_move + current_position[Z_AXIS]);
do_blocking_move_to_z(amplification * z_align_move + current_position.z);
} // for (zstepper)
// Back to normal stepper operations
@ -299,20 +308,22 @@ void GcodeSuite::M422() {
return;
}
const float x_pos = parser.floatval('X', z_auto_align_xpos[zstepper]);
if (!WITHIN(x_pos, X_MIN_POS, X_MAX_POS)) {
const xy_pos_t pos = {
parser.floatval('X', z_auto_align_pos[zstepper].x),
parser.floatval('Y', z_auto_align_pos[zstepper].y)
};
if (!WITHIN(pos.x, X_MIN_POS, X_MAX_POS)) {
SERIAL_ECHOLNPGM("?(X) out of bounds.");
return;
}
const float y_pos = parser.floatval('Y', z_auto_align_ypos[zstepper]);
if (!WITHIN(y_pos, Y_MIN_POS, Y_MAX_POS)) {
if (!WITHIN(pos.y, Y_MIN_POS, Y_MAX_POS)) {
SERIAL_ECHOLNPGM("?(Y) out of bounds.");
return;
}
z_auto_align_xpos[zstepper] = x_pos;
z_auto_align_ypos[zstepper] = y_pos;
z_auto_align_pos[zstepper] = pos;
}
#endif // Z_STEPPER_AUTO_ALIGN

154
Marlin/src/gcode/calibrate/G425.cpp
View File

@ -61,17 +61,17 @@
enum side_t : uint8_t { TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES };
static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER;
static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS;
static constexpr xy_float_t nod = { CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER };
struct measurements_t {
static constexpr float dimensions[XYZ] = CALIBRATION_OBJECT_DIMENSIONS;
static constexpr float true_center[XYZ] = CALIBRATION_OBJECT_CENTER;
xyz_pos_t obj_center = true_center; // Non-static must be assigned from xyz_pos_t
float obj_center[XYZ] = CALIBRATION_OBJECT_CENTER;
float obj_side[NUM_SIDES];
float obj_side[NUM_SIDES], backlash[NUM_SIDES];
xyz_float_t pos_error;
float backlash[NUM_SIDES];
float pos_error[XYZ];
float nozzle_outer_dimension[2] = {CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER};
xy_float_t nozzle_outer_dimension = nod;
};
#define TEMPORARY_SOFT_ENDSTOP_STATE(enable) REMEMBER(tes, soft_endstops_enabled, enable);
@ -88,29 +88,8 @@ struct measurements_t {
#define TEMPORARY_BACKLASH_SMOOTHING(value)
#endif
/**
* Move to a particular location. Up to three individual axes
* and their destinations can be specified, in any order.
*/
inline void move_to(
const AxisEnum a1 = NO_AXIS, const float p1 = 0,
const AxisEnum a2 = NO_AXIS, const float p2 = 0,
const AxisEnum a3 = NO_AXIS, const float p3 = 0
) {
set_destination_from_current();
// Note: The order of p1, p2, p3 may not correspond to X, Y, Z
if (a1 != NO_AXIS) destination[a1] = p1;
if (a2 != NO_AXIS) destination[a2] = p2;
if (a3 != NO_AXIS) destination[a3] = p3;
// Make sure coordinates are within bounds
destination[X_AXIS] = _MAX(_MIN(destination[X_AXIS], X_MAX_POS), X_MIN_POS);
destination[Y_AXIS] = _MAX(_MIN(destination[Y_AXIS], Y_MAX_POS), Y_MIN_POS);
destination[Z_AXIS] = _MAX(_MIN(destination[Z_AXIS], Z_MAX_POS), Z_MIN_POS);
// Move to position
do_blocking_move_to(destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
inline void calibration_move() {
do_blocking_move_to(current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL));
}
/**
@ -121,10 +100,12 @@ inline void move_to(
*/
inline void park_above_object(measurements_t &m, const float uncertainty) {
// Move to safe distance above calibration object
move_to(Z_AXIS, m.obj_center[Z_AXIS] + m.dimensions[Z_AXIS] / 2 + uncertainty);
current_position.z = m.obj_center.z + dimensions.z / 2 + uncertainty;
calibration_move();
// Move to center of calibration object in XY
move_to(X_AXIS, m.obj_center[X_AXIS], Y_AXIS, m.obj_center[Y_AXIS]);
current_position = xy_pos_t(m.obj_center);
calibration_move();
}
#if HOTENDS > 1
@ -139,14 +120,9 @@ inline void park_above_object(measurements_t &m, const float uncertainty) {
#if HAS_HOTEND_OFFSET
inline void normalize_hotend_offsets() {
for (uint8_t e = 1; e < HOTENDS; e++) {
hotend_offset[X_AXIS][e] -= hotend_offset[X_AXIS][0];
hotend_offset[Y_AXIS][e] -= hotend_offset[Y_AXIS][0];
hotend_offset[Z_AXIS][e] -= hotend_offset[Z_AXIS][0];
}
hotend_offset[X_AXIS][0] = 0;
hotend_offset[Y_AXIS][0] = 0;
hotend_offset[Z_AXIS][0] = 0;
for (uint8_t e = 1; e < HOTENDS; e++)
hotend_offset[e] -= hotend_offset[0];
hotend_offset[0].reset();
}
#endif
@ -175,7 +151,7 @@ float measuring_movement(const AxisEnum axis, const int dir, const bool stop_sta
const feedRate_t mms = fast ? MMM_TO_MMS(CALIBRATION_FEEDRATE_FAST) : MMM_TO_MMS(CALIBRATION_FEEDRATE_SLOW);
const float limit = fast ? 50 : 5;
set_destination_from_current();
destination = current_position;
for (float travel = 0; travel < limit; travel += step) {
destination[axis] += dir * step;
do_blocking_move_to(destination, mms);
@ -199,7 +175,7 @@ inline float measure(const AxisEnum axis, const int dir, const bool stop_state,
const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN;
// Save position
set_destination_from_current();
destination = current_position;
const float start_pos = destination[axis];
const float measured_pos = measuring_movement(axis, dir, stop_state, fast);
// Measure backlash
@ -223,7 +199,7 @@ inline float measure(const AxisEnum axis, const int dir, const bool stop_state,
* to find out height of edge
*/
inline void probe_side(measurements_t &m, const float uncertainty, const side_t side, const bool probe_top_at_edge=false) {
const float dimensions[] = CALIBRATION_OBJECT_DIMENSIONS;
const xyz_float_t dimensions = CALIBRATION_OBJECT_DIMENSIONS;
AxisEnum axis;
float dir;
@ -232,7 +208,7 @@ inline void probe_side(measurements_t &m, const float uncertainty, const side_t
switch (side) {
case TOP: {
const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
m.obj_center[Z_AXIS] = measurement - dimensions[Z_AXIS] / 2;
m.obj_center.z = measurement - dimensions.z / 2;
m.obj_side[TOP] = measurement;
return;
}
@ -240,22 +216,24 @@ inline void probe_side(measurements_t &m, const float uncertainty, const side_t
case FRONT: axis = Y_AXIS; dir = 1; break;
case LEFT: axis = X_AXIS; dir = 1; break;
case BACK: axis = Y_AXIS; dir = -1; break;
default:
return;
default: return;
}
if (probe_top_at_edge) {
// Probe top nearest the side we are probing
move_to(axis, m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]));
current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]);
calibration_move();
m.obj_side[TOP] = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty);
m.obj_center[Z_AXIS] = m.obj_side[TOP] - dimensions[Z_AXIS] / 2;
m.obj_center.z = m.obj_side[TOP] - dimensions.z / 2;
}
// Move to safe distance to the side of the calibration object
move_to(axis, m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty));
current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty);
calibration_move();
// Plunge below the side of the calibration object and measure
move_to(Z_AXIS, m.obj_side[TOP] - CALIBRATION_NOZZLE_TIP_HEIGHT * 0.7);
current_position.z = m.obj_side[TOP] - CALIBRATION_NOZZLE_TIP_HEIGHT * 0.7;
calibration_move();
const float measurement = measure(axis, dir, true, &m.backlash[side], uncertainty);
m.obj_center[axis] = measurement + dir * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2);
m.obj_side[side] = measurement;
@ -294,36 +272,36 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
// Compute the measured center of the calibration object.
#if HAS_X_CENTER
m.obj_center[X_AXIS] = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2;
m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2;
#endif
#if HAS_Y_CENTER
m.obj_center[Y_AXIS] = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2;
m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2;
#endif
// Compute the outside diameter of the nozzle at the height
// at which it makes contact with the calibration object
#if HAS_X_CENTER
m.nozzle_outer_dimension[X_AXIS] = m.obj_side[RIGHT] - m.obj_side[LEFT] - m.dimensions[X_AXIS];
m.nozzle_outer_dimension.x = m.obj_side[RIGHT] - m.obj_side[LEFT] - dimensions.x;
#endif
#if HAS_Y_CENTER
m.nozzle_outer_dimension[Y_AXIS] = m.obj_side[BACK] - m.obj_side[FRONT] - m.dimensions[Y_AXIS];
m.nozzle_outer_dimension.y = m.obj_side[BACK] - m.obj_side[FRONT] - dimensions.y;
#endif
park_above_object(m, uncertainty);
// The difference between the known and the measured location
// of the calibration object is the positional error
m.pos_error[X_AXIS] = (0
m.pos_error.x = (0
#if HAS_X_CENTER
+ m.true_center[X_AXIS] - m.obj_center[X_AXIS]
+ true_center.x - m.obj_center.x
#endif
);
m.pos_error[Y_AXIS] = (0
m.pos_error.y = (0
#if HAS_Y_CENTER
+ m.true_center[Y_AXIS] - m.obj_center[Y_AXIS]
+ true_center.y - m.obj_center.y
#endif
);
m.pos_error[Z_AXIS] = m.true_center[Z_AXIS] - m.obj_center[Z_AXIS];
m.pos_error.z = true_center.z - m.obj_center.z;
}
#if ENABLED(CALIBRATION_REPORTING)
@ -348,12 +326,12 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
inline void report_measured_center(const measurements_t &m) {
SERIAL_ECHOLNPGM("Center:");
#if HAS_X_CENTER
SERIAL_ECHOLNPAIR(" X", m.obj_center[X_AXIS]);
SERIAL_ECHOLNPAIR(" X", m.obj_center.x);
#endif
#if HAS_Y_CENTER
SERIAL_ECHOLNPAIR(" Y", m.obj_center[Y_AXIS]);
SERIAL_ECHOLNPAIR(" Y", m.obj_center.y);
#endif
SERIAL_ECHOLNPAIR(" Z", m.obj_center[Z_AXIS]);
SERIAL_ECHOLNPAIR(" Z", m.obj_center.z);
SERIAL_EOL();
}
@ -380,12 +358,12 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
SERIAL_ECHO(int(active_extruder));
SERIAL_ECHOLNPGM(" Positional Error:");
#if HAS_X_CENTER
SERIAL_ECHOLNPAIR(" X", m.pos_error[X_AXIS]);
SERIAL_ECHOLNPAIR(" X", m.pos_error.x);
#endif
#if HAS_Y_CENTER
SERIAL_ECHOLNPAIR(" Y", m.pos_error[Y_AXIS]);
SERIAL_ECHOLNPAIR(" Y", m.pos_error.y);
#endif
SERIAL_ECHOLNPAIR(" Z", m.pos_error[Z_AXIS]);
SERIAL_ECHOLNPAIR(" Z", m.pos_error.z);
SERIAL_EOL();
}
@ -393,10 +371,10 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:");
#if HAS_X_CENTER || HAS_Y_CENTER
#if HAS_X_CENTER
SERIAL_ECHOLNPAIR(" X", m.nozzle_outer_dimension[X_AXIS]);
SERIAL_ECHOLNPAIR(" X", m.nozzle_outer_dimension.x);
#endif
#if HAS_Y_CENTER
SERIAL_ECHOLNPAIR(" Y", m.nozzle_outer_dimension[Y_AXIS]);
SERIAL_ECHOLNPAIR(" Y", m.nozzle_outer_dimension.y);
#endif
#else
UNUSED(m);
@ -410,7 +388,7 @@ inline void probe_sides(measurements_t &m, const float uncertainty) {
//
inline void report_hotend_offsets() {
for (uint8_t e = 1; e < HOTENDS; e++)
SERIAL_ECHOLNPAIR("T", int(e), " Hotend Offset X", hotend_offset[X_AXIS][e], " Y", hotend_offset[Y_AXIS][e], " Z", hotend_offset[Z_AXIS][e]);
SERIAL_ECHOLNPAIR("T", int(e), " Hotend Offset X", hotend_offset[e].x, " Y", hotend_offset[e].y, " Z", hotend_offset[e].z);
}
#endif
@ -434,49 +412,40 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) {
#if ENABLED(BACKLASH_GCODE)
#if HAS_X_CENTER
backlash.distance_mm[X_AXIS] = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
backlash.distance_mm.x = (m.backlash[LEFT] + m.backlash[RIGHT]) / 2;
#elif ENABLED(CALIBRATION_MEASURE_LEFT)
backlash.distance_mm[X_AXIS] = m.backlash[LEFT];
backlash.distance_mm.x = m.backlash[LEFT];
#elif ENABLED(CALIBRATION_MEASURE_RIGHT)
backlash.distance_mm[X_AXIS] = m.backlash[RIGHT];
backlash.distance_mm.x = m.backlash[RIGHT];
#endif
#if HAS_Y_CENTER
backlash.distance_mm[Y_AXIS] = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
backlash.distance_mm.y = (m.backlash[FRONT] + m.backlash[BACK]) / 2;
#elif ENABLED(CALIBRATION_MEASURE_FRONT)
backlash.distance_mm[Y_AXIS] = m.backlash[FRONT];
backlash.distance_mm.y = m.backlash[FRONT];
#elif ENABLED(CALIBRATION_MEASURE_BACK)
backlash.distance_mm[Y_AXIS] = m.backlash[BACK];
backlash.distance_mm.y = m.backlash[BACK];
#endif
backlash.distance_mm[Z_AXIS] = m.backlash[TOP];
backlash.distance_mm.z = m.backlash[TOP];
#endif
}
#if ENABLED(BACKLASH_GCODE)
// Turn on backlash compensation and move in all
// directions to take up any backlash
{
// New scope for TEMPORARY_BACKLASH_CORRECTION
TEMPORARY_BACKLASH_CORRECTION(all_on);
TEMPORARY_BACKLASH_SMOOTHING(0.0f);
move_to(
X_AXIS, current_position[X_AXIS] + 3,
Y_AXIS, current_position[Y_AXIS] + 3,
Z_AXIS, current_position[Z_AXIS] + 3
);
move_to(
X_AXIS, current_position[X_AXIS] - 3,
Y_AXIS, current_position[Y_AXIS] - 3,
Z_AXIS, current_position[Z_AXIS] - 3
);
const xyz_float_t move = { 3, 3, 3 };
current_position += move; calibration_move();
current_position -= move; calibration_move();
}
#endif
}
inline void update_measurements(measurements_t &m, const AxisEnum axis) {
const float true_center[XYZ] = CALIBRATION_OBJECT_CENTER;
current_position[axis] += m.pos_error[axis];
m.obj_center[axis] = true_center[axis];
m.pos_error[axis] = 0;
@ -508,12 +477,12 @@ inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const
// Adjust the hotend offset
#if HAS_HOTEND_OFFSET
#if HAS_X_CENTER
hotend_offset[X_AXIS][extruder] += m.pos_error[X_AXIS];
hotend_offset[extruder].x += m.pos_error.x;
#endif
#if HAS_Y_CENTER
hotend_offset[Y_AXIS][extruder] += m.pos_error[Y_AXIS];
hotend_offset[extruder].y += m.pos_error.y;
#endif
hotend_offset[Z_AXIS][extruder] += m.pos_error[Z_AXIS];
hotend_offset[extruder].z += m.pos_error.z;
normalize_hotend_offsets();
#endif
@ -589,7 +558,8 @@ inline void calibrate_all() {
// Do a slow and precise calibration of the toolheads
calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNCERTAIN);
move_to(X_AXIS, 150); // Park nozzle away from calibration object
current_position.x = X_CENTER;
calibration_move(); // Park nozzle away from calibration object
}
/**

34
Marlin/src/gcode/calibrate/M48.cpp
View File

@ -74,13 +74,14 @@ void GcodeSuite::M48() {
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
float X_current = current_position[X_AXIS],
Y_current = current_position[Y_AXIS];
xy_float_t next_pos = current_position;
const float X_probe_location = parser.linearval('X', X_current + probe_offset[X_AXIS]),
Y_probe_location = parser.linearval('Y', Y_current + probe_offset[Y_AXIS]);
const xy_pos_t probe_pos = {
parser.linearval('X', next_pos.x + probe_offset.x),
parser.linearval('Y', next_pos.y + probe_offset.y)
};
if (!position_is_reachable_by_probe(X_probe_location, Y_probe_location)) {
if (!position_is_reachable_by_probe(probe_pos)) {
SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
return;
}
@ -116,7 +117,7 @@ void GcodeSuite::M48() {
float mean = 0.0, sigma = 0.0, min = 99999.9, max = -99999.9, sample_set[n_samples];
// Move to the first point, deploy, and probe
const float t = probe_at_point(X_probe_location, Y_probe_location, raise_after, verbose_level);
const float t = probe_at_point(probe_pos, raise_after, verbose_level);
bool probing_good = !isnan(t);
if (probing_good) {
@ -165,32 +166,31 @@ void GcodeSuite::M48() {
while (angle < 0.0) angle += 360.0; // outside of this range. It looks like they behave correctly with
// numbers outside of the range, but just to be safe we clamp them.
X_current = X_probe_location - probe_offset[X_AXIS] + cos(RADIANS(angle)) * radius;
Y_current = Y_probe_location - probe_offset[Y_AXIS] + sin(RADIANS(angle)) * radius;
next_pos.set(probe_pos.x - probe_offset.x + cos(RADIANS(angle)) * radius,
probe_pos.y - probe_offset.y + sin(RADIANS(angle)) * radius);
#if DISABLED(DELTA)
LIMIT(X_current, X_MIN_POS, X_MAX_POS);
LIMIT(Y_current, Y_MIN_POS, Y_MAX_POS);
LIMIT(next_pos.x, X_MIN_POS, X_MAX_POS);
LIMIT(next_pos.y, Y_MIN_POS, Y_MAX_POS);
#else
// If we have gone out too far, we can do a simple fix and scale the numbers
// back in closer to the origin.
while (!position_is_reachable_by_probe(X_current, Y_current)) {
X_current *= 0.8;
Y_current *= 0.8;
while (!position_is_reachable_by_probe(next_pos)) {
next_pos *= 0.8;
if (verbose_level > 3)
SERIAL_ECHOLNPAIR("Moving inward: X", X_current, " Y", Y_current);
SERIAL_ECHOLNPAIR("Moving inward: X", next_pos.x, " Y", next_pos.y);
}
#endif
if (verbose_level > 3)
SERIAL_ECHOLNPAIR("Going to: X", X_current, " Y", Y_current, " Z", current_position[Z_AXIS]);
SERIAL_ECHOLNPAIR("Going to: X", next_pos.x, " Y", next_pos.y);
do_blocking_move_to_xy(X_current, Y_current);
do_blocking_move_to_xy(next_pos);
} // n_legs loop
} // n_legs
// Probe a single point
sample_set[n] = probe_at_point(X_probe_location, Y_probe_location, raise_after, 0);
sample_set[n] = probe_at_point(probe_pos, raise_after, 0);
// Break the loop if the probe fails
probing_good = !isnan(sample_set[n]);

22
Marlin/src/gcode/calibrate/M665.cpp
View File

@ -43,14 +43,14 @@
* Z = Gamma (Tower 3) angle trim
*/
void GcodeSuite::M665() {
if (parser.seen('H')) delta_height = parser.value_linear_units();
if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units();
if (parser.seen('R')) delta_radius = parser.value_linear_units();
if (parser.seen('S')) delta_segments_per_second = parser.value_float();
if (parser.seen('B')) delta_calibration_radius = parser.value_float();
if (parser.seen('X')) delta_tower_angle_trim[A_AXIS] = parser.value_float();
if (parser.seen('Y')) delta_tower_angle_trim[B_AXIS] = parser.value_float();
if (parser.seen('Z')) delta_tower_angle_trim[C_AXIS] = parser.value_float();
if (parser.seen('H')) delta_height = parser.value_linear_units();
if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units();
if (parser.seen('R')) delta_radius = parser.value_linear_units();
if (parser.seen('S')) delta_segments_per_second = parser.value_float();
if (parser.seen('B')) delta_calibration_radius = parser.value_float();
if (parser.seen('X')) delta_tower_angle_trim.a = parser.value_float();
if (parser.seen('Y')) delta_tower_angle_trim.b = parser.value_float();
if (parser.seen('Z')) delta_tower_angle_trim.c = parser.value_float();
recalc_delta_settings();
}
@ -76,13 +76,13 @@
#if HAS_SCARA_OFFSET
if (parser.seenval('Z')) scara_home_offset[Z_AXIS] = parser.value_linear_units();
if (parser.seenval('Z')) scara_home_offset.z = parser.value_linear_units();
const bool hasA = parser.seenval('A'), hasP = parser.seenval('P'), hasX = parser.seenval('X');
const uint8_t sumAPX = hasA + hasP + hasX;
if (sumAPX) {
if (sumAPX == 1)
scara_home_offset[A_AXIS] = parser.value_float();
scara_home_offset.a = parser.value_float();
else {
SERIAL_ERROR_MSG("Only one of A, P, or X is allowed.");
return;
@ -93,7 +93,7 @@
const uint8_t sumBTY = hasB + hasT + hasY;
if (sumBTY) {
if (sumBTY == 1)
scara_home_offset[B_AXIS] = parser.value_float();
scara_home_offset.b = parser.value_float();
else {
SERIAL_ERROR_MSG("Only one of B, T, or Y is allowed.");
return;

10
Marlin/src/gcode/config/M200-M205.cpp
View File

@ -152,17 +152,17 @@ void GcodeSuite::M205() {
}
#endif
#if HAS_CLASSIC_JERK
if (parser.seen('X')) planner.max_jerk[X_AXIS] = parser.value_linear_units();
if (parser.seen('Y')) planner.max_jerk[Y_AXIS] = parser.value_linear_units();
if (parser.seen('X')) planner.max_jerk.x = parser.value_linear_units();
if (parser.seen('Y')) planner.max_jerk.y = parser.value_linear_units();
if (parser.seen('Z')) {
planner.max_jerk[Z_AXIS] = parser.value_linear_units();
planner.max_jerk.z = parser.value_linear_units();
#if HAS_MESH
if (planner.max_jerk[Z_AXIS] <= 0.1f)
if (planner.max_jerk.z <= 0.1f)
SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses.");
#endif
}
#if !BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units();
if (parser.seen('E')) planner.max_jerk.e = parser.value_linear_units();
#endif
#endif
}

14
Marlin/src/gcode/config/M218.cpp
View File

@ -44,27 +44,27 @@ void GcodeSuite::M218() {
const int8_t target_extruder = get_target_extruder_from_command();
if (target_extruder < 0) return;
if (parser.seenval('X')) hotend_offset[X_AXIS][target_extruder] = parser.value_linear_units();
if (parser.seenval('Y')) hotend_offset[Y_AXIS][target_extruder] = parser.value_linear_units();
if (parser.seenval('Z')) hotend_offset[Z_AXIS][target_extruder] = parser.value_linear_units();
if (parser.seenval('X')) hotend_offset[target_extruder].x = parser.value_linear_units();
if (parser.seenval('Y')) hotend_offset[target_extruder].y = parser.value_linear_units();
if (parser.seenval('Z')) hotend_offset[target_extruder].z = parser.value_linear_units();
if (!parser.seen("XYZ")) {
SERIAL_ECHO_START();
SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
HOTEND_LOOP() {
SERIAL_CHAR(' ');
SERIAL_ECHO(hotend_offset[X_AXIS][e]);
SERIAL_ECHO(hotend_offset[e].x);
SERIAL_CHAR(',');
SERIAL_ECHO(hotend_offset[Y_AXIS][e]);
SERIAL_ECHO(hotend_offset[e].y);
SERIAL_CHAR(',');
SERIAL_ECHO_F(hotend_offset[Z_AXIS][e], 3);
SERIAL_ECHO_F(hotend_offset[e].z, 3);
}
SERIAL_EOL();
}
#if ENABLED(DELTA)
if (target_extruder == active_extruder)
do_blocking_move_to_xy(current_position[X_AXIS], current_position[Y_AXIS], planner.settings.max_feedrate_mm_s[X_AXIS]);
do_blocking_move_to_xy(current_position, planner.settings.max_feedrate_mm_s[X_AXIS]);
#endif
}

2
Marlin/src/gcode/config/M92.cpp
View File

@ -77,7 +77,7 @@ void GcodeSuite::M92() {
if (value < 20) {
float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab.
#if HAS_CLASSIC_JERK && !BOTH(JUNCTION_DEVIATION, LIN_ADVANCE)
planner.max_jerk[E_AXIS] *= factor;
planner.max_jerk.e *= factor;
#endif
planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor;
planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor;

12
Marlin/src/gcode/control/M211.cpp
View File

@ -33,18 +33,14 @@
* Usage: M211 S1 to enable, M211 S0 to disable, M211 alone for report
*/
void GcodeSuite::M211() {
const xyz_pos_t l_soft_min = soft_endstop.min.asLogical(),
l_soft_max = soft_endstop.max.asLogical();
SERIAL_ECHO_START();
SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS);
if (parser.seen('S')) soft_endstops_enabled = parser.value_bool();
serialprint_onoff(soft_endstops_enabled);
SERIAL_ECHOPGM(MSG_SOFT_MIN);
SERIAL_ECHOPAIR( MSG_X, LOGICAL_X_POSITION(soft_endstop[X_AXIS].min));
SERIAL_ECHOPAIR(" " MSG_Y, LOGICAL_Y_POSITION(soft_endstop[Y_AXIS].min));
SERIAL_ECHOPAIR(" " MSG_Z, LOGICAL_Z_POSITION(soft_endstop[Z_AXIS].min));
SERIAL_ECHOPGM(MSG_SOFT_MAX);
SERIAL_ECHOPAIR( MSG_X, LOGICAL_X_POSITION(soft_endstop[X_AXIS].max));
SERIAL_ECHOPAIR(" " MSG_Y, LOGICAL_Y_POSITION(soft_endstop[Y_AXIS].max));
SERIAL_ECHOLNPAIR(" " MSG_Z, LOGICAL_Z_POSITION(soft_endstop[Z_AXIS].max));
print_xyz(l_soft_min, PSTR(MSG_SOFT_MIN), PSTR(" "));
print_xyz(l_soft_max, PSTR(MSG_SOFT_MAX));
}
#endif

8
Marlin/src/gcode/control/M605.cpp
View File

@ -79,9 +79,9 @@
}
mirrored_duplication_mode = true;
stepper.set_directions();
float x_jog = current_position[X_AXIS] - .1;
float x_jog = current_position.x - .1;
for (uint8_t i = 2; --i;) {
planner.buffer_line(x_jog, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, 0);
planner.buffer_line(x_jog, current_position.y, current_position.z, current_position.e, feedrate_mm_s, 0);
x_jog += .1;
}
return;
@ -122,7 +122,7 @@
DEBUG_ECHOPAIR("\nActive Ext: ", int(active_extruder));
if (!active_extruder_parked) DEBUG_ECHOPGM(" NOT ");
DEBUG_ECHOPGM(" parked.");
DEBUG_ECHOPAIR("\nactive_extruder_x_pos: ", current_position[X_AXIS]);
DEBUG_ECHOPAIR("\nactive_extruder_x_pos: ", current_position.x);
DEBUG_ECHOPAIR("\ninactive_extruder_x_pos: ", inactive_extruder_x_pos);
DEBUG_ECHOPAIR("\nextruder_duplication_enabled: ", int(extruder_duplication_enabled));
DEBUG_ECHOPAIR("\nduplicate_extruder_x_offset: ", duplicate_extruder_x_offset);
@ -138,7 +138,7 @@
HOTEND_LOOP() {
DEBUG_ECHOPAIR(" T", int(e));
LOOP_XYZ(a) DEBUG_ECHOPAIR(" hotend_offset[", axis_codes[a], "_AXIS][", int(e), "]=", hotend_offset[a][e]);
LOOP_XYZ(a) DEBUG_ECHOPAIR(" hotend_offset[", int(e), "].", axis_codes[a] | 0x20, "=", hotend_offset[e][a]);
DEBUG_EOL();
}
DEBUG_EOL();

32
Marlin/src/gcode/feature/camera/M240.cpp
View File

@ -48,8 +48,8 @@
#if ENABLED(ADVANCED_PAUSE_FEATURE)
do_pause_e_move(length, fr_mm_s);
#else
current_position[E_AXIS] += length / planner.e_factor[active_extruder];
planner.buffer_line(current_position, fr_mm_s, active_extruder);
current_position.e += length / planner.e_factor[active_extruder];
line_to_current_position(fr_mm_s);
#endif
}
}
@ -97,10 +97,10 @@ void GcodeSuite::M240() {
if (axis_unhomed_error()) return;
const float old_pos[XYZ] = {
current_position[X_AXIS] + parser.linearval('A'),
current_position[Y_AXIS] + parser.linearval('B'),
current_position[Z_AXIS]
const xyz_pos_t old_pos = {
current_position.x + parser.linearval('A'),
current_position.y + parser.linearval('B'),
current_position.z
};
#ifdef PHOTO_RETRACT_MM
@ -121,22 +121,22 @@ void GcodeSuite::M240() {
feedRate_t fr_mm_s = MMM_TO_MMS(parser.linearval('F'));
if (fr_mm_s) NOLESS(fr_mm_s, 10.0f);
constexpr float photo_position[XYZ] = PHOTO_POSITION;
float raw[XYZ] = {
parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : photo_position[X_AXIS],
parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_position[Y_AXIS],
(parser.seenval('Z') ? parser.value_linear_units() : photo_position[Z_AXIS]) + current_position[Z_AXIS]
constexpr xyz_pos_t photo_position = PHOTO_POSITION;
xyz_pos_t raw = {
parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : photo_position.x,
parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_position.y,
(parser.seenval('Z') ? parser.value_linear_units() : photo_position.z) + current_position.z
};
apply_motion_limits(raw);
do_blocking_move_to(raw, fr_mm_s);
#ifdef PHOTO_SWITCH_POSITION
constexpr float photo_switch_position[2] = PHOTO_SWITCH_POSITION;
const float sraw[] = {
parser.seenval('I') ? RAW_X_POSITION(parser.value_linear_units()) : photo_switch_position[X_AXIS],
parser.seenval('J') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_switch_position[Y_AXIS]
constexpr xy_pos_t photo_switch_position = PHOTO_SWITCH_POSITION;
const xy_pos_t sraw = {
parser.seenval('I') ? RAW_X_POSITION(parser.value_linear_units()) : photo_switch_position.x,
parser.seenval('J') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_switch_position.y
};
do_blocking_move_to_xy(sraw[X_AXIS], sraw[Y_AXIS], get_homing_bump_feedrate(X_AXIS));
do_blocking_move_to_xy(sraw, get_homing_bump_feedrate(X_AXIS));
#if PHOTO_SWITCH_MS > 0
safe_delay(parser.intval('D', PHOTO_SWITCH_MS));
#endif

5
Marlin/src/gcode/feature/pause/M125.cpp
View File

@ -58,7 +58,7 @@ void GcodeSuite::M125() {
#endif
);
point_t park_point = NOZZLE_PARK_POINT;
xyz_pos_t park_point = NOZZLE_PARK_POINT;
// Move XY axes to filament change position or given position
if (parser.seenval('X')) park_point.x = RAW_X_POSITION(parser.linearval('X'));
@ -68,8 +68,7 @@ void GcodeSuite::M125() {
if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
#if HAS_HOTEND_OFFSET && NONE(DUAL_X_CARRIAGE, DELTA)
park_point.x += hotend_offset[X_AXIS][active_extruder];
park_point.y += hotend_offset[Y_AXIS][active_extruder];
park_point += hotend_offset[active_extruder];
#endif
#if ENABLED(SDSUPPORT)

6
Marlin/src/gcode/feature/pause/M600.cpp
View File

@ -60,7 +60,6 @@
* Default values are used for omitted arguments.
*/
void GcodeSuite::M600() {
point_t park_point = NOZZLE_PARK_POINT;
#if ENABLED(MIXING_EXTRUDER)
const int8_t target_e_stepper = get_target_e_stepper_from_command();
@ -119,6 +118,8 @@ void GcodeSuite::M600() {
#endif
);
xyz_pos_t park_point NOZZLE_PARK_POINT;
// Lift Z axis
if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
@ -127,8 +128,7 @@ void GcodeSuite::M600() {
if (parser.seenval('Y')) park_point.y = parser.linearval('Y');
#if HAS_HOTEND_OFFSET && NONE(DUAL_X_CARRIAGE, DELTA)
park_point.x += hotend_offset[X_AXIS][active_extruder];
park_point.y += hotend_offset[Y_AXIS][active_extruder];
park_point += hotend_offset[active_extruder];
#endif
#if ENABLED(MMU2_MENUS)

13
Marlin/src/gcode/feature/pause/M701_M702.cpp
View File

@ -28,7 +28,6 @@
#include "../../../Marlin.h"
#include "../../../module/motion.h"
#include "../../../module/temperature.h"
#include "../../../libs/point_t.h"
#if EXTRUDERS > 1
#include "../../../module/tool_change.h"
@ -57,7 +56,7 @@
* Default values are used for omitted arguments.
*/
void GcodeSuite::M701() {
point_t park_point = NOZZLE_PARK_POINT;
xyz_pos_t park_point = NOZZLE_PARK_POINT;
#if ENABLED(NO_MOTION_BEFORE_HOMING)
// Don't raise Z if the machine isn't homed
@ -97,7 +96,7 @@ void GcodeSuite::M701() {
// Lift Z axis
if (park_point.z > 0)
do_blocking_move_to_z(_MIN(current_position[Z_AXIS] + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
do_blocking_move_to_z(_MIN(current_position.z + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
// Load filament
#if ENABLED(PRUSA_MMU2)
@ -116,7 +115,7 @@ void GcodeSuite::M701() {
// Restore Z axis
if (park_point.z > 0)
do_blocking_move_to_z(_MAX(current_position[Z_AXIS] - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
do_blocking_move_to_z(_MAX(current_position.z - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
#if EXTRUDERS > 1 && DISABLED(PRUSA_MMU2)
// Restore toolhead if it was changed
@ -146,7 +145,7 @@ void GcodeSuite::M701() {
* Default values are used for omitted arguments.
*/
void GcodeSuite::M702() {
point_t park_point = NOZZLE_PARK_POINT;
xyz_pos_t park_point = NOZZLE_PARK_POINT;
#if ENABLED(NO_MOTION_BEFORE_HOMING)
// Don't raise Z if the machine isn't homed
@ -196,7 +195,7 @@ void GcodeSuite::M702() {
// Lift Z axis
if (park_point.z > 0)
do_blocking_move_to_z(_MIN(current_position[Z_AXIS] + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
do_blocking_move_to_z(_MIN(current_position.z + park_point.z, Z_MAX_POS), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
// Unload filament
#if ENABLED(PRUSA_MMU2)
@ -226,7 +225,7 @@ void GcodeSuite::M702() {
// Restore Z axis
if (park_point.z > 0)
do_blocking_move_to_z(_MAX(current_position[Z_AXIS] - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
do_blocking_move_to_z(_MAX(current_position.z - park_point.z, 0), feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
#if EXTRUDERS > 1 && DISABLED(PRUSA_MMU2)
// Restore toolhead if it was changed

10
Marlin/src/gcode/feature/trinamic/M122.cpp
View File

@ -31,8 +31,8 @@
* M122: Debug TMC drivers
*/
void GcodeSuite::M122() {
bool print_axis[XYZE] = { false, false, false, false },
print_all = true;
xyze_bool_t print_axis = { false, false, false, false };
bool print_all = true;
LOOP_XYZE(i) if (parser.seen(axis_codes[i])) { print_axis[i] = true; print_all = false; }
if (print_all) LOOP_XYZE(i) print_axis[i] = true;
@ -45,12 +45,12 @@ void GcodeSuite::M122() {
#endif
if (parser.seen('V'))
tmc_get_registers(print_axis[X_AXIS], print_axis[Y_AXIS], print_axis[Z_AXIS], print_axis[E_AXIS]);
tmc_get_registers(print_axis.x, print_axis.y, print_axis.z, print_axis.e);
else
tmc_report_all(print_axis[X_AXIS], print_axis[Y_AXIS], print_axis[Z_AXIS], print_axis[E_AXIS]);
tmc_report_all(print_axis.x, print_axis.y, print_axis.z, print_axis.e);
#endif
test_tmc_connection(print_axis[X_AXIS], print_axis[Y_AXIS], print_axis[Z_AXIS], print_axis[E_AXIS]);
test_tmc_connection(print_axis.x, print_axis.y, print_axis.z, print_axis.e);
}
#endif // HAS_TRINAMIC

16
Marlin/src/gcode/feature/trinamic/M911-M914.cpp
View File

@ -104,25 +104,25 @@
*/
void GcodeSuite::M912() {
#if M91x_SOME_X
const bool hasX = parser.seen(axis_codes[X_AXIS]);
const bool hasX = parser.seen(axis_codes.x);
#else
constexpr bool hasX = false;
#endif
#if M91x_SOME_Y
const bool hasY = parser.seen(axis_codes[Y_AXIS]);
const bool hasY = parser.seen(axis_codes.y);
#else
constexpr bool hasY = false;
#endif
#if M91x_SOME_Z
const bool hasZ = parser.seen(axis_codes[Z_AXIS]);
const bool hasZ = parser.seen(axis_codes.z);
#else
constexpr bool hasZ = false;
#endif
#if M91x_SOME_E
const bool hasE = parser.seen(axis_codes[E_AXIS]);
const bool hasE = parser.seen(axis_codes.e);
#else
constexpr bool hasE = false;
#endif
@ -130,7 +130,7 @@
const bool hasNone = !hasX && !hasY && !hasZ && !hasE;
#if M91x_SOME_X
const int8_t xval = int8_t(parser.byteval(axis_codes[X_AXIS], 0xFF));
const int8_t xval = int8_t(parser.byteval(axis_codes.x, 0xFF));
#if M91x_USE(X)
if (hasNone || xval == 1 || (hasX && xval < 0)) tmc_clear_otpw(stepperX);
#endif
@ -140,7 +140,7 @@
#endif
#if M91x_SOME_Y
const int8_t yval = int8_t(parser.byteval(axis_codes[Y_AXIS], 0xFF));
const int8_t yval = int8_t(parser.byteval(axis_codes.y, 0xFF));
#if M91x_USE(Y)
if (hasNone || yval == 1 || (hasY && yval < 0)) tmc_clear_otpw(stepperY);
#endif
@ -150,7 +150,7 @@
#endif
#if M91x_SOME_Z
const int8_t zval = int8_t(parser.byteval(axis_codes[Z_AXIS], 0xFF));
const int8_t zval = int8_t(parser.byteval(axis_codes.z, 0xFF));
#if M91x_USE(Z)
if (hasNone || zval == 1 || (hasZ && zval < 0)) tmc_clear_otpw(stepperZ);
#endif
@ -163,7 +163,7 @@
#endif
#if M91x_SOME_E
const int8_t eval = int8_t(parser.byteval(axis_codes[E_AXIS], 0xFF));
const int8_t eval = int8_t(parser.byteval(axis_codes.e, 0xFF));
#if M91x_USE_E(0)
if (hasNone || eval == 0 || (hasE && eval < 0)) tmc_clear_otpw(stepperE0);
#endif

19
Marlin/src/gcode/gcode.cpp
View File

@ -49,12 +49,13 @@ GcodeSuite gcode;
millis_t GcodeSuite::previous_move_ms;
static constexpr bool ar_init[XYZE] = AXIS_RELATIVE_MODES;
// Relative motion mode for each logical axis
static constexpr xyze_bool_t ar_init = AXIS_RELATIVE_MODES;
uint8_t GcodeSuite::axis_relative = (
(ar_init[X_AXIS] ? _BV(REL_X) : 0)
| (ar_init[Y_AXIS] ? _BV(REL_Y) : 0)
| (ar_init[Z_AXIS] ? _BV(REL_Z) : 0)
| (ar_init[E_AXIS] ? _BV(REL_E) : 0)
(ar_init.x ? _BV(REL_X) : 0)
| (ar_init.y ? _BV(REL_Y) : 0)
| (ar_init.z ? _BV(REL_Z) : 0)
| (ar_init.e ? _BV(REL_E) : 0)
);
#if ENABLED(HOST_KEEPALIVE_FEATURE)
@ -68,7 +69,7 @@ uint8_t GcodeSuite::axis_relative = (
#if ENABLED(CNC_COORDINATE_SYSTEMS)
int8_t GcodeSuite::active_coordinate_system = -1; // machine space
float GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
xyz_pos_t GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS];
#endif
/**
@ -112,7 +113,7 @@ int8_t GcodeSuite::get_target_e_stepper_from_command() {
* - Set the feedrate, if included
*/
void GcodeSuite::get_destination_from_command() {
bool seen[XYZE] = { false, false, false, false };
xyze_bool_t seen = { false, false, false, false };
LOOP_XYZE(i) {
if ( (seen[i] = parser.seenval(axis_codes[i])) ) {
const float v = parser.value_axis_units((AxisEnum)i);
@ -124,7 +125,7 @@ void GcodeSuite::get_destination_from_command() {
#if ENABLED(POWER_LOSS_RECOVERY) && !PIN_EXISTS(POWER_LOSS)
// Only update power loss recovery on moves with E
if (recovery.enabled && IS_SD_PRINTING() && seen[E_AXIS] && (seen[X_AXIS] || seen[Y_AXIS]))
if (recovery.enabled && IS_SD_PRINTING() && seen.e && (seen.x || seen.y))
recovery.save();
#endif
@ -133,7 +134,7 @@ void GcodeSuite::get_destination_from_command() {
#if ENABLED(PRINTCOUNTER)
if (!DEBUGGING(DRYRUN))
print_job_timer.incFilamentUsed(destination[E_AXIS] - current_position[E_AXIS]);
print_job_timer.incFilamentUsed(destination.e - current_position.e);
#endif
// Get ABCDHI mixing factors

2
Marlin/src/gcode/gcode.h
View File

@ -321,7 +321,7 @@ public:
#define MAX_COORDINATE_SYSTEMS 9
#if ENABLED(CNC_COORDINATE_SYSTEMS)
static int8_t active_coordinate_system;
static float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
static xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS];
static bool select_coordinate_system(const int8_t _new);
#endif

4
Marlin/src/gcode/geometry/G53-G59.cpp
View File

@ -36,9 +36,9 @@
bool GcodeSuite::select_coordinate_system(const int8_t _new) {
if (active_coordinate_system == _new) return false;
active_coordinate_system = _new;
float new_offset[XYZ] = { 0 };
xyz_float_t new_offset{0};
if (WITHIN(_new, 0, MAX_COORDINATE_SYSTEMS - 1))
COPY(new_offset, coordinate_system[_new]);
new_offset = coordinate_system[_new];
LOOP_XYZ(i) {
if (position_shift[i] != new_offset[i]) {
position_shift[i] = new_offset[i];

4
Marlin/src/gcode/geometry/G92.cpp
View File

@ -86,7 +86,7 @@ void GcodeSuite::G92() {
#elif HAS_POSITION_SHIFT
if (i == E_AXIS) {
didE = true;
current_position[E_AXIS] = v; // When using coordinate spaces, only E is set directly
current_position.e = v; // When using coordinate spaces, only E is set directly
}
else {
position_shift[i] += d; // Other axes simply offset the coordinate space
@ -102,7 +102,7 @@ void GcodeSuite::G92() {
#if ENABLED(CNC_COORDINATE_SYSTEMS)
// Apply workspace offset to the active coordinate system
if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1))
COPY(coordinate_system[active_coordinate_system], position_shift);
coordinate_system[active_coordinate_system] = position_shift;
#endif
if (didXYZ) sync_plan_position();

2
Marlin/src/gcode/geometry/M206_M428.cpp
View File

@ -63,7 +63,7 @@ void GcodeSuite::M206() {
void GcodeSuite::M428() {
if (axis_unhomed_error()) return;
float diff[XYZ];
xyz_float_t diff;
LOOP_XYZ(i) {
diff[i] = base_home_pos((AxisEnum)i) - current_position[i];
if (!WITHIN(diff[i], -20, 20) && home_dir((AxisEnum)i) > 0)

40
Marlin/src/gcode/host/M114.cpp
View File

@ -36,7 +36,7 @@
#include "../../core/debug_out.h"
#endif
void report_xyze(const float pos[], const uint8_t n = 4, const uint8_t precision = 3) {
void report_xyze(const xyze_pos_t &pos, const uint8_t n=4, const uint8_t precision=3) {
char str[12];
for (uint8_t a = 0; a < n; a++) {
SERIAL_CHAR(' ');
@ -47,22 +47,27 @@
SERIAL_EOL();
}
inline void report_xyz(const float pos[]) { report_xyze(pos, 3); }
void report_xyz(const xyz_pos_t &pos, const uint8_t precision=3) {
char str[12];
for (uint8_t a = X_AXIS; a <= Z_AXIS; a++) {
SERIAL_CHAR(' ');
SERIAL_CHAR(axis_codes[a]);
SERIAL_CHAR(':');
SERIAL_ECHO(dtostrf(pos[a], 1, precision, str));
}
SERIAL_EOL();
}
inline void report_xyz(const xyze_pos_t &pos) { report_xyze(pos, 3); }
void report_current_position_detail() {
SERIAL_ECHOPGM("\nLogical:");
const float logical[XYZ] = {
LOGICAL_X_POSITION(current_position[X_AXIS]),
LOGICAL_Y_POSITION(current_position[Y_AXIS]),
LOGICAL_Z_POSITION(current_position[Z_AXIS])
};
report_xyz(logical);
report_xyz(current_position.asLogical());
SERIAL_ECHOPGM("Raw: ");
report_xyz(current_position);
float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
xyze_pos_t leveled = current_position;
#if HAS_LEVELING
SERIAL_ECHOPGM("Leveled:");
@ -70,7 +75,7 @@
report_xyz(leveled);
SERIAL_ECHOPGM("UnLevel:");
float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] };
xyze_pos_t unleveled = leveled;
planner.unapply_leveling(unleveled);
report_xyz(unleveled);
#endif
@ -153,7 +158,7 @@
SERIAL_EOL();
#if IS_SCARA
const float deg[XYZ] = {
const xy_float_t deg = {
planner.get_axis_position_degrees(A_AXIS),
planner.get_axis_position_degrees(B_AXIS)
};
@ -162,17 +167,12 @@
#endif
SERIAL_ECHOPGM("FromStp:");
get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics)
const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], planner.get_axis_position_mm(E_AXIS) };
get_cartesian_from_steppers(); // writes 'cartes' (with forward kinematics)
xyze_pos_t from_steppers = { cartes.x, cartes.y, cartes.z, planner.get_axis_position_mm(E_AXIS) };
report_xyze(from_steppers);
const float diff[XYZE] = {
from_steppers[X_AXIS] - leveled[X_AXIS],
from_steppers[Y_AXIS] - leveled[Y_AXIS],
from_steppers[Z_AXIS] - leveled[Z_AXIS],
from_steppers[E_AXIS] - current_position[E_AXIS]
};
SERIAL_ECHOPGM("Differ: ");
const xyze_float_t diff = from_steppers - leveled;
SERIAL_ECHOPGM("Diff: ");
report_xyze(diff);
}

10
Marlin/src/gcode/motion/G0_G1.cpp
View File

@ -35,7 +35,7 @@
#include "../../module/stepper.h"
#endif
extern float destination[XYZE];
extern xyze_pos_t destination;
#if ENABLED(VARIABLE_G0_FEEDRATE)
feedRate_t fast_move_feedrate = MMM_TO_MMS(G0_FEEDRATE);
@ -87,12 +87,12 @@ void GcodeSuite::G0_G1(
if (MIN_AUTORETRACT <= MAX_AUTORETRACT) {
// When M209 Autoretract is enabled, convert E-only moves to firmware retract/recover moves
if (fwretract.autoretract_enabled && parser.seen('E') && !(parser.seen('X') || parser.seen('Y') || parser.seen('Z'))) {
const float echange = destination[E_AXIS] - current_position[E_AXIS];
const float echange = destination.e - current_position.e;
// Is this a retract or recover move?
if (WITHIN(ABS(echange), MIN_AUTORETRACT, MAX_AUTORETRACT) && fwretract.retracted[active_extruder] == (echange > 0.0)) {
current_position[E_AXIS] = destination[E_AXIS]; // Hide a G1-based retract/recover from calculations
sync_plan_position_e(); // AND from the planner
return fwretract.retract(echange < 0.0); // Firmware-based retract/recover (double-retract ignored)
current_position.e = destination.e; // Hide a G1-based retract/recover from calculations
sync_plan_position_e(); // AND from the planner
return fwretract.retract(echange < 0.0); // Firmware-based retract/recover (double-retract ignored)
}
}
}

73
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -50,9 +50,9 @@
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
const float (&cart)[XYZE], // Destination position
const float (&offset)[2], // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
const xyze_pos_t &cart, // Destination position
const ab_float_t &offset, // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
) {
#if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis;
@ -67,21 +67,21 @@ void plan_arc(
#endif
// Radius vector from center to current location
float r_P = -offset[0], r_Q = -offset[1];
ab_float_t rvec = -offset;
const float radius = HYPOT(r_P, r_Q),
const float radius = HYPOT(rvec.a, rvec.b),
#if ENABLED(AUTO_BED_LEVELING_UBL)
start_L = current_position[l_axis],
#endif
center_P = current_position[p_axis] - r_P,
center_Q = current_position[q_axis] - r_Q,
center_P = current_position[p_axis] - rvec.a,
center_Q = current_position[q_axis] - rvec.b,
rt_X = cart[p_axis] - center_P,
rt_Y = cart[q_axis] - center_Q,
linear_travel = cart[l_axis] - current_position[l_axis],
extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
extruder_travel = cart.e - current_position.e;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
if (angular_travel < 0) angular_travel += RADIANS(360);
#ifdef MIN_ARC_SEGMENTS
uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));
@ -133,7 +133,7 @@ void plan_arc(
* This is important when there are successive arc motions.
*/
// Vector rotation matrix values
float raw[XYZE];
xyze_pos_t raw;
const float theta_per_segment = angular_travel / segments,
linear_per_segment = linear_travel / segments,
extruder_per_segment = extruder_travel / segments,
@ -144,7 +144,7 @@ void plan_arc(
raw[l_axis] = current_position[l_axis];
// Initialize the extruder axis
raw[E_AXIS] = current_position[E_AXIS];
raw.e = current_position.e;
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
@ -168,10 +168,10 @@ void plan_arc(
#if N_ARC_CORRECTION > 1
if (--arc_recalc_count) {
// Apply vector rotation matrix to previous r_P / 1
const float r_new_Y = r_P * sin_T + r_Q * cos_T;
r_P = r_P * cos_T - r_Q * sin_T;
r_Q = r_new_Y;
// Apply vector rotation matrix to previous rvec.a / 1
const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
rvec.a = rvec.a * cos_T - rvec.b * sin_T;
rvec.b = r_new_Y;
}
else
#endif
@ -185,20 +185,20 @@ void plan_arc(
// To reduce stuttering, the sin and cos could be computed at different times.
// For now, compute both at the same time.
const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti;
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
}
// Update raw location
raw[p_axis] = center_P + r_P;
raw[q_axis] = center_Q + r_Q;
raw[p_axis] = center_P + rvec.a;
raw[q_axis] = center_Q + rvec.b;
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
UNUSED(linear_per_segment);
#else
raw[l_axis] += linear_per_segment;
#endif
raw[E_AXIS] += extruder_per_segment;
raw.e += extruder_per_segment;
apply_motion_limits(raw);
@ -215,7 +215,7 @@ void plan_arc(
}
// Ensure last segment arrives at target location.
COPY(raw, cart);
raw = cart;
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
#endif
@ -235,7 +235,7 @@ void plan_arc(
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
#endif
COPY(current_position, raw);
current_position = raw;
} // plan_arc
/**
@ -278,32 +278,27 @@ void GcodeSuite::G2_G3(const bool clockwise) {
relative_mode = relative_mode_backup;
#endif
float arc_offset[2] = { 0, 0 };
ab_float_t arc_offset = { 0, 0 };
if (parser.seenval('R')) {
const float r = parser.value_linear_units();
if (r) {
const float p1 = current_position[X_AXIS], q1 = current_position[Y_AXIS],
p2 = destination[X_AXIS], q2 = destination[Y_AXIS];
if (p2 != p1 || q2 != q1) {
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
dx = p2 - p1, dy = q2 - q1, // X and Y differences
d = HYPOT(dx, dy), // Linear distance between the points
dinv = 1/d, // Inverse of d
h = SQRT(sq(r) - sq(d * 0.5f)), // Distance to the arc pivot-point
mx = (p1 + p2) * 0.5f, my = (q1 + q2) * 0.5f,// Point between the two points
sx = -dy * dinv, sy = dx * dinv, // Slope of the perpendicular bisector
cx = mx + e * h * sx, cy = my + e * h * sy; // Pivot-point of the arc
arc_offset[0] = cx - p1;
arc_offset[1] = cy - q1;
const xy_pos_t p1 = current_position, p2 = destination;
if (p1 != p2) {
const xy_pos_t d = p2 - p1, m = (p1 + p2) * 0.5f; // XY distance and midpoint
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
len = d.magnitude(), // Total move length
h = SQRT(sq(r) - sq(len * 0.5f)); // Distance to the arc pivot-point
const xy_pos_t s = { d.x, -d.y }; // Inverse Slope of the perpendicular bisector
arc_offset = m + s * RECIPROCAL(len) * e * h - p1; // The calculated offset
}
}
}
else {
if (parser.seenval('I')) arc_offset[0] = parser.value_linear_units();
if (parser.seenval('J')) arc_offset[1] = parser.value_linear_units();
if (parser.seenval('I')) arc_offset.a = parser.value_linear_units();
if (parser.seenval('J')) arc_offset.b = parser.value_linear_units();
}
if (arc_offset[0] || arc_offset[1]) {
if (arc_offset) {
#if ENABLED(ARC_P_CIRCLES)
// P indicates number of circles to do

16
Marlin/src/gcode/motion/G5.cpp
View File

@ -27,11 +27,6 @@
#include "../../module/motion.h"
#include "../../module/planner_bezier.h"
void plan_cubic_move(const float (&cart)[XYZE], const float (&offset)[4]) {
cubic_b_spline(current_position, cart, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
COPY(current_position, cart);
}
/**
* Parameters interpreted according to:
* http://linuxcnc.org/docs/2.6/html/gcode/parser.html#sec:G5-Cubic-Spline
@ -57,14 +52,13 @@ void GcodeSuite::G5() {
get_destination_from_command();
const float offset[4] = {
parser.linearval('I'),
parser.linearval('J'),
parser.linearval('P'),
parser.linearval('Q')
const xy_pos_t offsets[2] = {
{ parser.linearval('I'), parser.linearval('J') },
{ parser.linearval('P'), parser.linearval('Q') }
};
plan_cubic_move(destination, offset);
cubic_b_spline(current_position, destination, offsets, MMS_SCALED(feedrate_mm_s), active_extruder);
current_position = destination;
}
}

22
Marlin/src/gcode/motion/M290.cpp
View File

@ -40,21 +40,21 @@
#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
FORCE_INLINE void mod_zprobe_zoffset(const float &offs) {
FORCE_INLINE void mod_probe_offset(const float &offs) {
if (true
#if ENABLED(BABYSTEP_HOTEND_Z_OFFSET)
&& active_extruder == 0
#endif
) {
probe_offset[Z_AXIS] += offs;
probe_offset.z += offs;
SERIAL_ECHO_START();
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET MSG_Z ": ", probe_offset[Z_AXIS]);
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET MSG_Z ": ", probe_offset.z);
}
else {
#if ENABLED(BABYSTEP_HOTEND_Z_OFFSET)
hotend_offset[Z_AXIS][active_extruder] -= offs;
hotend_offset[active_extruder].z -= offs;
SERIAL_ECHO_START();
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET MSG_Z ": ", hotend_offset[Z_AXIS][active_extruder]);
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET MSG_Z ": ", hotend_offset[active_extruder].z);
#endif
}
}
@ -81,7 +81,7 @@ void GcodeSuite::M290() {
const float offs = constrain(parser.value_axis_units((AxisEnum)a), -2, 2);
babystep.add_mm((AxisEnum)a, offs);
#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
if (a == Z_AXIS && (!parser.seen('P') || parser.value_bool())) mod_zprobe_zoffset(offs);
if (a == Z_AXIS && (!parser.seen('P') || parser.value_bool())) mod_probe_offset(offs);
#endif
}
#else
@ -89,7 +89,7 @@ void GcodeSuite::M290() {
const float offs = constrain(parser.value_axis_units(Z_AXIS), -2, 2);
babystep.add_mm(Z_AXIS, offs);
#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
if (!parser.seen('P') || parser.value_bool()) mod_zprobe_zoffset(offs);
if (!parser.seen('P') || parser.value_bool()) mod_probe_offset(offs);
#endif
}
#endif
@ -98,17 +98,17 @@ void GcodeSuite::M290() {
SERIAL_ECHO_START();
#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET " " MSG_Z, probe_offset[Z_AXIS]);
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET " " MSG_Z, probe_offset.z);
#endif
#if ENABLED(BABYSTEP_HOTEND_Z_OFFSET)
{
SERIAL_ECHOLNPAIR("Hotend ", int(active_extruder), "Offset"
#if ENABLED(BABYSTEP_XY)
" X", hotend_offset[X_AXIS][active_extruder],
" Y", hotend_offset[Y_AXIS][active_extruder],
" X", hotend_offset[active_extruder].x,
" Y", hotend_offset[active_extruder].y,
#endif
" Z", hotend_offset[Z_AXIS][active_extruder]
" Z", hotend_offset[active_extruder].z
);
}
#endif

11
Marlin/src/gcode/probe/G30.cpp
View File

@ -39,10 +39,10 @@
* E Engage the probe for each probe (default 1)
*/
void GcodeSuite::G30() {
const float xpos = parser.linearval('X', current_position[X_AXIS] + probe_offset[X_AXIS]),
ypos = parser.linearval('Y', current_position[Y_AXIS] + probe_offset[Y_AXIS]);
const xy_pos_t pos = { parser.linearval('X', current_position.x + probe_offset.x),
parser.linearval('Y', current_position.y + probe_offset.y) };
if (!position_is_reachable_by_probe(xpos, ypos)) return;
if (!position_is_reachable_by_probe(pos)) return;
// Disable leveling so the planner won't mess with us
#if HAS_LEVELING
@ -52,10 +52,9 @@ void GcodeSuite::G30() {
remember_feedrate_scaling_off();
const ProbePtRaise raise_after = parser.boolval('E', true) ? PROBE_PT_STOW : PROBE_PT_NONE;
const float measured_z = probe_at_point(xpos, ypos, raise_after, 1);
const float measured_z = probe_at_point(pos, raise_after, 1);
if (!isnan(measured_z))
SERIAL_ECHOLNPAIR("Bed X: ", FIXFLOAT(xpos), " Y: ", FIXFLOAT(ypos), " Z: ", FIXFLOAT(measured_z));
SERIAL_ECHOLNPAIR("Bed X: ", FIXFLOAT(pos.x), " Y: ", FIXFLOAT(pos.y), " Z: ", FIXFLOAT(measured_z));
restore_feedrate_and_scaling();

7
Marlin/src/gcode/probe/G38.cpp
View File

@ -48,7 +48,7 @@ inline bool G38_run_probe() {
#if MULTIPLE_PROBING > 1
// Get direction of move and retract
float retract_mm[XYZ];
xyz_float_t retract_mm;
LOOP_XYZ(i) {
const float dist = destination[i] - current_position[i];
retract_mm[i] = ABS(dist) < G38_MINIMUM_MOVE ? 0 : home_bump_mm((AxisEnum)i) * (dist > 0 ? -1 : 1);
@ -75,8 +75,7 @@ inline bool G38_run_probe() {
#if MULTIPLE_PROBING > 1
// Move away by the retract distance
set_destination_from_current();
LOOP_XYZ(i) destination[i] += retract_mm[i];
destination = current_position + retract_mm;
endstops.enable(false);
prepare_move_to_destination();
planner.synchronize();
@ -84,7 +83,7 @@ inline bool G38_run_probe() {
REMEMBER(fr, feedrate_mm_s, feedrate_mm_s * 0.25);
// Bump the target more slowly
LOOP_XYZ(i) destination[i] -= retract_mm[i] * 2;
destination -= retract_mm * 2;
G38_single_probe(move_value);
#endif

12
Marlin/src/gcode/probe/M851.cpp
View File

@ -35,18 +35,18 @@ void GcodeSuite::M851() {
// Show usage with no parameters
if (!parser.seen("XYZ")) {
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET " X", probe_offset[X_AXIS], " Y", probe_offset[Y_AXIS], " Z", probe_offset[Z_AXIS]);
SERIAL_ECHOLNPAIR(MSG_PROBE_OFFSET " X", probe_offset.x, " Y", probe_offset.y, " Z", probe_offset.z);
return;
}
float offs[XYZ] = { probe_offset[X_AXIS], probe_offset[Y_AXIS], probe_offset[Z_AXIS] };
xyz_pos_t offs = probe_offset;
bool ok = true;
if (parser.seenval('X')) {
const float x = parser.value_float();
if (WITHIN(x, -(X_BED_SIZE), X_BED_SIZE))
offs[X_AXIS] = x;
offs.x = x;
else {
SERIAL_ECHOLNPAIR("?X out of range (-", int(X_BED_SIZE), " to ", int(X_BED_SIZE), ")");
ok = false;
@ -56,7 +56,7 @@ void GcodeSuite::M851() {
if (parser.seenval('Y')) {
const float y = parser.value_float();
if (WITHIN(y, -(Y_BED_SIZE), Y_BED_SIZE))
offs[Y_AXIS] = y;
offs.y = y;
else {
SERIAL_ECHOLNPAIR("?Y out of range (-", int(Y_BED_SIZE), " to ", int(Y_BED_SIZE), ")");
ok = false;
@ -66,7 +66,7 @@ void GcodeSuite::M851() {
if (parser.seenval('Z')) {
const float z = parser.value_float();
if (WITHIN(z, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX))
offs[Z_AXIS] = z;
offs.z = z;
else {
SERIAL_ECHOLNPAIR("?Z out of range (", int(Z_PROBE_OFFSET_RANGE_MIN), " to ", int(Z_PROBE_OFFSET_RANGE_MAX), ")");
ok = false;
@ -74,7 +74,7 @@ void GcodeSuite::M851() {
}
// Save the new offsets
if (ok) COPY(probe_offset, offs);
if (ok) probe_offset = offs;
}
#endif // HAS_BED_PROBE

2
Marlin/src/gcode/scara/M360-M364.cpp
View File

@ -32,7 +32,7 @@
inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) {
if (IsRunning()) {
forward_kinematics_SCARA(delta_a, delta_b);
do_blocking_move_to_xy(cartes[X_AXIS], cartes[Y_AXIS]);
do_blocking_move_to_xy(cartes);
return true;
}
return false;