Add dynamic 3-point bed-leveling support
Adds G29 commands to register bed level points. When three points are registered, the plane of the bed is calculated and dynamic bed leveling takes effect. Add a warning if bed-leveling is enabled when MAX_JERK_Z is zero. In this case lookahead will always fail when bed-leveling is active since the Z-axis is not allowed to move during lookahead.
This commit is contained in:
Phil Hord
parent
47dfcf1a44
commit
77d8583cca
|
|
@ -0,0 +1,199 @@
|
|||
/** \file
|
||||
\brief Dynamic Z-height compensation for out-of-level print bed.
|
||||
*/
|
||||
|
||||
#include "bed_leveling.h"
|
||||
|
||||
#ifdef BED_LEVELING
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "dda_maths.h"
|
||||
#include "debug.h"
|
||||
#include "sersendf.h"
|
||||
#include "config_wrapper.h"
|
||||
|
||||
struct point {
|
||||
int32_t x, y, z;
|
||||
};
|
||||
|
||||
// Accept exactly three points for bed leveling
|
||||
static uint8_t level_index = 0;
|
||||
static struct point bed_level_map[3];
|
||||
|
||||
// Alias the three points
|
||||
static struct point const * const P = &bed_level_map[0];
|
||||
static struct point const * const Q = &bed_level_map[1];
|
||||
static struct point const * const R = &bed_level_map[2];
|
||||
|
||||
int bed_plane_calculate(void);
|
||||
|
||||
// Reset the bed level values to "unknown"
|
||||
void bed_level_reset() {
|
||||
level_index = 0;
|
||||
}
|
||||
|
||||
// Scale x and y down to tenths of mm to keep math from overflowing
|
||||
#define SCALE 100
|
||||
static int32_t scale_to_dum(int32_t a) {
|
||||
if (a<0) return (a-SCALE/2)/SCALE;
|
||||
return (a+SCALE/2)/SCALE;
|
||||
}
|
||||
|
||||
// Register a point on the bed plane; coordinates in um
|
||||
void bed_level_register(int32_t x, int32_t y, int32_t z) {
|
||||
// Scale x and y to tenths of mm; keep z in um
|
||||
x = scale_to_dum(x);
|
||||
y = scale_to_dum(y);
|
||||
|
||||
// Find a previous registered point at the same location or use a new location
|
||||
struct point * p = bed_level_map;
|
||||
int i = 0;
|
||||
for (; i < level_index; i++, p++) {
|
||||
if (p->x == x && p->y == y)
|
||||
break;
|
||||
}
|
||||
|
||||
// We can only handle three points
|
||||
if (i >= 3)
|
||||
return;
|
||||
|
||||
p->x = x;
|
||||
p->y = y;
|
||||
p->z = z;
|
||||
|
||||
// Bump the index if we just used a new location
|
||||
if (i == level_index)
|
||||
++level_index;
|
||||
|
||||
// Nothing more to do until we have three points
|
||||
if (level_index < 3)
|
||||
return;
|
||||
|
||||
// We have three points. Try to calculate the plane of the bed.
|
||||
if (!bed_plane_calculate())
|
||||
--level_index;
|
||||
}
|
||||
|
||||
// Pre-scaled coefficients of the planar equation,
|
||||
// Ax + By + Cz + K= 0
|
||||
//
|
||||
// When we have these coefficients, we're only going to use them relative to -1/C, so
|
||||
// Ac = -A/C; Bc = -B/C; Kc = 0 (because we translate a point to the origin)
|
||||
// f(x,y) = z = Ac*x + Bc*y + Kc
|
||||
static uint32_t Aq, Ar, Bq, Br, C;
|
||||
static int8_t Asign, Bsign;
|
||||
|
||||
int bed_leveling_active() {
|
||||
// No adjustment if not calibrated yet
|
||||
return level_index == 3;
|
||||
}
|
||||
|
||||
void bed_level_report() {
|
||||
sersendf_P(PSTR("Bed leveling status:"));
|
||||
if (!bed_leveling_active()) {
|
||||
sersendf_P(PSTR(" not"));
|
||||
}
|
||||
sersendf_P(PSTR(" active (%d) positions registered\n"), level_index);
|
||||
for (int i = 0; i < level_index; i++) {
|
||||
sersendf_P(PSTR(" %d: G29 S1 X%lq Y%lq Z%lq\n"),
|
||||
i+1, bed_level_map[i].x * SCALE, bed_level_map[i].y * SCALE, bed_level_map[i].z);
|
||||
}
|
||||
}
|
||||
|
||||
int32_t bed_level_adjustment(int32_t x, int32_t y) {
|
||||
int32_t za, zb;
|
||||
|
||||
// No adjustment if not calibrated yet
|
||||
if (!bed_leveling_active())
|
||||
return 0;
|
||||
|
||||
x = scale_to_dum(x);
|
||||
y = scale_to_dum(y);
|
||||
|
||||
x -= P->x;
|
||||
y -= P->y;
|
||||
|
||||
za = muldivQR(x, Aq, Ar, C);
|
||||
if (Asign)
|
||||
za = -za;
|
||||
|
||||
zb = muldivQR(y, Bq, Br, C);
|
||||
if (Bsign)
|
||||
zb = -zb;
|
||||
|
||||
return P->z - za - zb;
|
||||
}
|
||||
|
||||
|
||||
int bed_plane_calculate() {
|
||||
// Coefficients of the planar equation,
|
||||
// Ax + By + Cz + K = 0
|
||||
int32_t a, b, c;
|
||||
|
||||
// Draw two vectors on the plane, u = B-A and v = C-A
|
||||
int32_t Ui, Uj, Uk, Vi, Vj, Vk;
|
||||
|
||||
// U = vector(QP)
|
||||
Ui = Q->x - P->x;
|
||||
Uj = Q->y - P->y;
|
||||
Uk = Q->z - P->z;
|
||||
|
||||
// V = vector(RP)
|
||||
Vi = R->x - P->x;
|
||||
Vj = R->y - P->y;
|
||||
Vk = R->z - P->z;
|
||||
|
||||
// Find normal vector (a,b,c) = (U x V) and is perpendicular to the plane
|
||||
a = Uj*Vk - Uk*Vj;
|
||||
b = Uk*Vi - Ui*Vk;
|
||||
c = Ui*Vj - Uj*Vi;
|
||||
|
||||
// Notes:
|
||||
// * Ignore K (constant) by translating plane to pass through origin at P (0,0,0)
|
||||
// * if a==0 and b==0, the bed is already level; z-offset is still important
|
||||
// * if c==0 the bed is perpendicular or the points are colinear
|
||||
if (c == 0)
|
||||
return 0;
|
||||
|
||||
if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
|
||||
sersendf_P(PSTR("Coefficients: A:%ld B:%ld C:%ld\n"), a, b, c);
|
||||
|
||||
// muldiv requires positive parameters
|
||||
// remember the signs separately
|
||||
Asign = a < 0;
|
||||
Bsign = b < 0;
|
||||
if (Asign) a = -a;
|
||||
if (Bsign) b = -b;
|
||||
|
||||
// Combine A/C and B/C, so combine their signs, too
|
||||
if (c < 0) {
|
||||
c = -c;
|
||||
Asign = !Asign;
|
||||
Bsign = !Bsign;
|
||||
}
|
||||
|
||||
// Pre-calc the coefficients A/C and B/C
|
||||
Aq = a / c;
|
||||
Ar = a % c;
|
||||
Bq = b / c;
|
||||
Br = b % c;
|
||||
C = c;
|
||||
|
||||
int ret = 1;
|
||||
// Sanity check
|
||||
for (int i = 0; i < level_index; i++) {
|
||||
int32_t x=bed_level_map[i].x * SCALE, y=bed_level_map[i].y * SCALE;
|
||||
int32_t validation = bed_level_adjustment(x, y);
|
||||
if (labs(validation - bed_level_map[i].z) > 10) {
|
||||
sersendf_P(PSTR("!! Bed plane validation failed: Point %d: X:%lq Y:%lq Expected Z:%lq Calculated Z:%lq\n"),
|
||||
i+1, x, y, bed_level_map[i].z, validation);
|
||||
ret = 0; // invalidate results on error
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
#endif /* BED_LEVELING */
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#ifndef _BED_LEVELING_H
|
||||
#define _BED_LEVELING_H
|
||||
|
||||
#include "config_wrapper.h"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "dda.h"
|
||||
|
||||
#ifdef BED_LEVELING
|
||||
|
||||
// Clears all registered points and disables dynamic leveling
|
||||
void bed_level_reset(void);
|
||||
|
||||
// Returns true if bed leveling a plane is mapped and leveling is active
|
||||
int bed_leveling_active(void);
|
||||
|
||||
// Report information about bed leveling calculations
|
||||
void bed_level_report(void);
|
||||
|
||||
// Read the z-adjustment for the given x,y position
|
||||
int32_t bed_level_adjustment(int32_t x, int32_t y);
|
||||
|
||||
// Register a point as being "on the bed plane". Three points are required
|
||||
// to define a plane. After three non-colinear points are registered, the
|
||||
// adjustment is active and can be read from bed_level_adjustment.
|
||||
// Note: units for x, y and z are um but the three (X, Y) points should be
|
||||
// distinct enough in mm to define an accurate plane.
|
||||
void bed_level_register(int32_t x, int32_t y, int32_t z);
|
||||
|
||||
#endif /* BED_LEVELING */
|
||||
|
||||
static int32_t bed_level_offset(const axes_int32_t) __attribute__ ((always_inline));
|
||||
inline int32_t bed_level_offset(const axes_int32_t axis) {
|
||||
#ifdef BED_LEVELING
|
||||
return bed_level_adjustment(axis[X], axis[Y]);
|
||||
#else
|
||||
return 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
|
|||
*/
|
||||
#define MAX_JERK_X 200
|
||||
#define MAX_JERK_Y 200
|
||||
#define MAX_JERK_Z 0
|
||||
#define MAX_JERK_Z 20
|
||||
#define MAX_JERK_E 200
|
||||
|
||||
/** \def BED_LEVELING
|
||||
Define this to enable dynamic bed leveling using the G29 command and
|
||||
3-point planar bed mapping. Allows the printer to compensate dynamically
|
||||
for a print bed which is flat but is not quite level.
|
||||
Enabling bed-leveling requires about 2400 bytes of flash memory.
|
||||
*/
|
||||
//#define BED_LEVELING
|
||||
|
||||
|
||||
/***************************************************************************\
|
||||
* *
|
||||
|
|
|
|||
|
|
@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
|
|||
*/
|
||||
#define MAX_JERK_X 300
|
||||
#define MAX_JERK_Y 300
|
||||
#define MAX_JERK_Z 0
|
||||
#define MAX_JERK_Z 20
|
||||
#define MAX_JERK_E 300
|
||||
|
||||
/** \def BED_LEVELING
|
||||
Define this to enable dynamic bed leveling using the G29 command and
|
||||
3-point planar bed mapping. Allows the printer to compensate dynamically
|
||||
for a print bed which is flat but is not quite level.
|
||||
Enabling bed-leveling requires about 2400 bytes of flash memory.
|
||||
*/
|
||||
//#define BED_LEVELING
|
||||
|
||||
|
||||
/***************************************************************************\
|
||||
* *
|
||||
|
|
|
|||
|
|
@ -184,9 +184,17 @@ DEFINE_HOMING(z_negative, x_negative, y_negative, none)
|
|||
*/
|
||||
#define MAX_JERK_X 100
|
||||
#define MAX_JERK_Y 100
|
||||
#define MAX_JERK_Z 0
|
||||
#define MAX_JERK_Z 2
|
||||
#define MAX_JERK_E 200
|
||||
|
||||
/** \def BED_LEVELING
|
||||
Define this to enable dynamic bed leveling using the G29 command and
|
||||
3-point planar bed mapping. Allows the printer to compensate dynamically
|
||||
for a print bed which is flat but is not quite level.
|
||||
Enabling bed-leveling requires about 2400 bytes of flash memory.
|
||||
*/
|
||||
//#define BED_LEVELING
|
||||
|
||||
|
||||
/***************************************************************************\
|
||||
* *
|
||||
|
|
|
|||
|
|
@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
|
|||
*/
|
||||
#define MAX_JERK_X 200
|
||||
#define MAX_JERK_Y 200
|
||||
#define MAX_JERK_Z 0
|
||||
#define MAX_JERK_Z 20
|
||||
#define MAX_JERK_E 200
|
||||
|
||||
/** \def BED_LEVELING
|
||||
Define this to enable dynamic bed leveling using the G29 command and
|
||||
3-point planar bed mapping. Allows the printer to compensate dynamically
|
||||
for a print bed which is flat but is not quite level.
|
||||
Enabling bed-leveling requires about 2400 bytes of flash memory.
|
||||
*/
|
||||
//#define BED_LEVELING
|
||||
|
||||
|
||||
/***************************************************************************\
|
||||
* *
|
||||
|
|
|
|||
|
|
@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
|
|||
*/
|
||||
#define MAX_JERK_X 20
|
||||
#define MAX_JERK_Y 20
|
||||
#define MAX_JERK_Z 0
|
||||
#define MAX_JERK_Z 2
|
||||
#define MAX_JERK_E 200
|
||||
|
||||
/** \def BED_LEVELING
|
||||
Define this to enable dynamic bed leveling using the G29 command and
|
||||
3-point planar bed mapping. Allows the printer to compensate dynamically
|
||||
for a print bed which is flat but is not quite level.
|
||||
Enabling bed-leveling requires about 2400 bytes of flash memory.
|
||||
*/
|
||||
//#define BED_LEVELING
|
||||
|
||||
|
||||
/***************************************************************************\
|
||||
* *
|
||||
|
|
|
|||
|
|
@ -85,6 +85,15 @@
|
|||
#undef LOOKAHEAD
|
||||
#endif
|
||||
|
||||
/**
|
||||
LOOKAHEAD won't work if Z-jerk is zero and bed leveling is active
|
||||
because most moves will have Z-steps and lookahead will be skipped.
|
||||
*/
|
||||
#if defined BED_LEVELING && defined LOOKAHEAD && MAX_JERK_Z==0
|
||||
#warning When bed-leveling is activated, lookahead will be ineffective \
|
||||
because MAX_JERK_Z is zero.
|
||||
#endif
|
||||
|
||||
/**
|
||||
Silently discard EECONFIG on ARM. Silently to not disturb regression tests.
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ class MiscellaneousPage(wx.Panel, Page):
|
|||
self.font = font
|
||||
|
||||
self.labels = {'USE_INTERNAL_PULLUPS': "Use Internal Pullups",
|
||||
'BED_LEVELING': "Enable dynamic bed leveling",
|
||||
'Z_AUTODISABLE': "Z Autodisable",
|
||||
'EECONFIG': "Enable EEPROM Storage",
|
||||
'BANG_BANG': "Enable",
|
||||
|
|
@ -79,6 +80,10 @@ class MiscellaneousPage(wx.Panel, Page):
|
|||
cb = self.addCheckBox(k, self.onCheckBox)
|
||||
sz.Add(cb, pos = (7, 1))
|
||||
|
||||
k = 'BED_LEVELING'
|
||||
cb = self.addCheckBox(k, self.onCheckBox)
|
||||
sz.Add(cb, pos = (8, 1))
|
||||
|
||||
b = wx.StaticBox(self, wx.ID_ANY, "BANG BANG Bed Control")
|
||||
b.SetFont(font)
|
||||
sbox = wx.StaticBoxSizer(b, wx.VERTICAL)
|
||||
|
|
|
|||
|
|
@ -184,9 +184,17 @@ DEFINE_HOMING(x_negative, y_negative, z_negative, none)
|
|||
*/
|
||||
#define MAX_JERK_X 20
|
||||
#define MAX_JERK_Y 20
|
||||
#define MAX_JERK_Z 0
|
||||
#define MAX_JERK_Z 5
|
||||
#define MAX_JERK_E 200
|
||||
|
||||
/** \def BED_LEVELING
|
||||
Define this to enable dynamic bed leveling using the G29 command and
|
||||
3-point planar bed mapping. Allows the printer to compensate dynamically
|
||||
for a print bed which is flat but is not quite level.
|
||||
Enabling bed-leveling requires about 2400 bytes of flash memory.
|
||||
*/
|
||||
#define BED_LEVELING
|
||||
|
||||
|
||||
/***************************************************************************\
|
||||
* *
|
||||
|
|
|
|||
24
dda.c
24
dda.c
|
|
@ -21,6 +21,7 @@
|
|||
#include "pinio.h"
|
||||
#include "memory_barrier.h"
|
||||
#include "home.h"
|
||||
#include "bed_leveling.h"
|
||||
//#include "graycode.c"
|
||||
|
||||
#ifdef DC_EXTRUDER
|
||||
|
|
@ -112,7 +113,13 @@ void dda_init(void) {
|
|||
|
||||
This is needed for example after homing or a G92. The new location must be in startpoint already.
|
||||
*/
|
||||
void dda_new_startpoint(void) {
|
||||
void dda_new_startpoint() {
|
||||
if (DEBUG_DDA && (debug_flags & DEBUG_DDA)) {
|
||||
int32_t z_offset = bed_level_offset(startpoint.axis);
|
||||
sersendf_P(PSTR("\nReset: X %lq Y %lq Z %lq Zofs %lq F %lu\n"),
|
||||
startpoint.axis[X], startpoint.axis[Y],
|
||||
startpoint.axis[Z], z_offset, startpoint.F);
|
||||
}
|
||||
axes_um_to_steps(startpoint.axis, startpoint_steps.axis);
|
||||
startpoint_steps.axis[E] = um_to_steps(startpoint.axis[E], E);
|
||||
}
|
||||
|
|
@ -943,22 +950,12 @@ void update_current_position() {
|
|||
DDA *dda = mb_tail_dda;
|
||||
enum axis_e i;
|
||||
|
||||
static const axes_uint32_t PROGMEM steps_per_m_P = {
|
||||
STEPS_PER_M_X,
|
||||
STEPS_PER_M_Y,
|
||||
STEPS_PER_M_Z,
|
||||
STEPS_PER_M_E
|
||||
};
|
||||
|
||||
if (dda != NULL) {
|
||||
uint32_t axis_um;
|
||||
axes_int32_t delta_um;
|
||||
|
||||
for (i = X; i < AXIS_COUNT; i++) {
|
||||
axis_um = muldiv(move_state.steps[i],
|
||||
1000000,
|
||||
pgm_read_dword(&steps_per_m_P[i]));
|
||||
|
||||
axis_um = steps_to_um(move_state.steps[i], i);
|
||||
delta_um[i] = (int32_t)get_direction(dda, i) * axis_um;
|
||||
}
|
||||
|
||||
|
|
@ -968,6 +965,9 @@ void update_current_position() {
|
|||
current_position.axis[i] = dda->endpoint.axis[i] - delta_um[i];
|
||||
}
|
||||
|
||||
// Compensate for bed-leveling z-offset
|
||||
current_position.axis[Z] -= bed_level_offset(current_position.axis);
|
||||
|
||||
current_position.F = dda->endpoint.F;
|
||||
}
|
||||
else {
|
||||
|
|
|
|||
|
|
@ -6,28 +6,16 @@
|
|||
#include <stdlib.h>
|
||||
|
||||
#include "dda_maths.h"
|
||||
|
||||
#include "bed_leveling.h"
|
||||
|
||||
void
|
||||
carthesian_to_carthesian(const TARGET *startpoint, const TARGET *target,
|
||||
axes_uint32_t delta_um, axes_int32_t steps) {
|
||||
enum axis_e i;
|
||||
|
||||
for (i = X; i < E; i++) {
|
||||
delta_um[i] = (uint32_t)labs(target->axis[i] - startpoint->axis[i]);
|
||||
steps[i] = um_to_steps(target->axis[i], i);
|
||||
}
|
||||
|
||||
/* Replacing the above five lines with this costs about 200 bytes binary
|
||||
size on AVR, but also takes about 120 clock cycles less during movement
|
||||
preparation. The smaller version was kept for our Arduino Nano friends.
|
||||
delta_um[X] = (uint32_t)labs(target->axis[X] - startpoint->axis[X]);
|
||||
steps[X] = um_to_steps(target->axis[X], X);
|
||||
delta_um[Y] = (uint32_t)labs(target->axis[Y] - startpoint->axis[Y]);
|
||||
steps[Y] = um_to_steps(target->axis[Y], Y);
|
||||
delta_um[Z] = (uint32_t)labs(target->axis[Z] - startpoint->axis[Z]);
|
||||
steps[Z] = um_to_steps(target->axis[Z], Z);
|
||||
*/
|
||||
|
||||
axes_um_to_steps_cartesian(target->axis, steps);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -43,17 +31,15 @@ carthesian_to_corexy(const TARGET *startpoint, const TARGET *target,
|
|||
}
|
||||
|
||||
void axes_um_to_steps_cartesian(const axes_int32_t um, axes_int32_t steps) {
|
||||
enum axis_e i;
|
||||
|
||||
for (i = X; i < E; i++) {
|
||||
steps[i] = um_to_steps(um[i], i);
|
||||
}
|
||||
steps[X] = um_to_steps(um[X], X);
|
||||
steps[Y] = um_to_steps(um[Y], Y);
|
||||
steps[Z] = um_to_steps(um[Z] + bed_level_offset(um), Z);
|
||||
}
|
||||
|
||||
void axes_um_to_steps_corexy(const axes_int32_t um, axes_int32_t steps) {
|
||||
steps[X] = um_to_steps(um[X] + um[Y], X);
|
||||
steps[Y] = um_to_steps(um[X] - um[Y], Y);
|
||||
steps[Z] = um_to_steps(um[Z], Z);
|
||||
steps[Z] = um_to_steps(um[Z] + bed_level_offset(um), Z);
|
||||
}
|
||||
|
||||
void delta_to_axes_cartesian(axes_int32_t delta) {
|
||||
|
|
|
|||
|
|
@ -28,6 +28,13 @@ const axes_uint32_t PROGMEM axis_qr_P = {
|
|||
(uint32_t)STEPS_PER_M_E % UM_PER_METER
|
||||
};
|
||||
|
||||
const axes_uint32_t PROGMEM steps_per_m_P = {
|
||||
(uint32_t)STEPS_PER_M_X,
|
||||
(uint32_t)STEPS_PER_M_Y,
|
||||
(uint32_t)STEPS_PER_M_Z,
|
||||
(uint32_t)STEPS_PER_M_E
|
||||
};
|
||||
|
||||
/*!
|
||||
Integer multiply-divide algorithm. Returns the same as muldiv(multiplicand, multiplier, divisor), but also allowing to use precalculated quotients and remainders.
|
||||
|
||||
|
|
|
|||
|
|
@ -34,6 +34,13 @@ inline int32_t um_to_steps(int32_t distance, enum axis_e a) {
|
|||
pgm_read_dword(&axis_qr_P[a]), UM_PER_METER);
|
||||
}
|
||||
|
||||
extern const axes_uint32_t steps_per_m_P;
|
||||
|
||||
static int32_t steps_to_um(int32_t, enum axis_e) __attribute__ ((always_inline));
|
||||
inline int32_t steps_to_um(int32_t steps, enum axis_e a) {
|
||||
return muldiv(steps, UM_PER_METER, pgm_read_dword(&steps_per_m_P[a]));
|
||||
}
|
||||
|
||||
// approximate 2D distance
|
||||
uint32_t approx_distance(uint32_t dx, uint32_t dy);
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
#include "config_wrapper.h"
|
||||
#include "home.h"
|
||||
#include "sd.h"
|
||||
|
||||
#include "bed_leveling.h"
|
||||
|
||||
/// the current tool
|
||||
uint8_t tool;
|
||||
|
|
@ -224,6 +224,62 @@ void process_gcode_command() {
|
|||
}
|
||||
break;
|
||||
|
||||
#ifdef BED_LEVELING
|
||||
case 29:
|
||||
//? --- G29: Bed leveling registration ---
|
||||
//?
|
||||
//? Example: G29 S1
|
||||
//?
|
||||
//? Registers the Z-offset for a specific point on the print bed.
|
||||
//? In this case the current position is used as the registration
|
||||
//? point, but a different position can be specified by including
|
||||
//? the X, Y and Z coordinate values.
|
||||
//?
|
||||
//? Three points must be registered before the dynamic bed leveling
|
||||
//? feature is activated. Once three points are registered, the bed
|
||||
//? is mapped assuming a flat plane and Z-offsets are adjusted
|
||||
//? automatically during movements to follow the mapped plane. The
|
||||
//? adjusted position is not displayed to the client, for example
|
||||
//? in M114 results.
|
||||
//?
|
||||
//? The S value controls the action as follows:
|
||||
//? S0 displays the current bed leveling status
|
||||
//? S1 registers a new point on the 3-point plane mapping
|
||||
//? S5 clears all registered points and disables dynamic leveling
|
||||
//?
|
||||
//? G29 S1 X100 Y50 Z-0.3
|
||||
//?
|
||||
//? This command registers the specific point 100,50 => -0.3
|
||||
//?
|
||||
//? G29 S1
|
||||
//?
|
||||
//? This command registers the current head position as a point in
|
||||
//? the plane map.
|
||||
//?
|
||||
|
||||
queue_wait();
|
||||
|
||||
if (next_target.seen_S) {
|
||||
switch (next_target.S) {
|
||||
case 5: // reset bed leveling registration points
|
||||
bed_level_reset();
|
||||
break;
|
||||
|
||||
case 1: // Register a new registration point
|
||||
bed_level_register(next_target.target.axis[X], next_target.target.axis[Y], next_target.target.axis[Z]);
|
||||
break;
|
||||
|
||||
case 0: // Report leveling status
|
||||
bed_level_report();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Restore position, ignoring any axes included in G29 cmd
|
||||
next_target.target = startpoint;
|
||||
break;
|
||||
#endif /* BED_LEVELING */
|
||||
|
||||
case 90:
|
||||
//? --- G90: Set to Absolute Positioning ---
|
||||
//?
|
||||
|
|
|
|||
12
home.c
12
home.c
|
|
@ -1,5 +1,5 @@
|
|||
#include "home.h"
|
||||
|
||||
#include "bed_leveling.h"
|
||||
/** \file
|
||||
\brief Homing routines
|
||||
*/
|
||||
|
|
@ -159,6 +159,12 @@ void home_axis(enum axis_e n, int8_t dir, enum axis_endstop_e endstop_check) {
|
|||
queue_wait();
|
||||
set_axis_home_position(n, dir);
|
||||
dda_new_startpoint();
|
||||
|
||||
#ifdef BED_LEVELING
|
||||
// Move to calculated Z-plane offset
|
||||
if (n==Z)
|
||||
enqueue(&next_target.target);
|
||||
#endif /* BED_LEVELING */
|
||||
}
|
||||
|
||||
void set_axis_home_position(enum axis_e n, int8_t dir) {
|
||||
|
|
@ -198,4 +204,8 @@ void set_axis_home_position(enum axis_e n, int8_t dir) {
|
|||
}
|
||||
}
|
||||
startpoint.axis[n] = next_target.target.axis[n] = home_position;
|
||||
if (n == Z) {
|
||||
// Compensate for z-offset that will be added in by next move
|
||||
startpoint.axis[n] -= bed_level_offset(startpoint.axis);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,107 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
# Experiments with coefficients of a geometric plane
|
||||
|
||||
# Resources:
|
||||
# http://www.wolframalpha.com/input/?i=plane+through+(1,-2,0),(4,-2,-2),(4,1,4)&lk=3
|
||||
# ==> 2 x - 6 y + 3 z + 14 == 0
|
||||
|
||||
|
||||
# Translate a point relative to some origin
|
||||
def translate(point, origin):
|
||||
return tuple([a-b for a,b in zip(point, origin)])
|
||||
|
||||
# Given two points in 3d space, define a vector
|
||||
def vector(p1, p2):
|
||||
return tuple([b-a for a,b in zip(p1,p2)])
|
||||
|
||||
# Given two vectors in a plane, find the normal vector
|
||||
def normal(u, v):
|
||||
# A normal vector is the cross-product of two coplanar vectors
|
||||
return tuple([
|
||||
u[1]*v[2] - u[2]*v[1],
|
||||
u[2]*v[0] - u[0]*v[2],
|
||||
u[0]*v[1] - u[1]*v[0]
|
||||
])
|
||||
|
||||
def plane_from_three_points(P, Q, R):
|
||||
u = vector(P, Q)
|
||||
v = vector(P, R)
|
||||
n = normal(u, v)
|
||||
|
||||
# Find the coefficients
|
||||
(A,B,C) = n
|
||||
|
||||
# The equation of the plane is thus Ax+By+Cz+K=0.
|
||||
# Solve for K to get the final coefficient
|
||||
(x,y,z) = P
|
||||
K = -(A*x + B*y + C*z)
|
||||
|
||||
return (A, B, C, K)
|
||||
|
||||
# find the Z offset for any x,y
|
||||
# z = -(Ax + By + K) / C
|
||||
def calcz(x, y, plane, translation=(0,0,0)):
|
||||
(A,B,C,K) = plane
|
||||
(tx, ty, tz) = translation
|
||||
return -(A*(x-tx) + B*(y-ty) + K) / C + tz
|
||||
|
||||
|
||||
# Verify a point is on this plane
|
||||
def validate(plane, point):
|
||||
(A, B, C, K) = plane
|
||||
(x, y, z) = point
|
||||
return z == calcz(x, y, plane)
|
||||
|
||||
|
||||
def verify_plane(points):
|
||||
print ' ', '\n '.join([str(p) for p in points])
|
||||
|
||||
plane = plane_from_three_points( *points)
|
||||
print 'Plane coordinates: ', plane
|
||||
|
||||
if plane[2] == 0:
|
||||
print ' Error: points are colinear'
|
||||
return
|
||||
|
||||
valid = True
|
||||
for p in points:
|
||||
if not validate(plane, p):
|
||||
print "Failed: sample point not on plane, ", p
|
||||
valid = False
|
||||
print "Validation:", "Failed" if not valid else "Passed"
|
||||
|
||||
|
||||
|
||||
samples = [
|
||||
# canonical example
|
||||
[ (1,-2,0), (4,-2,-2), (4,1,4) ],
|
||||
|
||||
# three colinear points (infinite planes)
|
||||
[ (2,2,2), (4,4,4), (10,10,10) ],
|
||||
|
||||
# Extreme tilt example in mm
|
||||
[ (57,123,-5), (200,0,35), (0,207,2) ],
|
||||
|
||||
# Some more examples in um
|
||||
[ (0, 0, 1300), (200000, 200000, 3500), (0, 150000, -1000) ],
|
||||
[ (20000, 20000, -300), (220000, 120000, -1700), (120000, 220000, -700) ],
|
||||
|
||||
# some example in tenths of mm
|
||||
[ (200, 200, -300), (2200, 1200, -1700), (1200, 2200, -700) ],
|
||||
|
||||
[ (20000, 20000 , -300 ), (220000, 120000 , -1700 ), (120000, 220000 , -700 ) ],
|
||||
[ (200, 200, -300 ), (2200, 1200, -1700 ), (1200, 2200, -700 ) ]
|
||||
]
|
||||
|
||||
for points in samples:
|
||||
verify_plane(points)
|
||||
|
||||
print "====[Translated]========="
|
||||
# Translate plane to origin at P (simplifies by removing K coefficient)
|
||||
# A*x' + B*y' + C*z' = 0
|
||||
P = points[0]
|
||||
T = translate((0,0,0), P)
|
||||
xpoints = [translate(p, P) for p in points]
|
||||
verify_plane(xpoints)
|
||||
print "=========================\n"
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
(Test bed leveling)
|
||||
m111 s32
|
||||
|
||||
G28
|
||||
G29 S5
|
||||
G29 S1 X20.000 Y20.000 Z-2.000
|
||||
G29 S1 X220.000 Y120.000 Z-1.700
|
||||
G29 S1 X120.000 Y220.000 Z-0.700
|
||||
|
||||
g29 s0
|
||||
|
||||
m114
|
||||
g1 x100 y100
|
||||
m114
|
||||
G28
|
||||
M114
|
||||
|
||||
G4 ; dwell
|
||||
G1 x200 y0
|
||||
G4 ; dwell
|
||||
m114
|
||||
G1 x199
|
||||
G4 ; dwell
|
||||
G1 x198
|
||||
G4 ; dwell
|
||||
G1 x197
|
||||
G4 ; dwell
|
||||
G1 x196
|
||||
G4 ; dwell
|
||||
G1 x195
|
||||
G4 ; dwell
|
||||
G1 x194
|
||||
G4 ; dwell
|
||||
g28
|
||||
M114
|
||||
G1 x1 y1
|
||||
G4 ; dwell
|
||||
m114
|
||||
|
||||
G28
|
||||
G29 S5
|
||||
G29 S1 X20.000 Y20.000 Z-0.300
|
||||
G29 S1 X220.000 Y120.000 Z-1.700
|
||||
G29 S1 X120.000 Y220.000 Z-0.700
|
||||
|
||||
g29 s0
|
||||
Loading…
Reference in New Issue