josh
/
Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Planning move in mesh rewritten.

This commit is contained in:
michalprusa 2016-03-25 15:56:51 +01:00
parent 7f9e947954
commit 4263792793
1 changed files with 52 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
Firmware/Marlin_main.cpp
View File

@ -4624,75 +4624,63 @@ void calculate_delta(float cartesian[3])
#ifdef MESH_BED_LEVELING #ifdef MESH_BED_LEVELING
// This function is used to split lines on mesh borders so each segment is only part of one mesh area // This function is used to split lines on mesh borders so each segment is only part of one mesh area
void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t& extruder, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) { #define X_MID_POS (0.5f*(X_MIN_POS+X_MAX_POS))
if (!mbl.active) { #define Y_MID_POS (0.5f*(Y_MIN_POS+Y_MAX_POS))
plan_buffer_line(x, y, z, e, feed_rate, extruder);
set_current_to_destination(); void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t& extruder) {
return; int tileDiff = 0;
if (mbl.active) {
// In which of the four tiles the start and the end points fall?
// tileDiff is a bitmask,
// 1st bit indicates a crossing of the tile boundary in the X axis,
// 2nd bit indicates a crossing of the tile boundary in the Y axis.
tileDiff =
((current_position[X_AXIS] > X_MID_POS) | ((current_position[Y_AXIS] > Y_MID_POS) << 1)) ^
((x > X_MID_POS) | ((y > Y_MID_POS) << 1));
} }
int pix = mbl.select_x_index(current_position[X_AXIS]);
int piy = mbl.select_y_index(current_position[Y_AXIS]); // Normalized parameters for the crossing of the tile boundary.
int ix = mbl.select_x_index(x); float s1, s2;
int iy = mbl.select_y_index(y); // Interpolate a linear movement at the tile boundary, in X, Y and E axes.
pix = min(pix, MESH_NUM_X_POINTS - 2); #define INRPL_X(u) (current_position[X_AXIS] + (x - current_position[X_AXIS]) * u)
piy = min(piy, MESH_NUM_Y_POINTS - 2); #define INRPL_Y(u) (current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * u)
ix = min(ix, MESH_NUM_X_POINTS - 2); #define INRPL_E(u) (current_position[E_AXIS] + (e - current_position[E_AXIS]) * u)
iy = min(iy, MESH_NUM_Y_POINTS - 2); switch (tileDiff) {
if (pix == ix && piy == iy) { case 0:
// Start and end on same mesh square // The start and end points on same tile, or the mesh bed leveling is not active.
plan_buffer_line(x, y, z, e, feed_rate, extruder); break;
set_current_to_destination(); case 1:
return; // The start and end tiles differ in X only.
s1 = (X_MID_POS - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
plan_buffer_line(X_MID_POS, INRPL_Y(s1), z, INRPL_E(s1), feed_rate, extruder);
break;
case 2:
// The start and end tiles differ in Y only.
s1 = (Y_MID_POS - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
plan_buffer_line(INRPL_X(s1), Y_MID_POS, z, INRPL_E(s1), feed_rate, extruder);
break;
default:
// The start and end tiles differ in both coordinates.
// assert(tileDiff == 3);
s1 = (X_MID_POS - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
s2 = (Y_MID_POS - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
if (s1 < s2) {
plan_buffer_line(X_MID_POS, INRPL_Y(s1), z, INRPL_E(s1), feed_rate, extruder);
plan_buffer_line(INRPL_X(s2), Y_MID_POS, z, INRPL_E(s2), feed_rate, extruder);
} else {
plan_buffer_line(INRPL_X(s1), Y_MID_POS, z, INRPL_E(s1), feed_rate, extruder);
plan_buffer_line(X_MID_POS, INRPL_Y(s2), z, INRPL_E(s2), feed_rate, extruder);
}
} }
float nx, ny, ne, normalized_dist; #undef INRPL_X
if (ix > pix && (x_splits) & BIT(ix)) { #undef INRPL_Y
nx = mbl.get_x(ix); #undef INRPL_E
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]); // The rest of the path.
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist; plan_buffer_line(x, y, z, e, feed_rate, extruder);
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist; set_current_to_destination();
x_splits ^= BIT(ix);
}
else if (ix < pix && (x_splits) & BIT(pix)) {
nx = mbl.get_x(pix);
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
x_splits ^= BIT(pix);
}
else if (iy > piy && (y_splits) & BIT(iy)) {
ny = mbl.get_y(iy);
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
y_splits ^= BIT(iy);
}
else if (iy < piy && (y_splits) & BIT(piy)) {
ny = mbl.get_y(piy);
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
y_splits ^= BIT(piy);
}
else {
// Already split on a border
plan_buffer_line(x, y, z, e, feed_rate, extruder);
set_current_to_destination();
return;
}
// Do the split and look for more borders
destination[X_AXIS] = nx;
destination[Y_AXIS] = ny;
destination[E_AXIS] = ne;
mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
destination[X_AXIS] = x;
destination[Y_AXIS] = y;
destination[E_AXIS] = e;
mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
} }
#endif // MESH_BED_LEVELING #endif // MESH_BED_LEVELING
void prepare_move() void prepare_move()
{ {
clamp_to_software_endstops(destination); clamp_to_software_endstops(destination);