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

Fix up ubl_motion indentation

pull/1/head
Scott Lahteine 7 years ago
parent
24f9bec6f4
commit
d568e586b7
1 changed files with 180 additions and 180 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 360
      Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp

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

@ -23,23 +23,23 @@
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#include "../bedlevel.h" #include "../bedlevel.h"
#include "../../../module/planner.h" #include "../../../module/planner.h"
#include "../../../module/stepper.h" #include "../../../module/stepper.h"
#include "../../../module/motion.h" #include "../../../module/motion.h"
#if ENABLED(DELTA) #if ENABLED(DELTA)
#include "../../../module/delta.h" #include "../../../module/delta.h"
#endif #endif
#include "../../../Marlin.h" #include "../../../Marlin.h"
#include <math.h> #include <math.h>
#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this #if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
inline void set_current_from_destination() { COPY(current_position, destination); } inline void set_current_from_destination() { COPY(current_position, destination); }
#else #else
extern void set_current_from_destination(); extern void set_current_from_destination();
#endif #endif
#if !UBL_SEGMENTED #if !UBL_SEGMENTED
@ -409,219 +409,219 @@
#else // UBL_SEGMENTED #else // UBL_SEGMENTED
#if IS_SCARA // scale the feed rate from mm/s to degrees/s #if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor, scara_oldA, scara_oldB; static float scara_feed_factor, scara_oldA, scara_oldB;
#endif
// 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 (&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 #endif
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, #if ENABLED(DELTA) // apply delta inverse_kinematics
// so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) { DELTA_RAW_IK();
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
#if ENABLED(SKEW_CORRECTION) #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
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 inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
// should move the feedrate scaling to scara inverse_kinematics
DELTA_RAW_IK(); const float adiff = FABS(delta[A_AXIS] - scara_oldA),
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder); bdiff = FABS(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS];
scara_oldB = delta[B_AXIS];
float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE] #else // CARTESIAN
// should move the feedrate scaling to scara inverse_kinematics
const float adiff = FABS(delta[A_AXIS] - scara_oldA), planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
bdiff = FABS(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS];
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], in_raw[E_AXIS], s_feedrate, active_extruder); #endif
}
#else // CARTESIAN #if IS_SCARA
#define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
#elif ENABLED(DELTA)
#define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND)
#else // CARTESIAN
#ifdef LEVELED_SEGMENT_LENGTH
#define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH
#else
#define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation)
#endif
#endif
planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder); /**
* Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
* This calls planner.buffer_segment multiple times for small incremental moves.
* Returns true if did NOT move, false if moved (requires current_position update).
*/
#endif bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
}
#if IS_SCARA if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
#define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm return true; // did not move, so current_position still accurate
#elif ENABLED(DELTA)
#define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND) const float total[XYZE] = {
#else // CARTESIAN rtarget[X_AXIS] - current_position[X_AXIS],
#ifdef LEVELED_SEGMENT_LENGTH rtarget[Y_AXIS] - current_position[Y_AXIS],
#define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH rtarget[Z_AXIS] - current_position[Z_AXIS],
#else rtarget[E_AXIS] - current_position[E_AXIS]
#define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation) };
#endif
const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
#if IS_KINEMATIC
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
uint16_t segments = lroundf(delta_segments_per_second * seconds), // preferred number of segments for distance @ feedrate
seglimit = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments)
#else
uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
#endif #endif
/** NOLESS(segments, 1); // must have at least one segment
* Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics. const float inv_segments = 1.0 / segments; // divide once, multiply thereafter
* This calls planner.buffer_segment multiple times for small incremental moves.
* 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) { #if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary scara_oldA = stepper.get_axis_position_degrees(A_AXIS);
return true; // did not move, so current_position still accurate scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif
const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS], const float diff[XYZE] = {
rtarget[Y_AXIS] - current_position[Y_AXIS], total[X_AXIS] * inv_segments,
rtarget[Z_AXIS] - current_position[Z_AXIS], total[Y_AXIS] * inv_segments,
rtarget[E_AXIS] - current_position[E_AXIS] total[Z_AXIS] * inv_segments,
}; total[E_AXIS] * inv_segments
};
const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
// Note that E segment distance could vary slightly as z mesh height
#if IS_KINEMATIC // changes for each segment, but small enough to ignore.
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
uint16_t segments = lroundf(delta_segments_per_second * seconds), // preferred number of segments for distance @ feedrate float raw[XYZE] = {
seglimit = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length current_position[X_AXIS],
NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments) current_position[Y_AXIS],
#else current_position[Z_AXIS],
uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length current_position[E_AXIS]
#endif };
NOLESS(segments, 1); // must have at least one segment // Only compute leveling per segment if ubl active and target below z_fade_height.
const float inv_segments = 1.0 / segments; // divide once, multiply thereafter if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
#if IS_SCARA // scale the feed rate from mm/s to degrees/s LOOP_XYZE(i) raw[i] += diff[i];
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate; ubl_buffer_segment_raw(raw, feedrate);
scara_oldA = stepper.get_axis_position_degrees(A_AXIS);
scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif
const float diff[XYZE] = {
total[X_AXIS] * inv_segments,
total[Y_AXIS] * inv_segments,
total[Z_AXIS] * inv_segments,
total[E_AXIS] * inv_segments
};
// Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore.
float raw[XYZE] = {
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
};
// 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) {
LOOP_XYZE(i) raw[i] += diff[i];
ubl_buffer_segment_raw(raw, feedrate);
}
ubl_buffer_segment_raw(rtarget, feedrate);
return false; // moved but did not set_current_from_destination();
} }
ubl_buffer_segment_raw(rtarget, feedrate);
return false; // moved but did not set_current_from_destination();
}
// Otherwise perform per-segment leveling // Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]); const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
#endif #endif
// increment to first segment destination // increment to first segment destination
LOOP_XYZE(i) raw[i] += diff[i]; LOOP_XYZE(i) raw[i] += diff[i];
for(;;) { // for each mesh cell encountered during the move for(;;) { // for each mesh cell encountered during the move
// Compute mesh cell invariants that remain constant for all segments within cell. // Compute mesh cell invariants that remain constant for all segments within cell.
// Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter) // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
// the bilinear interpolation from the adjacent cell within the mesh will still work. // the bilinear interpolation from the adjacent cell within the mesh will still work.
// Inner loop will exit each time (because out of cell bounds) but will come back // Inner loop will exit each time (because out of cell bounds) but will come back
// in top of loop and again re-find same adjacent cell and use it, just less efficient // in top of loop and again re-find same adjacent cell and use it, just less efficient
// for mesh inset area. // for mesh inset area.
int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)), int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1); cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1); cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
y0 = mesh_index_to_ypos(cell_yi); y0 = mesh_index_to_ypos(cell_yi);
float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_active (G29 A) if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_active (G29 A)
if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell, if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
float cx = raw[X_AXIS] - x0, // cell-relative x and y float cx = raw[X_AXIS] - x0, // cell-relative x and y
cy = raw[Y_AXIS] - y0; cy = raw[Y_AXIS] - y0;
const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right) const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right) z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell) float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell)
const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1 z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell) float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
// float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop) // float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop)
// As subsequent segments step through this cell, the z_cxy0 intercept will change // As subsequent segments step through this cell, the z_cxy0 intercept will change
// and the z_cxym slope will change, both as a function of cx within the cell, and // and the z_cxym slope will change, both as a function of cx within the cell, and
// each change by a constant for fixed segment lengths. // each change by a constant for fixed segment lengths.
const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0 const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0
z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym
for(;;) { // for all segments within this mesh cell for(;;) { // for all segments within this mesh cell
if (--segments == 0) // if this is last segment, use rtarget for exact if (--segments == 0) // if this is last segment, use rtarget for exact
COPY(raw, rtarget); COPY(raw, rtarget);
const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
* fade_scaling_factor // apply fade factor to interpolated mesh height * fade_scaling_factor // apply fade factor to interpolated mesh height
#endif #endif
; ;
const float z = raw[Z_AXIS]; const float z = raw[Z_AXIS];
raw[Z_AXIS] += z_cxcy; raw[Z_AXIS] += z_cxcy;
ubl_buffer_segment_raw(raw, feedrate); ubl_buffer_segment_raw(raw, feedrate);
raw[Z_AXIS] = z; raw[Z_AXIS] = z;
if (segments == 0) // done with last segment if (segments == 0) // done with last segment
return false; // did not set_current_from_destination() return false; // did not set_current_from_destination()
LOOP_XYZE(i) raw[i] += diff[i]; LOOP_XYZE(i) raw[i] += diff[i];
cx += diff[X_AXIS]; cx += diff[X_AXIS];
cy += diff[Y_AXIS]; cy += diff[Y_AXIS];
if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next
break; break;
// Next segment still within same mesh cell, adjust the per-segment // Next segment still within same mesh cell, adjust the per-segment
// slope and intercept to compute next z height. // slope and intercept to compute next z height.
z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0 z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
} // segment loop } // segment loop
} // cell loop } // cell loop
} }
#endif // UBL_SEGMENTED #endif // UBL_SEGMENTED

Write Preview
Loading…
Cancel
Save