diff --git a/Documentation/MeshBedLeveling.md b/Documentation/MeshBedLeveling.md
new file mode 100644
index 0000000000..21eabb6b09
--- /dev/null
+++ b/Documentation/MeshBedLeveling.md
@@ -0,0 +1,76 @@
+==============================================
+Instructions for configuring Mesh Bed Leveling
+==============================================
+
+Background
+----------
+
+This mesh based method of leveling/compensating can compensate for an non-flat bed. There are various opinions about doing this. It was primarily written to compensate a RigidBot BIG bed (40x30cm) that was somewhat bent.
+
+Currently there is no automatic way to probe the bed like the Auto Bed Leveling feature. This might soon be implemented though, stay tuned.
+
+Theory
+------
+
+The bed is manually probed in a grid maner. During a print the Z axis compensation will be interpolated within each square using a bi-linear method. Because the grid squares can be tilting in different directions a printing move can be split on the borders of the grid squares. During fast travel moves one can sometimes notice a de-acceleration on these borders. 
+
+Mesh point probing can either be carried out from the display, or by issuing `G29` commands.
+
+The Z-endstop should be set slightly above the bed. An opto endstop is preferable but a switch with a metal arm that allow some travel though should also work.
+
+Configuration
+-------------
+
+In `Configuration.h` there are two options that can be enabled.
+
+`MESH_BED_LEVELING` will enable mesh bed leveling.<br/>
+`MANUAL_BED_LEVELING` will add the menu option for bed leveling.
+
+There are also some values that can be set.
+
+Following four define the area to cover. Default 10mm from max bed size
+
+`MESH_MIN_X`<br/>
+`MESH_MAX_X`<br/>
+`MESH_MIN_Y`<br/>
+`MESH_MAX_Y`
+
+Following two define the number of points to probe, total number will be these two multiplied. Default is 3x3 points. Don't probe more than 7x7 points (software limited)
+
+`MESH_NUM_X_POINTS`<br/> 
+`MESH_NUM_Y_POINTS`<br/>
+
+The following will set the Z-endstop height during probing. When initiating a bed leveling probing, a homing will take place and the Z-endstop will be set to this height so lowering through the endstop can take place and the bed should be within this distance. Default is 4mm
+
+`MESH_HOME_SEARCH_Z`
+
+The probed points will also be saved in the EEPROM if it has been enables. Otherwise a new probe sequence needs to be made next time the printer has been turned on.
+
+Probing the bed with the display
+--------------------------------
+
+If `MANUAL_BED_LEVELING` has been enabled then will a `Level bed` menu option be available in the `Prepare` menu.
+
+When selecting this option the printer will first do a homing, and then travel to the first probe point. There it will wait. By turning the encoder on the display the hotend can now be lowered until it touches the bed. Using a paper to feel the distance when it gets close. Pressing the encoder/button will store this point and then travel to the next point. Repeating this until all points have been probed.
+
+If the EEPROM has been enable it can be good to issue a `M500` to get these points saved.
+
+Issuing a `G29` will return the state of the mesh leveling.
+
+Probing the bed with G-codes
+----------------------------
+
+Probing the bed by G-codes follows the sequence much like doing it with the display.
+
+`G29` or `G29 S0` will return the state bed leveling.
+
+`G29 S1` will initiate the bed leveling, homing and traveling to the first point to probe.
+
+Then use your preferred Printer controller program, i.e. Printrun, to lower the hotend until it touches the bed. Using a paper to feel the distance when it gets close.
+
+`G29 S2` will store the point and travel to the next point until last point has been probed.
+
+Note
+----
+
+Depending how firm feel you aim for on the paper you can use the `Z offset` option in Slic3r to compensate a slight height diff. (I like the paper loose so I needed to put `-0.05` in Slic3r)
\ No newline at end of file
diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index fc6d6573ea..c471ec4366 100644
--- a/Marlin/Conditionals.h
+++ b/Marlin/Conditionals.h
@@ -390,16 +390,5 @@
     #define WRITE_FAN(v) WRITE(FAN_PIN, v)
   #endif
 
-  /**
-   * Sampling period of the temperature routine
-   * This override comes originally from temperature.cpp
-   * The Configuration.h option is basically ignored.
-   */
-  #ifdef PID_dT
-    #undef PID_dT
-  #endif
-  #define PID_dT ((OVERSAMPLENR * 12.0)/(F_CPU / 64.0 / 256.0))
-
-
 #endif //CONFIGURATION_LCD
 #endif //CONDITIONALS_H
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index a1bfd9d9ba..760355b896 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -209,7 +208,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp
index 29cc0412ae..fc485e2262 100644
--- a/Marlin/ConfigurationStore.cpp
+++ b/Marlin/ConfigurationStore.cpp
@@ -67,6 +67,9 @@
  *
  *  filament_size (x4)
  *
+ * Z_DUAL_ENDSTOPS
+ *  z_endstop_adj
+ *
  */
 #include "Marlin.h"
 #include "language.h"
@@ -165,6 +168,10 @@ void Config_StoreSettings()  {
     EEPROM_WRITE_VAR(i, delta_radius);              // 1 float
     EEPROM_WRITE_VAR(i, delta_diagonal_rod);        // 1 float
     EEPROM_WRITE_VAR(i, delta_segments_per_second); // 1 float
+  #elif defined(Z_DUAL_ENDSTOPS)
+    EEPROM_WRITE_VAR(i, z_endstop_adj);            // 1 floats
+    dummy = 0.0f;
+    for (int q=5; q--;) EEPROM_WRITE_VAR(i, dummy);
   #else
     dummy = 0.0f;
     for (int q=6; q--;) EEPROM_WRITE_VAR(i, dummy);
@@ -326,7 +333,12 @@ void Config_RetrieveSettings() {
       EEPROM_READ_VAR(i, delta_radius);               // 1 float
       EEPROM_READ_VAR(i, delta_diagonal_rod);         // 1 float
       EEPROM_READ_VAR(i, delta_segments_per_second);  // 1 float
+    #elif defined(Z_DUAL_ENDSTOPS)
+      EEPROM_READ_VAR(i, z_endstop_adj);
+      dummy = 0.0f;
+      for (int q=5; q--;) EEPROM_READ_VAR(i, dummy);
     #else
+      dummy = 0.0f;
       for (int q=6; q--;) EEPROM_READ_VAR(i, dummy);
     #endif
 
@@ -459,6 +471,8 @@ void Config_ResetDefault() {
     delta_diagonal_rod =  DELTA_DIAGONAL_ROD;
     delta_segments_per_second =  DELTA_SEGMENTS_PER_SECOND;
     recalc_delta_settings(delta_radius, delta_diagonal_rod);
+  #elif defined(Z_DUAL_ENDSTOPS)
+    z_endstop_adj = 0;
   #endif
 
   #ifdef ULTIPANEL
@@ -629,6 +643,14 @@ void Config_PrintSettings(bool forReplay) {
     SERIAL_ECHOPAIR(" R", delta_radius );
     SERIAL_ECHOPAIR(" S", delta_segments_per_second );
     SERIAL_EOL;
+  #elif defined(Z_DUAL_ENDSTOPS)
+    SERIAL_ECHO_START;
+    if (!forReplay) {
+      SERIAL_ECHOLNPGM("Z2 Endstop adjustement (mm):");
+      SERIAL_ECHO_START;
+    }
+    SERIAL_ECHOPAIR("  M666 Z", z_endstop_adj );
+    SERIAL_EOL;  
   #endif // DELTA
 
   #ifdef PIDTEMP
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 7d6182bf58..9ff7644f88 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -100,6 +100,31 @@
 // On a RAMPS (or other 5 driver) motherboard, using this feature will limit you to using 1 extruder.
 //#define Z_DUAL_STEPPER_DRIVERS
 
+#ifdef Z_DUAL_STEPPER_DRIVERS
+
+// Z_DUAL_ENDSTOPS is a feature to enable the use of 2 endstops for both Z steppers - Let's call them Z stepper and Z2 stepper.
+// That way the machine is capable to align the bed during home, since both Z steppers are homed. 
+// There is also an implementation of M666 (software endstops adjustment) to this feature.
+// After Z homing, this adjustment is applied to just one of the steppers in order to align the bed.
+// One just need to home the Z axis and measure the distance difference between both Z axis and apply the math: Z adjust = Z - Z2.
+// If the Z stepper axis is closer to the bed, the measure Z > Z2 (yes, it is.. think about it) and the Z adjust would be positive.
+// Play a little bit with small adjustments (0.5mm) and check the behaviour.
+// The M119 (endstops report) will start reporting the Z2 Endstop as well.
+
+#define Z_DUAL_ENDSTOPS
+
+#ifdef Z_DUAL_ENDSTOPS
+  #define Z2_STEP_PIN E2_STEP_PIN           // Stepper to be used to Z2 axis.
+  #define Z2_DIR_PIN E2_DIR_PIN
+  #define Z2_ENABLE_PIN E2_ENABLE_PIN
+  #define Z2_MAX_PIN 36                     //Endstop used for Z2 axis. In this case I'm using XMAX in a Rumba Board (pin 36)
+  const bool Z2_MAX_ENDSTOP_INVERTING = false;
+  #define DISABLE_XMAX_ENDSTOP              //Better to disable the XMAX to avoid conflict. Just rename "XMAX_ENDSTOP" by the endstop you are using for Z2 axis.
+#endif
+
+
+#endif
+
 // Same again but for Y Axis.
 //#define Y_DUAL_STEPPER_DRIVERS
 
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index cc1f111188..bbd7ac3dbb 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -242,6 +242,8 @@ extern float home_offset[3];
   extern float delta_diagonal_rod;
   extern float delta_segments_per_second;
   void recalc_delta_settings(float radius, float diagonal_rod);
+#elif defined(Z_DUAL_ENDSTOPS)
+extern float z_endstop_adj;
 #endif
 #ifdef SCARA
   extern float axis_scaling[3];  // Build size scaling
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5109118190..6130ac8f50 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -248,6 +248,8 @@ float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
 float home_offset[3] = { 0, 0, 0 };
 #ifdef DELTA
   float endstop_adj[3] = { 0, 0, 0 };
+#elif defined(Z_DUAL_ENDSTOPS)
+  float z_endstop_adj = 0;
 #endif
 
 float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
@@ -393,7 +395,9 @@ static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
 static bool relative_mode = false;  //Determines Absolute or Relative Coordinates
 
 static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
+#ifdef SDSUPPORT
 static bool fromsd[BUFSIZE];
+#endif //!SDSUPPORT
 static int bufindr = 0;
 static int bufindw = 0;
 static int buflen = 0;
@@ -653,10 +657,12 @@ void setup()
   SERIAL_ECHO(freeMemory());
   SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
   SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
+  #ifdef SDSUPPORT
   for(int8_t i = 0; i < BUFSIZE; i++)
   {
     fromsd[i] = false;
   }
+  #endif //!SDSUPPORT
 
   // loads data from EEPROM if available else uses defaults (and resets step acceleration rate)
   Config_RetrieveSettings();
@@ -762,8 +768,9 @@ void get_command()
         return;
       }
       cmdbuffer[bufindw][serial_count] = 0; //terminate string
-
+      #ifdef SDSUPPORT
       fromsd[bufindw] = false;
+      #endif //!SDSUPPORT
       if(strchr(cmdbuffer[bufindw], 'N') != NULL)
       {
         strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
@@ -973,7 +980,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
 
   static float x_home_pos(int extruder) {
     if (extruder == 0)
-      return base_home_pos(X_AXIS) + add_homing[X_AXIS];
+    return base_home_pos(X_AXIS) + home_offset[X_AXIS];
     else
       // In dual carriage mode the extruder offset provides an override of the
       // second X-carriage offset when homed - otherwise X2_HOME_POS is used.
@@ -1166,6 +1173,7 @@ static void run_z_probe() {
     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();
+    endstops_hit_on_purpose();
 
     // move back down slowly to find bed
     
@@ -1183,6 +1191,7 @@ static void run_z_probe() {
     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();
+    endstops_hit_on_purpose();
 
     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
@@ -1387,11 +1396,11 @@ static float probe_pt(float x, float y, float z_before, ProbeAction retract_acti
   if (verbose_level > 2) {
     SERIAL_PROTOCOLPGM(MSG_BED);
     SERIAL_PROTOCOLPGM(" X: ");
-    SERIAL_PROTOCOL(x + 0.0001);
+    SERIAL_PROTOCOL_F(x, 3);
     SERIAL_PROTOCOLPGM(" Y: ");
-    SERIAL_PROTOCOL(y + 0.0001);
+    SERIAL_PROTOCOL_F(y, 3);
     SERIAL_PROTOCOLPGM(" Z: ");
-    SERIAL_PROTOCOL(measured_z + 0.0001);
+    SERIAL_PROTOCOL_F(measured_z, 3);
     SERIAL_EOL;
   }
   return measured_z;
@@ -1487,6 +1496,9 @@ static void homeaxis(int axis) {
       }
     #endif
 #endif // Z_PROBE_SLED
+    #ifdef Z_DUAL_ENDSTOPS
+      if (axis==Z_AXIS) In_Homing_Process(true);
+    #endif
     destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
     feedrate = homing_feedrate[axis];
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
@@ -1512,6 +1524,27 @@ static void homeaxis(int axis) {
 
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
     st_synchronize();
+    #ifdef Z_DUAL_ENDSTOPS
+      if (axis==Z_AXIS)
+      {
+        feedrate = homing_feedrate[axis];
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+        if (axis_home_dir > 0)
+        {
+          destination[axis] = (-1) * fabs(z_endstop_adj);
+          if (z_endstop_adj > 0) Lock_z_motor(true); else Lock_z2_motor(true);
+        } else {
+          destination[axis] = fabs(z_endstop_adj);
+          if (z_endstop_adj < 0) Lock_z_motor(true); else Lock_z2_motor(true);        
+        }
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        Lock_z_motor(false);
+        Lock_z2_motor(false);
+        In_Homing_Process(false);
+      }
+    #endif
+
 #ifdef DELTA
     // retrace by the amount specified in endstop_adj
     if (endstop_adj[axis] * axis_home_dir < 0) {
@@ -1754,7 +1787,7 @@ inline void gcode_G28() {
 
   enable_endstops(true);
 
-  for (int i = X_AXIS; i <= Z_AXIS; i++) destination[i] = current_position[i];
+  for (int i = X_AXIS; i <= NUM_AXIS; i++) destination[i] = current_position[i];
 
   feedrate = 0.0;
 
@@ -1944,7 +1977,7 @@ inline void gcode_G28() {
     if (code_seen(axis_codes[Z_AXIS]) && code_value_long() != 0)
       current_position[Z_AXIS] = code_value() + home_offset[Z_AXIS];
 
-    #ifdef ENABLE_AUTO_BED_LEVELING
+    #if defined(ENABLE_AUTO_BED_LEVELING) && (Z_HOME_DIR < 0)
       if (home_all_axis || code_seen(axis_codes[Z_AXIS]))
         current_position[Z_AXIS] += zprobe_zoffset;  //Add Z_Probe offset (the distance is negative)
     #endif
@@ -2083,6 +2116,9 @@ inline void gcode_G28() {
    *
    *  S  Set the XY travel speed between probe points (in mm/min)
    *
+   *  D  Dry-Run mode. Just evaluate the bed Topology - It does not apply or clean the rotation Matrix
+   *     Useful to check the topology after a first run of G29.
+   *
    *  V  Set the verbose level (0-4). Example: "G29 V3"
    *
    *  T  Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
@@ -2124,6 +2160,7 @@ inline void gcode_G28() {
       }
     }
 
+    bool dryrun = code_seen('D') || code_seen('d');
     bool enhanced_g29 = code_seen('E') || code_seen('e');
 
     #ifdef AUTO_BED_LEVELING_GRID
@@ -2133,7 +2170,10 @@ inline void gcode_G28() {
     #endif
 
       if (verbose_level > 0)
+      {
         SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n");
+        if (dryrun) SERIAL_ECHOLN("Running in DRY-RUN mode");
+      }
 
       int auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS;
       #ifndef DELTA
@@ -2190,22 +2230,27 @@ inline void gcode_G28() {
 
     st_synchronize();
 
-    #ifdef DELTA
-      reset_bed_level();
-    #else
+    if (!dryrun)
+    {
+      #ifdef DELTA
+        reset_bed_level();
+      #else
 
-    // 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 during 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]);
-  #endif
+      // 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 during 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]);
+
+      #endif
+    }
+    
     setup_for_endstop_move();
 
     feedrate = homing_feedrate[Z_AXIS];
@@ -2248,13 +2293,11 @@ inline void gcode_G28() {
           xStart = 0;
           xStop = auto_bed_leveling_grid_points;
           xInc = 1;
-          zig = false;
         }
         else {
           xStart = auto_bed_leveling_grid_points - 1;
           xStop = -1;
           xInc = -1;
-          zig = true;
         }
 
         #ifndef DELTA
@@ -2341,7 +2384,7 @@ inline void gcode_G28() {
         SERIAL_PROTOCOLPGM("+-----------+\n");
 
         for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
-          for (int xx = auto_bed_leveling_grid_points - 1; xx >= 0; xx--) {
+          for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
             int ind = yy * auto_bed_leveling_grid_points + xx;
             float diff = eqnBVector[ind] - mean;
             if (diff >= 0.0)
@@ -2357,12 +2400,12 @@ inline void gcode_G28() {
       } //do_topography_map
 
 
-      set_bed_level_equation_lsq(plane_equation_coefficients);
+      if (!dryrun) set_bed_level_equation_lsq(plane_equation_coefficients);
       free(plane_equation_coefficients);
-    #else
-      extrapolate_unprobed_bed_level();
+    #else //Delta
+      if (!dryrun) extrapolate_unprobed_bed_level();
       print_bed_level();
-    #endif
+    #endif //Delta
 
     #else // !AUTO_BED_LEVELING_GRID
 
@@ -2381,25 +2424,27 @@ inline void gcode_G28() {
         z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeEngageAndRetract, verbose_level);
       }
       clean_up_after_endstop_move();
-      set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
+      if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
 
     #endif // !AUTO_BED_LEVELING_GRID
 
   #ifndef DELTA
-    if (verbose_level > 0)
-      plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
+    if (verbose_level > 0) plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
 
     // 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];
+    if (!dryrun)
+    {
+      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]);
+      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]);
+    }
   #endif
 
   #ifdef Z_PROBE_SLED
@@ -3452,6 +3497,11 @@ inline void gcode_M119() {
     SERIAL_PROTOCOLPGM(MSG_Z_MAX);
     SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
   #endif
+  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
+    SERIAL_PROTOCOLPGM(MSG_Z2_MAX);
+    SERIAL_PROTOCOLLN(((READ(Z2_MAX_PIN)^Z2_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
+  #endif
+  
 }
 
 /**
@@ -3645,6 +3695,16 @@ inline void gcode_M206() {
       }
     }
   }
+#elif defined(Z_DUAL_ENDSTOPS)
+  /**
+   * M666: For Z Dual Endstop setup, set z axis offset to the z2 axis.
+   */
+  inline void gcode_M666() {
+   if (code_seen('Z')) z_endstop_adj = code_value();
+   SERIAL_ECHOPAIR("Z Endstop Adjustment set to (mm):", z_endstop_adj );
+   SERIAL_EOL;
+  }
+  
 #endif // DELTA
 
 #ifdef FWRETRACT
@@ -4894,6 +4954,10 @@ void process_commands() {
         case 666: // M666 set delta endstop adjustment
           gcode_M666();
           break;
+      #elif defined(Z_DUAL_ENDSTOPS)
+        case 666: // M666 set delta endstop adjustment
+          gcode_M666();
+          break;
       #endif // DELTA
 
       #ifdef FWRETRACT
diff --git a/Marlin/configurator/config/Configuration.h b/Marlin/configurator/config/Configuration.h
index 71cbdebaec..bfc62f5789 100644
--- a/Marlin/configurator/config/Configuration.h
+++ b/Marlin/configurator/config/Configuration.h
@@ -193,7 +193,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -218,7 +217,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/Felix/Configuration.h b/Marlin/example_configurations/Felix/Configuration.h
index e9801813f8..865a551677 100644
--- a/Marlin/example_configurations/Felix/Configuration.h
+++ b/Marlin/example_configurations/Felix/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
   // Felix 2.0+ electronics with v4 Hotend
   #define DEFAULT_Kp 12
@@ -199,7 +198,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/Felix/Configuration_DUAL.h b/Marlin/example_configurations/Felix/Configuration_DUAL.h
index e9e4623cab..e9e0adba41 100644
--- a/Marlin/example_configurations/Felix/Configuration_DUAL.h
+++ b/Marlin/example_configurations/Felix/Configuration_DUAL.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
   // Felix 2.0+ electronics with v4 Hotend
   #define DEFAULT_Kp 12
@@ -199,7 +198,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index c5b0243d57..5c426a5da7 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -215,7 +214,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/K8200/Configuration.h b/Marlin/example_configurations/K8200/Configuration.h
index bc0f3e5dfe..96a261caf3 100644
--- a/Marlin/example_configurations/K8200/Configuration.h
+++ b/Marlin/example_configurations/K8200/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -214,7 +213,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/SCARA/Configuration.h b/Marlin/example_configurations/SCARA/Configuration.h
index d42bebe3af..30dc731840 100644
--- a/Marlin/example_configurations/SCARA/Configuration.h
+++ b/Marlin/example_configurations/SCARA/Configuration.h
@@ -202,7 +202,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -238,7 +237,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index 481b591255..7a395d012d 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -214,7 +213,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h
index 4194575ef8..da53629a8f 100644
--- a/Marlin/example_configurations/delta/generic/Configuration.h
+++ b/Marlin/example_configurations/delta/generic/Configuration.h
@@ -217,7 +217,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -242,7 +241,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
index 9e27c95ce6..37da72020c 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h
@@ -218,7 +218,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -243,7 +242,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/makibox/Configuration.h b/Marlin/example_configurations/makibox/Configuration.h
index f6561b3af8..3e8c07fe46 100644
--- a/Marlin/example_configurations/makibox/Configuration.h
+++ b/Marlin/example_configurations/makibox/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // Ultimaker
@@ -209,7 +208,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration.h b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
index 17928b536b..22e16e4eeb 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration.h
@@ -184,7 +184,6 @@ Here are some standard links for getting your machine calibrated:
                                   // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
   #define PID_INTEGRAL_DRIVE_MAX PID_MAX  //limit for the integral term
   #define K1 0.95 //smoothing factor within the PID
-  #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine
 
 // If you are using a pre-configured hotend then you can use one of the value sets by uncommenting it
 // J-Head Mk V-B
@@ -214,7 +213,7 @@ Here are some standard links for getting your machine calibrated:
 // Select PID or bang-bang with PIDTEMPBED. If bang-bang, BED_LIMIT_SWITCHING will enable hysteresis
 //
 // Uncomment this to enable PID on the bed. It uses the same frequency PWM as the extruder.
-// If your PID_dT above is the default, and correct for your hardware/configuration, that means 7.689Hz,
+// If your PID_dT is the default, and correct for your hardware/configuration, that means 7.689Hz,
 // which is fine for driving a square wave into a resistive load and does not significantly impact you FET heating.
 // This also works fine on a Fotek SSR-10DA Solid State Relay into a 250W heater.
 // If your configuration is significantly different than this and you don't understand the issues involved, you probably
diff --git a/Marlin/language.h b/Marlin/language.h
index aa8851b3af..3f2291a947 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -131,6 +131,7 @@
 #define MSG_Y_MAX                           "y_max: "
 #define MSG_Z_MIN                           "z_min: "
 #define MSG_Z_MAX                           "z_max: "
+#define MSG_Z2_MAX                          "z2_max: "
 #define MSG_M119_REPORT                     "Reporting endstop status"
 #define MSG_ENDSTOP_HIT                     "TRIGGERED"
 #define MSG_ENDSTOP_OPEN                    "open"
diff --git a/Marlin/pins.h b/Marlin/pins.h
index 88c5cc78cb..939dab5e66 100644
--- a/Marlin/pins.h
+++ b/Marlin/pins.h
@@ -178,6 +178,35 @@
   #define Z_MIN_PIN          -1
 #endif
 
+#ifdef DISABLE_XMAX_ENDSTOP
+  #undef X_MAX_PIN
+  #define X_MAX_PIN          -1
+#endif
+
+#ifdef DISABLE_XMIN_ENDSTOP
+  #undef X_MIN_PIN 
+  #define X_MIN_PIN          -1
+#endif
+
+#ifdef DISABLE_YMAX_ENDSTOP
+  #define Y_MAX_PIN          -1
+#endif
+
+#ifdef DISABLE_YMIN_ENDSTOP
+  #undef Y_MIN_PIN
+  #define Y_MIN_PIN          -1
+#endif
+
+#ifdef DISABLE_ZMAX_ENDSTOP
+  #undef Z_MAX_PIN
+  #define Z_MAX_PIN          -1
+#endif
+
+#ifdef DISABLE_ZMIN_ENDSTOP
+  #undef Z_MIN_PIN 
+  #define Z_MIN_PIN          -1
+#endif
+
 #define SENSITIVE_PINS { 0, 1, X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, Y_STEP_PIN, Y_DIR_PIN, Y_ENABLE_PIN, Y_MIN_PIN, Y_MAX_PIN, Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, Z_MIN_PIN, Z_MAX_PIN, PS_ON_PIN, \
                         HEATER_BED_PIN, FAN_PIN, \
                         _E0_PINS _E1_PINS _E2_PINS _E3_PINS \
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index d3651b6b41..8be4b98af4 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -48,6 +48,12 @@ block_t *current_block;  // A pointer to the block currently being traced
 static unsigned char out_bits;        // The next stepping-bits to be output
 static unsigned int cleaning_buffer_counter;  
 
+#ifdef Z_DUAL_ENDSTOPS
+  static bool performing_homing = false, 
+              locked_z_motor = false, 
+              locked_z2_motor = false;
+#endif
+
 // Counter variables for the bresenham line tracer
 static long counter_x, counter_y, counter_z, counter_e;
 volatile static unsigned long step_events_completed; // The number of step events executed in the current block
@@ -84,7 +90,13 @@ static bool old_x_min_endstop = false,
             old_y_min_endstop = false,
             old_y_max_endstop = false,
             old_z_min_endstop = false,
+            #ifndef Z_DUAL_ENDSTOPS
             old_z_max_endstop = false;
+            #else
+              old_z_max_endstop = false,
+              old_z2_min_endstop = false,
+              old_z2_max_endstop = false;
+            #endif
 
 static bool check_endstops = true;
 
@@ -128,7 +140,23 @@ volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
 
 #ifdef Z_DUAL_STEPPER_DRIVERS
   #define Z_APPLY_DIR(v,Q) { Z_DIR_WRITE(v); Z2_DIR_WRITE(v); }
-  #define Z_APPLY_STEP(v,Q) { Z_STEP_WRITE(v); Z2_STEP_WRITE(v); }
+  #ifdef Z_DUAL_ENDSTOPS
+    #define Z_APPLY_STEP(v,Q) \
+    if (performing_homing) { \
+      if (Z_HOME_DIR > 0) {\
+        if (!(old_z_max_endstop && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
+        if (!(old_z2_max_endstop && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
+      } else {\
+        if (!(old_z_min_endstop && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
+        if (!(old_z2_min_endstop && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
+      } \
+    } else { \
+      Z_STEP_WRITE(v); \
+      Z2_STEP_WRITE(v); \
+    }
+  #else
+    #define Z_APPLY_STEP(v,Q) Z_STEP_WRITE(v), Z2_STEP_WRITE(v)
+  #endif
 #else
   #define Z_APPLY_DIR(v,Q) Z_DIR_WRITE(v)
   #define Z_APPLY_STEP(v,Q) Z_STEP_WRITE(v)
@@ -465,28 +493,66 @@ ISR(TIMER1_COMPA_vect) {
     }
 
     if (TEST(out_bits, Z_AXIS)) {   // -direction
-      Z_DIR_WRITE(INVERT_Z_DIR);
-      #ifdef Z_DUAL_STEPPER_DRIVERS
-        Z2_DIR_WRITE(INVERT_Z_DIR);
-      #endif
-
+      Z_APPLY_DIR(INVERT_Z_DIR,0);
       count_direction[Z_AXIS] = -1;
-      if (check_endstops) {
-        #if defined(Z_MIN_PIN) && Z_MIN_PIN >= 0
-          UPDATE_ENDSTOP(z, Z, min, MIN);
+      if (check_endstops) 
+      {
+        #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
+          #ifndef Z_DUAL_ENDSTOPS
+            UPDATE_ENDSTOP(z, Z, min, MIN);
+          #else
+            bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+            #if defined(Z2_MIN_PIN) && Z2_MIN_PIN > -1
+              bool z2_min_endstop=(READ(Z2_MIN_PIN) != Z2_MIN_ENDSTOP_INVERTING);
+            #else
+              bool z2_min_endstop=z_min_endstop;
+            #endif
+            if(((z_min_endstop && old_z_min_endstop) || (z2_min_endstop && old_z2_min_endstop)) && (current_block->steps[Z_AXIS] > 0))
+            {
+              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+              endstop_z_hit=true;
+              if (!(performing_homing) || ((performing_homing)&&(z_min_endstop && old_z_min_endstop)&&(z2_min_endstop && old_z2_min_endstop))) //if not performing home or if both endstops were trigged during homing...
+              {
+                step_events_completed = current_block->step_event_count;
+              } 
+            }
+            old_z_min_endstop = z_min_endstop;
+            old_z2_min_endstop = z2_min_endstop;
+          #endif
         #endif
       }
     }
     else { // +direction
-      Z_DIR_WRITE(!INVERT_Z_DIR);
-      #ifdef Z_DUAL_STEPPER_DRIVERS
-        Z2_DIR_WRITE(!INVERT_Z_DIR);
-      #endif
-
+      Z_APPLY_DIR(!INVERT_Z_DIR,0);
       count_direction[Z_AXIS] = 1;
       if (check_endstops) {
         #if defined(Z_MAX_PIN) && Z_MAX_PIN >= 0
-          UPDATE_ENDSTOP(z, Z, max, MAX);
+          #ifndef Z_DUAL_ENDSTOPS
+            UPDATE_ENDSTOP(z, Z, max, MAX);
+          #else
+            bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
+            #if defined(Z2_MAX_PIN) && Z2_MAX_PIN > -1
+              bool z2_max_endstop=(READ(Z2_MAX_PIN) != Z2_MAX_ENDSTOP_INVERTING);
+            #else
+              bool z2_max_endstop=z_max_endstop;
+            #endif
+            if(((z_max_endstop && old_z_max_endstop) || (z2_max_endstop && old_z2_max_endstop)) && (current_block->steps[Z_AXIS] > 0))
+            {
+              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+              endstop_z_hit=true;
+
+//              if (z_max_endstop && old_z_max_endstop) SERIAL_ECHOLN("z_max_endstop = true");
+//              if (z2_max_endstop && old_z2_max_endstop) SERIAL_ECHOLN("z2_max_endstop = true");
+
+            
+              if (!(performing_homing) || ((performing_homing)&&(z_max_endstop && old_z_max_endstop)&&(z2_max_endstop && old_z2_max_endstop))) //if not performing home or if both endstops were trigged during homing...
+              {
+                step_events_completed = current_block->step_event_count;
+              } 
+            }
+            old_z_max_endstop = z_max_endstop;
+            old_z2_max_endstop = z2_max_endstop;
+          #endif
         #endif
       }
     }
@@ -845,6 +911,13 @@ void st_init() {
     #endif
   #endif
 
+  #if defined(Z2_MAX_PIN) && Z2_MAX_PIN >= 0
+    SET_INPUT(Z2_MAX_PIN);
+    #ifdef ENDSTOPPULLUP_ZMAX
+      WRITE(Z2_MAX_PIN,HIGH);
+    #endif
+  #endif  
+  
   #define AXIS_INIT(axis, AXIS, PIN) \
     AXIS ##_STEP_INIT; \
     AXIS ##_STEP_WRITE(INVERT_## PIN ##_STEP_PIN); \
@@ -1174,3 +1247,9 @@ void microstep_readings() {
     SERIAL_PROTOCOLLN(digitalRead(E1_MS2_PIN));
   #endif
 }
+
+#ifdef Z_DUAL_ENDSTOPS
+  void In_Homing_Process(bool state) { performing_homing = state; }
+  void Lock_z_motor(bool state) { locked_z_motor = state; }
+  void Lock_z2_motor(bool state) { locked_z2_motor = state; }
+#endif
diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index a1f2916090..d6c17d60f6 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -97,6 +97,12 @@ void digipot_current(uint8_t driver, int current);
 void microstep_init();
 void microstep_readings();
 
+#ifdef Z_DUAL_ENDSTOPS
+  void In_Homing_Process(bool state);
+  void Lock_z_motor(bool state);
+  void Lock_z2_motor(bool state);
+#endif
+
 #ifdef BABYSTEPPING
   void babystep(const uint8_t axis,const bool direction); // perform a short step with a single stepper motor, outside of any convention
 #endif
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 713d0312f6..cb70013246 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -45,6 +45,10 @@
   #define K2 (1.0-K1)
 #endif
 
+#if defined(PIDTEMPBED) || defined(PIDTEMP)
+  #define PID_dT ((OVERSAMPLENR * 12.0)/(F_CPU / 64.0 / 256.0))
+#endif
+
 //===========================================================================
 //============================= public variables ============================
 //===========================================================================
@@ -576,6 +580,12 @@ void manage_heater() {
 
   updateTemperaturesFromRawValues();
 
+  #ifdef HEATER_0_USES_MAX6675
+    float ct = current_temperature[0];
+    if (ct > min(HEATER_0_MAXTEMP, 1023)) max_temp_error(0);
+    if (ct < max(HEATER_0_MINTEMP, 0.01)) min_temp_error(0);
+  #endif //HEATER_0_USES_MAX6675
+
   unsigned long ms = millis();
 
   // Loop through all extruders
@@ -607,7 +617,7 @@ void manage_heater() {
     #ifdef TEMP_SENSOR_1_AS_REDUNDANT
       if (fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
         disable_heater();
-        _temp_error(-1, MSG_EXTRUDER_SWITCHED_OFF, MSG_ERR_REDUNDANT_TEMP);
+        _temp_error(0, PSTR(MSG_EXTRUDER_SWITCHED_OFF), PSTR(MSG_ERR_REDUNDANT_TEMP));
       }
     #endif //TEMP_SENSOR_1_AS_REDUNDANT
 
@@ -1162,20 +1172,40 @@ enum TempState {
   StartupDelay // Startup, delay initial temp reading a tiny bit so the hardware can settle
 };
 
+#ifdef TEMP_SENSOR_1_AS_REDUNDANT
+  #define TEMP_SENSOR_COUNT 2
+#else
+  #define TEMP_SENSOR_COUNT EXTRUDERS
+#endif
+
+static unsigned long raw_temp_value[TEMP_SENSOR_COUNT] = { 0 };
+static unsigned long raw_temp_bed_value = 0;
+
+static void set_current_temp_raw() {
+  #ifndef HEATER_0_USES_MAX6675
+    current_temperature_raw[0] = raw_temp_value[0];
+  #endif
+  #if EXTRUDERS > 1
+    current_temperature_raw[1] = raw_temp_value[1];
+    #if EXTRUDERS > 2
+      current_temperature_raw[2] = raw_temp_value[2];
+      #if EXTRUDERS > 3
+        current_temperature_raw[3] = raw_temp_value[3];
+      #endif
+    #endif
+  #endif
+  #ifdef TEMP_SENSOR_1_AS_REDUNDANT
+    redundant_temperature_raw = raw_temp_value[1];
+  #endif
+  current_temperature_bed_raw = raw_temp_bed_value;
+}
+
 //
 // Timer 0 is shared with millies
 //
 ISR(TIMER0_COMPB_vect) {
-  #ifdef TEMP_SENSOR_1_AS_REDUNDANT
-    #define TEMP_SENSOR_COUNT 2
-  #else 
-    #define TEMP_SENSOR_COUNT EXTRUDERS
-  #endif
-
   //these variables are only accesible from the ISR, but static, so they don't lose their value
   static unsigned char temp_count = 0;
-  static unsigned long raw_temp_value[TEMP_SENSOR_COUNT] = { 0 };
-  static unsigned long raw_temp_bed_value = 0;
   static TempState temp_state = StartupDelay;
   static unsigned char pwm_count = BIT(SOFT_PWM_SCALE);
 
@@ -1478,22 +1508,7 @@ ISR(TIMER0_COMPB_vect) {
 
   if (temp_count >= OVERSAMPLENR) { // 10 * 16 * 1/(16000000/64/256)  = 164ms.
     if (!temp_meas_ready) { //Only update the raw values if they have been read. Else we could be updating them during reading.
-      #ifndef HEATER_0_USES_MAX6675
-        current_temperature_raw[0] = raw_temp_value[0];
-      #endif
-      #if EXTRUDERS > 1
-        current_temperature_raw[1] = raw_temp_value[1];
-        #if EXTRUDERS > 2
-          current_temperature_raw[2] = raw_temp_value[2];
-          #if EXTRUDERS > 3
-            current_temperature_raw[3] = raw_temp_value[3];
-          #endif
-        #endif
-      #endif
-      #ifdef TEMP_SENSOR_1_AS_REDUNDANT
-        redundant_temperature_raw = raw_temp_value[1];
-      #endif
-      current_temperature_bed_raw = raw_temp_bed_value;
+      set_current_temp_raw();
     } //!temp_meas_ready
 
     // Filament Sensor - can be read any time since IIR filtering is used
@@ -1506,11 +1521,7 @@ ISR(TIMER0_COMPB_vect) {
     for (int i = 0; i < TEMP_SENSOR_COUNT; i++) raw_temp_value[i] = 0;
     raw_temp_bed_value = 0;
 
-    #ifdef HEATER_0_USES_MAX6675
-      float ct = current_temperature[0];
-      if (ct > min(HEATER_0_MAXTEMP, 1023)) max_temp_error(0);
-      if (ct < max(HEATER_0_MINTEMP, 0.01)) min_temp_error(0);
-    #else
+    #ifndef HEATER_0_USES_MAX6675
       #if HEATER_0_RAW_LO_TEMP > HEATER_0_RAW_HI_TEMP
         #define GE0 <=
       #else
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 7a8e714158..7be9fb1df9 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -204,7 +204,7 @@ static void menu_action_setting_edit_callback_long5(const char* pstr, unsigned l
   #define MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(type, label, args...) MENU_ITEM(setting_edit_callback_ ## type, label, PSTR(label), ## args)
 #endif //!ENCODER_RATE_MULTIPLIER
 #define END_MENU() \
-    if (encoderLine >= _menuItemNr) encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; encoderLine = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM;\
+    if (encoderLine >= _menuItemNr) { encoderPosition = _menuItemNr * ENCODER_STEPS_PER_MENU_ITEM - 1; encoderLine = encoderPosition / ENCODER_STEPS_PER_MENU_ITEM; }\
     if (encoderLine >= currentMenuViewOffset + LCD_HEIGHT) { currentMenuViewOffset = encoderLine - LCD_HEIGHT + 1; lcdDrawUpdate = 1; _lineNr = currentMenuViewOffset - 1; _drawLineNr = -1; } \
     } } while(0)
 
diff --git a/README.md b/README.md
index 01540cdad3..e281d3602a 100644
--- a/README.md
+++ b/README.md
@@ -11,6 +11,7 @@
     * [Filament Sensor](/Documentation/FilamentSensor.md)
     * [Ramps Servo Power](/Documentation/RampsServoPower.md)
     * [LCD Language - Font - System](Documentation/LCDLanguageFont.md)
+    * [Mesh Bed Leveling](/Documentation/MeshBedLeveling.md)
 
 ##### [RepRap.org Wiki Page](http://reprap.org/wiki/Marlin)