Bed Auto Leveling feature
Check the Readme for instruction how to enable and configure the feature
This commit is contained in:
parent
1bda6bf862
commit
253dfc4bc1
@ -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 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.
|
#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
|
// Travel limits after homing
|
||||||
#define X_MAX_POS 205
|
#define X_MAX_POS 205
|
||||||
#define X_MIN_POS 0
|
#define X_MIN_POS 0
|
||||||
#define Y_MAX_POS 205
|
#define Y_MAX_POS 205
|
||||||
#define Y_MIN_POS 0
|
#define Y_MIN_POS 0
|
||||||
#define Z_MAX_POS 200
|
#define Z_MAX_POS 200
|
||||||
|
|
||||||
|
#ifndef ENABLE_AUTO_BED_LEVELING
|
||||||
#define Z_MIN_POS 0
|
#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 X_MAX_LENGTH (X_MAX_POS - X_MIN_POS)
|
||||||
#define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS)
|
#define Y_MAX_LENGTH (Y_MAX_POS - Y_MIN_POS)
|
||||||
|
52
Marlin/Marlin.ino
Normal file
52
Marlin/Marlin.ino
Normal 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
|
@ -29,6 +29,10 @@
|
|||||||
|
|
||||||
#include "Marlin.h"
|
#include "Marlin.h"
|
||||||
|
|
||||||
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||||
|
#include "vector_3.h"
|
||||||
|
#endif // ENABLE_AUTO_BED_LEVELING
|
||||||
|
|
||||||
#include "ultralcd.h"
|
#include "ultralcd.h"
|
||||||
#include "planner.h"
|
#include "planner.h"
|
||||||
#include "stepper.h"
|
#include "stepper.h"
|
||||||
@ -63,6 +67,8 @@
|
|||||||
// G10 - retract filament according to settings of M207
|
// G10 - retract filament according to settings of M207
|
||||||
// G11 - retract recover filament according to settings of M208
|
// G11 - retract recover filament according to settings of M208
|
||||||
// G28 - Home all Axis
|
// 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
|
// G90 - Use Absolute Coordinates
|
||||||
// G91 - Use Relative Coordinates
|
// G91 - Use Relative Coordinates
|
||||||
// G92 - Set current position to cordinates given
|
// 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)
|
// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
|
||||||
// M304 - Set bed PID parameters P I and D
|
// M304 - Set bed PID parameters P I and D
|
||||||
// M400 - Finish all moves
|
// M400 - Finish all moves
|
||||||
|
// M401 - Lower z-probe if present
|
||||||
|
// M402 - Raise z-probe if present
|
||||||
// M500 - stores paramters in EEPROM
|
// M500 - stores paramters in EEPROM
|
||||||
// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
|
// 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.
|
// 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
|
#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()
|
void setup()
|
||||||
@ -756,6 +769,143 @@ static void axis_is_at_home(int axis) {
|
|||||||
max_pos[axis] = base_max_pos(axis) + add_homeing[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) {
|
static void homeaxis(int axis) {
|
||||||
#define HOMEAXIS_DO(LETTER) \
|
#define HOMEAXIS_DO(LETTER) \
|
||||||
((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))
|
((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
|
// Engage Servo endstop if enabled
|
||||||
#ifdef SERVO_ENDSTOPS
|
#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) {
|
if (servo_endstops[axis] > -1) {
|
||||||
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2]);
|
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]);
|
servos[servo_endstops[axis]].write(servo_endstop_angles[axis * 2 + 1]);
|
||||||
}
|
}
|
||||||
#endif
|
#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)
|
#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
|
||||||
@ -826,7 +984,9 @@ void process_commands()
|
|||||||
{
|
{
|
||||||
unsigned long codenum; //throw away variable
|
unsigned long codenum; //throw away variable
|
||||||
char *starpos = NULL;
|
char *starpos = NULL;
|
||||||
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||||
|
float x_tmp, y_tmp, z_tmp, real_z;
|
||||||
|
#endif
|
||||||
if(code_seen('G'))
|
if(code_seen('G'))
|
||||||
{
|
{
|
||||||
switch((int)code_value())
|
switch((int)code_value())
|
||||||
@ -898,6 +1058,11 @@ void process_commands()
|
|||||||
break;
|
break;
|
||||||
#endif //FWRETRACT
|
#endif //FWRETRACT
|
||||||
case 28: //G28 Home all Axis one at a time
|
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_feedrate = feedrate;
|
||||||
saved_feedmultiply = feedmultiply;
|
saved_feedmultiply = feedmultiply;
|
||||||
feedmultiply = 100;
|
feedmultiply = 100;
|
||||||
@ -1045,6 +1210,122 @@ void process_commands()
|
|||||||
previous_millis_cmd = millis();
|
previous_millis_cmd = millis();
|
||||||
endstops_hit_on_purpose();
|
endstops_hit_on_purpose();
|
||||||
break;
|
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
|
case 90: // G90
|
||||||
relative_mode = false;
|
relative_mode = false;
|
||||||
break;
|
break;
|
||||||
@ -1787,7 +2068,14 @@ void process_commands()
|
|||||||
if (code_seen('S')) {
|
if (code_seen('S')) {
|
||||||
servo_position = code_value();
|
servo_position = code_value();
|
||||||
if ((servo_index >= 0) && (servo_index < NUM_SERVOS)) {
|
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);
|
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 {
|
else {
|
||||||
SERIAL_ECHO_START;
|
SERIAL_ECHO_START;
|
||||||
@ -1938,6 +2226,19 @@ void process_commands()
|
|||||||
st_synchronize();
|
st_synchronize();
|
||||||
}
|
}
|
||||||
break;
|
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
|
case 500: // M500 Store settings in EEPROM
|
||||||
{
|
{
|
||||||
Config_StoreSettings();
|
Config_StoreSettings();
|
||||||
|
@ -262,6 +262,9 @@ uint8_t Servo::attach(int pin)
|
|||||||
uint8_t Servo::attach(int pin, int min, int max)
|
uint8_t Servo::attach(int pin, int min, int max)
|
||||||
{
|
{
|
||||||
if(this->servoIndex < MAX_SERVOS ) {
|
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
|
pinMode( pin, OUTPUT) ; // set servo pin to output
|
||||||
servos[this->servoIndex].Pin.nbr = pin;
|
servos[this->servoIndex].Pin.nbr = pin;
|
||||||
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
|
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
|
||||||
|
@ -123,6 +123,9 @@ public:
|
|||||||
int read(); // returns current pulse width as an angle between 0 and 180 degrees
|
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)
|
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
|
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:
|
private:
|
||||||
uint8_t servoIndex; // index into the channel data for this servo
|
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
|
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
|
||||||
|
@ -75,6 +75,15 @@ float max_e_jerk;
|
|||||||
float mintravelfeedrate;
|
float mintravelfeedrate;
|
||||||
unsigned long axis_steps_per_sqr_second[NUM_AXIS];
|
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
|
// The current position of the tool in absolute steps
|
||||||
long position[4]; //rescaled from extern when axis_steps_per_unit are changed by gcode
|
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
|
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
|
// 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
|
// 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.
|
// 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)
|
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
|
// Calculate the buffer head after we push this byte
|
||||||
int next_buffer_head = next_block_index(block_buffer_head);
|
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();
|
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
|
// The target position of the tool in absolute steps
|
||||||
// Calculate target position 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
|
//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();
|
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)
|
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[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
|
||||||
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
|
position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
|
||||||
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
||||||
|
@ -26,6 +26,10 @@
|
|||||||
|
|
||||||
#include "Marlin.h"
|
#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
|
// 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.
|
// the source g-code and may never actually be reached if acceleration management is active.
|
||||||
typedef struct {
|
typedef struct {
|
||||||
@ -67,15 +71,33 @@ typedef struct {
|
|||||||
volatile char busy;
|
volatile char busy;
|
||||||
} block_t;
|
} 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
|
// Initialize the motion plan subsystem
|
||||||
void plan_init();
|
void plan_init();
|
||||||
|
|
||||||
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
|
// 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.
|
// 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);
|
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.
|
// 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);
|
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);
|
void plan_set_e_position(const float &e);
|
||||||
|
|
||||||
|
|
||||||
|
@ -969,6 +969,14 @@ long st_get_position(uint8_t axis)
|
|||||||
return count_pos;
|
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()
|
void finishAndDisableSteppers()
|
||||||
{
|
{
|
||||||
st_synchronize();
|
st_synchronize();
|
||||||
|
@ -61,6 +61,11 @@ void st_set_e_position(const long &e);
|
|||||||
// Get current position in steps
|
// Get current position in steps
|
||||||
long st_get_position(uint8_t axis);
|
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
|
// 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.
|
// to notify the subsystem that it is time to go to work.
|
||||||
void st_wake_up();
|
void st_wake_up();
|
||||||
|
202
Marlin/vector_3.cpp
Normal file
202
Marlin/vector_3.cpp
Normal 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
62
Marlin/vector_3.h
Normal 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
68
README.md
@ -48,6 +48,7 @@ Features:
|
|||||||
* Configurable serial port to support connection of wireless adaptors.
|
* Configurable serial port to support connection of wireless adaptors.
|
||||||
* Automatic operation of extruder/cold-end cooling fans based on nozzle temperature
|
* Automatic operation of extruder/cold-end cooling fans based on nozzle temperature
|
||||||
* RC Servo Support, specify angle or duration for continuous rotation servos.
|
* 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.
|
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
|
* G10 - retract filament according to settings of M207
|
||||||
* G11 - retract recover filament according to settings of M208
|
* G11 - retract recover filament according to settings of M208
|
||||||
* G28 - Home all Axis
|
* 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
|
* G90 - Use Absolute Coordinates
|
||||||
* G91 - Use Relative Coordinates
|
* G91 - Use Relative Coordinates
|
||||||
* G92 - Set current position to cordinates given
|
* 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)
|
* M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
|
||||||
* M304 - Set bed PID parameters P I and D
|
* M304 - Set bed PID parameters P I and D
|
||||||
* M400 - Finish all moves
|
* M400 - Finish all moves
|
||||||
|
* M401 - Lower z-probe if present
|
||||||
|
* M402 - Raise z-probe if present
|
||||||
* M500 - stores paramters in EEPROM
|
* M500 - stores paramters in EEPROM
|
||||||
* M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
|
* 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.
|
* 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.
|
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 ;-)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user