Bed Auto Leveling feature

Check the Readme for instruction how to enable and configure the feature

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)

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 <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

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

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

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

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]);     

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);
 
 

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();

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();

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 <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
+

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

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<temperature> sets the target temperature. (default target temperature = 150C)
 *  M304 - Set bed PID parameters P I and D
 *  M400 - Finish all moves
+*  M401 - Lower z-probe if present
+*  M402 - Raise z-probe if present
 *  M500 - stores paramters in EEPROM
 *  M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
 *  M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
@@ -249,6 +254,69 @@ If all goes well the firmware is uploading
 
 That's ok.  Enjoy Silky Smooth Printing.
 
+===============================================
+Instructions for configuring Bed Auto Leveling
+===============================================
+Uncomment the "ENABLE_AUTO_BED_LEVELING" define (commented by default)
+
+You will probably need a swivel Z-MIN endstop in the extruder. A rc servo do a great job.
+Check the system working here: http://www.youtube.com/watch?v=3IKMeOYz-1Q (Enable English subtitles)
+Teasing ;-) video: http://www.youtube.com/watch?v=x8eqSQNAyro
+
+In order to get the servo working, you need to enable:
+
+* \#define NUM_SERVOS 1 // Servo index starts with 0 for M280 command
+
+* \#define SERVO_ENDSTOPS {-1, -1, 0} // Servo index for X, Y, Z. Disable with -1
+
+* \#define SERVO_ENDSTOP_ANGLES {0,0, 0,0, 165,60} // X,Y,Z Axis Extend and Retract angles
+
+
+The first define tells firmware how many servos you have.
+The second tells what axis this servo will be attached to. In the example above, we have a servo in Z axis.
+The third one tells the angle in 2 situations: Probing (165º) and resting (60º). Check this with command M280 P0 S{angle}
+
+Next you need to define the Z endstop (probe) offset from hotend.
+My preferred method:
+
+* a) Make a small mark in the bed with a marker/felt-tip pen.
+* b) Place the hotend tip as *exactly* as possible on the mark, touching the bed. Raise the hotend 0.1mm (a regular paper thickness) and zero all axis (G92 X0 Y0 Z0);
+* d) Raise the hotend 10mm (or more) for probe clearance, lower the Z probe (Z-Endstop) with M401 and place it just on that mark by moving X, Y and Z;
+* e) Lower the Z in 0.1mm steps, with the probe always touching the mark (it may be necessary to adjust X and Y as well) until you hear the "click" meaning the mechanical endstop was trigged. You can confirm with M119;
+* f) Now you have the probe in the same place as your hotend tip was before. Perform a M114 and write down the values, for example: X:24.3 Y:-31.4 Z:5.1;
+* g) You can raise the z probe with M402 command;
+* h) Fill the defines bellow multiplying the values by "-1" (just change the signal)
+
+
+* \#define X_PROBE_OFFSET_FROM_EXTRUDER -24.3
+* \#define Y_PROBE_OFFSET_FROM_EXTRUDER 31.4
+* \#define Z_PROBE_OFFSET_FROM_EXTRUDER -5.1
+
+
+The following options define the probing positions. These are good starting values.
+I recommend to keep a better clearance from borders in the first run and then make the probes as close as possible to borders:
+
+* \#define LEFT_PROBE_BED_POSITION 30
+* \#define RIGHT_PROBE_BED_POSITION 140
+* \#define BACK_PROBE_BED_POSITION 140
+* \#define FRONT_PROBE_BED_POSITION 30
+
+A few more options:
+
+* \#define XY_TRAVEL_SPEED 6000
+
+X and Y axis travel speed between probes, in mm/min.
+Bear in mind that really fast moves may render step skipping. 6000 mm/min (100mm/s) is a good value.
+
+* \#define Z_RAISE_BEFORE_PROBING 10
+* \#define Z_RAISE_BETWEEN_PROBINGS 10
+
+The Z axis is lifted when traveling to the first probe point by Z_RAISE_BEFORE_PROBING value
+and then lifted when traveling from first to second and second to third point by Z_RAISE_BETWEEN_PROBINGS.
+All values are in mm as usual. 
+
+That's it.. enjoy never having to calibrate your Z endstop neither leveling your bed by hand anymore ;-)
+