SunRed/Marlin-Artillery-M600
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Bed Auto Leveling feature

Browse source 
Check the Readme for instruction how to enable and configure the feature
This commit is contained in:
Alex Borro 2013-09-29 13:20:06 -03:00
parent 1bda6bf862
commit 253dfc4bc1
12 changed files with 806 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

37
Marlin/Configuration.h
View file

@ -292,13 +292,50 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#define min_software_endstops true // If true, axis won't move to coordinates less than HOME_POS.
#define max_software_endstops true // If true, axis won't move to coordinates greater than the defined lengths below.
//============================= Bed Auto Leveling ===========================
//#define ENABLE_AUTO_BED_LEVELING // Delete the comment to enable (remove // at the start of the line)
#ifdef ENABLE_AUTO_BED_LEVELING
// these are the positions on the bed to do the probing
#define LEFT_PROBE_BED_POSITION 15
#define RIGHT_PROBE_BED_POSITION 170
#define BACK_PROBE_BED_POSITION 180
#define FRONT_PROBE_BED_POSITION 20
// these are the offsets to the prob relative to the extruder tip (Hotend - Probe)
#define X_PROBE_OFFSET_FROM_EXTRUDER -25
#define Y_PROBE_OFFSET_FROM_EXTRUDER -29
#define Z_PROBE_OFFSET_FROM_EXTRUDER -12.35
#define XY_TRAVEL_SPEED 8000 // X and Y axis travel speed between probes, in mm/min
#define Z_RAISE_BEFORE_PROBING 15 //How much the extruder will be raised before traveling to the first probing point.
#define Z_RAISE_BETWEEN_PROBINGS 5 //How much the extruder will be raised when traveling from between next probing points
//If defined, the Probe servo will be turned on only during movement and then turned off to avoid jerk
//The value is the delay to turn the servo off after powered on - depends on the servo speed; 300ms is good value, but you can try lower it.
// You MUST HAVE the SERVO_ENDSTOPS defined to use here a value higher than zero otherwise your code will not compile.
// #define PROBE_SERVO_DEACTIVATION_DELAY 300
#endif
// Travel limits after homing
#define X_MAX_POS 205
#define X_MIN_POS 0
#define Y_MAX_POS 205
#define Y_MIN_POS 0
#define Z_MAX_POS 200
#ifndef ENABLE_AUTO_BED_LEVELING
#define Z_MIN_POS 0
#else
#define Z_MIN_POS (-1*Z_PROBE_OFFSET_FROM_EXTRUDER) //With Auto Bed Leveling, the Z_MIN MUST have the same distance as Z_PROBE
#endif
#define X_MAX_LENGTH (X_MAX_POS - X_MIN_POS)
#define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS)

52
Marlin/Marlin.ino Normal file
View file

@ -0,0 +1,52 @@
/* -*- c++ -*- */
/*
Reprap firmware 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/>.
*/
/*
This firmware is a mashup between Sprinter and grbl.
(https://github.com/kliment/Sprinter)
(https://github.com/simen/grbl/tree)
It has preliminary support for Matthew Roberts advance algorithm
http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
*/
/* All the implementation is done in *.cpp files to get better compatibility with avr-gcc without the Arduino IDE */
/* Use this file to help the Arduino IDE find which Arduino libraries are needed and to keep documentation on GCode */
#include "Configuration.h"
#include "pins.h"
#ifdef ULTRA_LCD
#if defined(LCD_I2C_TYPE_PCF8575)
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#elif defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008)
#include <Wire.h>
#include <LiquidTWI2.h>
#elif defined(DOGLCD)
#include <U8glib.h> // library for graphics LCD by Oli Kraus (https://code.google.com/p/u8glib/)
#else
#include <LiquidCrystal.h> // library for character LCD
#endif
#endif
#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
#include <SPI.h>
#endif

303
Marlin/Marlin_main.cpp
View file

@ -29,6 +29,10 @@
#include "Marlin.h"
#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
#endif // ENABLE_AUTO_BED_LEVELING
#include "ultralcd.h"
#include "planner.h"
#include "stepper.h"
@ -63,6 +67,8 @@
// G10 - retract filament according to settings of M207
// G11 - retract recover filament according to settings of M208
// G28 - Home all Axis
// G29 - Detailed Z-Probe, probes the bed at 3 points. You must de at the home position for this to work correctly.
// G30 - Single Z Probe, probes bed at current XY location.
// G90 - Use Absolute Coordinates
// G91 - Use Relative Coordinates
// G92 - Set current position to cordinates given
@ -133,6 +139,8 @@
// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
// M304 - Set bed PID parameters P I and D
// M400 - Finish all moves
// M401 - Lower z-probe if present
// M402 - Raise z-probe if present
// M500 - stores paramters in EEPROM
// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
// M502 - reverts to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
@ -388,6 +396,11 @@ void servo_init()
}
}
#endif
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_endstops[Z_AXIS]].detach();
#endif
}
void setup()
@ -756,6 +769,143 @@ static void axis_is_at_home(int axis) {
max_pos[axis] = base_max_pos(axis) + add_homeing[axis];
}
#ifdef ENABLE_AUTO_BED_LEVELING
static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yFront, float z_at_xLeft_yBack) {
plan_bed_level_matrix.set_to_identity();
vector_3 xLeftyFront = vector_3(LEFT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, z_at_xLeft_yFront);
vector_3 xLeftyBack = vector_3(LEFT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION, z_at_xLeft_yBack);
vector_3 xRightyFront = vector_3(RIGHT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, z_at_xRight_yFront);
vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
vector_3 planeNormal = vector_3::cross(yPositive, xPositive).get_normal();
//planeNormal.debug("planeNormal");
//yPositive.debug("yPositive");
matrix_3x3 bedLevel = matrix_3x3::create_look_at(planeNormal, yPositive);
//bedLevel.debug("bedLevel");
//plan_bed_level_matrix.debug("bed level before");
//vector_3 uncorrected_position = plan_get_position_mm();
//uncorrected_position.debug("position before");
// and set our bed level equation to do the right thing
plan_bed_level_matrix = matrix_3x3::create_inverse(bedLevel);
//plan_bed_level_matrix.debug("bed level after");
vector_3 corrected_position = plan_get_position();
//corrected_position.debug("position after");
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z;
// but the bed at 0 so we don't go below it.
current_position[Z_AXIS] = -Z_PROBE_OFFSET_FROM_EXTRUDER;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
static void run_z_probe() {
plan_bed_level_matrix.set_to_identity();
feedrate = homing_feedrate[Z_AXIS];
// move down until you find the bed
float zPosition = -10;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
// we have to let the planner know where we are right now as it is not where we said to go.
zPosition = st_get_position_mm(Z_AXIS);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
// move up the retract distance
zPosition += home_retract_mm(Z_AXIS);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
// move back down slowly to find bed
feedrate = homing_feedrate[Z_AXIS]/4;
zPosition -= home_retract_mm(Z_AXIS) * 2;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
// make sure the planner knows where we are as it may be a bit different than we last said to move to
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
static void do_blocking_move_to(float x, float y, float z) {
float oldFeedRate = feedrate;
feedrate = XY_TRAVEL_SPEED;
current_position[X_AXIS] = x;
current_position[Y_AXIS] = y;
current_position[Z_AXIS] = z;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
st_synchronize();
feedrate = oldFeedRate;
}
static void do_blocking_move_relative(float offset_x, float offset_y, float offset_z) {
do_blocking_move_to(current_position[X_AXIS] + offset_x, current_position[Y_AXIS] + offset_y, current_position[Z_AXIS] + offset_z);
}
static void setup_for_endstop_move() {
saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
previous_millis_cmd = millis();
enable_endstops(true);
}
static void clean_up_after_endstop_move() {
#ifdef ENDSTOPS_ONLY_FOR_HOMING
enable_endstops(false);
#endif
feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply;
previous_millis_cmd = millis();
}
static void engage_z_probe() {
// Engage Z Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
if (servo_endstops[Z_AXIS] > -1) {
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
servos[servo_endstops[Z_AXIS]].attach(0);
#endif
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2]);
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_endstops[Z_AXIS]].detach();
#endif
}
#endif
}
static void retract_z_probe() {
// Retract Z Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
if (servo_endstops[Z_AXIS] > -1) {
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
servos[servo_endstops[Z_AXIS]].attach(0);
#endif
servos[servo_endstops[Z_AXIS]].write(servo_endstop_angles[Z_AXIS * 2 + 1]);
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_endstops[Z_AXIS]].detach();
#endif
}
#endif
}
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
static void homeaxis(int axis) {
#define HOMEAXIS_DO(LETTER) \
((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
@ -772,6 +922,10 @@ static void homeaxis(int axis) {
// Engage Servo endstop if enabled
#ifdef SERVO_ENDSTOPS
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
if (axis==Z_AXIS) engage_z_probe();
else
#endif
if (servo_endstops[axis] > -1) {
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
}
@ -818,6 +972,10 @@ static void homeaxis(int axis) {
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
}
#endif
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
if (axis==Z_AXIS) retract_z_probe();
#endif
}
}
#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
@ -826,7 +984,9 @@ void process_commands()
{
unsigned long codenum; //throw away variable
char *starpos = NULL;
#ifdef ENABLE_AUTO_BED_LEVELING
float x_tmp, y_tmp, z_tmp, real_z;
#endif
if(code_seen('G'))
{
switch((int)code_value())
@ -898,6 +1058,11 @@ void process_commands()
break;
#endif //FWRETRACT
case 28: //G28 Home all Axis one at a time
#ifdef ENABLE_AUTO_BED_LEVELING
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
#endif //ENABLE_AUTO_BED_LEVELING
saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
@ -1045,6 +1210,122 @@ void process_commands()
previous_millis_cmd = millis();
endstops_hit_on_purpose();
break;
#ifdef ENABLE_AUTO_BED_LEVELING
case 29: // G29 Detailed Z-Probe, probes the bed at 3 points.
{
#if Z_MIN_PIN == -1
#error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin."
#endif
st_synchronize();
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
//vector_3 corrected_position = plan_get_position_mm();
//corrected_position.debug("position before G29");
plan_bed_level_matrix.set_to_identity();
vector_3 uncorrected_position = plan_get_position();
//uncorrected_position.debug("position durring G29");
current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
// prob 1
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
do_blocking_move_to(LEFT_PROBE_BED_POSITION - X_PROBE_OFFSET_FROM_EXTRUDER, BACK_PROBE_BED_POSITION - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
engage_z_probe(); // Engage Z Servo endstop if available
run_z_probe();
float z_at_xLeft_yBack = current_position[Z_AXIS];
SERIAL_PROTOCOLPGM("Bed x: ");
SERIAL_PROTOCOL(LEFT_PROBE_BED_POSITION);
SERIAL_PROTOCOLPGM(" y: ");
SERIAL_PROTOCOL(BACK_PROBE_BED_POSITION);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("\n");
// prob 2
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
do_blocking_move_to(LEFT_PROBE_BED_POSITION - X_PROBE_OFFSET_FROM_EXTRUDER, FRONT_PROBE_BED_POSITION - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
run_z_probe();
float z_at_xLeft_yFront = current_position[Z_AXIS];
SERIAL_PROTOCOLPGM("Bed x: ");
SERIAL_PROTOCOL(LEFT_PROBE_BED_POSITION);
SERIAL_PROTOCOLPGM(" y: ");
SERIAL_PROTOCOL(FRONT_PROBE_BED_POSITION);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("\n");
// prob 3
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
// the current position will be updated by the blocking move so the head will not lower on this next call.
do_blocking_move_to(RIGHT_PROBE_BED_POSITION - X_PROBE_OFFSET_FROM_EXTRUDER, FRONT_PROBE_BED_POSITION - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
run_z_probe();
float z_at_xRight_yFront = current_position[Z_AXIS];
SERIAL_PROTOCOLPGM("Bed x: ");
SERIAL_PROTOCOL(RIGHT_PROBE_BED_POSITION);
SERIAL_PROTOCOLPGM(" y: ");
SERIAL_PROTOCOL(FRONT_PROBE_BED_POSITION);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("\n");
clean_up_after_endstop_move();
set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
retract_z_probe(); // Retract Z Servo endstop if available
st_synchronize();
// The following code correct the Z height difference from z-probe position and hotend tip position.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// When the bed is uneven, this height must be corrected.
real_z = float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
z_tmp = current_position[Z_AXIS];
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
break;
case 30: // G30 Single Z Probe
{
engage_z_probe(); // Engage Z Servo endstop if available
st_synchronize();
// TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
run_z_probe();
SERIAL_PROTOCOLPGM("Bed Position X: ");
SERIAL_PROTOCOL(current_position[X_AXIS]);
SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL(current_position[Y_AXIS]);
SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("\n");
clean_up_after_endstop_move();
retract_z_probe(); // Retract Z Servo endstop if available
}
break;
#endif // ENABLE_AUTO_BED_LEVELING
case 90: // G90
relative_mode = false;
break;
@ -1787,7 +2068,14 @@ void process_commands()
if (code_seen('S')) {
servo_position = code_value();
if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
servos[servo_index].attach(0);
#endif
servos[servo_index].write(servo_position);
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
delay(PROBE_SERVO_DEACTIVATION_DELAY);
servos[servo_index].detach();
#endif
}
else {
SERIAL_ECHO_START;
@ -1938,6 +2226,19 @@ void process_commands()
st_synchronize();
}
break;
#if defined(ENABLE_AUTO_BED_LEVELING) && defined(SERVO_ENDSTOPS)
case 401:
{
engage_z_probe(); // Engage Z Servo endstop if available
}
break;
case 402:
{
retract_z_probe(); // Retract Z Servo endstop if enabled
}
break;
#endif
case 500: // M500 Store settings in EEPROM
{
Config_StoreSettings();

3
Marlin/Servo.cpp
View file

@ -262,6 +262,9 @@ uint8_t Servo::attach(int pin)
uint8_t Servo::attach(int pin, int min, int max)
{
if(this->servoIndex < MAX_SERVOS ) {
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
if (pin > 0) this->pin = pin; else pin = this->pin;
#endif
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128

3
Marlin/Servo.h
View file

@ -123,6 +123,9 @@ public:
int read(); // returns current pulse width as an angle between 0 and 180 degrees
int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
bool attached(); // return true if this servo is attached, otherwise false
#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
int pin; // store the hw pin of the servo
#endif
private:
uint8_t servoIndex; // index into the channel data for this servo
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH

39
Marlin/planner.cpp
View file

@ -75,6 +75,15 @@ float max_e_jerk;
float mintravelfeedrate;
unsigned long axis_steps_per_sqr_second[NUM_AXIS];
#ifdef ENABLE_AUTO_BED_LEVELING
// this holds the required transform to compensate for bed level
matrix_3x3 plan_bed_level_matrix = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
};
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
// The current position of the tool in absolute steps
long position[4]; //rescaled from extern when axis_steps_per_unit are changed by gcode
static float previous_speed[4]; // Speed of previous path line segment
@ -513,7 +522,11 @@ 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
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)
#endif //ENABLE_AUTO_BED_LEVELING
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
@ -527,6 +540,10 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
lcd_update();
}
#ifdef ENABLE_AUTO_BED_LEVELING
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // ENABLE_AUTO_BED_LEVELING
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
@ -919,8 +936,30 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
st_wake_up();
}
#ifdef ENABLE_AUTO_BED_LEVELING
vector_3 plan_get_position() {
vector_3 position = vector_3(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS), st_get_position_mm(Z_AXIS));
//position.debug("in plan_get position");
//plan_bed_level_matrix.debug("in plan_get bed_level");
matrix_3x3 inverse = matrix_3x3::create_inverse(plan_bed_level_matrix);
//inverse.debug("in plan_get inverse");
position.apply_rotation(inverse);
//position.debug("after rotation");
return position;
}
#endif // ENABLE_AUTO_BED_LEVELING
#ifdef ENABLE_AUTO_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
position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);

22
Marlin/planner.h
View file

@ -26,6 +26,10 @@
#include "Marlin.h"
#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
#endif // ENABLE_AUTO_BED_LEVELING
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
// the source g-code and may never actually be reached if acceleration management is active.
typedef struct {
@ -67,15 +71,33 @@ typedef struct {
volatile char busy;
} block_t;
#ifdef ENABLE_AUTO_BED_LEVELING
// this holds the required transform to compensate for bed level
extern matrix_3x3 plan_bed_level_matrix;
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
// Initialize the motion plan subsystem
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
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
vector_3 plan_get_position();
#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
// Set position. Used for G92 instructions.
#ifdef ENABLE_AUTO_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
void plan_set_e_position(const float &e);

8
Marlin/stepper.cpp
View file

@ -969,6 +969,14 @@ long st_get_position(uint8_t axis)
return count_pos;
}
#ifdef ENABLE_AUTO_BED_LEVELING
float st_get_position_mm(uint8_t axis)
{
float steper_position_in_steps = st_get_position(axis);
return steper_position_in_steps / axis_steps_per_unit[axis];
}
#endif // ENABLE_AUTO_BED_LEVELING
void finishAndDisableSteppers()
{
st_synchronize();

11
Marlin/stepper.h
View file

@ -44,9 +44,9 @@
#define REV_E_DIR() WRITE(E0_DIR_PIN, INVERT_E0_DIR)
#endif
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
extern bool abort_on_endstop_hit;
#endif
#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
extern bool abort_on_endstop_hit;
#endif
// Initialize and start the stepper motor subsystem
void st_init();
@ -61,6 +61,11 @@ void st_set_e_position(const long &e);
// Get current position in steps
long st_get_position(uint8_t axis);
#ifdef ENABLE_AUTO_BED_LEVELING
// Get current position in mm
float st_get_position_mm(uint8_t axis);
#endif //ENABLE_AUTO_BED_LEVELING
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this
// to notify the subsystem that it is time to go to work.
void st_wake_up();

202
Marlin/vector_3.cpp Normal file
View file

@ -0,0 +1,202 @@
/*
vector_3.cpp - Vector library for bed leveling
Copyright (c) 2012 Lars Brubaker. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include <math.h>
#include "Marlin.h"
#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
vector_3::vector_3()
{
this->x = 0;
this->y = 0;
this->z = 0;
}
vector_3::vector_3(float x, float y, float z)
{
this->x = x;
this->y = y;
this->z = z;
}
vector_3 vector_3::cross(vector_3 left, vector_3 right)
{
return vector_3(left.y * right.z - left.z * right.y,
left.z * right.x - left.x * right.z,
left.x * right.y - left.y * right.x);
}
vector_3 vector_3::operator+(vector_3 v)
{
return vector_3((x + v.x), (y + v.y), (z + v.z));
}
vector_3 vector_3::operator-(vector_3 v)
{
return vector_3((x - v.x), (y - v.y), (z - v.z));
}
vector_3 vector_3::get_normal()
{
vector_3 normalized = vector_3(x, y, z);
normalized.normalize();
return normalized;
}
float vector_3::get_length()
{
float length = sqrt((x * x) + (y * y) + (z * z));
return length;
}
void vector_3::normalize()
{
float length = get_length();
x /= length;
y /= length;
z /= length;
}
void vector_3::apply_rotation(matrix_3x3 matrix)
{
float resultX = x * matrix.matrix[3*0+0] + y * matrix.matrix[3*1+0] + z * matrix.matrix[3*2+0];
float resultY = x * matrix.matrix[3*0+1] + y * matrix.matrix[3*1+1] + z * matrix.matrix[3*2+1];
float resultZ = x * matrix.matrix[3*0+2] + y * matrix.matrix[3*1+2] + z * matrix.matrix[3*2+2];
x = resultX;
y = resultY;
z = resultZ;
}
void vector_3::debug(char* title)
{
SERIAL_PROTOCOL(title);
SERIAL_PROTOCOLPGM(" x: ");
SERIAL_PROTOCOL(x);
SERIAL_PROTOCOLPGM(" y: ");
SERIAL_PROTOCOL(y);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL(z);
SERIAL_PROTOCOLPGM("\n");
}
void apply_rotation_xyz(matrix_3x3 matrix, float &x, float& y, float& z)
{
vector_3 vector = vector_3(x, y, z);
vector.apply_rotation(matrix);
x = vector.x;
y = vector.y;
z = vector.z;
}
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2)
{
//row_0.debug("row_0");
//row_1.debug("row_1");
//row_2.debug("row_2");
matrix_3x3 new_matrix;
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z;
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z;
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z;
//new_matrix.debug("new_matrix");
return new_matrix;
}
void matrix_3x3::set_to_identity()
{
matrix[0] = 1; matrix[1] = 0; matrix[2] = 0;
matrix[3] = 0; matrix[4] = 1; matrix[5] = 0;
matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
}
matrix_3x3 matrix_3x3::create_look_at(vector_3 target, vector_3 up)
{
// There are lots of examples of look at code on the internet that don't do all these noramize and also find the position
// through several dot products. The problem with them is that they have a bit of error in that all the vectors arn't normal and need to be.
vector_3 z_row = vector_3(-target.x, -target.y, -target.z).get_normal();
vector_3 x_row = vector_3::cross(up, z_row).get_normal();
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
//x_row.debug("x_row");
//y_row.debug("y_row");
//z_row.debug("z_row");
matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, y_row.x, z_row.x),
vector_3(x_row.y, y_row.y, z_row.y),
vector_3(x_row.z, y_row.z, z_row.z));
//rot.debug("rot");
return rot;
}
matrix_3x3 matrix_3x3::create_inverse(matrix_3x3 original)
{
//original.debug("original");
float* A = original.matrix;
float determinant =
+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2])
- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0])
+ A[0 * 3 + 2] * (A[1 * 3 + 0] * A[2 * 3 + 1] - A[1 * 3 + 1] * A[2 * 3 + 0]);
matrix_3x3 inverse;
inverse.matrix[0 * 3 + 0] = +(A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) / determinant;
inverse.matrix[0 * 3 + 1] = -(A[0 * 3 + 1] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 1]) / determinant;
inverse.matrix[0 * 3 + 2] = +(A[0 * 3 + 1] * A[1 * 3 + 2] - A[0 * 3 + 2] * A[1 * 3 + 1]) / determinant;
inverse.matrix[1 * 3 + 0] = -(A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) / determinant;
inverse.matrix[1 * 3 + 1] = +(A[0 * 3 + 0] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 0]) / determinant;
inverse.matrix[1 * 3 + 2] = -(A[0 * 3 + 0] * A[1 * 3 + 2] - A[1 * 3 + 0] * A[0 * 3 + 2]) / determinant;
inverse.matrix[2 * 3 + 0] = +(A[1 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[1 * 3 + 1]) / determinant;
inverse.matrix[2 * 3 + 1] = -(A[0 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[0 * 3 + 1]) / determinant;
inverse.matrix[2 * 3 + 2] = +(A[0 * 3 + 0] * A[1 * 3 + 1] - A[1 * 3 + 0] * A[0 * 3 + 1]) / determinant;
vector_3 row0 = vector_3(inverse.matrix[0 * 3 + 0], inverse.matrix[0 * 3 + 1], inverse.matrix[0 * 3 + 2]);
vector_3 row1 = vector_3(inverse.matrix[1 * 3 + 0], inverse.matrix[1 * 3 + 1], inverse.matrix[1 * 3 + 2]);
vector_3 row2 = vector_3(inverse.matrix[2 * 3 + 0], inverse.matrix[2 * 3 + 1], inverse.matrix[2 * 3 + 2]);
row0.normalize();
row1.normalize();
row2.normalize();
inverse = matrix_3x3::create_from_rows(row0, row1, row2);
//inverse.debug("inverse");
return inverse;
}
void matrix_3x3::debug(char* title)
{
SERIAL_PROTOCOL(title);
SERIAL_PROTOCOL("\n");
int count = 0;
for(int i=0; i<3; i++)
{
for(int j=0; j<3; j++)
{
SERIAL_PROTOCOL(matrix[count]);
SERIAL_PROTOCOLPGM(" ");
count++;
}
SERIAL_PROTOCOLPGM("\n");
}
}
#endif // #ifdef ENABLE_AUTO_BED_LEVELING

62
Marlin/vector_3.h Normal file
View file

@ -0,0 +1,62 @@
/*
vector_3.cpp - Vector library for bed leveling
Copyright (c) 2012 Lars Brubaker. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#ifndef VECTOR_3_H
#define VECTOR_3_H
#ifdef ENABLE_AUTO_BED_LEVELING
class matrix_3x3;
struct vector_3
{
float x, y, z;
vector_3();
vector_3(float x, float y, float z);
static vector_3 cross(vector_3 a, vector_3 b);
vector_3 operator+(vector_3 v);
vector_3 operator-(vector_3 v);
void normalize();
float get_length();
vector_3 get_normal();
void debug(char* title);
void apply_rotation(matrix_3x3 matrix);
};
struct matrix_3x3
{
float matrix[9];
static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
static matrix_3x3 create_look_at(vector_3 target, vector_3 up);
static matrix_3x3 create_inverse(matrix_3x3 original);
void set_to_identity();
void debug(char* title);
};
void apply_rotation_xyz(matrix_3x3 rotationMatrix, float &x, float& y, float& z);
#endif // ENABLE_AUTO_BED_LEVELING
#endif // VECTOR_3_H

68
README.md
View file

@ -48,6 +48,7 @@ Features:
* Configurable serial port to support connection of wireless adaptors.
* Automatic operation of extruder/cold-end cooling fans based on nozzle temperature
* RC Servo Support, specify angle or duration for continuous rotation servos.
* Bed Auto Leveling.
The default baudrate is 250000. This baudrate has less jitter and hence errors than the usual 115200 baud, but is less supported by drivers and host-environments.
@ -142,6 +143,8 @@ Implemented G Codes:
* G10 - retract filament according to settings of M207
* G11 - retract recover filament according to settings of M208
* G28 - Home all Axis
* G29 - Detailed Z-Probe, probes the bed at 3 points. You must de at the home position for this to work correctly.
* G30 - Single Z Probe, probes bed at current XY location.
* G90 - Use Absolute Coordinates
* G91 - Use Relative Coordinates
* G92 - Set current position to cordinates given
@ -210,6 +213,8 @@ M Codes
* M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
* M304 - Set bed PID parameters P I and D
* M400 - Finish all moves
* M401 - Lower z-probe if present
* M402 - Raise z-probe if present
* M500 - stores paramters in EEPROM
* M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
* M502 - reverts to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
@ -249,6 +254,69 @@ If all goes well the firmware is uploading
That's ok. Enjoy Silky Smooth Printing.
===============================================
Instructions for configuring Bed Auto Leveling
===============================================
Uncomment the "ENABLE_AUTO_BED_LEVELING" define (commented by default)
You will probably need a swivel Z-MIN endstop in the extruder. A rc servo do a great job.
Check the system working here: http://www.youtube.com/watch?v=3IKMeOYz-1Q (Enable English subtitles)
Teasing ;-) video: http://www.youtube.com/watch?v=x8eqSQNAyro
In order to get the servo working, you need to enable:
* \#define NUM_SERVOS 1 // Servo index starts with 0 for M280 command
* \#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
* \#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 165,60} // X,Y,Z Axis Extend and Retract angles
The first define tells firmware how many servos you have.
The second tells what axis this servo will be attached to. In the example above, we have a servo in Z axis.
The third one tells the angle in 2 situations: Probing (165º) and resting (60º). Check this with command M280 P0 S{angle}
Next you need to define the Z endstop (probe) offset from hotend.
My preferred method:
* a) Make a small mark in the bed with a marker/felt-tip pen.
* b) Place the hotend tip as *exactly* as possible on the mark, touching the bed. Raise the hotend 0.1mm (a regular paper thickness) and zero all axis (G92 X0 Y0 Z0);
* d) Raise the hotend 10mm (or more) for probe clearance, lower the Z probe (Z-Endstop) with M401 and place it just on that mark by moving X, Y and Z;
* e) Lower the Z in 0.1mm steps, with the probe always touching the mark (it may be necessary to adjust X and Y as well) until you hear the "click" meaning the mechanical endstop was trigged. You can confirm with M119;
* f) Now you have the probe in the same place as your hotend tip was before. Perform a M114 and write down the values, for example: X:24.3 Y:-31.4 Z:5.1;
* g) You can raise the z probe with M402 command;
* h) Fill the defines bellow multiplying the values by "-1" (just change the signal)
* \#define X_PROBE_OFFSET_FROM_EXTRUDER -24.3
* \#define Y_PROBE_OFFSET_FROM_EXTRUDER 31.4
* \#define Z_PROBE_OFFSET_FROM_EXTRUDER -5.1
The following options define the probing positions. These are good starting values.
I recommend to keep a better clearance from borders in the first run and then make the probes as close as possible to borders:
* \#define LEFT_PROBE_BED_POSITION 30
* \#define RIGHT_PROBE_BED_POSITION 140
* \#define BACK_PROBE_BED_POSITION 140
* \#define FRONT_PROBE_BED_POSITION 30
A few more options:
* \#define XY_TRAVEL_SPEED 6000
X and Y axis travel speed between probes, in mm/min.
Bear in mind that really fast moves may render step skipping. 6000 mm/min (100mm/s) is a good value.
* \#define Z_RAISE_BEFORE_PROBING 10
* \#define Z_RAISE_BETWEEN_PROBINGS 10
The Z axis is lifted when traveling to the first probe point by Z_RAISE_BEFORE_PROBING value
and then lifted when traveling from first to second and second to third point by Z_RAISE_BETWEEN_PROBINGS.
All values are in mm as usual.
That's it.. enjoy never having to calibrate your Z endstop neither leveling your bed by hand anymore ;-)