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

Comment/cleanup of motion code

pull/1/head
Scott Lahteine 7 years ago
parent
046a1ad331
commit
000b3b3117
6 changed files with 78 additions and 105 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. 18
      Marlin/src/feature/bedlevel/abl/abl.cpp
  2. 18
      Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
  3. 129
      Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
  4. 11
      Marlin/src/module/motion.cpp
  5. 4
      Marlin/src/module/planner.h
  6. 3
      Marlin/src/module/stepper.cpp

18
Marlin/src/feature/bedlevel/abl/abl.cpp
View File

@ -366,6 +366,7 @@ float bilinear_z_offset(const float raw[XYZ]) {
* splitting the move where it crosses grid borders.
*/
void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
// Get current and destination cells for this line
int cx1 = CELL_INDEX(X, current_position[X_AXIS]),
cy1 = CELL_INDEX(Y, current_position[Y_AXIS]),
cx2 = CELL_INDEX(X, destination[X_AXIS]),
@ -375,8 +376,8 @@ float bilinear_z_offset(const float raw[XYZ]) {
cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2);
cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 2);
// Start and end in the same cell? No split needed.
if (cx1 == cx2 && cy1 == cy2) {
// Start and end on same mesh square
buffer_line_to_destination(fr_mm_s);
set_current_from_destination();
return;
@ -385,25 +386,30 @@ float bilinear_z_offset(const float raw[XYZ]) {
#define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
float normalized_dist, end[XYZE];
// Split at the left/front border of the right/top square
const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
// Crosses on the X and not already split on this X?
// The x_splits flags are insurance against rounding errors.
if (cx2 != cx1 && TEST(x_splits, gcx)) {
// Split on the X grid line
CBI(x_splits, gcx);
COPY(end, destination);
destination[X_AXIS] = bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx;
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = LINE_SEGMENT_END(Y);
CBI(x_splits, gcx);
}
// Crosses on the Y and not already split on this Y?
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
// Split on the Y grid line
CBI(y_splits, gcy);
COPY(end, destination);
destination[Y_AXIS] = bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy;
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = LINE_SEGMENT_END(X);
CBI(y_splits, gcy);
}
else {
// Already split on a border
// Must already have been split on these border(s)
// This should be a rare case.
buffer_line_to_destination(fr_mm_s);
set_current_from_destination();
return;

18
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
View File

@ -59,6 +59,7 @@
* splitting the move where it crosses mesh borders.
*/
void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
// Get current and destination cells for this line
int cx1 = mbl.cell_index_x(current_position[X_AXIS]),
cy1 = mbl.cell_index_y(current_position[Y_AXIS]),
cx2 = mbl.cell_index_x(destination[X_AXIS]),
@ -68,8 +69,8 @@
NOMORE(cx2, GRID_MAX_POINTS_X - 2);
NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
// Start and end in the same cell? No split needed.
if (cx1 == cx2 && cy1 == cy2) {
// Start and end on same mesh square
buffer_line_to_destination(fr_mm_s);
set_current_from_destination();
return;
@ -78,25 +79,30 @@
#define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
float normalized_dist, end[XYZE];
// Split at the left/front border of the right/top square
const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
// Crosses on the X and not already split on this X?
// The x_splits flags are insurance against rounding errors.
if (cx2 != cx1 && TEST(x_splits, gcx)) {
// Split on the X grid line
CBI(x_splits, gcx);
COPY(end, destination);
destination[X_AXIS] = mbl.index_to_xpos[gcx];
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = MBL_SEGMENT_END(Y);
CBI(x_splits, gcx);
}
// Crosses on the Y and not already split on this Y?
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
// Split on the Y grid line
CBI(y_splits, gcy);
COPY(end, destination);
destination[Y_AXIS] = mbl.index_to_ypos[gcy];
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = MBL_SEGMENT_END(X);
CBI(y_splits, gcy);
}
else {
// Already split on a border
// Must already have been split on these border(s)
// This should be a rare case.
buffer_line_to_destination(fr_mm_s);
set_current_from_destination();
return;

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

@ -475,30 +475,17 @@
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
// so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw(const float &rx, const float &ry, const float rz, const float &e, const float &fr) {
inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
#if ENABLED(DELTA) // apply delta inverse_kinematics
const float delta_A = rz + SQRT( delta_diagonal_rod_2_tower[A_AXIS]
- HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
delta_tower[A_AXIS][Y_AXIS] - ry ));
const float delta_B = rz + SQRT( delta_diagonal_rod_2_tower[B_AXIS]
- HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
delta_tower[B_AXIS][Y_AXIS] - ry ));
const float delta_C = rz + SQRT( delta_diagonal_rod_2_tower[C_AXIS]
- HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
delta_tower[C_AXIS][Y_AXIS] - ry ));
planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder);
DELTA_RAW_IK();
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
const float lseg[XYZ] = { rx, ry, rz };
inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
// should move the feedrate scaling to scara inverse_kinematics
inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
// should move the feedrate scaling to scara inverse_kinematics
const float adiff = FABS(delta[A_AXIS] - scara_oldA),
bdiff = FABS(delta[B_AXIS] - scara_oldB);
@ -506,11 +493,11 @@
scara_oldB = delta[B_AXIS];
float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], e, s_feedrate, active_extruder);
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN
planner._buffer_line(rx, ry, rz, e, fr, active_extruder);
planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
#endif
@ -528,12 +515,14 @@
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
const float tot_dx = rtarget[X_AXIS] - current_position[X_AXIS],
tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS],
tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS],
tot_de = rtarget[E_AXIS] - current_position[E_AXIS];
const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS],
rtarget[Z_AXIS] - current_position[Z_AXIS],
rtarget[E_AXIS] - current_position[E_AXIS]
};
const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy); // total horizontal xy distance
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
@ -553,41 +542,30 @@
scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif
const float seg_dx = tot_dx * inv_segments,
seg_dy = tot_dy * inv_segments,
seg_dz = tot_dz * inv_segments,
seg_de = tot_de * inv_segments;
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 seg_rx = current_position[X_AXIS],
seg_ry = current_position[Y_AXIS],
seg_rz = current_position[Z_AXIS],
seg_le = current_position[E_AXIS];
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
do {
if (--segments) { // not the last segment
seg_rx += seg_dx;
seg_ry += seg_dy;
seg_rz += seg_dz;
seg_le += seg_de;
} else { // last segment, use exact destination
seg_rx = rtarget[X_AXIS];
seg_ry = rtarget[Y_AXIS];
seg_rz = rtarget[Z_AXIS];
seg_le = rtarget[E_AXIS];
}
ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz, seg_le, feedrate);
} while (segments);
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();
}
@ -598,10 +576,7 @@
#endif
// increment to first segment destination
seg_rx += seg_dx;
seg_ry += seg_dy;
seg_rz += seg_dz;
seg_le += seg_de;
LOOP_XYZE(i) raw[i] += diff[i];
for(;;) { // for each mesh cell encountered during the move
@ -612,8 +587,8 @@
// in top of loop and again re-find same adjacent cell and use it, just less efficient
// for mesh inset area.
int8_t cell_xi = (seg_rx - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
cell_yi = (seg_ry - (MESH_MIN_Y)) * (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_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
@ -631,8 +606,8 @@
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
float cx = seg_rx - x0, // cell-relative x and y
cy = seg_ry - y0;
float cx = raw[X_AXIS] - x0, // cell-relative x and y
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)
z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
@ -650,40 +625,34 @@
// 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.
const float z_sxy0 = z_xmy0 * seg_dx, // per-segment adjustment to z_cxy0
z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx; // per-segment adjustment to z_cxym
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
for(;;) { // for all segments within this mesh cell
float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy
if (--segments == 0) // if this is last segment, use rtarget for exact
COPY(raw, rtarget);
float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height
#endif
if (--segments == 0) { // if this is last segment, use rtarget for exact
seg_rx = rtarget[X_AXIS];
seg_ry = rtarget[Y_AXIS];
seg_rz = rtarget[Z_AXIS];
seg_le = rtarget[E_AXIS];
}
ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate);
const float z = raw[Z_AXIS];
raw[Z_AXIS] += z_cxcy;
ubl_buffer_segment_raw(raw, feedrate);
raw[Z_AXIS] = z;
if (segments == 0) // done with last segment
return false; // did not set_current_from_destination()
seg_rx += seg_dx;
seg_ry += seg_dy;
seg_rz += seg_dz;
seg_le += seg_de;
LOOP_XYZE(i) raw[i] += diff[i];
cx += seg_dx;
cy += seg_dy;
cx += diff[X_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;
}
// Next segment still within same mesh cell, adjust the per-segment
// slope and intercept to compute next z height.

11
Marlin/src/module/motion.cpp
View File

@ -587,12 +587,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
float raw[XYZE];
COPY(raw, current_position);
// Drop one segment so the last move is to the exact target.
// If there's only 1 segment, loops will be skipped entirely.
--segments;
// Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) {
while (--segments) {
static millis_t next_idle_ms = millis() + 200UL;
thermalManager.manage_heater(); // This returns immediately if not really needed.
@ -691,16 +688,12 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOLNPAIR(" segments=", segments);
// Drop one segment so the last move is to the exact target.
// If there's only 1 segment, loops will be skipped entirely.
--segments;
// Get the raw current position as starting point
float raw[XYZE];
COPY(raw, current_position);
// Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) {
while (--segments) {
static millis_t next_idle_ms = millis() + 200UL;
thermalManager.manage_heater(); // This returns immediately if not really needed.
if (ELAPSED(millis(), next_idle_ms)) {

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

@ -505,8 +505,8 @@ class Planner {
/**
* Get the index of the next / previous block in the ring buffer
*/
static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); }
static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); }
static int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); }
static int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
/**
* Calculate the distance (not time) it takes to accelerate

3
Marlin/src/module/stepper.cpp
View File

@ -409,8 +409,7 @@ void Stepper::isr() {
// If there is no current block, attempt to pop one from the buffer
if (!current_block) {
// Anything in the buffer?
current_block = planner.get_current_block();
if (current_block) {
if ((current_block = planner.get_current_block())) {
trapezoid_generator_reset();
// Initialize Bresenham counters to 1/2 the ceiling

Write Preview
Loading…
Cancel
Save