WIP. Adding bed leveling code.

This commit is contained in:
Edward Patel 2015-03-15 10:43:26 +01:00
parent d0d12962e0
commit 0e51e53813
6 changed files with 184 additions and 13 deletions

View file

@ -372,6 +372,22 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
//const bool FIL_RUNOUT_INVERTING = true; // Should be uncommented and true or false should assigned
//#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
//===========================================================================
//============================ Manual Bed Leveling ==========================
//===========================================================================
#define MANUAL_BED_LEVELING // Add display menu option for bed leveling
#define MESH_BED_LEVELING // Enable mesh bed leveling
#if defined(MESH_BED_LEVELING)
#define MESH_MIN_X 10
#define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
#define MESH_MIN_Y 10
#define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
#define MESH_NUM_X_POINTS 4
#define MESH_NUM_Y_POINTS 3
#endif // MESH_BED_LEVELING
//===========================================================================
//============================= Bed Auto Leveling ===========================
//===========================================================================

View file

@ -41,6 +41,10 @@
#define SERVO_LEVELING defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
#if defined(MESH_BED_LEVELING)
#include "mesh_bed_leveling.h"
#endif // MESH_BED_LEVELING
#include "ultralcd.h"
#include "planner.h"
#include "stepper.h"
@ -4987,6 +4991,65 @@ void calculate_delta(float cartesian[3])
}
#endif
#if defined(MESH_BED_LEVELING)
#if !defined(MIN)
#define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
#endif // ! MIN
// 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)
{
int pix = mbl.select_x_index(current_position[X_AXIS]);
int piy = mbl.select_y_index(current_position[Y_AXIS]);
int ix = mbl.select_x_index(x);
int iy = mbl.select_y_index(y);
pix = MIN(pix, MESH_NUM_X_POINTS-2);
piy = MIN(piy, MESH_NUM_Y_POINTS-2);
ix = MIN(ix, MESH_NUM_X_POINTS-2);
iy = MIN(iy, MESH_NUM_Y_POINTS-2);
if (ix > pix && (x_splits)&(1<<ix)) {
float nx = mbl.get_x(ix);
float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
float ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
x_splits ^= 1 << ix;
mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
return;
} else if (ix < pix && (x_splits)&(1<<pix)) {
float nx = mbl.get_x(pix);
float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
float ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
x_splits ^= 1 << pix;
mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
return;
} else if (iy > piy && (y_splits)&(1<<iy)) {
float ny = mbl.get_y(iy);
float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
float nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
y_splits ^= 1 << iy;
mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
return;
} else if (iy < piy && (y_splits)&(1<<piy)) {
float ny = mbl.get_y(piy);
float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
float nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
y_splits ^= 1 << piy;
mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
return;
}
plan_buffer_line(x, y, z, e, feedrate, extruder);
for(int8_t i=0; i < NUM_AXIS; i++) {
current_position[i] = destination[i];
}
}
#endif // MESH_BED_LEVELING
void prepare_move()
{
clamp_to_software_endstops(destination);
@ -5102,10 +5165,14 @@ for (int s = 1; s <= steps; s++) {
#if ! (defined DELTA || defined SCARA)
// Do not use feedmultiply for E or Z only moves
if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
}
else {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
} else {
#if defined(MESH_BED_LEVELING)
mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
return;
#else
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
#endif // MESH_BED_LEVELING
}
#endif // !(DELTA || SCARA)

View file

@ -0,0 +1,7 @@
#include "mesh_bed_leveling.h"
#if defined(MESH_BED_LEVELING)
mesh_bed_leveling mbl;
#endif // MESH_BED_LEVELING

View file

@ -0,0 +1,69 @@
#include "Marlin.h"
#if defined(MESH_BED_LEVELING)
#define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
#define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1))
class mesh_bed_leveling {
public:
float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
mesh_bed_leveling() {
reset();
}
void reset() {
for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
for (int x=0; x<MESH_NUM_X_POINTS; x++) {
z_values[y][x] = 0;
}
}
}
float get_x(int i) { return MESH_MIN_X + MESH_X_DIST*i; }
float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST*i; }
void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
int select_x_index(float x) {
int i = 1;
while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) {
i++;
}
return i-1;
}
int select_y_index(float y) {
int i = 1;
while (y > get_y(i) && i < MESH_NUM_Y_POINTS-1) {
i++;
}
return i-1;
}
float calc_z0(float a0, float a1, float z1, float a2, float z2) {
float delta_z = (z2 - z1)/(a2 - a1);
float delta_a = a0 - a1;
return z1 + delta_a * delta_z;
}
float get_z(float x0, float y0) {
int x_index = select_x_index(x0);
int y_index = select_y_index(y0);
float z1 = calc_z0(x0,
get_x(x_index), z_values[y_index][x_index],
get_x(x_index+1), z_values[y_index][x_index+1]);
float z2 = calc_z0(x0,
get_x(x_index), z_values[y_index+1][x_index],
get_x(x_index+1), z_values[y_index+1][x_index+1]);
float z0 = calc_z0(y0,
get_y(y_index), z1,
get_y(y_index+1), z2);
return z0;
}
};
extern mesh_bed_leveling mbl;
#endif // MESH_BED_LEVELING

View file

@ -58,6 +58,10 @@
#include "ultralcd.h"
#include "language.h"
#if defined(MESH_BED_LEVELING)
#include "mesh_bed_leveling.h"
#endif // MESH_BED_LEVELING
//===========================================================================
//============================= public variables ============================
//===========================================================================
@ -530,7 +534,7 @@ float junction_deviation = 0.1;
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
#ifdef ENABLE_AUTO_BED_LEVELING
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder)
#else
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
@ -548,6 +552,10 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
lcd_update();
}
#if defined(MESH_BED_LEVELING)
z += mbl.get_z(x, y);
#endif // MESH_BED_LEVELING
#ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // ENABLE_AUTO_BED_LEVELING
@ -1078,14 +1086,17 @@ vector_3 plan_get_position() {
}
#endif // ENABLE_AUTO_BED_LEVELING
#ifdef ENABLE_AUTO_BED_LEVELING
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float &e)
{
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#else
void plan_set_position(const float &x, const float &y, const float &z, const float &e)
#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
{
#endif // ENABLE_AUTO_BED_LEVELING
#if defined(ENABLE_AUTO_BED_LEVELING)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#elif defined(MESH_BED_LEVELING)
z += mbl.get_z(x, y);
#endif // ENABLE_AUTO_BED_LEVELING
position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);

View file

@ -82,21 +82,22 @@ void plan_init();
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimaters. Feed rate specifies the speed of the motion.
#ifdef ENABLE_AUTO_BED_LEVELING
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
// Get the position applying the bed level matrix if enabled
#if defined(ENABLE_AUTO_BED_LEVELING)
vector_3 plan_get_position();
#endif // ENABLE_AUTO_BED_LEVELING
#else
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
#endif // ENABLE_AUTO_BED_LEVELING
#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
// Set position. Used for G92 instructions.
#ifdef ENABLE_AUTO_BED_LEVELING
#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
void plan_set_position(float x, float y, float z, const float &e);
#else
void plan_set_position(const float &x, const float &y, const float &z, const float &e);
#endif // ENABLE_AUTO_BED_LEVELING
#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
void plan_set_e_position(const float &e);