Consolidate "bedlevel" code
This commit is contained in:
123
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
Normal file
123
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
Normal file
@ -0,0 +1,123 @@
|
||||
/**
|
||||
* Marlin 3D Printer Firmware
|
||||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
||||
*
|
||||
* Based on Sprinter and grbl.
|
||||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "../../../inc/MarlinConfig.h"
|
||||
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
|
||||
#include "mesh_bed_leveling.h"
|
||||
|
||||
#include "../../../module/motion.h"
|
||||
#include "../../../feature/bedlevel/bedlevel.h"
|
||||
|
||||
mesh_bed_leveling mbl;
|
||||
|
||||
uint8_t mesh_bed_leveling::status;
|
||||
|
||||
float mesh_bed_leveling::z_offset,
|
||||
mesh_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
|
||||
mesh_bed_leveling::index_to_xpos[GRID_MAX_POINTS_X],
|
||||
mesh_bed_leveling::index_to_ypos[GRID_MAX_POINTS_Y];
|
||||
|
||||
mesh_bed_leveling::mesh_bed_leveling() {
|
||||
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; ++i)
|
||||
index_to_xpos[i] = MESH_MIN_X + i * (MESH_X_DIST);
|
||||
for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; ++i)
|
||||
index_to_ypos[i] = MESH_MIN_Y + i * (MESH_Y_DIST);
|
||||
reset();
|
||||
}
|
||||
|
||||
void mesh_bed_leveling::reset() {
|
||||
status = MBL_STATUS_NONE;
|
||||
z_offset = 0;
|
||||
ZERO(z_values);
|
||||
}
|
||||
|
||||
/**
|
||||
* Prepare a mesh-leveled linear move in a Cartesian setup,
|
||||
* 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) {
|
||||
int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X)),
|
||||
cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y)),
|
||||
cx2 = mbl.cell_index_x(RAW_X_POSITION(destination[X_AXIS])),
|
||||
cy2 = mbl.cell_index_y(RAW_Y_POSITION(destination[Y_AXIS]));
|
||||
NOMORE(cx1, GRID_MAX_POINTS_X - 2);
|
||||
NOMORE(cy1, GRID_MAX_POINTS_Y - 2);
|
||||
NOMORE(cx2, GRID_MAX_POINTS_X - 2);
|
||||
NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
|
||||
|
||||
if (cx1 == cx2 && cy1 == cy2) {
|
||||
// Start and end on same mesh square
|
||||
line_to_destination(fr_mm_s);
|
||||
set_current_to_destination();
|
||||
return;
|
||||
}
|
||||
|
||||
#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);
|
||||
if (cx2 != cx1 && TEST(x_splits, gcx)) {
|
||||
COPY(end, destination);
|
||||
destination[X_AXIS] = LOGICAL_X_POSITION(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);
|
||||
}
|
||||
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
|
||||
COPY(end, destination);
|
||||
destination[Y_AXIS] = LOGICAL_Y_POSITION(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
|
||||
line_to_destination(fr_mm_s);
|
||||
set_current_to_destination();
|
||||
return;
|
||||
}
|
||||
|
||||
destination[Z_AXIS] = MBL_SEGMENT_END(Z);
|
||||
destination[E_AXIS] = MBL_SEGMENT_END(E);
|
||||
|
||||
// Do the split and look for more borders
|
||||
mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
|
||||
|
||||
// Restore destination from stack
|
||||
COPY(destination, end);
|
||||
mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
|
||||
}
|
||||
|
||||
void mbl_mesh_report() {
|
||||
SERIAL_PROTOCOLLNPGM("Num X,Y: " STRINGIFY(GRID_MAX_POINTS_X) "," STRINGIFY(GRID_MAX_POINTS_Y));
|
||||
SERIAL_PROTOCOLPGM("Z offset: "); SERIAL_PROTOCOL_F(mbl.z_offset, 5);
|
||||
SERIAL_PROTOCOLLNPGM("\nMeasured points:");
|
||||
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 5,
|
||||
[](const uint8_t ix, const uint8_t iy) { return mbl.z_values[ix][iy]; }
|
||||
);
|
||||
}
|
||||
|
||||
#endif // MESH_BED_LEVELING
|
129
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
Normal file
129
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
Normal file
@ -0,0 +1,129 @@
|
||||
/**
|
||||
* Marlin 3D Printer Firmware
|
||||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
||||
*
|
||||
* Based on Sprinter and grbl.
|
||||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _MESH_BED_LEVELING_H_
|
||||
#define _MESH_BED_LEVELING_H_
|
||||
|
||||
#include "../../../inc/MarlinConfig.h"
|
||||
|
||||
enum MeshLevelingState {
|
||||
MeshReport,
|
||||
MeshStart,
|
||||
MeshNext,
|
||||
MeshSet,
|
||||
MeshSetZOffset,
|
||||
MeshReset
|
||||
};
|
||||
|
||||
enum MBLStatus {
|
||||
MBL_STATUS_NONE = 0,
|
||||
MBL_STATUS_HAS_MESH_BIT = 0,
|
||||
MBL_STATUS_ACTIVE_BIT = 1
|
||||
};
|
||||
|
||||
#define MESH_X_DIST ((MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_POINTS_X - 1))
|
||||
#define MESH_Y_DIST ((MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_POINTS_Y - 1))
|
||||
|
||||
class mesh_bed_leveling {
|
||||
public:
|
||||
static uint8_t status; // Has Mesh and Is Active bits
|
||||
static float z_offset,
|
||||
z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
|
||||
index_to_xpos[GRID_MAX_POINTS_X],
|
||||
index_to_ypos[GRID_MAX_POINTS_Y];
|
||||
|
||||
mesh_bed_leveling();
|
||||
|
||||
static void reset();
|
||||
|
||||
static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
|
||||
|
||||
static bool active() { return TEST(status, MBL_STATUS_ACTIVE_BIT); }
|
||||
static void set_active(const bool onOff) { onOff ? SBI(status, MBL_STATUS_ACTIVE_BIT) : CBI(status, MBL_STATUS_ACTIVE_BIT); }
|
||||
static bool has_mesh() { return TEST(status, MBL_STATUS_HAS_MESH_BIT); }
|
||||
static void set_has_mesh(const bool onOff) { onOff ? SBI(status, MBL_STATUS_HAS_MESH_BIT) : CBI(status, MBL_STATUS_HAS_MESH_BIT); }
|
||||
|
||||
static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) {
|
||||
px = index % (GRID_MAX_POINTS_X);
|
||||
py = index / (GRID_MAX_POINTS_X);
|
||||
if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag
|
||||
}
|
||||
|
||||
static void set_zigzag_z(const int8_t index, const float &z) {
|
||||
int8_t px, py;
|
||||
zigzag(index, px, py);
|
||||
set_z(px, py, z);
|
||||
}
|
||||
|
||||
static int8_t cell_index_x(const float &x) {
|
||||
int8_t cx = (x - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
|
||||
return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2);
|
||||
}
|
||||
|
||||
static int8_t cell_index_y(const float &y) {
|
||||
int8_t cy = (y - (MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
|
||||
return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2);
|
||||
}
|
||||
|
||||
static int8_t probe_index_x(const float &x) {
|
||||
int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST));
|
||||
return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
|
||||
}
|
||||
|
||||
static int8_t probe_index_y(const float &y) {
|
||||
int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST));
|
||||
return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
|
||||
}
|
||||
|
||||
static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
|
||||
const float delta_z = (z2 - z1) / (a2 - a1),
|
||||
delta_a = a0 - a1;
|
||||
return z1 + delta_a * delta_z;
|
||||
}
|
||||
|
||||
static float get_z(const float &x0, const float &y0
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
, const float &factor
|
||||
#endif
|
||||
) {
|
||||
const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0);
|
||||
const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy], index_to_xpos[cx + 1], z_values[cx + 1][cy]),
|
||||
z2 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy + 1], index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]),
|
||||
z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2);
|
||||
|
||||
return z_offset + z0
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
* factor
|
||||
#endif
|
||||
;
|
||||
}
|
||||
};
|
||||
|
||||
extern mesh_bed_leveling mbl;
|
||||
|
||||
// Support functions, which may be embedded in the class later
|
||||
|
||||
void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF);
|
||||
|
||||
void mbl_mesh_report();
|
||||
|
||||
#endif // _MESH_BED_LEVELING_H_
|
Reference in New Issue
Block a user