Comment/cleanup of motion code

This commit is contained in:
Scott Lahteine 2017-12-01 21:43:44 -06:00
parent 046a1ad331
commit 000b3b3117
6 changed files with 78 additions and 105 deletions

View file

@ -366,6 +366,7 @@ float bilinear_z_offset(const float raw[XYZ]) {
* splitting the move where it crosses grid borders. * 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) { 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]), int cx1 = CELL_INDEX(X, current_position[X_AXIS]),
cy1 = CELL_INDEX(Y, current_position[Y_AXIS]), cy1 = CELL_INDEX(Y, current_position[Y_AXIS]),
cx2 = CELL_INDEX(X, destination[X_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); cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2);
cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 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) { if (cx1 == cx2 && cy1 == cy2) {
// Start and end on same mesh square
buffer_line_to_destination(fr_mm_s); buffer_line_to_destination(fr_mm_s);
set_current_from_destination(); set_current_from_destination();
return; 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) #define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
float normalized_dist, end[XYZE]; 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); 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)) { if (cx2 != cx1 && TEST(x_splits, gcx)) {
// Split on the X grid line
CBI(x_splits, gcx);
COPY(end, destination); COPY(end, destination);
destination[X_AXIS] = bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx; 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]); normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = LINE_SEGMENT_END(Y); 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)) { else if (cy2 != cy1 && TEST(y_splits, gcy)) {
// Split on the Y grid line
CBI(y_splits, gcy);
COPY(end, destination); COPY(end, destination);
destination[Y_AXIS] = bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy; 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]); normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = LINE_SEGMENT_END(X); destination[X_AXIS] = LINE_SEGMENT_END(X);
CBI(y_splits, gcy);
} }
else { 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); buffer_line_to_destination(fr_mm_s);
set_current_from_destination(); set_current_from_destination();
return; return;

View file

@ -59,6 +59,7 @@
* splitting the move where it crosses mesh borders. * 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) { 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]), int cx1 = mbl.cell_index_x(current_position[X_AXIS]),
cy1 = mbl.cell_index_y(current_position[Y_AXIS]), cy1 = mbl.cell_index_y(current_position[Y_AXIS]),
cx2 = mbl.cell_index_x(destination[X_AXIS]), cx2 = mbl.cell_index_x(destination[X_AXIS]),
@ -68,8 +69,8 @@
NOMORE(cx2, GRID_MAX_POINTS_X - 2); NOMORE(cx2, GRID_MAX_POINTS_X - 2);
NOMORE(cy2, GRID_MAX_POINTS_Y - 2); NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
// Start and end in the same cell? No split needed.
if (cx1 == cx2 && cy1 == cy2) { if (cx1 == cx2 && cy1 == cy2) {
// Start and end on same mesh square
buffer_line_to_destination(fr_mm_s); buffer_line_to_destination(fr_mm_s);
set_current_from_destination(); set_current_from_destination();
return; return;
@ -78,25 +79,30 @@
#define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist) #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
float normalized_dist, end[XYZE]; 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); 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)) { if (cx2 != cx1 && TEST(x_splits, gcx)) {
// Split on the X grid line
CBI(x_splits, gcx);
COPY(end, destination); COPY(end, destination);
destination[X_AXIS] = mbl.index_to_xpos[gcx]; destination[X_AXIS] = mbl.index_to_xpos[gcx];
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = MBL_SEGMENT_END(Y); 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)) { else if (cy2 != cy1 && TEST(y_splits, gcy)) {
// Split on the Y grid line
CBI(y_splits, gcy);
COPY(end, destination); COPY(end, destination);
destination[Y_AXIS] = mbl.index_to_ypos[gcy]; destination[Y_AXIS] = mbl.index_to_ypos[gcy];
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = MBL_SEGMENT_END(X); destination[X_AXIS] = MBL_SEGMENT_END(X);
CBI(y_splits, gcy);
} }
else { 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); buffer_line_to_destination(fr_mm_s);
set_current_from_destination(); set_current_from_destination();
return; return;

View file

@ -475,30 +475,17 @@
// 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_line directly here. Per-segmented leveling and kinematics performed first. // 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 #if ENABLED(DELTA) // apply delta inverse_kinematics
const float delta_A = rz + SQRT( delta_diagonal_rod_2_tower[A_AXIS] DELTA_RAW_IK();
- HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx, planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
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);
#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)
const float lseg[XYZ] = { rx, ry, rz }; inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
// should move the feedrate scaling to scara inverse_kinematics
inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
// should move the feedrate scaling to scara inverse_kinematics
const float adiff = FABS(delta[A_AXIS] - scara_oldA), const float adiff = FABS(delta[A_AXIS] - scara_oldA),
bdiff = FABS(delta[B_AXIS] - scara_oldB); bdiff = FABS(delta[B_AXIS] - scara_oldB);
@ -506,11 +493,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_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 #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 #endif
@ -528,12 +515,14 @@
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
const float tot_dx = rtarget[X_AXIS] - current_position[X_AXIS], const float total[XYZE] = {
tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS], rtarget[X_AXIS] - current_position[X_AXIS],
tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS], rtarget[Y_AXIS] - current_position[Y_AXIS],
tot_de = rtarget[E_AXIS] - current_position[E_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 #if IS_KINEMATIC
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate 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); scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif #endif
const float seg_dx = tot_dx * inv_segments, const float diff[XYZE] = {
seg_dy = tot_dy * inv_segments, total[X_AXIS] * inv_segments,
seg_dz = tot_dz * inv_segments, total[Y_AXIS] * inv_segments,
seg_de = tot_de * inv_segments; total[Z_AXIS] * inv_segments,
total[E_AXIS] * inv_segments
};
// Note that E segment distance could vary slightly as z mesh height // Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore. // changes for each segment, but small enough to ignore.
float seg_rx = current_position[X_AXIS], float raw[XYZE] = {
seg_ry = current_position[Y_AXIS], current_position[X_AXIS],
seg_rz = current_position[Z_AXIS], current_position[Y_AXIS],
seg_le = current_position[E_AXIS]; current_position[Z_AXIS],
current_position[E_AXIS]
};
// Only compute leveling per segment if ubl active and target below z_fade_height. // 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 if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
do { LOOP_XYZE(i) raw[i] += diff[i];
ubl_buffer_segment_raw(raw, feedrate);
if (--segments) { // not the last segment }
seg_rx += seg_dx; ubl_buffer_segment_raw(rtarget, feedrate);
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);
return false; // moved but did not set_current_from_destination(); return false; // moved but did not set_current_from_destination();
} }
@ -598,10 +576,7 @@
#endif #endif
// increment to first segment destination // increment to first segment destination
seg_rx += seg_dx; LOOP_XYZE(i) raw[i] += diff[i];
seg_ry += seg_dy;
seg_rz += seg_dz;
seg_le += seg_de;
for(;;) { // for each mesh cell encountered during the move 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 // 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 = (seg_rx - (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 = (seg_ry - (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);
@ -631,8 +606,8 @@
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 = seg_rx - x0, // cell-relative x and y float cx = raw[X_AXIS] - x0, // cell-relative x and y
cy = seg_ry - 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)
@ -650,40 +625,34 @@
// 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 * seg_dx, // 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)) * seg_dx; // 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
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) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height
#endif #endif
if (--segments == 0) { // if this is last segment, use rtarget for exact const float z = raw[Z_AXIS];
seg_rx = rtarget[X_AXIS]; raw[Z_AXIS] += z_cxcy;
seg_ry = rtarget[Y_AXIS]; ubl_buffer_segment_raw(raw, feedrate);
seg_rz = rtarget[Z_AXIS]; raw[Z_AXIS] = z;
seg_le = rtarget[E_AXIS];
}
ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate);
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()
seg_rx += seg_dx; LOOP_XYZE(i) raw[i] += diff[i];
seg_ry += seg_dy;
seg_rz += seg_dz;
seg_le += seg_de;
cx += seg_dx; cx += diff[X_AXIS];
cy += seg_dy; 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.

View file

@ -587,12 +587,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
float raw[XYZE]; float raw[XYZE];
COPY(raw, current_position); 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 // Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) { while (--segments) {
static millis_t next_idle_ms = millis() + 200UL; static millis_t next_idle_ms = millis() + 200UL;
thermalManager.manage_heater(); // This returns immediately if not really needed. 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_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOLNPAIR(" segments=", segments); // 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 // Get the raw current position as starting point
float raw[XYZE]; float raw[XYZE];
COPY(raw, current_position); COPY(raw, current_position);
// Calculate and execute the segments // Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) { while (--segments) {
static millis_t next_idle_ms = millis() + 200UL; static millis_t next_idle_ms = millis() + 200UL;
thermalManager.manage_heater(); // This returns immediately if not really needed. thermalManager.manage_heater(); // This returns immediately if not really needed.
if (ELAPSED(millis(), next_idle_ms)) { if (ELAPSED(millis(), next_idle_ms)) {

View file

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

View file

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