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

Skew Correction for UBL 

Also remove unused grid slicing function when using UBL segmented.
pull/1/head
Scott Lahteine 7 years ago
parent
3d796d8040
commit
3255712343
4 changed files with 53 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      Marlin/src/feature/bedlevel/ubl/ubl.h
  2. 47
      Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
  3. 18
      Marlin/src/module/planner.cpp
  4. 24
      Marlin/src/module/planner.h

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);
}
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 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) (_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
* 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]),
@ -66,10 +64,10 @@
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_cartesian()"));
@ -416,12 +414,19 @@
// 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.
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
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)
@ -434,11 +439,11 @@
scara_oldB = delta[B_AXIS];
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
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
}
@ -461,7 +466,7 @@
* 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
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) {
#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;
@ -678,14 +671,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/src/module/planner.h
View File

@ -345,6 +345,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

Write Preview
Loading…
Cancel
Save