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

Skew Correction for UBL

Browse Source 
Also remove unused grid slicing function when using UBL segmented.
This commit is contained in:
Scott Lahteine 2017-12-09 07:50:55 -06:00
parent 3d796d8040
commit 3255712343
4 changed files with 53 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/src/feature/bedlevel/ubl/ubl.h
View File

@ -321,8 +321,8 @@ class unified_bed_leveling {
return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST); return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
} }
static bool prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate);
static void line_to_destination_cartesian(const float &fr, const uint8_t e); static void line_to_destination_cartesian(const float &fr, const uint8_t e);
static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
#define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1]) #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
#define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1)) #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))

47
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@ -47,18 +47,16 @@
* as possible to determine if this is the case. If this move is within the same cell, we will * 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 * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/ */
const float start[XYZE] = { #if ENABLED(SKEW_CORRECTION)
current_position[X_AXIS], // For skew correction just adjust the destination point and we're done
current_position[Y_AXIS], float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
current_position[Z_AXIS], end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
current_position[E_AXIS] planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
}, planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
end[XYZE] = { #else
destination[X_AXIS], const float (&start)[XYZE] = current_position,
destination[Y_AXIS], (&end)[XYZE] = destination;
destination[Z_AXIS], #endif
destination[E_AXIS]
};
const int cell_start_xi = get_cell_index_x(start[X_AXIS]), const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]), cell_start_yi = get_cell_index_y(start[Y_AXIS]),
@ -66,10 +64,10 @@
cell_dest_yi = get_cell_index_y(end[Y_AXIS]); cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
if (g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]); SERIAL_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]); SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]); SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", end[E_AXIS]); SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL(); SERIAL_EOL();
debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()")); debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
@ -416,12 +414,19 @@
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
// so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first. // so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw(const float (&raw)[XYZE], const float &fr) { inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
#if ENABLED(SKEW_CORRECTION)
float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#else
const float (&raw)[XYZE] = in_raw;
#endif
#if ENABLED(DELTA) // apply delta inverse_kinematics #if ENABLED(DELTA) // apply delta inverse_kinematics
DELTA_RAW_IK(); DELTA_RAW_IK();
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder); planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
@ -434,11 +439,11 @@
scara_oldB = delta[B_AXIS]; scara_oldB = delta[B_AXIS];
float s_feedrate = max(adiff, bdiff) * scara_feed_factor; float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder); planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN #else // CARTESIAN
planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder); planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
#endif #endif
} }
@ -461,7 +466,7 @@
* Returns true if did NOT move, false if moved (requires current_position update). * Returns true if did NOT move, false if moved (requires current_position update).
*/ */
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate) { bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate return true; // did not move, so current_position still accurate

18
Marlin/src/module/planner.cpp
View File

@ -580,14 +580,7 @@ void Planner::calculate_volumetric_multipliers() {
void Planner::apply_leveling(float &rx, float &ry, float &rz) { void Planner::apply_leveling(float &rx, float &ry, float &rz) {
#if ENABLED(SKEW_CORRECTION) #if ENABLED(SKEW_CORRECTION)
if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) { skew(rx, ry, rz);
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;
}
}
#endif #endif
if (!leveling_active) return; if (!leveling_active) return;
@ -678,14 +671,7 @@ void Planner::calculate_volumetric_multipliers() {
} }
#if ENABLED(SKEW_CORRECTION) #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)) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
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;
}
}
#endif #endif
} }

24
Marlin/src/module/planner.h
View File

@ -345,6 +345,30 @@ class Planner {
#endif #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 #if PLANNER_LEVELING
#define ARG_X float rx #define ARG_X float rx