diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index fe22331d0..502ebd438 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -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) diff --git a/Marlin/Marlin.ino b/Marlin/Marlin.ino new file mode 100644 index 000000000..2d6211c97 --- /dev/null +++ b/Marlin/Marlin.ino @@ -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 . + */ + +/* + 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 + #include + #elif defined(LCD_I2C_TYPE_MCP23017) || defined(LCD_I2C_TYPE_MCP23008) + #include + #include + #elif defined(DOGLCD) + #include // library for graphics LCD by Oli Kraus (https://code.google.com/p/u8glib/) + #else + #include // library for character LCD + #endif +#endif + +#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1 +#include +#endif diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 65b829bdc..932af1355 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -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 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(); diff --git a/Marlin/Servo.cpp b/Marlin/Servo.cpp index 47c16aa71..5f8c7efe3 100644 --- a/Marlin/Servo.cpp +++ b/Marlin/Servo.cpp @@ -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 diff --git a/Marlin/Servo.h b/Marlin/Servo.h index 17c99f797..f2e0be1a9 100644 --- a/Marlin/Servo.h +++ b/Marlin/Servo.h @@ -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 diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 8eff19175..008c8d257 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -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]); diff --git a/Marlin/planner.h b/Marlin/planner.h index 597eeb1c0..9df017460 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -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); diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 7d738ac5e..8b39a41d6 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -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(); diff --git a/Marlin/stepper.h b/Marlin/stepper.h index ac9dd8af5..82b41c90d 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -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(); diff --git a/Marlin/vector_3.cpp b/Marlin/vector_3.cpp new file mode 100644 index 000000000..8c8a0e1dc --- /dev/null +++ b/Marlin/vector_3.cpp @@ -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 +#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 + diff --git a/Marlin/vector_3.h b/Marlin/vector_3.h new file mode 100644 index 000000000..b08c336e8 --- /dev/null +++ b/Marlin/vector_3.h @@ -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 diff --git a/README.md b/README.md index 0b0b74e08..de5528f05 100644 --- a/README.md +++ b/README.md @@ -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 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 ;-) +