MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Extend Skew Correction to UBL

Browse Source
This commit is contained in:
Scott Lahteine 2017-12-09 04:55:02 -06:00
parent ba48ce8586
commit bdf69db0a8
4 changed files with 59 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Marlin/SanityCheck.h
View File

@ -1539,9 +1539,6 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m
#endif
#if ENABLED(SKEW_CORRECTION)
#if ENABLED(AUTO_BED_LEVELING_UBL) && !ENABLED(SEGMENT_LEVELED_MOVES)
#error "SKEW_CORRECTION with AUTO_BED_LEVELING_UBL requires SEGMENT_LEVELED_MOVES."
#endif
#if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD))
#error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD."
#endif

18
Marlin/planner.cpp
View File

@ -569,14 +569,7 @@ void Planner::calculate_volumetric_multipliers() {
void Planner::apply_leveling(float &rx, float &ry, float &rz) {
#if ENABLED(SKEW_CORRECTION)
if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) {
const float tempry = ry - (rz * planner.yz_skew_factor),
temprx = rx - (ry * planner.xy_skew_factor) - (rz * (planner.xz_skew_factor - (planner.xy_skew_factor * planner.yz_skew_factor)));
if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
rx = temprx;
ry = tempry;
}
}
skew(rx, ry, rz);
#endif
if (!leveling_active) return;
@ -667,14 +660,7 @@ void Planner::calculate_volumetric_multipliers() {
}
#if ENABLED(SKEW_CORRECTION)
if (WITHIN(raw[X_AXIS], X_MIN_POS, X_MAX_POS) && WITHIN(raw[Y_AXIS], Y_MIN_POS, Y_MAX_POS)) {
const float temprx = raw[X_AXIS] + raw[Y_AXIS] * planner.xy_skew_factor + raw[Z_AXIS] * planner.xz_skew_factor,
tempry = raw[Y_AXIS] + raw[Z_AXIS] * planner.yz_skew_factor;
if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
raw[X_AXIS] = temprx;
raw[Y_AXIS] = tempry;
}
}
unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#endif
}

24
Marlin/planner.h
View File

@ -341,6 +341,30 @@ class Planner {
#endif
#if ENABLED(SKEW_CORRECTION)
FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {
const float sx = cx - (cy * xy_skew_factor) - (cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor))),
sy = cy - (cz * yz_skew_factor);
if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
cx = sx; cy = sy;
}
}
}
FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor,
sy = cy + cz * yz_skew_factor;
if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
cx = sx; cy = sy;
}
}
}
#endif // SKEW_CORRECTION
#if PLANNER_LEVELING
#define ARG_X float rx

56
Marlin/ubl_motion.cpp
View File

@ -44,18 +44,16 @@
* as possible to determine if this is the case. If this move is within the same cell, we will
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/
const float start[XYZE] = {
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
},
end[XYZE] = {
destination[X_AXIS],
destination[Y_AXIS],
destination[Z_AXIS],
destination[E_AXIS]
};
#if ENABLED(SKEW_CORRECTION)
// For skew correction just adjust the destination point and we're done
float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
#else
const float (&start)[XYZE] = current_position,
(&end)[XYZE] = destination;
#endif
const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]),
@ -63,13 +61,13 @@
cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
if (g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
SERIAL_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]);
SERIAL_CHAR(')');
SERIAL_EOL();
debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
}
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
@ -89,7 +87,7 @@
set_current_from_destination();
if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()"));
return;
}
@ -132,7 +130,7 @@
planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
if (g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()"));
set_current_from_destination();
return;
@ -238,7 +236,7 @@
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()"));
//
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
@ -302,7 +300,7 @@
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
@ -396,7 +394,7 @@
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
@ -460,9 +458,17 @@
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&in_target)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
if (!position_is_reachable(in_target[X_AXIS], in_target[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
#if ENABLED(SKEW_CORRECTION)
// For skew correction just adjust the destination point and we're done
float rtarget[XYZE] = { in_target[X_AXIS], in_target[Y_AXIS], in_target[Z_AXIS], in_target[E_AXIS] };
planner.skew(rtarget[X_AXIS], rtarget[Y_AXIS], rtarget[Z_AXIS]);
#else
const float (&rtarget)[XYZE] = in_target;
#endif
const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS],
@ -507,6 +513,10 @@
current_position[E_AXIS]
};
#if ENABLED(SKEW_CORRECTION)
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#endif
// Only compute leveling per segment if ubl active and target below z_fade_height.
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {