Initial split-up of G-code handlers by category

Marlin/src/Marlin.cpp

@@ -25,220 +25,7 @@
  *
  * This firmware is a mashup between Sprinter and grbl.
  *  - https://github.com/kliment/Sprinter
- *  - https://github.com/simen/grbl/tree
- */
-
-/**
- * -----------------
- * G-Codes in Marlin
- * -----------------
- *
- * Helpful G-code references:
- *  - http://linuxcnc.org/handbook/gcode/g-code.html
- *  - http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
- *
- * Help to document Marlin's G-codes online:
- *  - http://reprap.org/wiki/G-code
- *  - https://github.com/MarlinFirmware/MarlinDocumentation
- *
- * -----------------
- *
- * "G" Codes
- *
- * G0   -> G1
- * G1   - Coordinated Movement X Y Z E
- * G2   - CW ARC
- * G3   - CCW ARC
- * G4   - Dwell S<seconds> or P<milliseconds>
- * G5   - Cubic B-spline with XYZE destination and IJPQ offsets
- * G10  - Retract filament according to settings of M207 (Requires FWRETRACT)
- * G11  - Retract recover filament according to settings of M208 (Requires FWRETRACT)
- * G12  - Clean tool (Requires NOZZLE_CLEAN_FEATURE)
- * G17  - Select Plane XY (Requires CNC_WORKSPACE_PLANES)
- * G18  - Select Plane ZX (Requires CNC_WORKSPACE_PLANES)
- * G19  - Select Plane YZ (Requires CNC_WORKSPACE_PLANES)
- * G20  - Set input units to inches (Requires INCH_MODE_SUPPORT)
- * G21  - Set input units to millimeters (Requires INCH_MODE_SUPPORT)
- * G26  - Mesh Validation Pattern (Requires UBL_G26_MESH_VALIDATION)
- * G27  - Park Nozzle (Requires NOZZLE_PARK_FEATURE)
- * G28  - Home one or more axes
- * G29  - Start or continue the bed leveling probe procedure (Requires bed leveling)
- * G30  - Single Z probe, probes bed at X Y location (defaults to current XY location)
- * G31  - Dock sled (Z_PROBE_SLED only)
- * G32  - Undock sled (Z_PROBE_SLED only)
- * G33  - Delta Auto-Calibration (Requires DELTA_AUTO_CALIBRATION)
- * G38  - Probe in any direction using the Z_MIN_PROBE (Requires G38_PROBE_TARGET)
- * G42  - Coordinated move to a mesh point (Requires AUTO_BED_LEVELING_UBL)
- * G90  - Use Absolute Coordinates
- * G91  - Use Relative Coordinates
- * G92  - Set current position to coordinates given
- *
- * "M" Codes
- *
- * M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
- * M1   -> M0
- * M3   - Turn laser/spindle on, set spindle/laser speed/power, set rotation to clockwise
- * M4   - Turn laser/spindle on, set spindle/laser speed/power, set rotation to counter-clockwise
- * M5   - Turn laser/spindle off
- * M17  - Enable/Power all stepper motors
- * M18  - Disable all stepper motors; same as M84
- * M20  - List SD card. (Requires SDSUPPORT)
- * M21  - Init SD card. (Requires SDSUPPORT)
- * M22  - Release SD card. (Requires SDSUPPORT)
- * M23  - Select SD file: "M23 /path/file.gco". (Requires SDSUPPORT)
- * M24  - Start/resume SD print. (Requires SDSUPPORT)
- * M25  - Pause SD print. (Requires SDSUPPORT)
- * M26  - Set SD position in bytes: "M26 S12345". (Requires SDSUPPORT)
- * M27  - Report SD print status. (Requires SDSUPPORT)
- * M28  - Start SD write: "M28 /path/file.gco". (Requires SDSUPPORT)
- * M29  - Stop SD write. (Requires SDSUPPORT)
- * M30  - Delete file from SD: "M30 /path/file.gco"
- * M31  - Report time since last M109 or SD card start to serial.
- * M32  - Select file and start SD print: "M32 [S<bytepos>] !/path/file.gco#". (Requires SDSUPPORT)
- *        Use P to run other files as sub-programs: "M32 P !filename#"
- *        The '#' is necessary when calling from within sd files, as it stops buffer prereading
- * M33  - Get the longname version of a path. (Requires LONG_FILENAME_HOST_SUPPORT)
- * M34  - Set SD Card sorting options. (Requires SDCARD_SORT_ALPHA)
- * M42  - Change pin status via gcode: M42 P<pin> S<value>. LED pin assumed if P is omitted.
- * M43  - Display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
- * M48  - Measure Z Probe repeatability: M48 P<points> X<pos> Y<pos> V<level> E<engage> L<legs>. (Requires Z_MIN_PROBE_REPEATABILITY_TEST)
- * M75  - Start the print job timer.
- * M76  - Pause the print job timer.
- * M77  - Stop the print job timer.
- * M78  - Show statistical information about the print jobs. (Requires PRINTCOUNTER)
- * M80  - Turn on Power Supply. (Requires POWER_SUPPLY > 0)
- * M81  - Turn off Power Supply. (Requires POWER_SUPPLY > 0)
- * M82  - Set E codes absolute (default).
- * M83  - Set E codes relative while in Absolute (G90) mode.
- * M84  - Disable steppers until next move, or use S<seconds> to specify an idle
- *        duration after which steppers should turn off. S0 disables the timeout.
- * M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
- * M92  - Set planner.axis_steps_per_mm for one or more axes.
- * M100 - Watch Free Memory (for debugging) (Requires M100_FREE_MEMORY_WATCHER)
- * M104 - Set extruder target temp.
- * M105 - Report current temperatures.
- * M106 - Fan on.
- * M107 - Fan off.
- * M108 - Break out of heating loops (M109, M190, M303). With no controller, breaks out of M0/M1. (Requires EMERGENCY_PARSER)
- * M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
- *        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
- *        If AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
- * M110 - Set the current line number. (Used by host printing)
- * M111 - Set debug flags: "M111 S<flagbits>". See flag bits defined in enum.h.
- * M112 - Emergency stop.
- * M113 - Get or set the timeout interval for Host Keepalive "busy" messages. (Requires HOST_KEEPALIVE_FEATURE)
- * M114 - Report current position.
- * M115 - Report capabilities. (Extended capabilities requires EXTENDED_CAPABILITIES_REPORT)
- * M117 - Display a message on the controller screen. (Requires an LCD)
- * M118 - Display a message in the host console.
- * M119 - Report endstops status.
- * M120 - Enable endstops detection.
- * M121 - Disable endstops detection.
- * M125 - Save current position and move to filament change position. (Requires PARK_HEAD_ON_PAUSE)
- * M126 - Solenoid Air Valve Open. (Requires BARICUDA)
- * M127 - Solenoid Air Valve Closed. (Requires BARICUDA)
- * M128 - EtoP Open. (Requires BARICUDA)
- * M129 - EtoP Closed. (Requires BARICUDA)
- * M140 - Set bed target temp. S<temp>
- * M145 - Set heatup values for materials on the LCD. H<hotend> B<bed> F<fan speed> for S<material> (0=PLA, 1=ABS)
- * M149 - Set temperature units. (Requires TEMPERATURE_UNITS_SUPPORT)
- * M150 - Set Status LED Color as R<red> U<green> B<blue>. Values 0-255. (Requires BLINKM, RGB_LED, RGBW_LED, or PCA9632)
- * M155 - Auto-report temperatures with interval of S<seconds>. (Requires AUTO_REPORT_TEMPERATURES)
- * M163 - Set a single proportion for a mixing extruder. (Requires MIXING_EXTRUDER)
- * M164 - Save the mix as a virtual extruder. (Requires MIXING_EXTRUDER and MIXING_VIRTUAL_TOOLS)
- * M165 - Set the proportions for a mixing extruder. Use parameters ABCDHI to set the mixing factors. (Requires MIXING_EXTRUDER)
- * M190 - Sxxx Wait for bed current temp to reach target temp. ** Waits only when heating! **
- *        Rxxx Wait for bed current temp to reach target temp. ** Waits for heating or cooling. **
- * M200 - Set filament diameter, D<diameter>, setting E axis units to cubic. (Use S0 to revert to linear units.)
- * M201 - Set max acceleration in units/s^2 for print moves: "M201 X<accel> Y<accel> Z<accel> E<accel>"
- * M202 - Set max acceleration in units/s^2 for travel moves: "M202 X<accel> Y<accel> Z<accel> E<accel>" ** UNUSED IN MARLIN! **
- * M203 - Set maximum feedrate: "M203 X<fr> Y<fr> Z<fr> E<fr>" in units/sec.
- * M204 - Set default acceleration in units/sec^2: P<printing> R<extruder_only> T<travel>
- * M205 - Set advanced settings. Current units apply:
-            S<print> T<travel> minimum speeds
-            B<minimum segment time>
-            X<max X jerk>, Y<max Y jerk>, Z<max Z jerk>, E<max E jerk>
- * M206 - Set additional homing offset. (Disabled by NO_WORKSPACE_OFFSETS or DELTA)
- * M207 - Set Retract Length: S<length>, Feedrate: F<units/min>, and Z lift: Z<distance>. (Requires FWRETRACT)
- * M208 - Set Recover (unretract) Additional (!) Length: S<length> and Feedrate: F<units/min>. (Requires FWRETRACT)
- * M209 - Turn Automatic Retract Detection on/off: S<0|1> (For slicers that don't support G10/11). (Requires FWRETRACT)
-          Every normal extrude-only move will be classified as retract depending on the direction.
- * M211 - Enable, Disable, and/or Report software endstops: S<0|1> (Requires MIN_SOFTWARE_ENDSTOPS or MAX_SOFTWARE_ENDSTOPS)
- * M218 - Set a tool offset: "M218 T<index> X<offset> Y<offset>". (Requires 2 or more extruders)
- * M220 - Set Feedrate Percentage: "M220 S<percent>" (i.e., "FR" on the LCD)
- * M221 - Set Flow Percentage: "M221 S<percent>"
- * M226 - Wait until a pin is in a given state: "M226 P<pin> S<state>"
- * M240 - Trigger a camera to take a photograph. (Requires CHDK or PHOTOGRAPH_PIN)
- * M250 - Set LCD contrast: "M250 C<contrast>" (0-63). (Requires LCD support)
- * M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS)
- * M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS)
- * M280 - Set servo position absolute: "M280 P<index> S<angle|µs>". (Requires servos)
- * M300 - Play beep sound S<frequency Hz> P<duration ms>
- * M301 - Set PID parameters P I and D. (Requires PIDTEMP)
- * M302 - Allow cold extrudes, or set the minimum extrude S<temperature>. (Requires PREVENT_COLD_EXTRUSION)
- * M303 - PID relay autotune S<temperature> sets the target temperature. Default 150C. (Requires PIDTEMP)
- * M304 - Set bed PID parameters P I and D. (Requires PIDTEMPBED)
- * M350 - Set microstepping mode. (Requires digital microstepping pins.)
- * M351 - Toggle MS1 MS2 pins directly. (Requires digital microstepping pins.)
- * M355 - Set Case Light on/off and set brightness. (Requires CASE_LIGHT_PIN)
- * M380 - Activate solenoid on active extruder. (Requires EXT_SOLENOID)
- * M381 - Disable all solenoids. (Requires EXT_SOLENOID)
- * M400 - Finish all moves.
- * M401 - Lower Z probe. (Requires a probe)
- * M402 - Raise Z probe. (Requires a probe)
- * M404 - Display or set the Nominal Filament Width: "W<diameter>". (Requires FILAMENT_WIDTH_SENSOR)
- * M405 - Enable Filament Sensor flow control. "M405 D<delay_cm>". (Requires FILAMENT_WIDTH_SENSOR)
- * M406 - Disable Filament Sensor flow control. (Requires FILAMENT_WIDTH_SENSOR)
- * M407 - Display measured filament diameter in millimeters. (Requires FILAMENT_WIDTH_SENSOR)
- * M410 - Quickstop. Abort all planned moves.
- * M420 - Enable/Disable Leveling (with current values) S1=enable S0=disable (Requires MESH_BED_LEVELING or ABL)
- * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<units> Y<units> Z<units> (Requires MESH_BED_LEVELING or AUTO_BED_LEVELING_UBL)
- * M428 - Set the home_offset based on the current_position. Nearest edge applies. (Disabled by NO_WORKSPACE_OFFSETS or DELTA)
- * M500 - Store parameters in EEPROM. (Requires EEPROM_SETTINGS)
- * M501 - Restore parameters from EEPROM. (Requires EEPROM_SETTINGS)
- * M502 - Revert to the default "factory settings". ** Does not write them to EEPROM! **
- * M503 - Print the current settings (in memory): "M503 S<verbose>". S0 specifies compact output.
- * M540 - Enable/disable SD card abort on endstop hit: "M540 S<state>". (Requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
- * M600 - Pause for filament change: "M600 X<pos> Y<pos> Z<raise> E<first_retract> L<later_retract>". (Requires ADVANCED_PAUSE_FEATURE)
- * M665 - Set delta configurations: "M665 L<diagonal rod> R<delta radius> S<segments/s> A<rod A trim mm> B<rod B trim mm> C<rod C trim mm> I<tower A trim angle> J<tower B trim angle> K<tower C trim angle>" (Requires DELTA)
- * M666 - Set delta endstop adjustment. (Requires DELTA)
- * M605 - Set dual x-carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
- * M851 - Set Z probe's Z offset in current units. (Negative = below the nozzle.)
- * M860 - Report the position of position encoder modules.
- * M861 - Report the status of position encoder modules.
- * M862 - Perform an axis continuity test for position encoder modules.
- * M863 - Perform steps-per-mm calibration for position encoder modules.
- * M864 - Change position encoder module I2C address.
- * M865 - Check position encoder module firmware version.
- * M866 - Report or reset position encoder module error count.
- * M867 - Enable/disable or toggle error correction for position encoder modules.
- * M868 - Report or set position encoder module error correction threshold.
- * M869 - Report position encoder module error.
- * M900 - Get and/or Set advance K factor and WH/D ratio. (Requires LIN_ADVANCE)
- * M906 - Set or get motor current in milliamps using axis codes X, Y, Z, E. Report values if no axis codes given. (Requires HAVE_TMC2130)
- * M907 - Set digital trimpot motor current using axis codes. (Requires a board with digital trimpots)
- * M908 - Control digital trimpot directly. (Requires DAC_STEPPER_CURRENT or DIGIPOTSS_PIN)
- * M909 - Print digipot/DAC current value. (Requires DAC_STEPPER_CURRENT)
- * M910 - Commit digipot/DAC value to external EEPROM via I2C. (Requires DAC_STEPPER_CURRENT)
- * M911 - Report stepper driver overtemperature pre-warn condition. (Requires HAVE_TMC2130)
- * M912 - Clear stepper driver overtemperature pre-warn condition flag. (Requires HAVE_TMC2130)
- * M913 - Set HYBRID_THRESHOLD speed. (Requires HYBRID_THRESHOLD)
- * M914 - Set SENSORLESS_HOMING sensitivity. (Requires SENSORLESS_HOMING)
- *
- * M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
- * M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
- * M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
- * M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
- * M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
- *
- * ************ Custom codes - This can change to suit future G-code regulations
- * M928 - Start SD logging: "M928 filename.gco". Stop with M29. (Requires SDSUPPORT)
- * M999 - Restart after being stopped by error
- *
- * "T" Codes
- *
- * T0-T3 - Select an extruder (tool) by index: "T<n> F<units/min>"
- *
+ *  - https://github.com/simen/grbl
  */
 
 #include "Marlin.h"
@@ -316,7 +103,6 @@
 #endif
 
 #if ENABLED(M100_FREE_MEMORY_WATCHER)
-  void gcode_M100();
   void M100_dump_routine(const char * const title, const char *start, const char *end);
 #endif
 
@@ -344,6 +130,10 @@
                            || isnan(ubl.z_values[0][0]))
 #endif
 
+#if ENABLED(SENSORLESS_HOMING)
+  #include "feature/tmc2130.h"
+#endif
+
 bool Running = true;
 
 /**
@@ -3328,7 +3118,7 @@ void dwell(millis_t time) {
 #endif
 
 #if ENABLED(CNC_WORKSPACE_PLANES)
-  #include "gcode/feature/clean/G17-G19.h"
+  #include "gcode/geometry/G17-G19.h"
 #endif
 
 #if ENABLED(INCH_MODE_SUPPORT)
@@ -3732,11 +3522,11 @@ void report_current_position() {
 #endif
 
 #if defined(CHDK) || HAS_PHOTOGRAPH
-  #include "gcode/control/M240.h"
+  #include "gcode/feature/camera/M240.h"
 #endif
 
 #if HAS_LCD_CONTRAST
-  #include "gcode/control/M250.h"
+  #include "gcode/lcd/M250.h"
 #endif
 
 #if ENABLED(PREVENT_COLD_EXTRUSION)

Marlin/src/Marlin.h

@@ -194,8 +194,6 @@ inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
  */
 extern int16_t feedrate_percentage;
 
-#define MMM_TO_MMS(MM_M) ((MM_M)/60.0)
-#define MMS_TO_MMM(MM_S) ((MM_S)*60.0)
 #define MMS_SCALED(MM_S) ((MM_S)*feedrate_percentage*0.01)
 
 extern bool axis_relative_modes[];

Marlin/src/core/macros.h

@@ -172,6 +172,9 @@
 #define PENDING(NOW,SOON) ((long)(NOW-(SOON))<0)
 #define ELAPSED(NOW,SOON) (!PENDING(NOW,SOON))
 
+#define MMM_TO_MMS(MM_M) ((MM_M)/60.0)
+#define MMS_TO_MMM(MM_S) ((MM_S)*60.0)
+
 #define NOOP do{} while(0)
 
 #define CEILING(x,y) (((x) + (y) - 1) / (y))

Marlin/src/feature/I2CPositionEncoder.h

@@ -341,15 +341,4 @@ class I2CPositionEncodersMgr {
 
 extern I2CPositionEncodersMgr I2CPEM;
 
-FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); }
-FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); }
-FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); }
-FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); }
-FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); }
-FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); }
-FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); }
-FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); }
-FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); }
-FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); }
-
 #endif //I2CPOSENC_H

Marlin/src/feature/tmc2130.cpp

@@ -30,8 +30,6 @@
 #include "../libs/duration_t.h"
 #include "../module/stepper_indirection.h"
 
-#include <TMC2130Stepper.h>
-
 #ifdef AUTOMATIC_CURRENT_CONTROL
   bool auto_current_control = 0;
 #endif

Marlin/src/feature/tmc2130.h

@@ -23,8 +23,16 @@
 #ifndef _TMC2130_H_
 #define _TMC2130_H_
 
+#include <TMC2130Stepper.h>
+
+#include "../inc/MarlinConfig.h"
+
 extern bool auto_current_control;
 
 void tmc2130_checkOverTemp(void);
 
+#if ENABLED(SENSORLESS_HOMING)
+  void tmc2130_sensorless_homing(TMC2130Stepper &st, bool enable=true);
+#endif
+
 #endif // _TMC2130_H_

Marlin/src/gcode/calibrate/G26.h

@@ -0,0 +1,27 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void gcode_G26() {
+
+  ubl.G26();
+
+}

Marlin/src/gcode/calibrate/G28.h

@@ -0,0 +1,324 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "common.h"
+
+#if HOTENDS > 1
+  #include "../control/tool_change.h"
+#endif
+
+#if ENABLED(QUICK_HOME)
+
+  static void quick_home_xy() {
+
+    // Pretend the current position is 0,0
+    current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
+    sync_plan_position();
+
+    const int x_axis_home_dir =
+      #if ENABLED(DUAL_X_CARRIAGE)
+        x_home_dir(active_extruder)
+      #else
+        home_dir(X_AXIS)
+      #endif
+    ;
+
+    const float mlx = max_length(X_AXIS),
+                mly = max_length(Y_AXIS),
+                mlratio = mlx > mly ? mly / mlx : mlx / mly,
+                fr_mm_s = min(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0);
+
+    do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * home_dir(Y_AXIS), fr_mm_s);
+    endstops.hit_on_purpose(); // clear endstop hit flags
+    current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
+  }
+
+#endif // QUICK_HOME
+
+#if ENABLED(Z_SAFE_HOMING)
+
+  inline void home_z_safely() {
+
+    // Disallow Z homing if X or Y are unknown
+    if (!axis_known_position[X_AXIS] || !axis_known_position[Y_AXIS]) {
+      LCD_MESSAGEPGM(MSG_ERR_Z_HOMING);
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPGM(MSG_ERR_Z_HOMING);
+      return;
+    }
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Z_SAFE_HOMING >>>");
+    #endif
+
+    SYNC_PLAN_POSITION_KINEMATIC();
+
+    /**
+     * Move the Z probe (or just the nozzle) to the safe homing point
+     */
+    destination[X_AXIS] = LOGICAL_X_POSITION(Z_SAFE_HOMING_X_POINT);
+    destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT);
+    destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
+
+    #if HOMING_Z_WITH_PROBE
+      destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
+      destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
+    #endif
+
+    if (position_is_reachable_xy(destination[X_AXIS], destination[Y_AXIS])) {
+
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
+      #endif
+
+      // This causes the carriage on Dual X to unpark
+      #if ENABLED(DUAL_X_CARRIAGE)
+        active_extruder_parked = false;
+      #endif
+
+      do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
+      HOMEAXIS(Z);
+    }
+    else {
+      LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
+    }
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< Z_SAFE_HOMING");
+    #endif
+  }
+
+#endif // Z_SAFE_HOMING
+
+/**
+ * G28: Home all axes according to settings
+ *
+ * Parameters
+ *
+ *  None  Home to all axes with no parameters.
+ *        With QUICK_HOME enabled XY will home together, then Z.
+ *
+ * Cartesian parameters
+ *
+ *  X   Home to the X endstop
+ *  Y   Home to the Y endstop
+ *  Z   Home to the Z endstop
+ *
+ */
+void gcode_G28(const bool always_home_all) {
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) {
+      SERIAL_ECHOLNPGM(">>> gcode_G28");
+      log_machine_info();
+    }
+  #endif
+
+  // Wait for planner moves to finish!
+  stepper.synchronize();
+
+  // Cancel the active G29 session
+  #if ENABLED(PROBE_MANUALLY)
+    g29_in_progress = false;
+  #endif
+
+  // Disable the leveling matrix before homing
+  #if HAS_LEVELING
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      const bool ubl_state_at_entry = leveling_is_active();
+    #endif
+    set_bed_leveling_enabled(false);
+  #endif
+
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    workspace_plane = PLANE_XY;
+  #endif
+
+  // Always home with tool 0 active
+  #if HOTENDS > 1
+    const uint8_t old_tool_index = active_extruder;
+    tool_change(0, 0, true);
+  #endif
+
+  #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
+    extruder_duplication_enabled = false;
+  #endif
+
+  setup_for_endstop_or_probe_move();
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> endstops.enable(true)");
+  #endif
+  endstops.enable(true); // Enable endstops for next homing move
+
+  #if ENABLED(DELTA)
+
+    home_delta();
+    UNUSED(always_home_all);
+
+  #else // NOT DELTA
+
+    const bool homeX = always_home_all || parser.seen('X'),
+               homeY = always_home_all || parser.seen('Y'),
+               homeZ = always_home_all || parser.seen('Z'),
+               home_all = (!homeX && !homeY && !homeZ) || (homeX && homeY && homeZ);
+
+    set_destination_to_current();
+
+    #if Z_HOME_DIR > 0  // If homing away from BED do Z first
+
+      if (home_all || homeZ) {
+        HOMEAXIS(Z);
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) DEBUG_POS("> HOMEAXIS(Z)", current_position);
+        #endif
+      }
+
+    #else
+
+      if (home_all || homeX || homeY) {
+        // Raise Z before homing any other axes and z is not already high enough (never lower z)
+        destination[Z_AXIS] = LOGICAL_Z_POSITION(Z_HOMING_HEIGHT);
+        if (destination[Z_AXIS] > current_position[Z_AXIS]) {
+
+          #if ENABLED(DEBUG_LEVELING_FEATURE)
+            if (DEBUGGING(LEVELING))
+              SERIAL_ECHOLNPAIR("Raise Z (before homing) to ", destination[Z_AXIS]);
+          #endif
+
+          do_blocking_move_to_z(destination[Z_AXIS]);
+        }
+      }
+
+    #endif
+
+    #if ENABLED(QUICK_HOME)
+
+      if (home_all || (homeX && homeY)) quick_home_xy();
+
+    #endif
+
+    #if ENABLED(HOME_Y_BEFORE_X)
+
+      // Home Y
+      if (home_all || homeY) {
+        HOMEAXIS(Y);
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) DEBUG_POS("> homeY", current_position);
+        #endif
+      }
+
+    #endif
+
+    // Home X
+    if (home_all || homeX) {
+
+      #if ENABLED(DUAL_X_CARRIAGE)
+
+        // Always home the 2nd (right) extruder first
+        active_extruder = 1;
+        HOMEAXIS(X);
+
+        // Remember this extruder's position for later tool change
+        inactive_extruder_x_pos = RAW_X_POSITION(current_position[X_AXIS]);
+
+        // Home the 1st (left) extruder
+        active_extruder = 0;
+        HOMEAXIS(X);
+
+        // Consider the active extruder to be parked
+        COPY(raised_parked_position, current_position);
+        delayed_move_time = 0;
+        active_extruder_parked = true;
+
+      #else
+
+        HOMEAXIS(X);
+
+      #endif
+
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(LEVELING)) DEBUG_POS("> homeX", current_position);
+      #endif
+    }
+
+    #if DISABLED(HOME_Y_BEFORE_X)
+      // Home Y
+      if (home_all || homeY) {
+        HOMEAXIS(Y);
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) DEBUG_POS("> homeY", current_position);
+        #endif
+      }
+    #endif
+
+    // Home Z last if homing towards the bed
+    #if Z_HOME_DIR < 0
+      if (home_all || homeZ) {
+        #if ENABLED(Z_SAFE_HOMING)
+          home_z_safely();
+        #else
+          HOMEAXIS(Z);
+        #endif
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) DEBUG_POS("> (home_all || homeZ) > final", current_position);
+        #endif
+      } // home_all || homeZ
+    #endif // Z_HOME_DIR < 0
+
+    SYNC_PLAN_POSITION_KINEMATIC();
+
+  #endif // !DELTA (gcode_G28)
+
+  endstops.not_homing();
+
+  #if ENABLED(DELTA) && ENABLED(DELTA_HOME_TO_SAFE_ZONE)
+    // move to a height where we can use the full xy-area
+    do_blocking_move_to_z(delta_clip_start_height);
+  #endif
+
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+    set_bed_leveling_enabled(ubl_state_at_entry);
+  #endif
+
+  clean_up_after_endstop_or_probe_move();
+
+  // Restore the active tool after homing
+  #if HOTENDS > 1
+    tool_change(old_tool_index, 0,
+      #if ENABLED(PARKING_EXTRUDER)
+        false // fetch the previous toolhead
+      #else
+        true
+      #endif
+    );
+  #endif
+
+  lcd_refresh();
+
+  report_current_position();
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< gcode_G28");
+  #endif
+}

Marlin/src/gcode/calibrate/G29-abl.h

@@ -0,0 +1,946 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ABL_GRID
+  #if ENABLED(PROBE_Y_FIRST)
+    #define PR_OUTER_VAR xCount
+    #define PR_OUTER_END abl_grid_points_x
+    #define PR_INNER_VAR yCount
+    #define PR_INNER_END abl_grid_points_y
+  #else
+    #define PR_OUTER_VAR yCount
+    #define PR_OUTER_END abl_grid_points_y
+    #define PR_INNER_VAR xCount
+    #define PR_INNER_END abl_grid_points_x
+  #endif
+#endif
+
+/**
+ * G29: Detailed Z probe, probes the bed at 3 or more points.
+ *      Will fail if the printer has not been homed with G28.
+ *
+ * Enhanced G29 Auto Bed Leveling Probe Routine
+ *
+ *  D  Dry-Run mode. Just evaluate the bed Topology - Don't apply
+ *     or alter the bed level data. Useful to check the topology
+ *     after a first run of G29.
+ *
+ *  J  Jettison current bed leveling data
+ *
+ *  V  Set the verbose level (0-4). Example: "G29 V3"
+ *
+ * Parameters With LINEAR leveling only:
+ *
+ *  P  Set the size of the grid that will be probed (P x P points).
+ *     Example: "G29 P4"
+ *
+ *  X  Set the X size of the grid that will be probed (X x Y points).
+ *     Example: "G29 X7 Y5"
+ *
+ *  Y  Set the Y size of the grid that will be probed (X x Y points).
+ *
+ *  T  Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
+ *     This is useful for manual bed leveling and finding flaws in the bed (to
+ *     assist with part placement).
+ *     Not supported by non-linear delta printer bed leveling.
+ *
+ * Parameters With LINEAR and BILINEAR leveling only:
+ *
+ *  S  Set the XY travel speed between probe points (in units/min)
+ *
+ *  F  Set the Front limit of the probing grid
+ *  B  Set the Back limit of the probing grid
+ *  L  Set the Left limit of the probing grid
+ *  R  Set the Right limit of the probing grid
+ *
+ * Parameters with DEBUG_LEVELING_FEATURE only:
+ *
+ *  C  Make a totally fake grid with no actual probing.
+ *     For use in testing when no probing is possible.
+ *
+ * Parameters with BILINEAR leveling only:
+ *
+ *  Z  Supply an additional Z probe offset
+ *
+ * Extra parameters with PROBE_MANUALLY:
+ *
+ *  To do manual probing simply repeat G29 until the procedure is complete.
+ *  The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort.
+ *
+ *  Q  Query leveling and G29 state
+ *
+ *  A  Abort current leveling procedure
+ *
+ * Extra parameters with BILINEAR only:
+ *
+ *  W  Write a mesh point. (If G29 is idle.)
+ *  I  X index for mesh point
+ *  J  Y index for mesh point
+ *  X  X for mesh point, overrides I
+ *  Y  Y for mesh point, overrides J
+ *  Z  Z for mesh point. Otherwise, raw current Z.
+ *
+ * Without PROBE_MANUALLY:
+ *
+ *  E  By default G29 will engage the Z probe, test the bed, then disengage.
+ *     Include "E" to engage/disengage the Z probe for each sample.
+ *     There's no extra effect if you have a fixed Z probe.
+ *
+ */
+void gcode_G29() {
+
+  // G29 Q is also available if debugging
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    const bool query = parser.seen('Q');
+    const uint8_t old_debug_flags = marlin_debug_flags;
+    if (query) marlin_debug_flags |= DEBUG_LEVELING;
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_POS(">>> gcode_G29", current_position);
+      log_machine_info();
+    }
+    marlin_debug_flags = old_debug_flags;
+    #if DISABLED(PROBE_MANUALLY)
+      if (query) return;
+    #endif
+  #endif
+
+  #if ENABLED(PROBE_MANUALLY)
+    const bool seenA = parser.seen('A'), seenQ = parser.seen('Q'), no_action = seenA || seenQ;
+  #endif
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY)
+    const bool faux = parser.boolval('C');
+  #elif ENABLED(PROBE_MANUALLY)
+    const bool faux = no_action;
+  #else
+    bool constexpr faux = false;
+  #endif
+
+  // Don't allow auto-leveling without homing first
+  if (axis_unhomed_error()) return;
+
+  // Define local vars 'static' for manual probing, 'auto' otherwise
+  #if ENABLED(PROBE_MANUALLY)
+    #define ABL_VAR static
+  #else
+    #define ABL_VAR
+  #endif
+
+  ABL_VAR int verbose_level;
+  ABL_VAR float xProbe, yProbe, measured_z;
+  ABL_VAR bool dryrun, abl_should_enable;
+
+  #if ENABLED(PROBE_MANUALLY) || ENABLED(AUTO_BED_LEVELING_LINEAR)
+    ABL_VAR int abl_probe_index;
+  #endif
+
+  #if HAS_SOFTWARE_ENDSTOPS && ENABLED(PROBE_MANUALLY)
+    ABL_VAR bool enable_soft_endstops = true;
+  #endif
+
+  #if ABL_GRID
+
+    #if ENABLED(PROBE_MANUALLY)
+      ABL_VAR uint8_t PR_OUTER_VAR;
+      ABL_VAR  int8_t PR_INNER_VAR;
+    #endif
+
+    ABL_VAR int left_probe_bed_position, right_probe_bed_position, front_probe_bed_position, back_probe_bed_position;
+    ABL_VAR float xGridSpacing = 0, yGridSpacing = 0;
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+      ABL_VAR uint8_t abl_grid_points_x = GRID_MAX_POINTS_X,
+                      abl_grid_points_y = GRID_MAX_POINTS_Y;
+      ABL_VAR bool do_topography_map;
+    #else // Bilinear
+      uint8_t constexpr abl_grid_points_x = GRID_MAX_POINTS_X,
+                        abl_grid_points_y = GRID_MAX_POINTS_Y;
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(PROBE_MANUALLY)
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+        ABL_VAR int abl2;
+      #else // Bilinear
+        int constexpr abl2 = GRID_MAX_POINTS;
+      #endif
+    #endif
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      ABL_VAR float zoffset;
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      ABL_VAR int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+
+      ABL_VAR float eqnAMatrix[GRID_MAX_POINTS * 3], // "A" matrix of the linear system of equations
+                    eqnBVector[GRID_MAX_POINTS],     // "B" vector of Z points
+                    mean;
+    #endif
+
+  #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+    int constexpr abl2 = 3;
+
+    // Probe at 3 arbitrary points
+    ABL_VAR vector_3 points[3] = {
+      vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, 0),
+      vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, 0),
+      vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, 0)
+    };
+
+  #endif // AUTO_BED_LEVELING_3POINT
+
+  #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+    struct linear_fit_data lsf_results;
+    incremental_LSF_reset(&lsf_results);
+  #endif
+
+  /**
+   * On the initial G29 fetch command parameters.
+   */
+  if (!g29_in_progress) {
+
+    #if ENABLED(PROBE_MANUALLY) || ENABLED(AUTO_BED_LEVELING_LINEAR)
+      abl_probe_index = -1;
+    #endif
+
+    abl_should_enable = leveling_is_active();
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      if (parser.seen('W')) {
+        if (!leveling_is_valid()) {
+          SERIAL_ERROR_START();
+          SERIAL_ERRORLNPGM("No bilinear grid");
+          return;
+        }
+
+        const float z = parser.floatval('Z', RAW_CURRENT_POSITION(Z));
+        if (!WITHIN(z, -10, 10)) {
+          SERIAL_ERROR_START();
+          SERIAL_ERRORLNPGM("Bad Z value");
+          return;
+        }
+
+        const float x = parser.floatval('X', NAN),
+                    y = parser.floatval('Y', NAN);
+        int8_t i = parser.byteval('I', -1),
+               j = parser.byteval('J', -1);
+
+        if (!isnan(x) && !isnan(y)) {
+          // Get nearest i / j from x / y
+          i = (x - LOGICAL_X_POSITION(bilinear_start[X_AXIS]) + 0.5 * xGridSpacing) / xGridSpacing;
+          j = (y - LOGICAL_Y_POSITION(bilinear_start[Y_AXIS]) + 0.5 * yGridSpacing) / yGridSpacing;
+          i = constrain(i, 0, GRID_MAX_POINTS_X - 1);
+          j = constrain(j, 0, GRID_MAX_POINTS_Y - 1);
+        }
+        if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) {
+          set_bed_leveling_enabled(false);
+          z_values[i][j] = z;
+          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+            bed_level_virt_interpolate();
+          #endif
+          set_bed_leveling_enabled(abl_should_enable);
+        }
+        return;
+      } // parser.seen('W')
+
+    #endif
+
+    #if HAS_LEVELING
+
+      // Jettison bed leveling data
+      if (parser.seen('J')) {
+        reset_bed_level();
+        return;
+      }
+
+    #endif
+
+    verbose_level = parser.intval('V');
+    if (!WITHIN(verbose_level, 0, 4)) {
+      SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-4).");
+      return;
+    }
+
+    dryrun = parser.boolval('D')
+      #if ENABLED(PROBE_MANUALLY)
+        || no_action
+      #endif
+    ;
+
+    #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      do_topography_map = verbose_level > 2 || parser.boolval('T');
+
+      // X and Y specify points in each direction, overriding the default
+      // These values may be saved with the completed mesh
+      abl_grid_points_x = parser.intval('X', GRID_MAX_POINTS_X);
+      abl_grid_points_y = parser.intval('Y', GRID_MAX_POINTS_Y);
+      if (parser.seenval('P')) abl_grid_points_x = abl_grid_points_y = parser.value_int();
+
+      if (abl_grid_points_x < 2 || abl_grid_points_y < 2) {
+        SERIAL_PROTOCOLLNPGM("?Number of probe points is implausible (2 minimum).");
+        return;
+      }
+
+      abl2 = abl_grid_points_x * abl_grid_points_y;
+      mean = 0;
+
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      zoffset = parser.linearval('Z');
+
+    #endif
+
+    #if ABL_GRID
+
+      xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_SPEED));
+
+      left_probe_bed_position = (int)parser.linearval('L', LOGICAL_X_POSITION(LEFT_PROBE_BED_POSITION));
+      right_probe_bed_position = (int)parser.linearval('R', LOGICAL_X_POSITION(RIGHT_PROBE_BED_POSITION));
+      front_probe_bed_position = (int)parser.linearval('F', LOGICAL_Y_POSITION(FRONT_PROBE_BED_POSITION));
+      back_probe_bed_position = (int)parser.linearval('B', LOGICAL_Y_POSITION(BACK_PROBE_BED_POSITION));
+
+      const bool left_out_l = left_probe_bed_position < LOGICAL_X_POSITION(MIN_PROBE_X),
+                 left_out = left_out_l || left_probe_bed_position > right_probe_bed_position - (MIN_PROBE_EDGE),
+                 right_out_r = right_probe_bed_position > LOGICAL_X_POSITION(MAX_PROBE_X),
+                 right_out = right_out_r || right_probe_bed_position < left_probe_bed_position + MIN_PROBE_EDGE,
+                 front_out_f = front_probe_bed_position < LOGICAL_Y_POSITION(MIN_PROBE_Y),
+                 front_out = front_out_f || front_probe_bed_position > back_probe_bed_position - (MIN_PROBE_EDGE),
+                 back_out_b = back_probe_bed_position > LOGICAL_Y_POSITION(MAX_PROBE_Y),
+                 back_out = back_out_b || back_probe_bed_position < front_probe_bed_position + MIN_PROBE_EDGE;
+
+      if (left_out || right_out || front_out || back_out) {
+        if (left_out) {
+          out_of_range_error(PSTR("(L)eft"));
+          left_probe_bed_position = left_out_l ? LOGICAL_X_POSITION(MIN_PROBE_X) : right_probe_bed_position - (MIN_PROBE_EDGE);
+        }
+        if (right_out) {
+          out_of_range_error(PSTR("(R)ight"));
+          right_probe_bed_position = right_out_r ? LOGICAL_Y_POSITION(MAX_PROBE_X) : left_probe_bed_position + MIN_PROBE_EDGE;
+        }
+        if (front_out) {
+          out_of_range_error(PSTR("(F)ront"));
+          front_probe_bed_position = front_out_f ? LOGICAL_Y_POSITION(MIN_PROBE_Y) : back_probe_bed_position - (MIN_PROBE_EDGE);
+        }
+        if (back_out) {
+          out_of_range_error(PSTR("(B)ack"));
+          back_probe_bed_position = back_out_b ? LOGICAL_Y_POSITION(MAX_PROBE_Y) : front_probe_bed_position + MIN_PROBE_EDGE;
+        }
+        return;
+      }
+
+      // probe at the points of a lattice grid
+      xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (abl_grid_points_x - 1);
+      yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (abl_grid_points_y - 1);
+
+    #endif // ABL_GRID
+
+    if (verbose_level > 0) {
+      SERIAL_PROTOCOLLNPGM("G29 Auto Bed Leveling");
+      if (dryrun) SERIAL_PROTOCOLLNPGM("Running in DRY-RUN mode");
+    }
+
+    stepper.synchronize();
+
+    // Disable auto bed leveling during G29
+    planner.abl_enabled = false;
+
+    if (!dryrun) {
+      // Re-orient the current position without leveling
+      // based on where the steppers are positioned.
+      set_current_from_steppers_for_axis(ALL_AXES);
+
+      // Sync the planner to where the steppers stopped
+      SYNC_PLAN_POSITION_KINEMATIC();
+    }
+
+    #if HAS_BED_PROBE
+      // Deploy the probe. Probe will raise if needed.
+      if (DEPLOY_PROBE()) {
+        planner.abl_enabled = abl_should_enable;
+        return;
+      }
+    #endif
+
+    if (!faux) setup_for_endstop_or_probe_move();
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      #if ENABLED(PROBE_MANUALLY)
+        if (!no_action)
+      #endif
+      if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
+        || yGridSpacing != bilinear_grid_spacing[Y_AXIS]
+        || left_probe_bed_position != LOGICAL_X_POSITION(bilinear_start[X_AXIS])
+        || front_probe_bed_position != LOGICAL_Y_POSITION(bilinear_start[Y_AXIS])
+      ) {
+        if (dryrun) {
+          // Before reset bed level, re-enable to correct the position
+          planner.abl_enabled = abl_should_enable;
+        }
+        // Reset grid to 0.0 or "not probed". (Also disables ABL)
+        reset_bed_level();
+
+        // Initialize a grid with the given dimensions
+        bilinear_grid_spacing[X_AXIS] = xGridSpacing;
+        bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
+        bilinear_start[X_AXIS] = RAW_X_POSITION(left_probe_bed_position);
+        bilinear_start[Y_AXIS] = RAW_Y_POSITION(front_probe_bed_position);
+
+        // Can't re-enable (on error) until the new grid is written
+        abl_should_enable = false;
+      }
+
+    #endif // AUTO_BED_LEVELING_BILINEAR
+
+    #if ENABLED(AUTO_BED_LEVELING_3POINT)
+
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling");
+      #endif
+
+      // Probe at 3 arbitrary points
+      points[0].z = points[1].z = points[2].z = 0;
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+  } // !g29_in_progress
+
+  #if ENABLED(PROBE_MANUALLY)
+
+    // For manual probing, get the next index to probe now.
+    // On the first probe this will be incremented to 0.
+    if (!no_action) {
+      ++abl_probe_index;
+      g29_in_progress = true;
+    }
+
+    // Abort current G29 procedure, go back to idle state
+    if (seenA && g29_in_progress) {
+      SERIAL_PROTOCOLLNPGM("Manual G29 aborted");
+      #if HAS_SOFTWARE_ENDSTOPS
+        soft_endstops_enabled = enable_soft_endstops;
+      #endif
+      planner.abl_enabled = abl_should_enable;
+      g29_in_progress = false;
+      #if ENABLED(LCD_BED_LEVELING)
+        lcd_wait_for_move = false;
+      #endif
+    }
+
+    // Query G29 status
+    if (verbose_level || seenQ) {
+      SERIAL_PROTOCOLPGM("Manual G29 ");
+      if (g29_in_progress) {
+        SERIAL_PROTOCOLPAIR("point ", min(abl_probe_index + 1, abl2));
+        SERIAL_PROTOCOLLNPAIR(" of ", abl2);
+      }
+      else
+        SERIAL_PROTOCOLLNPGM("idle");
+    }
+
+    if (no_action) return;
+
+    if (abl_probe_index == 0) {
+      // For the initial G29 save software endstop state
+      #if HAS_SOFTWARE_ENDSTOPS
+        enable_soft_endstops = soft_endstops_enabled;
+      #endif
+    }
+    else {
+      // For G29 after adjusting Z.
+      // Save the previous Z before going to the next point
+      measured_z = current_position[Z_AXIS];
+
+      #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+        mean += measured_z;
+        eqnBVector[abl_probe_index] = measured_z;
+        eqnAMatrix[abl_probe_index + 0 * abl2] = xProbe;
+        eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe;
+        eqnAMatrix[abl_probe_index + 2 * abl2] = 1;
+
+        incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);
+
+      #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+        z_values[xCount][yCount] = measured_z + zoffset;
+
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_PROTOCOLPAIR("Save X", xCount);
+            SERIAL_PROTOCOLPAIR(" Y", yCount);
+            SERIAL_PROTOCOLLNPAIR(" Z", measured_z + zoffset);
+          }
+        #endif
+
+      #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+        points[abl_probe_index].z = measured_z;
+
+      #endif
+    }
+
+    //
+    // If there's another point to sample, move there with optional lift.
+    //
+
+    #if ABL_GRID
+
+      // Skip any unreachable points
+      while (abl_probe_index < abl2) {
+
+        // Set xCount, yCount based on abl_probe_index, with zig-zag
+        PR_OUTER_VAR = abl_probe_index / PR_INNER_END;
+        PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END);
+
+        // Probe in reverse order for every other row/column
+        bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_END) & 1);
+
+        if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR;
+
+        const float xBase = xCount * xGridSpacing + left_probe_bed_position,
+                    yBase = yCount * yGridSpacing + front_probe_bed_position;
+
+        xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
+        yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));
+
+        #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+          indexIntoAB[xCount][yCount] = abl_probe_index;
+        #endif
+
+        // Keep looping till a reachable point is found
+        if (position_is_reachable_xy(xProbe, yProbe)) break;
+        ++abl_probe_index;
+      }
+
+      // Is there a next point to move to?
+      if (abl_probe_index < abl2) {
+        _manual_goto_xy(xProbe, yProbe); // Can be used here too!
+        #if HAS_SOFTWARE_ENDSTOPS
+          // Disable software endstops to allow manual adjustment
+          // If G29 is not completed, they will not be re-enabled
+          soft_endstops_enabled = false;
+        #endif
+        return;
+      }
+      else {
+
+        // Leveling done! Fall through to G29 finishing code below
+
+        SERIAL_PROTOCOLLNPGM("Grid probing done.");
+
+        // Re-enable software endstops, if needed
+        #if HAS_SOFTWARE_ENDSTOPS
+          soft_endstops_enabled = enable_soft_endstops;
+        #endif
+      }
+
+    #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+      // Probe at 3 arbitrary points
+      if (abl_probe_index < 3) {
+        xProbe = LOGICAL_X_POSITION(points[abl_probe_index].x);
+        yProbe = LOGICAL_Y_POSITION(points[abl_probe_index].y);
+        #if HAS_SOFTWARE_ENDSTOPS
+          // Disable software endstops to allow manual adjustment
+          // If G29 is not completed, they will not be re-enabled
+          soft_endstops_enabled = false;
+        #endif
+        return;
+      }
+      else {
+
+        SERIAL_PROTOCOLLNPGM("3-point probing done.");
+
+        // Re-enable software endstops, if needed
+        #if HAS_SOFTWARE_ENDSTOPS
+          soft_endstops_enabled = enable_soft_endstops;
+        #endif
+
+        if (!dryrun) {
+          vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
+          if (planeNormal.z < 0) {
+            planeNormal.x *= -1;
+            planeNormal.y *= -1;
+            planeNormal.z *= -1;
+          }
+          planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+          // Can't re-enable (on error) until the new grid is written
+          abl_should_enable = false;
+        }
+
+      }
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+  #else // !PROBE_MANUALLY
+  {
+    const bool stow_probe_after_each = parser.boolval('E');
+
+    measured_z = 0;
+
+    #if ABL_GRID
+
+      bool zig = PR_OUTER_END & 1;  // Always end at RIGHT and BACK_PROBE_BED_POSITION
+
+      // Outer loop is Y with PROBE_Y_FIRST disabled
+      for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
+
+        int8_t inStart, inStop, inInc;
+
+        if (zig) { // away from origin
+          inStart = 0;
+          inStop = PR_INNER_END;
+          inInc = 1;
+        }
+        else {     // towards origin
+          inStart = PR_INNER_END - 1;
+          inStop = -1;
+          inInc = -1;
+        }
+
+        zig ^= true; // zag
+
+        // Inner loop is Y with PROBE_Y_FIRST enabled
+        for (int8_t PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; PR_INNER_VAR += inInc) {
+
+          float xBase = left_probe_bed_position + xGridSpacing * xCount,
+                yBase = front_probe_bed_position + yGridSpacing * yCount;
+
+          xProbe = FLOOR(xBase + (xBase < 0 ? 0 : 0.5));
+          yProbe = FLOOR(yBase + (yBase < 0 ? 0 : 0.5));
+
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+            indexIntoAB[xCount][yCount] = ++abl_probe_index; // 0...
+          #endif
+
+          #if IS_KINEMATIC
+            // Avoid probing outside the round or hexagonal area
+            if (!position_is_reachable_by_probe_xy(xProbe, yProbe)) continue;
+          #endif
+
+          measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
+
+          if (isnan(measured_z)) {
+            planner.abl_enabled = abl_should_enable;
+            break;
+          }
+
+          #if ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+            mean += measured_z;
+            eqnBVector[abl_probe_index] = measured_z;
+            eqnAMatrix[abl_probe_index + 0 * abl2] = xProbe;
+            eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe;
+            eqnAMatrix[abl_probe_index + 2 * abl2] = 1;
+
+            incremental_LSF(&lsf_results, xProbe, yProbe, measured_z);
+
+          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+            z_values[xCount][yCount] = measured_z + zoffset;
+
+          #endif
+
+          abl_should_enable = false;
+          idle();
+
+        } // inner
+      } // outer
+
+    #elif ENABLED(AUTO_BED_LEVELING_3POINT)
+
+      // Probe at 3 arbitrary points
+
+      for (uint8_t i = 0; i < 3; ++i) {
+        // Retain the last probe position
+        xProbe = LOGICAL_X_POSITION(points[i].x);
+        yProbe = LOGICAL_Y_POSITION(points[i].y);
+        measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
+        if (isnan(measured_z)) {
+          planner.abl_enabled = abl_should_enable;
+          break;
+        }
+        points[i].z = measured_z;
+      }
+
+      if (!dryrun && !isnan(measured_z)) {
+        vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
+        if (planeNormal.z < 0) {
+          planeNormal.x *= -1;
+          planeNormal.y *= -1;
+          planeNormal.z *= -1;
+        }
+        planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+        // Can't re-enable (on error) until the new grid is written
+        abl_should_enable = false;
+      }
+
+    #endif // AUTO_BED_LEVELING_3POINT
+
+    // Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
+    if (STOW_PROBE()) {
+      planner.abl_enabled = abl_should_enable;
+      measured_z = NAN;
+    }
+  }
+  #endif // !PROBE_MANUALLY
+
+  //
+  // G29 Finishing Code
+  //
+  // Unless this is a dry run, auto bed leveling will
+  // definitely be enabled after this point.
+  //
+  // If code above wants to continue leveling, it should
+  // return or loop before this point.
+  //
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position);
+  #endif
+
+  #if ENABLED(PROBE_MANUALLY)
+    g29_in_progress = false;
+    #if ENABLED(LCD_BED_LEVELING)
+      lcd_wait_for_move = false;
+    #endif
+  #endif
+
+  // Calculate leveling, print reports, correct the position
+  if (!isnan(measured_z)) {
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      if (!dryrun) extrapolate_unprobed_bed_level();
+      print_bilinear_leveling_grid();
+
+      refresh_bed_level();
+
+      #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+        print_bilinear_leveling_grid_virt();
+      #endif
+
+    #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
+
+      // For LINEAR leveling calculate matrix, print reports, correct the position
+
+      /**
+       * solve the plane equation ax + by + d = z
+       * A is the matrix with rows [x y 1] for all the probed points
+       * B is the vector of the Z positions
+       * the normal vector to the plane is formed by the coefficients of the
+       * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
+       * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
+       */
+      float plane_equation_coefficients[3];
+
+      finish_incremental_LSF(&lsf_results);
+      plane_equation_coefficients[0] = -lsf_results.A;  // We should be able to eliminate the '-' on these three lines and down below
+      plane_equation_coefficients[1] = -lsf_results.B;  // but that is not yet tested.
+      plane_equation_coefficients[2] = -lsf_results.D;
+
+      mean /= abl2;
+
+      if (verbose_level) {
+        SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
+        SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
+        SERIAL_PROTOCOLPGM(" b: ");
+        SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
+        SERIAL_PROTOCOLPGM(" d: ");
+        SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
+        SERIAL_EOL();
+        if (verbose_level > 2) {
+          SERIAL_PROTOCOLPGM("Mean of sampled points: ");
+          SERIAL_PROTOCOL_F(mean, 8);
+          SERIAL_EOL();
+        }
+      }
+
+      // Create the matrix but don't correct the position yet
+      if (!dryrun)
+        planner.bed_level_matrix = matrix_3x3::create_look_at(
+          vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1)    // We can eliminate the '-' here and up above
+        );
+
+      // Show the Topography map if enabled
+      if (do_topography_map) {
+
+        SERIAL_PROTOCOLLNPGM("\nBed Height Topography:\n"
+                               "   +--- BACK --+\n"
+                               "   |           |\n"
+                               " L |    (+)    | R\n"
+                               " E |           | I\n"
+                               " F | (-) N (+) | G\n"
+                               " T |           | H\n"
+                               "   |    (-)    | T\n"
+                               "   |           |\n"
+                               "   O-- FRONT --+\n"
+                               " (0,0)");
+
+        float min_diff = 999;
+
+        for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
+          for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
+            int ind = indexIntoAB[xx][yy];
+            float diff = eqnBVector[ind] - mean,
+                  x_tmp = eqnAMatrix[ind + 0 * abl2],
+                  y_tmp = eqnAMatrix[ind + 1 * abl2],
+                  z_tmp = 0;
+
+            apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
+
+            NOMORE(min_diff, eqnBVector[ind] - z_tmp);
+
+            if (diff >= 0.0)
+              SERIAL_PROTOCOLPGM(" +");   // Include + for column alignment
+            else
+              SERIAL_PROTOCOLCHAR(' ');
+            SERIAL_PROTOCOL_F(diff, 5);
+          } // xx
+          SERIAL_EOL();
+        } // yy
+        SERIAL_EOL();
+
+        if (verbose_level > 3) {
+          SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
+
+          for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
+            for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
+              int ind = indexIntoAB[xx][yy];
+              float x_tmp = eqnAMatrix[ind + 0 * abl2],
+                    y_tmp = eqnAMatrix[ind + 1 * abl2],
+                    z_tmp = 0;
+
+              apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
+
+              float diff = eqnBVector[ind] - z_tmp - min_diff;
+              if (diff >= 0.0)
+                SERIAL_PROTOCOLPGM(" +");
+              // Include + for column alignment
+              else
+                SERIAL_PROTOCOLCHAR(' ');
+              SERIAL_PROTOCOL_F(diff, 5);
+            } // xx
+            SERIAL_EOL();
+          } // yy
+          SERIAL_EOL();
+        }
+      } //do_topography_map
+
+    #endif // AUTO_BED_LEVELING_LINEAR
+
+    #if ABL_PLANAR
+
+      // For LINEAR and 3POINT leveling correct the current position
+
+      if (verbose_level > 0)
+        planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
+
+      if (!dryrun) {
+        //
+        // Correct the current XYZ position based on the tilted plane.
+        //
+
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
+        #endif
+
+        float converted[XYZ];
+        COPY(converted, current_position);
+
+        planner.abl_enabled = true;
+        planner.unapply_leveling(converted); // use conversion machinery
+        planner.abl_enabled = false;
+
+        // Use the last measured distance to the bed, if possible
+        if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
+          && NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
+        ) {
+          const float simple_z = current_position[Z_AXIS] - measured_z;
+          #if ENABLED(DEBUG_LEVELING_FEATURE)
+            if (DEBUGGING(LEVELING)) {
+              SERIAL_ECHOPAIR("Z from Probe:", simple_z);
+              SERIAL_ECHOPAIR("  Matrix:", converted[Z_AXIS]);
+              SERIAL_ECHOLNPAIR("  Discrepancy:", simple_z - converted[Z_AXIS]);
+            }
+          #endif
+          converted[Z_AXIS] = simple_z;
+        }
+
+        // The rotated XY and corrected Z are now current_position
+        COPY(current_position, converted);
+
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
+        #endif
+      }
+
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+      if (!dryrun) {
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("G29 uncorrected Z:", current_position[Z_AXIS]);
+        #endif
+
+        // Unapply the offset because it is going to be immediately applied
+        // and cause compensation movement in Z
+        current_position[Z_AXIS] -= bilinear_z_offset(current_position);
+
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR(" corrected Z:", current_position[Z_AXIS]);
+        #endif
+      }
+
+    #endif // ABL_PLANAR
+
+    #ifdef Z_PROBE_END_SCRIPT
+      #if ENABLED(DEBUG_LEVELING_FEATURE)
+        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
+      #endif
+      enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
+      stepper.synchronize();
+    #endif
+
+    // Auto Bed Leveling is complete! Enable if possible.
+    planner.abl_enabled = dryrun ? abl_should_enable : true;
+  } // !isnan(measured_z)
+
+  // Restore state after probing
+  if (!faux) clean_up_after_endstop_or_probe_move();
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< gcode_G29");
+  #endif
+
+  report_current_position();
+
+  KEEPALIVE_STATE(IN_HANDLER);
+
+  if (planner.abl_enabled)
+    SYNC_PLAN_POSITION_KINEMATIC();
+}

Marlin/src/gcode/calibrate/G29-mbl.h

@@ -0,0 +1,200 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "../../libs/buzzer.h"
+#include "../../lcd/ultralcd.h"
+
+// Save 130 bytes with non-duplication of PSTR
+void echo_not_entered() { SERIAL_PROTOCOLLNPGM(" not entered."); }
+
+void mbl_mesh_report() {
+  SERIAL_PROTOCOLLNPGM("Num X,Y: " STRINGIFY(GRID_MAX_POINTS_X) "," STRINGIFY(GRID_MAX_POINTS_Y));
+  SERIAL_PROTOCOLPGM("Z offset: "); SERIAL_PROTOCOL_F(mbl.z_offset, 5);
+  SERIAL_PROTOCOLLNPGM("\nMeasured points:");
+  print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 5,
+    [](const uint8_t ix, const uint8_t iy) { return mbl.z_values[ix][iy]; }
+  );
+}
+
+void mesh_probing_done() {
+  mbl.set_has_mesh(true);
+  home_all_axes();
+  set_bed_leveling_enabled(true);
+  #if ENABLED(MESH_G28_REST_ORIGIN)
+    current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS);
+    set_destination_to_current();
+    line_to_destination(homing_feedrate(Z_AXIS));
+    stepper.synchronize();
+  #endif
+}
+
+/**
+ * G29: Mesh-based Z probe, probes a grid and produces a
+ *      mesh to compensate for variable bed height
+ *
+ * Parameters With MESH_BED_LEVELING:
+ *
+ *  S0              Produce a mesh report
+ *  S1              Start probing mesh points
+ *  S2              Probe the next mesh point
+ *  S3 Xn Yn Zn.nn  Manually modify a single point
+ *  S4 Zn.nn        Set z offset. Positive away from bed, negative closer to bed.
+ *  S5              Reset and disable mesh
+ *
+ * The S0 report the points as below
+ *
+ *  +----> X-axis  1-n
+ *  |
+ *  |
+ *  v Y-axis  1-n
+ *
+ */
+void gcode_G29() {
+
+  static int mbl_probe_index = -1;
+  #if HAS_SOFTWARE_ENDSTOPS
+    static bool enable_soft_endstops;
+  #endif
+
+  const MeshLevelingState state = (MeshLevelingState)parser.byteval('S', (int8_t)MeshReport);
+  if (!WITHIN(state, 0, 5)) {
+    SERIAL_PROTOCOLLNPGM("S out of range (0-5).");
+    return;
+  }
+
+  int8_t px, py;
+
+  switch (state) {
+    case MeshReport:
+      if (leveling_is_valid()) {
+        SERIAL_PROTOCOLLNPAIR("State: ", leveling_is_active() ? MSG_ON : MSG_OFF);
+        mbl_mesh_report();
+      }
+      else
+        SERIAL_PROTOCOLLNPGM("Mesh bed leveling has no data.");
+      break;
+
+    case MeshStart:
+      mbl.reset();
+      mbl_probe_index = 0;
+      enqueue_and_echo_commands_P(PSTR("G28\nG29 S2"));
+      break;
+
+    case MeshNext:
+      if (mbl_probe_index < 0) {
+        SERIAL_PROTOCOLLNPGM("Start mesh probing with \"G29 S1\" first.");
+        return;
+      }
+      // For each G29 S2...
+      if (mbl_probe_index == 0) {
+        #if HAS_SOFTWARE_ENDSTOPS
+          // For the initial G29 S2 save software endstop state
+          enable_soft_endstops = soft_endstops_enabled;
+        #endif
+      }
+      else {
+        // For G29 S2 after adjusting Z.
+        mbl.set_zigzag_z(mbl_probe_index - 1, current_position[Z_AXIS]);
+        #if HAS_SOFTWARE_ENDSTOPS
+          soft_endstops_enabled = enable_soft_endstops;
+        #endif
+      }
+      // If there's another point to sample, move there with optional lift.
+      if (mbl_probe_index < GRID_MAX_POINTS) {
+        mbl.zigzag(mbl_probe_index, px, py);
+        _manual_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]);
+
+        #if HAS_SOFTWARE_ENDSTOPS
+          // Disable software endstops to allow manual adjustment
+          // If G29 is not completed, they will not be re-enabled
+          soft_endstops_enabled = false;
+        #endif
+
+        mbl_probe_index++;
+      }
+      else {
+        // One last "return to the bed" (as originally coded) at completion
+        current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS) + MANUAL_PROBE_HEIGHT;
+        line_to_current_position();
+        stepper.synchronize();
+
+        // After recording the last point, activate home and activate
+        mbl_probe_index = -1;
+        SERIAL_PROTOCOLLNPGM("Mesh probing done.");
+        BUZZ(100, 659);
+        BUZZ(100, 698);
+        mesh_probing_done();
+      }
+      break;
+
+    case MeshSet:
+      if (parser.seenval('X')) {
+        px = parser.value_int() - 1;
+        if (!WITHIN(px, 0, GRID_MAX_POINTS_X - 1)) {
+          SERIAL_PROTOCOLLNPGM("X out of range (1-" STRINGIFY(GRID_MAX_POINTS_X) ").");
+          return;
+        }
+      }
+      else {
+        SERIAL_CHAR('X'); echo_not_entered();
+        return;
+      }
+
+      if (parser.seenval('Y')) {
+        py = parser.value_int() - 1;
+        if (!WITHIN(py, 0, GRID_MAX_POINTS_Y - 1)) {
+          SERIAL_PROTOCOLLNPGM("Y out of range (1-" STRINGIFY(GRID_MAX_POINTS_Y) ").");
+          return;
+        }
+      }
+      else {
+        SERIAL_CHAR('Y'); echo_not_entered();
+        return;
+      }
+
+      if (parser.seenval('Z')) {
+        mbl.z_values[px][py] = parser.value_linear_units();
+      }
+      else {
+        SERIAL_CHAR('Z'); echo_not_entered();
+        return;
+      }
+      break;
+
+    case MeshSetZOffset:
+      if (parser.seenval('Z')) {
+        mbl.z_offset = parser.value_linear_units();
+      }
+      else {
+        SERIAL_CHAR('Z'); echo_not_entered();
+        return;
+      }
+      break;
+
+    case MeshReset:
+      reset_bed_level();
+      break;
+
+  } // switch(state)
+
+  report_current_position();
+}

Marlin/src/gcode/calibrate/G29-ubl.h

@@ -0,0 +1,27 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void gcode_G29() {
+
+  ubl.G29();
+
+}

Marlin/src/gcode/calibrate/G29.h

@@ -0,0 +1,65 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY)
+
+  #if ENABLED(PROBE_MANUALLY) && ENABLED(LCD_BED_LEVELING)
+    extern bool lcd_wait_for_move;
+  #endif
+
+  inline void _manual_goto_xy(const float &x, const float &y) {
+    const float old_feedrate_mm_s = feedrate_mm_s;
+    #if MANUAL_PROBE_HEIGHT > 0
+      const float prev_z = current_position[Z_AXIS];
+      feedrate_mm_s = homing_feedrate(Z_AXIS);
+      current_position[Z_AXIS] = LOGICAL_Z_POSITION(MANUAL_PROBE_HEIGHT);
+      line_to_current_position();
+    #endif
+
+    feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED);
+    current_position[X_AXIS] = LOGICAL_X_POSITION(x);
+    current_position[Y_AXIS] = LOGICAL_Y_POSITION(y);
+    line_to_current_position();
+
+    #if MANUAL_PROBE_HEIGHT > 0
+      feedrate_mm_s = homing_feedrate(Z_AXIS);
+      current_position[Z_AXIS] = prev_z; // move back to the previous Z.
+      line_to_current_position();
+    #endif
+
+    feedrate_mm_s = old_feedrate_mm_s;
+    stepper.synchronize();
+
+    #if ENABLED(PROBE_MANUALLY) && ENABLED(LCD_BED_LEVELING)
+      lcd_wait_for_move = false;
+    #endif
+  }
+
+#endif
+
+#if ENABLED(MESH_BED_LEVELING)
+  #include "G29-mbl.h"
+#elif ENABLED(AUTO_BED_LEVELING_UBL)
+  #include "G29-ubl.h"
+#elif HAS_ABL
+  #include "G29-abl.h"
+#endif

Marlin/src/gcode/calibrate/G33.h

@@ -0,0 +1,451 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "common.h"
+
+#if HOTENDS > 1
+  #include "../control/tool_change.h"
+#endif
+
+/**
+ * G33 - Delta '1-4-7-point' Auto-Calibration
+ *       Calibrate height, endstops, delta radius, and tower angles.
+ *
+ * Parameters:
+ *
+ *   Pn  Number of probe points:
+ *
+ *      P1     Probe center and set height only.
+ *      P2     Probe center and towers. Set height, endstops, and delta radius.
+ *      P3     Probe all positions: center, towers and opposite towers. Set all.
+ *      P4-P7  Probe all positions at different locations and average them.
+ *
+ *   T0  Don't calibrate tower angle corrections
+ *
+ *   Cn.nn Calibration precision; when omitted calibrates to maximum precision
+ *
+ *   Fn  Force to run at least n iterations and takes the best result
+ *
+ *   Vn  Verbose level:
+ *
+ *      V0  Dry-run mode. Report settings and probe results. No calibration.
+ *      V1  Report settings
+ *      V2  Report settings and probe results
+ *
+ *   E   Engage the probe for each point
+ */
+
+void print_signed_float(const char * const prefix, const float &f) {
+  SERIAL_PROTOCOLPGM("  ");
+  serialprintPGM(prefix);
+  SERIAL_PROTOCOLCHAR(':');
+  if (f >= 0) SERIAL_CHAR('+');
+  SERIAL_PROTOCOL_F(f, 2);
+}
+
+inline void print_G33_settings(const bool end_stops, const bool tower_angles){ // TODO echo these to LCD ???
+  SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
+  if (end_stops) {
+    print_signed_float(PSTR("  Ex"), endstop_adj[A_AXIS]);
+    print_signed_float(PSTR("Ey"), endstop_adj[B_AXIS]);
+    print_signed_float(PSTR("Ez"), endstop_adj[C_AXIS]);
+    SERIAL_PROTOCOLPAIR("    Radius:", delta_radius);
+  }
+  SERIAL_EOL();
+  if (tower_angles) {
+    SERIAL_PROTOCOLPGM(".Tower angle :  ");
+    print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]);
+    print_signed_float(PSTR("Ty"), delta_tower_angle_trim[B_AXIS]);
+    SERIAL_PROTOCOLLNPGM("  Tz:+0.00");
+  }
+}
+
+void G33_cleanup(
+  #if HOTENDS > 1
+    const uint8_t old_tool_index
+  #endif
+) {
+  #if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
+    do_blocking_move_to_z(delta_clip_start_height);
+  #endif
+  STOW_PROBE();
+  clean_up_after_endstop_or_probe_move();
+  #if HOTENDS > 1
+    tool_change(old_tool_index, 0, true);
+  #endif
+}
+
+void gcode_G33() {
+
+  const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
+  if (!WITHIN(probe_points, 1, 7)) {
+    SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (1-7).");
+    return;
+  }
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 2)) {
+    SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-2).");
+    return;
+  }
+
+  const float calibration_precision = parser.floatval('C');
+  if (calibration_precision < 0) {
+    SERIAL_PROTOCOLLNPGM("?(C)alibration precision is implausible (>0).");
+    return;
+  }
+
+  const int8_t force_iterations = parser.intval('F', 0);
+  if (!WITHIN(force_iterations, 0, 30)) {
+    SERIAL_PROTOCOLLNPGM("?(F)orce iteration is implausible (0-30).");
+    return;
+  }
+
+  const bool towers_set           = parser.boolval('T', true),
+             stow_after_each      = parser.boolval('E'),
+             _1p_calibration      = probe_points == 1,
+             _4p_calibration      = probe_points == 2,
+             _4p_towers_points    = _4p_calibration && towers_set,
+             _4p_opposite_points  = _4p_calibration && !towers_set,
+             _7p_calibration      = probe_points >= 3,
+             _7p_half_circle      = probe_points == 3,
+             _7p_double_circle    = probe_points == 5,
+             _7p_triple_circle    = probe_points == 6,
+             _7p_quadruple_circle = probe_points == 7,
+             _7p_multi_circle     = _7p_double_circle || _7p_triple_circle || _7p_quadruple_circle,
+             _7p_intermed_points  = _7p_calibration && !_7p_half_circle;
+  const static char save_message[] PROGMEM = "Save with M500 and/or copy to Configuration.h";
+  const float dx = (X_PROBE_OFFSET_FROM_EXTRUDER),
+              dy = (Y_PROBE_OFFSET_FROM_EXTRUDER);
+  int8_t iterations = 0;
+  float test_precision,
+        zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
+        zero_std_dev_old = zero_std_dev,
+        zero_std_dev_min = zero_std_dev,
+        e_old[XYZ] = {
+          endstop_adj[A_AXIS],
+          endstop_adj[B_AXIS],
+          endstop_adj[C_AXIS]
+        },
+        dr_old = delta_radius,
+        zh_old = home_offset[Z_AXIS],
+        alpha_old = delta_tower_angle_trim[A_AXIS],
+        beta_old = delta_tower_angle_trim[B_AXIS];
+
+  if (!_1p_calibration) {  // test if the outer radius is reachable
+    const float circles = (_7p_quadruple_circle ? 1.5 :
+                           _7p_triple_circle    ? 1.0 :
+                           _7p_double_circle    ? 0.5 : 0),
+                r = (1 + circles * 0.1) * delta_calibration_radius;
+    for (uint8_t axis = 1; axis < 13; ++axis) {
+      const float a = RADIANS(180 + 30 * axis);
+      if (!position_is_reachable_xy(cos(a) * r, sin(a) * r)) {
+        SERIAL_PROTOCOLLNPGM("?(M665 B)ed radius is implausible.");
+        return;
+      }
+    }
+  }
+  SERIAL_PROTOCOLLNPGM("G33 Auto Calibrate");
+
+  stepper.synchronize();
+  #if HAS_LEVELING
+    reset_bed_level(); // After calibration bed-level data is no longer valid
+  #endif
+
+  #if HOTENDS > 1
+    const uint8_t old_tool_index = active_extruder;
+    tool_change(0, 0, true);
+    #define G33_CLEANUP() G33_cleanup(old_tool_index)
+  #else
+    #define G33_CLEANUP() G33_cleanup()
+  #endif
+
+  setup_for_endstop_or_probe_move();
+  endstops.enable(true);
+  if (!home_delta())
+    return;
+  endstops.not_homing();
+
+  // print settings
+
+  const char *checkingac = PSTR("Checking... AC"); // TODO: Make translatable string
+  serialprintPGM(checkingac);
+  if (verbose_level == 0) SERIAL_PROTOCOLPGM(" (DRY-RUN)");
+  SERIAL_EOL();
+  lcd_setstatusPGM(checkingac);
+
+  print_G33_settings(!_1p_calibration, _7p_calibration && towers_set);
+
+  #if DISABLED(PROBE_MANUALLY)
+    const float measured_z = probe_pt(dx, dy, stow_after_each, 1, false); // 1st probe to set height
+    if (isnan(measured_z)) return G33_CLEANUP();
+    home_offset[Z_AXIS] -= measured_z;
+  #endif
+
+  do {
+
+    float z_at_pt[13] = { 0.0 };
+
+    test_precision = zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev;
+
+    iterations++;
+
+    // Probe the points
+
+    if (!_7p_half_circle && !_7p_triple_circle) { // probe the center
+      #if ENABLED(PROBE_MANUALLY)
+        z_at_pt[0] += lcd_probe_pt(0, 0);
+      #else
+        z_at_pt[0] += probe_pt(dx, dy, stow_after_each, 1, false);
+        if (isnan(z_at_pt[0])) return G33_CLEANUP();
+      #endif
+    }
+    if (_7p_calibration) { // probe extra center points
+      for (int8_t axis = _7p_multi_circle ? 11 : 9; axis > 0; axis -= _7p_multi_circle ? 2 : 4) {
+        const float a = RADIANS(180 + 30 * axis), r = delta_calibration_radius * 0.1;
+        #if ENABLED(PROBE_MANUALLY)
+          z_at_pt[0] += lcd_probe_pt(cos(a) * r, sin(a) * r);
+        #else
+          z_at_pt[0] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
+          if (isnan(z_at_pt[0])) return G33_CLEANUP();
+        #endif
+      }
+      z_at_pt[0] /= float(_7p_double_circle ? 7 : probe_points);
+    }
+    if (!_1p_calibration) {  // probe the radius
+      bool zig_zag = true;
+      const uint8_t start = _4p_opposite_points ? 3 : 1,
+                     step = _4p_calibration ? 4 : _7p_half_circle ? 2 : 1;
+      for (uint8_t axis = start; axis < 13; axis += step) {
+        const float zigadd = (zig_zag ? 0.5 : 0.0),
+                    offset_circles = _7p_quadruple_circle ? zigadd + 1.0 :
+                                     _7p_triple_circle    ? zigadd + 0.5 :
+                                     _7p_double_circle    ? zigadd : 0;
+        for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
+          const float a = RADIANS(180 + 30 * axis),
+                      r = delta_calibration_radius * (1 + circles * (zig_zag ? 0.1 : -0.1));
+          #if ENABLED(PROBE_MANUALLY)
+            z_at_pt[axis] += lcd_probe_pt(cos(a) * r, sin(a) * r);
+          #else
+            z_at_pt[axis] += probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1);
+            if (isnan(z_at_pt[axis])) return G33_CLEANUP();
+          #endif
+        }
+        zig_zag = !zig_zag;
+        z_at_pt[axis] /= (2 * offset_circles + 1);
+      }
+    }
+    if (_7p_intermed_points) // average intermediates to tower and opposites
+      for (uint8_t axis = 1; axis < 13; axis += 2)
+        z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0;
+
+    float S1 = z_at_pt[0],
+          S2 = sq(z_at_pt[0]);
+    int16_t N = 1;
+    if (!_1p_calibration) // std dev from zero plane
+      for (uint8_t axis = (_4p_opposite_points ? 3 : 1); axis < 13; axis += (_4p_calibration ? 4 : 2)) {
+        S1 += z_at_pt[axis];
+        S2 += sq(z_at_pt[axis]);
+        N++;
+      }
+    zero_std_dev_old = zero_std_dev;
+    zero_std_dev = round(SQRT(S2 / N) * 1000.0) / 1000.0 + 0.00001;
+
+    // Solve matrices
+
+    if ((zero_std_dev < test_precision && zero_std_dev > calibration_precision) || iterations <= force_iterations) {
+      if (zero_std_dev < zero_std_dev_min) {
+        COPY(e_old, endstop_adj);
+        dr_old = delta_radius;
+        zh_old = home_offset[Z_AXIS];
+        alpha_old = delta_tower_angle_trim[A_AXIS];
+        beta_old = delta_tower_angle_trim[B_AXIS];
+      }
+
+      float e_delta[XYZ] = { 0.0 }, r_delta = 0.0, t_alpha = 0.0, t_beta = 0.0;
+      const float r_diff = delta_radius - delta_calibration_radius,
+                  h_factor = 1.00 + r_diff * 0.001,                          //1.02 for r_diff = 20mm
+                  r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)),  //2.25 for r_diff = 20mm
+                  a_factor = 100.0 / delta_calibration_radius;               //1.25 for cal_rd = 80mm
+
+      #define ZP(N,I) ((N) * z_at_pt[I])
+      #define Z1000(I) ZP(1.00, I)
+      #define Z1050(I) ZP(h_factor, I)
+      #define Z0700(I) ZP(h_factor * 2.0 / 3.00, I)
+      #define Z0350(I) ZP(h_factor / 3.00, I)
+      #define Z0175(I) ZP(h_factor / 6.00, I)
+      #define Z2250(I) ZP(r_factor, I)
+      #define Z0750(I) ZP(r_factor / 3.00, I)
+      #define Z0375(I) ZP(r_factor / 6.00, I)
+      #define Z0444(I) ZP(a_factor * 4.0 / 9.0, I)
+      #define Z0888(I) ZP(a_factor * 8.0 / 9.0, I)
+
+      #if ENABLED(PROBE_MANUALLY)
+        test_precision = 0.00; // forced end
+      #endif
+
+      switch (probe_points) {
+        case 1:
+          test_precision = 0.00; // forced end
+          LOOP_XYZ(i) e_delta[i] = Z1000(0);
+          break;
+
+        case 2:
+          if (towers_set) {
+            e_delta[X_AXIS] = Z1050(0) + Z0700(1) - Z0350(5) - Z0350(9);
+            e_delta[Y_AXIS] = Z1050(0) - Z0350(1) + Z0700(5) - Z0350(9);
+            e_delta[Z_AXIS] = Z1050(0) - Z0350(1) - Z0350(5) + Z0700(9);
+            r_delta         = Z2250(0) - Z0750(1) - Z0750(5) - Z0750(9);
+          }
+          else {
+            e_delta[X_AXIS] = Z1050(0) - Z0700(7) + Z0350(11) + Z0350(3);
+            e_delta[Y_AXIS] = Z1050(0) + Z0350(7) - Z0700(11) + Z0350(3);
+            e_delta[Z_AXIS] = Z1050(0) + Z0350(7) + Z0350(11) - Z0700(3);
+            r_delta         = Z2250(0) - Z0750(7) - Z0750(11) - Z0750(3);
+          }
+          break;
+
+        default:
+          e_delta[X_AXIS] = Z1050(0) + Z0350(1) - Z0175(5) - Z0175(9) - Z0350(7) + Z0175(11) + Z0175(3);
+          e_delta[Y_AXIS] = Z1050(0) - Z0175(1) + Z0350(5) - Z0175(9) + Z0175(7) - Z0350(11) + Z0175(3);
+          e_delta[Z_AXIS] = Z1050(0) - Z0175(1) - Z0175(5) + Z0350(9) + Z0175(7) + Z0175(11) - Z0350(3);
+          r_delta         = Z2250(0) - Z0375(1) - Z0375(5) - Z0375(9) - Z0375(7) - Z0375(11) - Z0375(3);
+
+          if (towers_set) {
+            t_alpha = Z0444(1) - Z0888(5) + Z0444(9) + Z0444(7) - Z0888(11) + Z0444(3);
+            t_beta  = Z0888(1) - Z0444(5) - Z0444(9) + Z0888(7) - Z0444(11) - Z0444(3);
+          }
+          break;
+      }
+
+      LOOP_XYZ(axis) endstop_adj[axis] += e_delta[axis];
+      delta_radius += r_delta;
+      delta_tower_angle_trim[A_AXIS] += t_alpha;
+      delta_tower_angle_trim[B_AXIS] += t_beta;
+
+      // adjust delta_height and endstops by the max amount
+      const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
+      home_offset[Z_AXIS] -= z_temp;
+      LOOP_XYZ(i) endstop_adj[i] -= z_temp;
+
+      recalc_delta_settings(delta_radius, delta_diagonal_rod);
+    }
+    else if (zero_std_dev >= test_precision) {   // step one back
+      COPY(endstop_adj, e_old);
+      delta_radius = dr_old;
+      home_offset[Z_AXIS] = zh_old;
+      delta_tower_angle_trim[A_AXIS] = alpha_old;
+      delta_tower_angle_trim[B_AXIS] = beta_old;
+
+      recalc_delta_settings(delta_radius, delta_diagonal_rod);
+    }
+    NOMORE(zero_std_dev_min, zero_std_dev);
+
+    // print report
+
+    if (verbose_level != 1) {
+      SERIAL_PROTOCOLPGM(".    ");
+      print_signed_float(PSTR("c"), z_at_pt[0]);
+      if (_4p_towers_points || _7p_calibration) {
+        print_signed_float(PSTR("   x"), z_at_pt[1]);
+        print_signed_float(PSTR(" y"), z_at_pt[5]);
+        print_signed_float(PSTR(" z"), z_at_pt[9]);
+      }
+      if (!_4p_opposite_points) SERIAL_EOL();
+      if ((_4p_opposite_points) || _7p_calibration) {
+        if (_7p_calibration) {
+          SERIAL_CHAR('.');
+          SERIAL_PROTOCOL_SP(13);
+        }
+        print_signed_float(PSTR("  yz"), z_at_pt[7]);
+        print_signed_float(PSTR("zx"), z_at_pt[11]);
+        print_signed_float(PSTR("xy"), z_at_pt[3]);
+        SERIAL_EOL();
+      }
+    }
+    if (verbose_level != 0) {                                    // !dry run
+      if ((zero_std_dev >= test_precision || zero_std_dev <= calibration_precision) && iterations > force_iterations) {  // end iterations
+        SERIAL_PROTOCOLPGM("Calibration OK");
+        SERIAL_PROTOCOL_SP(36);
+        #if DISABLED(PROBE_MANUALLY)
+          if (zero_std_dev >= test_precision && !_1p_calibration)
+            SERIAL_PROTOCOLPGM("rolling back.");
+          else
+        #endif
+          {
+            SERIAL_PROTOCOLPGM("std dev:");
+            SERIAL_PROTOCOL_F(zero_std_dev_min, 3);
+          }
+        SERIAL_EOL();
+        char mess[21];
+        sprintf_P(mess, PSTR("Calibration sd:"));
+        if (zero_std_dev_min < 1)
+          sprintf_P(&mess[15], PSTR("0.%03i"), (int)round(zero_std_dev_min * 1000.0));
+        else
+          sprintf_P(&mess[15], PSTR("%03i.x"), (int)round(zero_std_dev_min));
+        lcd_setstatus(mess);
+        print_G33_settings(!_1p_calibration, _7p_calibration && towers_set);
+        serialprintPGM(save_message);
+        SERIAL_EOL();
+      }
+      else {                                                     // !end iterations
+        char mess[15];
+        if (iterations < 31)
+          sprintf_P(mess, PSTR("Iteration : %02i"), (int)iterations);
+        else
+          sprintf_P(mess, PSTR("No convergence"));
+        SERIAL_PROTOCOL(mess);
+        SERIAL_PROTOCOL_SP(36);
+        SERIAL_PROTOCOLPGM("std dev:");
+        SERIAL_PROTOCOL_F(zero_std_dev, 3);
+        SERIAL_EOL();
+        lcd_setstatus(mess);
+        print_G33_settings(!_1p_calibration, _7p_calibration && towers_set);
+      }
+    }
+    else {                                                       // dry run
+      const char *enddryrun = PSTR("End DRY-RUN");
+      serialprintPGM(enddryrun);
+      SERIAL_PROTOCOL_SP(39);
+      SERIAL_PROTOCOLPGM("std dev:");
+      SERIAL_PROTOCOL_F(zero_std_dev, 3);
+      SERIAL_EOL();
+
+      char mess[21];
+      sprintf_P(mess, enddryrun);
+      sprintf_P(&mess[11], PSTR(" sd:"));
+      if (zero_std_dev < 1)
+        sprintf_P(&mess[15], PSTR("0.%03i"), (int)round(zero_std_dev * 1000.0));
+      else
+        sprintf_P(&mess[15], PSTR("%03i.x"), (int)round(zero_std_dev));
+      lcd_setstatus(mess);
+    }
+
+    endstops.enable(true);
+    home_delta();
+    endstops.not_homing();
+
+  }
+  while ((zero_std_dev < test_precision && zero_std_dev > calibration_precision && iterations < 31) || iterations <= force_iterations);
+
+  G33_CLEANUP();
+}

Marlin/M100_Free_Mem_Chk.cpp → Marlin/src/gcode/calibrate/M100.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../../libs/hex_print_routines.h"
+
 /**
  * M100 Free Memory Watcher
  *
@@ -48,10 +50,6 @@
 #define M100_FREE_MEMORY_DUMPER     // Enable for the `M110 D` Dump sub-command
 #define M100_FREE_MEMORY_CORRUPTOR  // Enable for the `M100 C` Corrupt sub-command
 
-#include "MarlinConfig.h"
-
-#if ENABLED(M100_FREE_MEMORY_WATCHER)
-
 #define TEST_BYTE ((char) 0xE5)
 
 extern char command_queue[BUFSIZE][MAX_CMD_SIZE];
@@ -60,16 +58,11 @@ extern char* __brkval;
 extern size_t  __heap_start, __heap_end, __flp;
 extern char __bss_end;
 
-#include "Marlin.h"
-#include "gcode.h"
-#include "hex_print_routines.h"
-
 //
 // Utility functions
 //
 
 #define END_OF_HEAP() (__brkval ? __brkval : &__bss_end)
-int check_for_free_memory_corruption(const char * const title);
 
 // Location of a variable on its stack frame. Returns a value above
 // the stack (once the function returns to the caller).
@@ -137,18 +130,79 @@ int16_t count_test_bytes(const char * const ptr) {
     }
   }
 
-void M100_dump_routine(const char * const title, const char *start, const char *end) {
-  SERIAL_ECHOLN(title);
-  //
-  // Round the start and end locations to produce full lines of output
-  //
-  start = (char*)((uint16_t) start & 0xFFF0);
-  end   = (char*)((uint16_t) end   | 0x000F);
-  dump_free_memory(start, end);
-}
+  void M100_dump_routine(const char * const title, const char *start, const char *end) {
+    SERIAL_ECHOLN(title);
+    //
+    // Round the start and end locations to produce full lines of output
+    //
+    start = (char*)((uint16_t) start & 0xFFF0);
+    end   = (char*)((uint16_t) end   | 0x000F);
+    dump_free_memory(start, end);
+  }
 
 #endif // M100_FREE_MEMORY_DUMPER
 
+int check_for_free_memory_corruption(const char * const title) {
+  SERIAL_ECHO(title);
+
+  char *ptr = END_OF_HEAP(), *sp = top_of_stack();
+  int n = sp - ptr;
+
+  SERIAL_ECHOPAIR("\nfmc() n=", n);
+  SERIAL_ECHOPAIR("\n&__brkval: ", hex_address(&__brkval));
+  SERIAL_ECHOPAIR("=",             hex_address(__brkval));
+  SERIAL_ECHOPAIR("\n__bss_end: ", hex_address(&__bss_end));
+  SERIAL_ECHOPAIR(" sp=",          hex_address(sp));
+
+  if (sp < ptr)  {
+    SERIAL_ECHOPGM(" sp < Heap ");
+    // SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
+    // safe_delay(5);                  // this code can be enabled to pause the display as soon as the
+    // while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
+    //   idle();                       // being on pin-63 which is unassigend and available on most controller
+    // safe_delay(20);                 // boards.
+    // while ( !READ(63))
+    //   idle();
+    safe_delay(20);
+    #if ENABLED(M100_FREE_MEMORY_DUMPER)
+      M100_dump_routine("   Memory corruption detected with sp<Heap\n", (char*)0x1B80, (char*)0x21FF);
+    #endif
+  }
+
+  // Scan through the range looking for the biggest block of 0xE5's we can find
+  int block_cnt = 0;
+  for (int i = 0; i < n; i++) {
+    if (ptr[i] == TEST_BYTE) {
+      int16_t j = count_test_bytes(ptr + i);
+      if (j > 8) {
+        // SERIAL_ECHOPAIR("Found ", j);
+        // SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(ptr + i));
+        i += j;
+        block_cnt++;
+        SERIAL_ECHOPAIR(" (", block_cnt);
+        SERIAL_ECHOPAIR(") found=", j);
+        SERIAL_ECHOPGM("   ");
+      }
+    }
+  }
+  SERIAL_ECHOPAIR("  block_found=", block_cnt);
+
+  if (block_cnt != 1 || __brkval != 0x0000)
+    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
+
+  if (block_cnt == 0)       // Make sure the special case of no free blocks shows up as an
+    block_cnt = -1;         // error to the calling code!
+
+  SERIAL_ECHOPGM(" return=");
+  if (block_cnt == 1) {
+    SERIAL_CHAR('0');       // if the block_cnt is 1, nothing has broken up the free memory
+    SERIAL_EOL();             // area and it is appropriate to say 'no corruption'.
+    return 0;
+  }
+  SERIAL_ECHOLNPGM("true");
+  return block_cnt;
+}
+
 /**
  * M100 F
  *  Return the number of free bytes in the memory pool,
@@ -266,68 +320,3 @@ void gcode_M100() {
 
   #endif
 }
-
-int check_for_free_memory_corruption(const char * const title) {
-  SERIAL_ECHO(title);
-
-  char *ptr = END_OF_HEAP(), *sp = top_of_stack();
-  int n = sp - ptr;
-
-  SERIAL_ECHOPAIR("\nfmc() n=", n);
-  SERIAL_ECHOPAIR("\n&__brkval: ", hex_address(&__brkval));
-  SERIAL_ECHOPAIR("=",             hex_address(__brkval));
-  SERIAL_ECHOPAIR("\n__bss_end: ", hex_address(&__bss_end));
-  SERIAL_ECHOPAIR(" sp=",          hex_address(sp));
-
-  if (sp < ptr)  {
-    SERIAL_ECHOPGM(" sp < Heap ");
-    // SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
-    // safe_delay(5);                  // this code can be enabled to pause the display as soon as the
-    // while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
-    //   idle();                       // being on pin-63 which is unassigend and available on most controller
-    // safe_delay(20);                 // boards.
-    // while ( !READ(63))
-    //   idle();
-    safe_delay(20);
-    #ifdef M100_FREE_MEMORY_DUMPER
-      M100_dump_routine("   Memory corruption detected with sp<Heap\n", (char*)0x1B80, (char*)0x21FF);
-    #endif
-  }
-
-  // Scan through the range looking for the biggest block of 0xE5's we can find
-  int block_cnt = 0;
-  for (int i = 0; i < n; i++) {
-    if (ptr[i] == TEST_BYTE) {
-      int16_t j = count_test_bytes(ptr + i);
-      if (j > 8) {
-        // SERIAL_ECHOPAIR("Found ", j);
-        // SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(ptr + i));
-        i += j;
-        block_cnt++;
-        SERIAL_ECHOPAIR(" (", block_cnt);
-        SERIAL_ECHOPAIR(") found=", j);
-        SERIAL_ECHOPGM("   ");
-      }
-    }
-  }
-  SERIAL_ECHOPAIR("  block_found=", block_cnt);
-
-  if (block_cnt != 1 || __brkval != 0x0000)
-    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
-
-  if (block_cnt == 0)       // Make sure the special case of no free blocks shows up as an
-    block_cnt = -1;         // error to the calling code!
-
-  SERIAL_ECHOPGM(" return=");
-  if (block_cnt == 1) {
-    SERIAL_CHAR('0');       // if the block_cnt is 1, nothing has broken up the free memory
-    SERIAL_EOL();             // area and it is appropriate to say 'no corruption'.
-    return 0;
-  }
-  SERIAL_ECHOLNPGM("true");
-  return block_cnt;
-}
-
-#endif // M100_FREE_MEMORY_WATCHER
-
-

Marlin/src/gcode/calibrate/M420.h

@@ -0,0 +1,121 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M420: Enable/Disable Bed Leveling and/or set the Z fade height.
+ *
+ *   S[bool]   Turns leveling on or off
+ *   Z[height] Sets the Z fade height (0 or none to disable)
+ *   V[bool]   Verbose - Print the leveling grid
+ *
+ * With AUTO_BED_LEVELING_UBL only:
+ *
+ *   L[index]  Load UBL mesh from index (0 is default)
+ */
+void gcode_M420() {
+
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+    // L to load a mesh from the EEPROM
+    if (parser.seen('L')) {
+
+      #if ENABLED(EEPROM_SETTINGS)
+        const int8_t storage_slot = parser.has_value() ? parser.value_int() : ubl.state.storage_slot;
+        const int16_t a = settings.calc_num_meshes();
+
+        if (!a) {
+          SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
+          return;
+        }
+
+        if (!WITHIN(storage_slot, 0, a - 1)) {
+          SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
+          SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
+          return;
+        }
+
+        settings.load_mesh(storage_slot);
+        ubl.state.storage_slot = storage_slot;
+
+      #else
+
+        SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
+        return;
+
+      #endif
+    }
+
+    // L to load a mesh from the EEPROM
+    if (parser.seen('L') || parser.seen('V')) {
+      ubl.display_map(0);  // Currently only supports one map type
+      SERIAL_ECHOLNPAIR("UBL_MESH_VALID = ", UBL_MESH_VALID);
+      SERIAL_ECHOLNPAIR("ubl.state.storage_slot = ", ubl.state.storage_slot);
+    }
+
+  #endif // AUTO_BED_LEVELING_UBL
+
+  // V to print the matrix or mesh
+  if (parser.seen('V')) {
+    #if ABL_PLANAR
+      planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:"));
+    #else
+      if (leveling_is_valid()) {
+        #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          print_bilinear_leveling_grid();
+          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+            print_bilinear_leveling_grid_virt();
+          #endif
+        #elif ENABLED(MESH_BED_LEVELING)
+          SERIAL_ECHOLNPGM("Mesh Bed Level data:");
+          mbl_mesh_report();
+        #endif
+      }
+    #endif
+  }
+
+  const bool to_enable = parser.boolval('S');
+  if (parser.seen('S'))
+    set_bed_leveling_enabled(to_enable);
+
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units());
+  #endif
+
+  const bool new_status = leveling_is_active();
+
+  if (to_enable && !new_status) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_M420_FAILED);
+  }
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOLNPAIR("Bed Leveling ", new_status ? MSG_ON : MSG_OFF);
+
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Fade Height ");
+    if (planner.z_fade_height > 0.0)
+      SERIAL_ECHOLN(planner.z_fade_height);
+    else
+      SERIAL_ECHOLNPGM(MSG_OFF);
+  #endif
+}

Marlin/src/gcode/calibrate/M421-abl.h

@@ -0,0 +1,51 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ */
+void gcode_M421() {
+  int8_t ix = parser.intval('I', -1), iy = parser.intval('J', -1);
+  const bool hasI = ix >= 0,
+             hasJ = iy >= 0,
+             hasZ = parser.seen('Z'),
+             hasQ = !hasZ && parser.seen('Q');
+
+  if (!hasI || !hasJ || !(hasZ || hasQ)) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
+  }
+  else if (!WITHIN(ix, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1)) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
+  }
+  else {
+    z_values[ix][iy] = parser.value_linear_units() + (hasQ ? z_values[ix][iy] : 0);
+    #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+      bed_level_virt_interpolate();
+    #endif
+  }
+}

Marlin/src/gcode/calibrate/M421-mbl.h

@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 X<linear> Y<linear> Z<linear>
+ *   M421 X<linear> Y<linear> Q<offset>
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ */
+void gcode_M421() {
+  const bool hasX = parser.seen('X'), hasI = parser.seen('I');
+  const int8_t ix = hasI ? parser.value_int() : hasX ? mbl.probe_index_x(RAW_X_POSITION(parser.value_linear_units())) : -1;
+  const bool hasY = parser.seen('Y'), hasJ = parser.seen('J');
+  const int8_t iy = hasJ ? parser.value_int() : hasY ? mbl.probe_index_y(RAW_Y_POSITION(parser.value_linear_units())) : -1;
+  const bool hasZ = parser.seen('Z'), hasQ = !hasZ && parser.seen('Q');
+
+  if (int(hasI && hasJ) + int(hasX && hasY) != 1 || !(hasZ || hasQ)) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
+  }
+  else if (ix < 0 || iy < 0) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
+  }
+  else
+    mbl.set_z(ix, iy, parser.value_linear_units() + (hasQ ? mbl.z_values[ix][iy] : 0));
+}

Marlin/src/gcode/calibrate/M421-ubl.h

@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M421: Set a single Mesh Bed Leveling Z coordinate
+ *
+ * Usage:
+ *   M421 I<xindex> J<yindex> Z<linear>
+ *   M421 I<xindex> J<yindex> Q<offset>
+ *   M421 C Z<linear>
+ *   M421 C Q<offset>
+ */
+void gcode_M421() {
+  int8_t ix = parser.intval('I', -1), iy = parser.intval('J', -1);
+  const bool hasI = ix >= 0,
+             hasJ = iy >= 0,
+             hasC = parser.seen('C'),
+             hasZ = parser.seen('Z'),
+             hasQ = !hasZ && parser.seen('Q');
+
+  if (hasC) {
+    const mesh_index_pair location = ubl.find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, NULL, false);
+    ix = location.x_index;
+    iy = location.y_index;
+  }
+
+  if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ)) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
+  }
+  else if (!WITHIN(ix, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1)) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
+  }
+  else
+    ubl.z_values[ix][iy] = parser.value_linear_units() + (hasQ ? ubl.z_values[ix][iy] : 0);
+}

Marlin/src/gcode/calibrate/M48.h

@@ -0,0 +1,273 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M48: Z probe repeatability measurement function.
+ *
+ * Usage:
+ *   M48 <P#> <X#> <Y#> <V#> <E> <L#>
+ *     P = Number of sampled points (4-50, default 10)
+ *     X = Sample X position
+ *     Y = Sample Y position
+ *     V = Verbose level (0-4, default=1)
+ *     E = Engage Z probe for each reading
+ *     L = Number of legs of movement before probe
+ *     S = Schizoid (Or Star if you prefer)
+ *
+ * This function assumes the bed has been homed.  Specifically, that a G28 command
+ * as been issued prior to invoking the M48 Z probe repeatability measurement function.
+ * Any information generated by a prior G29 Bed leveling command will be lost and need to be
+ * regenerated.
+ */
+void gcode_M48() {
+
+  if (axis_unhomed_error()) return;
+
+  const int8_t verbose_level = parser.byteval('V', 1);
+  if (!WITHIN(verbose_level, 0, 4)) {
+    SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-4).");
+    return;
+  }
+
+  if (verbose_level > 0)
+    SERIAL_PROTOCOLLNPGM("M48 Z-Probe Repeatability Test");
+
+  const int8_t n_samples = parser.byteval('P', 10);
+  if (!WITHIN(n_samples, 4, 50)) {
+    SERIAL_PROTOCOLLNPGM("?Sample size not plausible (4-50).");
+    return;
+  }
+
+  const bool stow_probe_after_each = parser.boolval('E');
+
+  float X_current = current_position[X_AXIS],
+        Y_current = current_position[Y_AXIS];
+
+  const float X_probe_location = parser.linearval('X', X_current + X_PROBE_OFFSET_FROM_EXTRUDER),
+              Y_probe_location = parser.linearval('Y', Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER);
+
+  #if DISABLED(DELTA)
+    if (!WITHIN(X_probe_location, LOGICAL_X_POSITION(MIN_PROBE_X), LOGICAL_X_POSITION(MAX_PROBE_X))) {
+      out_of_range_error(PSTR("X"));
+      return;
+    }
+    if (!WITHIN(Y_probe_location, LOGICAL_Y_POSITION(MIN_PROBE_Y), LOGICAL_Y_POSITION(MAX_PROBE_Y))) {
+      out_of_range_error(PSTR("Y"));
+      return;
+    }
+  #else
+    if (!position_is_reachable_by_probe_xy(X_probe_location, Y_probe_location)) {
+      SERIAL_PROTOCOLLNPGM("? (X,Y) location outside of probeable radius.");
+      return;
+    }
+  #endif
+
+  bool seen_L = parser.seen('L');
+  uint8_t n_legs = seen_L ? parser.value_byte() : 0;
+  if (n_legs > 15) {
+    SERIAL_PROTOCOLLNPGM("?Number of legs in movement not plausible (0-15).");
+    return;
+  }
+  if (n_legs == 1) n_legs = 2;
+
+  const bool schizoid_flag = parser.boolval('S');
+  if (schizoid_flag && !seen_L) n_legs = 7;
+
+  /**
+   * Now get everything to the specified probe point So we can safely do a
+   * probe to get us close to the bed.  If the Z-Axis is far from the bed,
+   * we don't want to use that as a starting point for each probe.
+   */
+  if (verbose_level > 2)
+    SERIAL_PROTOCOLLNPGM("Positioning the probe...");
+
+  // Disable bed level correction in M48 because we want the raw data when we probe
+
+  #if HAS_LEVELING
+    const bool was_enabled = leveling_is_active();
+    set_bed_leveling_enabled(false);
+  #endif
+
+  setup_for_endstop_or_probe_move();
+
+  double mean = 0.0, sigma = 0.0, min = 99999.9, max = -99999.9, sample_set[n_samples];
+
+  // Move to the first point, deploy, and probe
+  const float t = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level);
+  bool probing_good = !isnan(t);
+
+  if (probing_good) {
+    randomSeed(millis());
+
+    for (uint8_t n = 0; n < n_samples; n++) {
+      if (n_legs) {
+        const int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
+        float angle = random(0.0, 360.0);
+        const float radius = random(
+          #if ENABLED(DELTA)
+            0.1250000000 * (DELTA_PROBEABLE_RADIUS),
+            0.3333333333 * (DELTA_PROBEABLE_RADIUS)
+          #else
+            5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE)
+          #endif
+        );
+
+        if (verbose_level > 3) {
+          SERIAL_ECHOPAIR("Starting radius: ", radius);
+          SERIAL_ECHOPAIR("   angle: ", angle);
+          SERIAL_ECHOPGM(" Direction: ");
+          if (dir > 0) SERIAL_ECHOPGM("Counter-");
+          SERIAL_ECHOLNPGM("Clockwise");
+        }
+
+        for (uint8_t l = 0; l < n_legs - 1; l++) {
+          double delta_angle;
+
+          if (schizoid_flag)
+            // The points of a 5 point star are 72 degrees apart.  We need to
+            // skip a point and go to the next one on the star.
+            delta_angle = dir * 2.0 * 72.0;
+
+          else
+            // If we do this line, we are just trying to move further
+            // around the circle.
+            delta_angle = dir * (float) random(25, 45);
+
+          angle += delta_angle;
+
+          while (angle > 360.0)   // We probably do not need to keep the angle between 0 and 2*PI, but the
+            angle -= 360.0;       // Arduino documentation says the trig functions should not be given values
+          while (angle < 0.0)     // outside of this range.   It looks like they behave correctly with
+            angle += 360.0;       // numbers outside of the range, but just to be safe we clamp them.
+
+          X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius;
+          Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius;
+
+          #if DISABLED(DELTA)
+            X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
+            Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
+          #else
+            // If we have gone out too far, we can do a simple fix and scale the numbers
+            // back in closer to the origin.
+            while (!position_is_reachable_by_probe_xy(X_current, Y_current)) {
+              X_current *= 0.8;
+              Y_current *= 0.8;
+              if (verbose_level > 3) {
+                SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
+                SERIAL_ECHOLNPAIR(", ", Y_current);
+              }
+            }
+          #endif
+          if (verbose_level > 3) {
+            SERIAL_PROTOCOLPGM("Going to:");
+            SERIAL_ECHOPAIR(" X", X_current);
+            SERIAL_ECHOPAIR(" Y", Y_current);
+            SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
+          }
+          do_blocking_move_to_xy(X_current, Y_current);
+        } // n_legs loop
+      } // n_legs
+
+      // Probe a single point
+      sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0);
+
+      // Break the loop if the probe fails
+      probing_good = !isnan(sample_set[n]);
+      if (!probing_good) break;
+
+      /**
+       * Get the current mean for the data points we have so far
+       */
+      double sum = 0.0;
+      for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
+      mean = sum / (n + 1);
+
+      NOMORE(min, sample_set[n]);
+      NOLESS(max, sample_set[n]);
+
+      /**
+       * Now, use that mean to calculate the standard deviation for the
+       * data points we have so far
+       */
+      sum = 0.0;
+      for (uint8_t j = 0; j <= n; j++)
+        sum += sq(sample_set[j] - mean);
+
+      sigma = SQRT(sum / (n + 1));
+      if (verbose_level > 0) {
+        if (verbose_level > 1) {
+          SERIAL_PROTOCOL(n + 1);
+          SERIAL_PROTOCOLPGM(" of ");
+          SERIAL_PROTOCOL((int)n_samples);
+          SERIAL_PROTOCOLPGM(": z: ");
+          SERIAL_PROTOCOL_F(sample_set[n], 3);
+          if (verbose_level > 2) {
+            SERIAL_PROTOCOLPGM(" mean: ");
+            SERIAL_PROTOCOL_F(mean, 4);
+            SERIAL_PROTOCOLPGM(" sigma: ");
+            SERIAL_PROTOCOL_F(sigma, 6);
+            SERIAL_PROTOCOLPGM(" min: ");
+            SERIAL_PROTOCOL_F(min, 3);
+            SERIAL_PROTOCOLPGM(" max: ");
+            SERIAL_PROTOCOL_F(max, 3);
+            SERIAL_PROTOCOLPGM(" range: ");
+            SERIAL_PROTOCOL_F(max-min, 3);
+          }
+          SERIAL_EOL();
+        }
+      }
+
+    } // n_samples loop
+  }
+
+  STOW_PROBE();
+
+  if (probing_good) {
+    SERIAL_PROTOCOLLNPGM("Finished!");
+
+    if (verbose_level > 0) {
+      SERIAL_PROTOCOLPGM("Mean: ");
+      SERIAL_PROTOCOL_F(mean, 6);
+      SERIAL_PROTOCOLPGM(" Min: ");
+      SERIAL_PROTOCOL_F(min, 3);
+      SERIAL_PROTOCOLPGM(" Max: ");
+      SERIAL_PROTOCOL_F(max, 3);
+      SERIAL_PROTOCOLPGM(" Range: ");
+      SERIAL_PROTOCOL_F(max-min, 3);
+      SERIAL_EOL();
+    }
+
+    SERIAL_PROTOCOLPGM("Standard Deviation: ");
+    SERIAL_PROTOCOL_F(sigma, 6);
+    SERIAL_EOL();
+    SERIAL_EOL();
+  }
+
+  clean_up_after_endstop_or_probe_move();
+
+  // Re-enable bed level correction if it had been on
+  #if HAS_LEVELING
+    set_bed_leveling_enabled(was_enabled);
+  #endif
+
+  report_current_position();
+}

Marlin/src/gcode/calibrate/M49.h

@@ -0,0 +1,27 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void gcode_M49() {
+  ubl.g26_debug_flag ^= true;
+  SERIAL_PROTOCOLPGM("UBL Debug Flag turned ");
+  serialprintPGM(ubl.g26_debug_flag ? PSTR("on.") : PSTR("off."));
+}

Marlin/src/gcode/calibrate/M665.h

@@ -0,0 +1,93 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ENABLED(DELTA)
+
+  /**
+   * M665: Set delta configurations
+   *
+   *    H = delta height
+   *    L = diagonal rod
+   *    R = delta radius
+   *    S = segments per second
+   *    B = delta calibration radius
+   *    X = Alpha (Tower 1) angle trim
+   *    Y = Beta (Tower 2) angle trim
+   *    Z = Rotate A and B by this angle
+   */
+  void gcode_M665() {
+    if (parser.seen('H')) {
+      home_offset[Z_AXIS] = parser.value_linear_units() - DELTA_HEIGHT;
+      update_software_endstops(Z_AXIS);
+    }
+    if (parser.seen('L')) delta_diagonal_rod             = parser.value_linear_units();
+    if (parser.seen('R')) delta_radius                   = parser.value_linear_units();
+    if (parser.seen('S')) delta_segments_per_second      = parser.value_float();
+    if (parser.seen('B')) delta_calibration_radius       = parser.value_float();
+    if (parser.seen('X')) delta_tower_angle_trim[A_AXIS] = parser.value_float();
+    if (parser.seen('Y')) delta_tower_angle_trim[B_AXIS] = parser.value_float();
+    if (parser.seen('Z')) { // rotate all 3 axis for Z = 0
+      delta_tower_angle_trim[A_AXIS] -= parser.value_float();
+      delta_tower_angle_trim[B_AXIS] -= parser.value_float();
+    }
+    recalc_delta_settings(delta_radius, delta_diagonal_rod);
+  }
+
+#elif IS_SCARA
+
+  /**
+   * M665: Set SCARA settings
+   *
+   * Parameters:
+   *
+   *   S[segments-per-second] - Segments-per-second
+   *   P[theta-psi-offset]    - Theta-Psi offset, added to the shoulder (A/X) angle
+   *   T[theta-offset]        - Theta     offset, added to the elbow    (B/Y) angle
+   *
+   *   A, P, and X are all aliases for the shoulder angle
+   *   B, T, and Y are all aliases for the elbow angle
+   */
+  void gcode_M665() {
+    if (parser.seen('S')) delta_segments_per_second = parser.value_float();
+
+    const bool hasA = parser.seen('A'), hasP = parser.seen('P'), hasX = parser.seen('X');
+    const uint8_t sumAPX = hasA + hasP + hasX;
+    if (sumAPX == 1)
+      home_offset[A_AXIS] = parser.value_float();
+    else if (sumAPX > 1) {
+      SERIAL_ERROR_START();
+      SERIAL_ERRORLNPGM("Only one of A, P, or X is allowed.");
+      return;
+    }
+
+    const bool hasB = parser.seen('B'), hasT = parser.seen('T'), hasY = parser.seen('Y');
+    const uint8_t sumBTY = hasB + hasT + hasY;
+    if (sumBTY == 1)
+      home_offset[B_AXIS] = parser.value_float();
+    else if (sumBTY > 1) {
+      SERIAL_ERROR_START();
+      SERIAL_ERRORLNPGM("Only one of B, T, or Y is allowed.");
+      return;
+    }
+  }
+
+#endif

Marlin/src/gcode/calibrate/M666.h

@@ -0,0 +1,66 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ENABLED(DELTA)
+
+  /**
+   * M666: Set delta endstop adjustment
+   */
+  void gcode_M666() {
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) {
+        SERIAL_ECHOLNPGM(">>> gcode_M666");
+      }
+    #endif
+    LOOP_XYZ(i) {
+      if (parser.seen(axis_codes[i])) {
+        endstop_adj[i] = parser.value_linear_units();
+        #if ENABLED(DEBUG_LEVELING_FEATURE)
+          if (DEBUGGING(LEVELING)) {
+            SERIAL_ECHOPAIR("endstop_adj[", axis_codes[i]);
+            SERIAL_ECHOLNPAIR("] = ", endstop_adj[i]);
+          }
+        #endif
+      }
+    }
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) {
+        SERIAL_ECHOLNPGM("<<< gcode_M666");
+      }
+    #endif
+    // normalize endstops so all are <=0; set the residue to delta height
+    const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
+    home_offset[Z_AXIS] -= z_temp;
+    LOOP_XYZ(i) endstop_adj[i] -= z_temp;
+  }
+
+#elif ENABLED(Z_DUAL_ENDSTOPS) // !DELTA && ENABLED(Z_DUAL_ENDSTOPS)
+
+  /**
+   * M666: For Z Dual Endstop setup, set z axis offset to the z2 axis.
+   */
+  void gcode_M666() {
+    if (parser.seen('Z')) z_endstop_adj = parser.value_linear_units();
+    SERIAL_ECHOLNPAIR("Z Endstop Adjustment set to (mm):", z_endstop_adj);
+  }
+
+#endif

Marlin/src/gcode/calibrate/common.h

@@ -0,0 +1,81 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#ifndef CALIBRATE_COMMON_H
+#define CALIBRATE_COMMON_H
+
+#if ENABLED(DELTA)
+
+  /**
+   * A delta can only safely home all axes at the same time
+   * This is like quick_home_xy() but for 3 towers.
+   */
+  inline bool home_delta() {
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) DEBUG_POS(">>> home_delta", current_position);
+    #endif
+    // Init the current position of all carriages to 0,0,0
+    ZERO(current_position);
+    sync_plan_position();
+
+    // Move all carriages together linearly until an endstop is hit.
+    current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (DELTA_HEIGHT + home_offset[Z_AXIS] + 10);
+    feedrate_mm_s = homing_feedrate(X_AXIS);
+    line_to_current_position();
+    stepper.synchronize();
+
+    // If an endstop was not hit, then damage can occur if homing is continued.
+    // This can occur if the delta height (DELTA_HEIGHT + home_offset[Z_AXIS]) is
+    // not set correctly.
+    if (!(Endstops::endstop_hit_bits & (_BV(X_MAX) | _BV(Y_MAX) | _BV(Z_MAX)))) {
+      LCD_MESSAGEPGM(MSG_ERR_HOMING_FAILED);
+      SERIAL_ERROR_START();
+      SERIAL_ERRORLNPGM(MSG_ERR_HOMING_FAILED);
+      return false;
+    }
+
+    endstops.hit_on_purpose(); // clear endstop hit flags
+
+    // At least one carriage has reached the top.
+    // Now re-home each carriage separately.
+    HOMEAXIS(A);
+    HOMEAXIS(B);
+    HOMEAXIS(C);
+
+    // Set all carriages to their home positions
+    // Do this here all at once for Delta, because
+    // XYZ isn't ABC. Applying this per-tower would
+    // give the impression that they are the same.
+    LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
+
+    SYNC_PLAN_POSITION_KINEMATIC();
+
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_delta", current_position);
+    #endif
+
+    return true;
+  }
+
+#endif // DELTA
+
+#endif // CALIBRATE_COMMON_H

Marlin/src/gcode/config/M200.h

@@ -0,0 +1,46 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M200: Set filament diameter and set E axis units to cubic units
+ *
+ *    T<extruder> - Optional extruder number. Current extruder if omitted.
+ *    D<linear> - Diameter of the filament. Use "D0" to switch back to linear units on the E axis.
+ */
+void gcode_M200() {
+
+  if (get_target_extruder_from_command(200)) return;
+
+  if (parser.seen('D')) {
+    // setting any extruder filament size disables volumetric on the assumption that
+    // slicers either generate in extruder values as cubic mm or as as filament feeds
+    // for all extruders
+    volumetric_enabled = (parser.value_linear_units() != 0.0);
+    if (volumetric_enabled) {
+      filament_size[target_extruder] = parser.value_linear_units();
+      // make sure all extruders have some sane value for the filament size
+      for (uint8_t i = 0; i < COUNT(filament_size); i++)
+        if (! filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
+    }
+  }
+  calculate_volumetric_multipliers();
+}

Marlin/src/gcode/config/M201.h

@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M201: Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+ *
+ *       With multiple extruders use T to specify which one.
+ */
+void gcode_M201() {
+
+  GET_TARGET_EXTRUDER(201);
+
+  LOOP_XYZE(i) {
+    if (parser.seen(axis_codes[i])) {
+      const uint8_t a = i + (i == E_AXIS ? TARGET_EXTRUDER : 0);
+      planner.max_acceleration_mm_per_s2[a] = parser.value_axis_units((AxisEnum)a);
+    }
+  }
+  // steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
+  planner.reset_acceleration_rates();
+}

Marlin/src/gcode/config/M203.h

@@ -0,0 +1,37 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec
+ *
+ *       With multiple extruders use T to specify which one.
+ */
+void gcode_M203() {
+
+  GET_TARGET_EXTRUDER(203);
+
+  LOOP_XYZE(i)
+    if (parser.seen(axis_codes[i])) {
+      const uint8_t a = i + (i == E_AXIS ? TARGET_EXTRUDER : 0);
+      planner.max_feedrate_mm_s[a] = parser.value_axis_units((AxisEnum)a);
+    }
+}

Marlin/src/gcode/config/M204.h

@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M204: Set Accelerations in units/sec^2 (M204 P1200 R3000 T3000)
+ *
+ *    P = Printing moves
+ *    R = Retract only (no X, Y, Z) moves
+ *    T = Travel (non printing) moves
+ *
+ *  Also sets minimum segment time in ms (B20000) to prevent buffer under-runs and M20 minimum feedrate
+ */
+void gcode_M204() {
+  if (parser.seen('S')) {  // Kept for legacy compatibility. Should NOT BE USED for new developments.
+    planner.travel_acceleration = planner.acceleration = parser.value_linear_units();
+    SERIAL_ECHOLNPAIR("Setting Print and Travel Acceleration: ", planner.acceleration);
+  }
+  if (parser.seen('P')) {
+    planner.acceleration = parser.value_linear_units();
+    SERIAL_ECHOLNPAIR("Setting Print Acceleration: ", planner.acceleration);
+  }
+  if (parser.seen('R')) {
+    planner.retract_acceleration = parser.value_linear_units();
+    SERIAL_ECHOLNPAIR("Setting Retract Acceleration: ", planner.retract_acceleration);
+  }
+  if (parser.seen('T')) {
+    planner.travel_acceleration = parser.value_linear_units();
+    SERIAL_ECHOLNPAIR("Setting Travel Acceleration: ", planner.travel_acceleration);
+  }
+}

Marlin/src/gcode/config/M205.h

@@ -0,0 +1,42 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M205: Set Advanced Settings
+ *
+ *    S = Min Feed Rate (units/s)
+ *    T = Min Travel Feed Rate (units/s)
+ *    B = Min Segment Time (µs)
+ *    X = Max X Jerk (units/sec^2)
+ *    Y = Max Y Jerk (units/sec^2)
+ *    Z = Max Z Jerk (units/sec^2)
+ *    E = Max E Jerk (units/sec^2)
+ */
+void gcode_M205() {
+  if (parser.seen('S')) planner.min_feedrate_mm_s = parser.value_linear_units();
+  if (parser.seen('T')) planner.min_travel_feedrate_mm_s = parser.value_linear_units();
+  if (parser.seen('B')) planner.min_segment_time = parser.value_millis();
+  if (parser.seen('X')) planner.max_jerk[X_AXIS] = parser.value_linear_units();
+  if (parser.seen('Y')) planner.max_jerk[Y_AXIS] = parser.value_linear_units();
+  if (parser.seen('Z')) planner.max_jerk[Z_AXIS] = parser.value_linear_units();
+  if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units();
+}

Marlin/src/gcode/config/M218.h

@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M218 - set hotend offset (in linear units)
+ *
+ *   T<tool>
+ *   X<xoffset>
+ *   Y<yoffset>
+ *   Z<zoffset> - Available with DUAL_X_CARRIAGE and SWITCHING_NOZZLE
+ */
+void gcode_M218() {
+  if (get_target_extruder_from_command(218) || target_extruder == 0) return;
+
+  if (parser.seenval('X')) hotend_offset[X_AXIS][target_extruder] = parser.value_linear_units();
+  if (parser.seenval('Y')) hotend_offset[Y_AXIS][target_extruder] = parser.value_linear_units();
+
+  #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(SWITCHING_NOZZLE) || ENABLED(PARKING_EXTRUDER)
+    if (parser.seenval('Z')) hotend_offset[Z_AXIS][target_extruder] = parser.value_linear_units();
+  #endif
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
+  HOTEND_LOOP() {
+    SERIAL_CHAR(' ');
+    SERIAL_ECHO(hotend_offset[X_AXIS][e]);
+    SERIAL_CHAR(',');
+    SERIAL_ECHO(hotend_offset[Y_AXIS][e]);
+    #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(SWITCHING_NOZZLE) || ENABLED(PARKING_EXTRUDER)
+      SERIAL_CHAR(',');
+      SERIAL_ECHO(hotend_offset[Z_AXIS][e]);
+    #endif
+  }
+  SERIAL_EOL();
+}

Marlin/src/gcode/config/M220.h

@@ -0,0 +1,28 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M220: Set speed percentage factor, aka "Feed Rate" (M220 S95)
+ */
+void gcode_M220() {
+  if (parser.seenval('S')) feedrate_percentage = parser.value_int();
+}

Marlin/src/gcode/config/M221.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M221: Set extrusion percentage (M221 T0 S95)
+ */
+void gcode_M221() {
+  if (get_target_extruder_from_command(221)) return;
+  if (parser.seenval('S'))
+    flow_percentage[target_extruder] = parser.value_int();
+}

Marlin/src/gcode/config/M301.h

@@ -0,0 +1,69 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M301: Set PID parameters P I D (and optionally C, L)
+ *
+ *   P[float] Kp term
+ *   I[float] Ki term (unscaled)
+ *   D[float] Kd term (unscaled)
+ *
+ * With PID_EXTRUSION_SCALING:
+ *
+ *   C[float] Kc term
+ *   L[float] LPQ length
+ */
+void gcode_M301() {
+
+  // multi-extruder PID patch: M301 updates or prints a single extruder's PID values
+  // default behaviour (omitting E parameter) is to update for extruder 0 only
+  const uint8_t e = parser.byteval('E'); // extruder being updated
+
+  if (e < HOTENDS) { // catch bad input value
+    if (parser.seen('P')) PID_PARAM(Kp, e) = parser.value_float();
+    if (parser.seen('I')) PID_PARAM(Ki, e) = scalePID_i(parser.value_float());
+    if (parser.seen('D')) PID_PARAM(Kd, e) = scalePID_d(parser.value_float());
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      if (parser.seen('C')) PID_PARAM(Kc, e) = parser.value_float();
+      if (parser.seen('L')) lpq_len = parser.value_float();
+      NOMORE(lpq_len, LPQ_MAX_LEN);
+    #endif
+
+    thermalManager.updatePID();
+    SERIAL_ECHO_START();
+    #if ENABLED(PID_PARAMS_PER_HOTEND)
+      SERIAL_ECHOPAIR(" e:", e); // specify extruder in serial output
+    #endif // PID_PARAMS_PER_HOTEND
+    SERIAL_ECHOPAIR(" p:", PID_PARAM(Kp, e));
+    SERIAL_ECHOPAIR(" i:", unscalePID_i(PID_PARAM(Ki, e)));
+    SERIAL_ECHOPAIR(" d:", unscalePID_d(PID_PARAM(Kd, e)));
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      //Kc does not have scaling applied above, or in resetting defaults
+      SERIAL_ECHOPAIR(" c:", PID_PARAM(Kc, e));
+    #endif
+    SERIAL_EOL();
+  }
+  else {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLN(MSG_INVALID_EXTRUDER);
+  }
+}

Marlin/src/gcode/config/M302.h

@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M302: Allow cold extrudes, or set the minimum extrude temperature
+ *
+ *       S<temperature> sets the minimum extrude temperature
+ *       P<bool> enables (1) or disables (0) cold extrusion
+ *
+ *  Examples:
+ *
+ *       M302         ; report current cold extrusion state
+ *       M302 P0      ; enable cold extrusion checking
+ *       M302 P1      ; disables cold extrusion checking
+ *       M302 S0      ; always allow extrusion (disables checking)
+ *       M302 S170    ; only allow extrusion above 170
+ *       M302 S170 P1 ; set min extrude temp to 170 but leave disabled
+ */
+void gcode_M302() {
+  const bool seen_S = parser.seen('S');
+  if (seen_S) {
+    thermalManager.extrude_min_temp = parser.value_celsius();
+    thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0);
+  }
+
+  if (parser.seen('P'))
+    thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0) || parser.value_bool();
+  else if (!seen_S) {
+    // Report current state
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPAIR("Cold extrudes are ", (thermalManager.allow_cold_extrude ? "en" : "dis"));
+    SERIAL_ECHOPAIR("abled (min temp ", thermalManager.extrude_min_temp);
+    SERIAL_ECHOLNPGM("C)");
+  }
+}

Marlin/src/gcode/config/M304.h

@@ -0,0 +1,34 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void gcode_M304() {
+  if (parser.seen('P')) thermalManager.bedKp = parser.value_float();
+  if (parser.seen('I')) thermalManager.bedKi = scalePID_i(parser.value_float());
+  if (parser.seen('D')) thermalManager.bedKd = scalePID_d(parser.value_float());
+
+  thermalManager.updatePID();
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPAIR(" p:", thermalManager.bedKp);
+  SERIAL_ECHOPAIR(" i:", unscalePID_i(thermalManager.bedKi));
+  SERIAL_ECHOLNPAIR(" d:", unscalePID_d(thermalManager.bedKd));
+}

Marlin/src/gcode/config/M43.h

@@ -0,0 +1,311 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "../../pins/pinsDebug.h"
+
+inline void toggle_pins() {
+  const bool I_flag = parser.boolval('I');
+  const int repeat = parser.intval('R', 1),
+            start = parser.intval('S'),
+            end = parser.intval('E', NUM_DIGITAL_PINS - 1),
+            wait = parser.intval('W', 500);
+
+  for (uint8_t pin = start; pin <= end; pin++) {
+    //report_pin_state_extended(pin, I_flag, false);
+    if (!VALID_PIN(pin)) continue;
+    if (!I_flag && pin_is_protected(pin)) {
+      report_pin_state_extended(pin, I_flag, true, "Untouched ");
+      SERIAL_EOL();
+    }
+    else {
+      report_pin_state_extended(pin, I_flag, true, "Pulsing   ");
+      #if AVR_AT90USB1286_FAMILY // Teensy IDEs don't know about these pins so must use FASTIO
+        if (pin == TEENSY_E2) {
+          SET_OUTPUT(TEENSY_E2);
+          for (int16_t j = 0; j < repeat; j++) {
+            WRITE(TEENSY_E2, LOW);  safe_delay(wait);
+            WRITE(TEENSY_E2, HIGH); safe_delay(wait);
+            WRITE(TEENSY_E2, LOW);  safe_delay(wait);
+          }
+        }
+        else if (pin == TEENSY_E3) {
+          SET_OUTPUT(TEENSY_E3);
+          for (int16_t j = 0; j < repeat; j++) {
+            WRITE(TEENSY_E3, LOW);  safe_delay(wait);
+            WRITE(TEENSY_E3, HIGH); safe_delay(wait);
+            WRITE(TEENSY_E3, LOW);  safe_delay(wait);
+          }
+        }
+        else
+      #endif
+      {
+        pinMode(pin, OUTPUT);
+        for (int16_t j = 0; j < repeat; j++) {
+          digitalWrite(pin, 0); safe_delay(wait);
+          digitalWrite(pin, 1); safe_delay(wait);
+          digitalWrite(pin, 0); safe_delay(wait);
+        }
+      }
+
+    }
+    SERIAL_EOL();
+  }
+  SERIAL_ECHOLNPGM("Done.");
+
+} // toggle_pins
+
+inline void servo_probe_test() {
+  #if !(NUM_SERVOS > 0 && HAS_SERVO_0)
+
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM("SERVO not setup");
+
+  #elif !HAS_Z_SERVO_ENDSTOP
+
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM("Z_ENDSTOP_SERVO_NR not setup");
+
+  #else // HAS_Z_SERVO_ENDSTOP
+
+    const uint8_t probe_index = parser.byteval('P', Z_ENDSTOP_SERVO_NR);
+
+    SERIAL_PROTOCOLLNPGM("Servo probe test");
+    SERIAL_PROTOCOLLNPAIR(".  using index:  ", probe_index);
+    SERIAL_PROTOCOLLNPAIR(".  deploy angle: ", z_servo_angle[0]);
+    SERIAL_PROTOCOLLNPAIR(".  stow angle:   ", z_servo_angle[1]);
+
+    bool probe_inverting;
+
+    #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+
+      #define PROBE_TEST_PIN Z_MIN_PIN
+
+      SERIAL_PROTOCOLLNPAIR(". probe uses Z_MIN pin: ", PROBE_TEST_PIN);
+      SERIAL_PROTOCOLLNPGM(". uses Z_MIN_ENDSTOP_INVERTING (ignores Z_MIN_PROBE_ENDSTOP_INVERTING)");
+      SERIAL_PROTOCOLPGM(". Z_MIN_ENDSTOP_INVERTING: ");
+
+      #if Z_MIN_ENDSTOP_INVERTING
+        SERIAL_PROTOCOLLNPGM("true");
+      #else
+        SERIAL_PROTOCOLLNPGM("false");
+      #endif
+
+      probe_inverting = Z_MIN_ENDSTOP_INVERTING;
+
+    #elif ENABLED(Z_MIN_PROBE_ENDSTOP)
+
+      #define PROBE_TEST_PIN Z_MIN_PROBE_PIN
+      SERIAL_PROTOCOLLNPAIR(". probe uses Z_MIN_PROBE_PIN: ", PROBE_TEST_PIN);
+      SERIAL_PROTOCOLLNPGM(". uses Z_MIN_PROBE_ENDSTOP_INVERTING (ignores Z_MIN_ENDSTOP_INVERTING)");
+      SERIAL_PROTOCOLPGM(". Z_MIN_PROBE_ENDSTOP_INVERTING: ");
+
+      #if Z_MIN_PROBE_ENDSTOP_INVERTING
+        SERIAL_PROTOCOLLNPGM("true");
+      #else
+        SERIAL_PROTOCOLLNPGM("false");
+      #endif
+
+      probe_inverting = Z_MIN_PROBE_ENDSTOP_INVERTING;
+
+    #endif
+
+    SERIAL_PROTOCOLLNPGM(". deploy & stow 4 times");
+    SET_INPUT_PULLUP(PROBE_TEST_PIN);
+    bool deploy_state, stow_state;
+    for (uint8_t i = 0; i < 4; i++) {
+      MOVE_SERVO(probe_index, z_servo_angle[0]); //deploy
+      safe_delay(500);
+      deploy_state = READ(PROBE_TEST_PIN);
+      MOVE_SERVO(probe_index, z_servo_angle[1]); //stow
+      safe_delay(500);
+      stow_state = READ(PROBE_TEST_PIN);
+    }
+    if (probe_inverting != deploy_state) SERIAL_PROTOCOLLNPGM("WARNING - INVERTING setting probably backwards");
+
+    refresh_cmd_timeout();
+
+    if (deploy_state != stow_state) {
+      SERIAL_PROTOCOLLNPGM("BLTouch clone detected");
+      if (deploy_state) {
+        SERIAL_PROTOCOLLNPGM(".  DEPLOYED state: HIGH (logic 1)");
+        SERIAL_PROTOCOLLNPGM(".  STOWED (triggered) state: LOW (logic 0)");
+      }
+      else {
+        SERIAL_PROTOCOLLNPGM(".  DEPLOYED state: LOW (logic 0)");
+        SERIAL_PROTOCOLLNPGM(".  STOWED (triggered) state: HIGH (logic 1)");
+      }
+      #if ENABLED(BLTOUCH)
+        SERIAL_PROTOCOLLNPGM("ERROR: BLTOUCH enabled - set this device up as a Z Servo Probe with inverting as true.");
+      #endif
+
+    }
+    else {                                           // measure active signal length
+      MOVE_SERVO(probe_index, z_servo_angle[0]);     // deploy
+      safe_delay(500);
+      SERIAL_PROTOCOLLNPGM("please trigger probe");
+      uint16_t probe_counter = 0;
+
+      // Allow 30 seconds max for operator to trigger probe
+      for (uint16_t j = 0; j < 500 * 30 && probe_counter == 0 ; j++) {
+
+        safe_delay(2);
+
+        if (0 == j % (500 * 1)) // keep cmd_timeout happy
+          refresh_cmd_timeout();
+
+        if (deploy_state != READ(PROBE_TEST_PIN)) { // probe triggered
+
+          for (probe_counter = 1; probe_counter < 50 && deploy_state != READ(PROBE_TEST_PIN); ++probe_counter)
+            safe_delay(2);
+
+          if (probe_counter == 50)
+            SERIAL_PROTOCOLLNPGM("Z Servo Probe detected"); // >= 100mS active time
+          else if (probe_counter >= 2)
+            SERIAL_PROTOCOLLNPAIR("BLTouch compatible probe detected - pulse width (+/- 4mS): ", probe_counter * 2); // allow 4 - 100mS pulse
+          else
+            SERIAL_PROTOCOLLNPGM("noise detected - please re-run test"); // less than 2mS pulse
+
+          MOVE_SERVO(probe_index, z_servo_angle[1]); //stow
+
+        }  // pulse detected
+
+      } // for loop waiting for trigger
+
+      if (probe_counter == 0) SERIAL_PROTOCOLLNPGM("trigger not detected");
+
+    } // measure active signal length
+
+  #endif
+
+} // servo_probe_test
+
+/**
+ * M43: Pin debug - report pin state, watch pins, toggle pins and servo probe test/report
+ *
+ *  M43         - report name and state of pin(s)
+ *                  P<pin>  Pin to read or watch. If omitted, reads all pins.
+ *                  I       Flag to ignore Marlin's pin protection.
+ *
+ *  M43 W       - Watch pins -reporting changes- until reset, click, or M108.
+ *                  P<pin>  Pin to read or watch. If omitted, read/watch all pins.
+ *                  I       Flag to ignore Marlin's pin protection.
+ *
+ *  M43 E<bool> - Enable / disable background endstop monitoring
+ *                  - Machine continues to operate
+ *                  - Reports changes to endstops
+ *                  - Toggles LED_PIN when an endstop changes
+ *                  - Can not reliably catch the 5mS pulse from BLTouch type probes
+ *
+ *  M43 T       - Toggle pin(s) and report which pin is being toggled
+ *                  S<pin>  - Start Pin number.   If not given, will default to 0
+ *                  L<pin>  - End Pin number.   If not given, will default to last pin defined for this board
+ *                  I<bool> - Flag to ignore Marlin's pin protection.   Use with caution!!!!
+ *                  R       - Repeat pulses on each pin this number of times before continueing to next pin
+ *                  W       - Wait time (in miliseconds) between pulses.  If not given will default to 500
+ *
+ *  M43 S       - Servo probe test
+ *                  P<index> - Probe index (optional - defaults to 0
+ */
+void gcode_M43() {
+
+  if (parser.seen('T')) {   // must be first or else its "S" and "E" parameters will execute endstop or servo test
+    toggle_pins();
+    return;
+  }
+
+  // Enable or disable endstop monitoring
+  if (parser.seen('E')) {
+    endstop_monitor_flag = parser.value_bool();
+    SERIAL_PROTOCOLPGM("endstop monitor ");
+    serialprintPGM(endstop_monitor_flag ? PSTR("en") : PSTR("dis"));
+    SERIAL_PROTOCOLLNPGM("abled");
+    return;
+  }
+
+  if (parser.seen('S')) {
+    servo_probe_test();
+    return;
+  }
+
+  // Get the range of pins to test or watch
+  const uint8_t first_pin = parser.byteval('P'),
+                last_pin = parser.seenval('P') ? first_pin : NUM_DIGITAL_PINS - 1;
+
+  if (first_pin > last_pin) return;
+
+  const bool ignore_protection = parser.boolval('I');
+
+  // Watch until click, M108, or reset
+  if (parser.boolval('W')) {
+    SERIAL_PROTOCOLLNPGM("Watching pins");
+    uint8_t pin_state[last_pin - first_pin + 1];
+    for (int8_t pin = first_pin; pin <= last_pin; pin++) {
+      if (!VALID_PIN(pin)) continue;
+      if (pin_is_protected(pin) && !ignore_protection) continue;
+      pinMode(pin, INPUT_PULLUP);
+      delay(1);
+      /*
+        if (IS_ANALOG(pin))
+          pin_state[pin - first_pin] = analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)); // int16_t pin_state[...]
+        else
+      //*/
+          pin_state[pin - first_pin] = digitalRead(pin);
+    }
+
+    #if HAS_RESUME_CONTINUE
+      wait_for_user = true;
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
+    #endif
+
+    for (;;) {
+      for (int8_t pin = first_pin; pin <= last_pin; pin++) {
+        if (!VALID_PIN(pin)) continue;
+        if (pin_is_protected(pin) && !ignore_protection) continue;
+        const byte val =
+          /*
+            IS_ANALOG(pin)
+              ? analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)) : // int16_t val
+              :
+          //*/
+            digitalRead(pin);
+        if (val != pin_state[pin - first_pin]) {
+          report_pin_state_extended(pin, ignore_protection, false);
+          pin_state[pin - first_pin] = val;
+        }
+      }
+
+      #if HAS_RESUME_CONTINUE
+        if (!wait_for_user) {
+          KEEPALIVE_STATE(IN_HANDLER);
+          break;
+        }
+      #endif
+
+      safe_delay(200);
+    }
+    return;
+  }
+
+  // Report current state of selected pin(s)
+  for (uint8_t pin = first_pin; pin <= last_pin; pin++)
+    if (VALID_PIN(pin)) report_pin_state_extended(pin, ignore_protection, true);
+}

Marlin/src/gcode/config/M540.h

@@ -0,0 +1,28 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>)
+ */
+void gcode_M540() {
+  if (parser.seen('S')) stepper.abort_on_endstop_hit = parser.value_bool();
+}

Marlin/src/gcode/config/M92.h

@@ -0,0 +1,51 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, and E.
+ *      (Follows the same syntax as G92)
+ *
+ *      With multiple extruders use T to specify which one.
+ */
+void gcode_M92() {
+
+  GET_TARGET_EXTRUDER(92);
+
+  LOOP_XYZE(i) {
+    if (parser.seen(axis_codes[i])) {
+      if (i == E_AXIS) {
+        const float value = parser.value_per_axis_unit((AxisEnum)(E_AXIS + TARGET_EXTRUDER));
+        if (value < 20.0) {
+          float factor = planner.axis_steps_per_mm[E_AXIS + TARGET_EXTRUDER] / value; // increase e constants if M92 E14 is given for netfab.
+          planner.max_jerk[E_AXIS] *= factor;
+          planner.max_feedrate_mm_s[E_AXIS + TARGET_EXTRUDER] *= factor;
+          planner.max_acceleration_steps_per_s2[E_AXIS + TARGET_EXTRUDER] *= factor;
+        }
+        planner.axis_steps_per_mm[E_AXIS + TARGET_EXTRUDER] = value;
+      }
+      else {
+        planner.axis_steps_per_mm[i] = parser.value_per_axis_unit((AxisEnum)i);
+      }
+    }
+  }
+  planner.refresh_positioning();
+}

Marlin/src/gcode/control/M108.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M108: Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
+ */
+void gcode_M108() {
+
+  wait_for_heatup = false;
+
+}

Marlin/src/gcode/control/M111.h

@@ -0,0 +1,61 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M111: Set the debug level
+ */
+void gcode_M111() {
+  if (parser.seen('S')) marlin_debug_flags = parser.byteval('S');
+
+  const static char str_debug_1[] PROGMEM = MSG_DEBUG_ECHO,
+                    str_debug_2[] PROGMEM = MSG_DEBUG_INFO,
+                    str_debug_4[] PROGMEM = MSG_DEBUG_ERRORS,
+                    str_debug_8[] PROGMEM = MSG_DEBUG_DRYRUN,
+                    str_debug_16[] PROGMEM = MSG_DEBUG_COMMUNICATION
+                    #if ENABLED(DEBUG_LEVELING_FEATURE)
+                      , str_debug_32[] PROGMEM = MSG_DEBUG_LEVELING
+                    #endif
+                    ;
+
+  const static char* const debug_strings[] PROGMEM = {
+    str_debug_1, str_debug_2, str_debug_4, str_debug_8, str_debug_16
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      , str_debug_32
+    #endif
+  };
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM(MSG_DEBUG_PREFIX);
+  if (marlin_debug_flags) {
+    uint8_t comma = 0;
+    for (uint8_t i = 0; i < COUNT(debug_strings); i++) {
+      if (TEST(marlin_debug_flags, i)) {
+        if (comma++) SERIAL_CHAR(',');
+        serialprintPGM((char*)pgm_read_word(&debug_strings[i]));
+      }
+    }
+  }
+  else {
+    SERIAL_ECHOPGM(MSG_DEBUG_OFF);
+  }
+  SERIAL_EOL();
+}

Marlin/src/gcode/control/M112.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M112: Emergency Stop
+ */
+void gcode_M112() {
+
+  kill(PSTR(MSG_KILLED));
+
+}

Marlin/src/gcode/control/M120_M121.h

@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M120: Enable endstops and set non-homing endstop state to "enabled"
+ */
+void gcode_M120() {
+
+  endstops.enable_globally(true);
+
+}
+
+/**
+ * M121: Disable endstops and set non-homing endstop state to "disabled"
+ */
+void gcode_M121() {
+
+  endstops.enable_globally(false);
+
+}

Marlin/src/gcode/control/M17.h

@@ -0,0 +1,29 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M17: Enable power on all stepper motors
+ */
+void gcode_M17() {
+  LCD_MESSAGEPGM(MSG_NO_MOVE);
+  enable_all_steppers();
+}

Marlin/src/gcode/control/M18_M84.h

@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(ULTRA_LCD)
+  extern bool defer_return_to_status;
+#endif
+
+/**
+ * M18, M84: Disable stepper motors
+ */
+void gcode_M18_M84() {
+  if (parser.seenval('S')) {
+    stepper_inactive_time = parser.value_millis_from_seconds();
+  }
+  else {
+    bool all_axis = !((parser.seen('X')) || (parser.seen('Y')) || (parser.seen('Z')) || (parser.seen('E')));
+    if (all_axis) {
+      stepper.finish_and_disable();
+    }
+    else {
+      stepper.synchronize();
+      if (parser.seen('X')) disable_X();
+      if (parser.seen('Y')) disable_Y();
+      if (parser.seen('Z')) disable_Z();
+      #if E0_ENABLE_PIN != X_ENABLE_PIN && E1_ENABLE_PIN != Y_ENABLE_PIN // Only enable on boards that have separate ENABLE_PINS
+        if (parser.seen('E')) disable_e_steppers();
+      #endif
+    }
+
+    #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(ULTRA_LCD)  // Only needed with an LCD
+      ubl_lcd_map_control = defer_return_to_status = false;
+    #endif
+  }
+}

Marlin/src/gcode/control/M211.h

@@ -0,0 +1,46 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M211: Enable, Disable, and/or Report software endstops
+ *
+ * Usage: M211 S1 to enable, M211 S0 to disable, M211 alone for report
+ */
+void gcode_M211() {
+  SERIAL_ECHO_START();
+  #if HAS_SOFTWARE_ENDSTOPS
+    if (parser.seen('S')) soft_endstops_enabled = parser.value_bool();
+    SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS);
+    serialprintPGM(soft_endstops_enabled ? PSTR(MSG_ON) : PSTR(MSG_OFF));
+  #else
+    SERIAL_ECHOPGM(MSG_SOFT_ENDSTOPS);
+    SERIAL_ECHOPGM(MSG_OFF);
+  #endif
+  SERIAL_ECHOPGM(MSG_SOFT_MIN);
+  SERIAL_ECHOPAIR(    MSG_X, soft_endstop_min[X_AXIS]);
+  SERIAL_ECHOPAIR(" " MSG_Y, soft_endstop_min[Y_AXIS]);
+  SERIAL_ECHOPAIR(" " MSG_Z, soft_endstop_min[Z_AXIS]);
+  SERIAL_ECHOPGM(MSG_SOFT_MAX);
+  SERIAL_ECHOPAIR(    MSG_X, soft_endstop_max[X_AXIS]);
+  SERIAL_ECHOPAIR(" " MSG_Y, soft_endstop_max[Y_AXIS]);
+  SERIAL_ECHOLNPAIR(" " MSG_Z, soft_endstop_max[Z_AXIS]);
+}

Marlin/src/gcode/control/M226.h

@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M226: Wait until the specified pin reaches the state required (M226 P<pin> S<state>)
+ */
+void gcode_M226() {
+  if (parser.seen('P')) {
+    const int pin_number = parser.value_int(),
+              pin_state = parser.intval('S', -1); // required pin state - default is inverted
+
+    if (WITHIN(pin_state, -1, 1) && pin_number > -1 && !pin_is_protected(pin_number)) {
+
+      int target = LOW;
+
+      stepper.synchronize();
+
+      pinMode(pin_number, INPUT);
+      switch (pin_state) {
+        case 1:
+          target = HIGH;
+          break;
+        case 0:
+          target = LOW;
+          break;
+        case -1:
+          target = !digitalRead(pin_number);
+          break;
+      }
+
+      while (digitalRead(pin_number) != target) idle();
+
+    } // pin_state -1 0 1 && pin_number > -1
+  } // parser.seen('P')
+}

Marlin/src/gcode/control/M280.h

@@ -0,0 +1,43 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M280: Get or set servo position. P<index> [S<angle>]
+ */
+void gcode_M280() {
+  if (!parser.seen('P')) return;
+  const int servo_index = parser.value_int();
+  if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    if (parser.seen('S'))
+      MOVE_SERVO(servo_index, parser.value_int());
+    else {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPAIR(" Servo ", servo_index);
+      SERIAL_ECHOLNPAIR(": ", servo[servo_index].read());
+    }
+  }
+  else {
+    SERIAL_ERROR_START();
+    SERIAL_ECHOPAIR("Servo ", servo_index);
+    SERIAL_ECHOLNPGM(" out of range");
+  }
+}

Marlin/src/gcode/control/M3-M5.h

@@ -0,0 +1,127 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M3: Spindle Clockwise
+ * M4: Spindle Counter-clockwise
+ *
+ *  S0 turns off spindle.
+ *
+ *  If no speed PWM output is defined then M3/M4 just turns it on.
+ *
+ *  At least 12.8KHz (50Hz * 256) is needed for spindle PWM.
+ *  Hardware PWM is required. ISRs are too slow.
+ *
+ * NOTE: WGM for timers 3, 4, and 5 must be either Mode 1 or Mode 5.
+ *       No other settings give a PWM signal that goes from 0 to 5 volts.
+ *
+ *       The system automatically sets WGM to Mode 1, so no special
+ *       initialization is needed.
+ *
+ *       WGM bits for timer 2 are automatically set by the system to
+ *       Mode 1. This produces an acceptable 0 to 5 volt signal.
+ *       No special initialization is needed.
+ *
+ * NOTE: A minimum PWM frequency of 50 Hz is needed. All prescaler
+ *       factors for timers 2, 3, 4, and 5 are acceptable.
+ *
+ *  SPINDLE_LASER_ENABLE_PIN needs an external pullup or it may power on
+ *  the spindle/laser during power-up or when connecting to the host
+ *  (usually goes through a reset which sets all I/O pins to tri-state)
+ *
+ *  PWM duty cycle goes from 0 (off) to 255 (always on).
+ */
+
+// Wait for spindle to come up to speed
+inline void delay_for_power_up() { dwell(SPINDLE_LASER_POWERUP_DELAY); }
+
+// Wait for spindle to stop turning
+inline void delay_for_power_down() { dwell(SPINDLE_LASER_POWERDOWN_DELAY); }
+
+/**
+ * ocr_val_mode() is used for debugging and to get the points needed to compute the RPM vs ocr_val line
+ *
+ * it accepts inputs of 0-255
+ */
+
+inline void ocr_val_mode() {
+  uint8_t spindle_laser_power = parser.value_byte();
+  WRITE(SPINDLE_LASER_ENABLE_PIN, SPINDLE_LASER_ENABLE_INVERT); // turn spindle on (active low)
+  if (SPINDLE_LASER_PWM_INVERT) spindle_laser_power = 255 - spindle_laser_power;
+  analogWrite(SPINDLE_LASER_PWM_PIN, spindle_laser_power);
+}
+
+void gcode_M3_M4(bool is_M3) {
+
+  stepper.synchronize();   // wait until previous movement commands (G0/G0/G2/G3) have completed before playing with the spindle
+  #if SPINDLE_DIR_CHANGE
+    const bool rotation_dir = (is_M3 != SPINDLE_INVERT_DIR);
+    if (SPINDLE_STOP_ON_DIR_CHANGE \
+       && READ(SPINDLE_LASER_ENABLE_PIN) == SPINDLE_LASER_ENABLE_INVERT \
+       && READ(SPINDLE_DIR_PIN) != rotation_dir
+    ) {
+      WRITE(SPINDLE_LASER_ENABLE_PIN, !SPINDLE_LASER_ENABLE_INVERT);  // turn spindle off
+      delay_for_power_down();
+    }
+    WRITE(SPINDLE_DIR_PIN, rotation_dir);
+  #endif
+
+  /**
+   * Our final value for ocr_val is an unsigned 8 bit value between 0 and 255 which usually means uint8_t.
+   * Went to uint16_t because some of the uint8_t calculations would sometimes give 1000 0000 rather than 1111 1111.
+   * Then needed to AND the uint16_t result with 0x00FF to make sure we only wrote the byte of interest.
+   */
+  #if ENABLED(SPINDLE_LASER_PWM)
+    if (parser.seen('O')) ocr_val_mode();
+    else {
+      const float spindle_laser_power = parser.floatval('S');
+      if (spindle_laser_power == 0) {
+        WRITE(SPINDLE_LASER_ENABLE_PIN, !SPINDLE_LASER_ENABLE_INVERT);                                    // turn spindle off (active low)
+        delay_for_power_down();
+      }
+      else {
+        int16_t ocr_val = (spindle_laser_power - (SPEED_POWER_INTERCEPT)) * (1.0 / (SPEED_POWER_SLOPE));  // convert RPM to PWM duty cycle
+        NOMORE(ocr_val, 255);                                                                             // limit to max the Atmel PWM will support
+        if (spindle_laser_power <= SPEED_POWER_MIN)
+          ocr_val = (SPEED_POWER_MIN - (SPEED_POWER_INTERCEPT)) * (1.0 / (SPEED_POWER_SLOPE));            // minimum setting
+        if (spindle_laser_power >= SPEED_POWER_MAX)
+          ocr_val = (SPEED_POWER_MAX - (SPEED_POWER_INTERCEPT)) * (1.0 / (SPEED_POWER_SLOPE));            // limit to max RPM
+        if (SPINDLE_LASER_PWM_INVERT) ocr_val = 255 - ocr_val;
+        WRITE(SPINDLE_LASER_ENABLE_PIN, SPINDLE_LASER_ENABLE_INVERT);                                     // turn spindle on (active low)
+        analogWrite(SPINDLE_LASER_PWM_PIN, ocr_val & 0xFF);                                               // only write low byte
+        delay_for_power_up();
+      }
+    }
+  #else
+    WRITE(SPINDLE_LASER_ENABLE_PIN, SPINDLE_LASER_ENABLE_INVERT); // turn spindle on (active low) if spindle speed option not enabled
+    delay_for_power_up();
+  #endif
+}
+
+/**
+* M5 turn off spindle
+*/
+void gcode_M5() {
+  stepper.synchronize();
+  WRITE(SPINDLE_LASER_ENABLE_PIN, !SPINDLE_LASER_ENABLE_INVERT);
+  delay_for_power_down();
+}

Marlin/src/gcode/control/M350.h

@@ -0,0 +1,33 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M350: Set axis microstepping modes. S sets mode for all drivers.
+ *
+ * Warning: Steps-per-unit remains unchanged.
+ */
+void gcode_M350() {
+  if (parser.seen('S')) for (int i = 0; i <= 4; i++) stepper.microstep_mode(i, parser.value_byte());
+  LOOP_XYZE(i) if (parser.seen(axis_codes[i])) stepper.microstep_mode(i, parser.value_byte());
+  if (parser.seen('B')) stepper.microstep_mode(4, parser.value_byte());
+  stepper.microstep_readings();
+}

Marlin/src/gcode/control/M351.h

@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M351: Toggle MS1 MS2 pins directly with axis codes X Y Z E B
+ *       S# determines MS1 or MS2, X# sets the pin high/low.
+ */
+void gcode_M351() {
+  if (parser.seenval('S')) switch (parser.value_byte()) {
+    case 1:
+      LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, parser.value_byte(), -1);
+      if (parser.seenval('B')) stepper.microstep_ms(4, parser.value_byte(), -1);
+      break;
+    case 2:
+      LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, parser.value_byte());
+      if (parser.seenval('B')) stepper.microstep_ms(4, -1, parser.value_byte());
+      break;
+  }
+  stepper.microstep_readings();
+}

Marlin/src/gcode/control/M380_M381.h

@@ -0,0 +1,93 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ENABLED(EXT_SOLENOID)
+
+void enable_solenoid(const uint8_t num) {
+  switch (num) {
+    case 0:
+      OUT_WRITE(SOL0_PIN, HIGH);
+      break;
+      #if HAS_SOLENOID_1 && EXTRUDERS > 1
+        case 1:
+          OUT_WRITE(SOL1_PIN, HIGH);
+          break;
+      #endif
+      #if HAS_SOLENOID_2 && EXTRUDERS > 2
+        case 2:
+          OUT_WRITE(SOL2_PIN, HIGH);
+          break;
+      #endif
+      #if HAS_SOLENOID_3 && EXTRUDERS > 3
+        case 3:
+          OUT_WRITE(SOL3_PIN, HIGH);
+          break;
+      #endif
+      #if HAS_SOLENOID_4 && EXTRUDERS > 4
+        case 4:
+          OUT_WRITE(SOL4_PIN, HIGH);
+          break;
+      #endif
+    default:
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPGM(MSG_INVALID_SOLENOID);
+      break;
+  }
+}
+
+void enable_solenoid_on_active_extruder() { enable_solenoid(active_extruder); }
+
+void disable_all_solenoids() {
+  OUT_WRITE(SOL0_PIN, LOW);
+  #if HAS_SOLENOID_1 && EXTRUDERS > 1
+    OUT_WRITE(SOL1_PIN, LOW);
+  #endif
+  #if HAS_SOLENOID_2 && EXTRUDERS > 2
+    OUT_WRITE(SOL2_PIN, LOW);
+  #endif
+  #if HAS_SOLENOID_3 && EXTRUDERS > 3
+    OUT_WRITE(SOL3_PIN, LOW);
+  #endif
+  #if HAS_SOLENOID_4 && EXTRUDERS > 4
+    OUT_WRITE(SOL4_PIN, LOW);
+  #endif
+}
+
+/**
+ * M380: Enable solenoid on the active extruder
+ */
+void gcode_M380() {
+
+  enable_solenoid_on_active_extruder();
+
+}
+
+/**
+ * M381: Disable all solenoids
+ */
+void gcode_M381() {
+
+  disable_all_solenoids();
+
+}
+
+#endif // EXT_SOLENOID

Marlin/src/gcode/control/M400.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M400: Finish all moves
+ */
+void gcode_M400() {
+
+  stepper.synchronize();
+
+}

Marlin/src/gcode/control/M410.h

@@ -0,0 +1,33 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M410: Quickstop - Abort all planned moves
+ *
+ * This will stop the carriages mid-move, so most likely they
+ * will be out of sync with the stepper position after this.
+ */
+void gcode_M410() {
+
+  quickstop_stepper();
+
+}

Marlin/src/gcode/control/M42.h

@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M42: Change pin status via GCode
+ *
+ *  P<pin>  Pin number (LED if omitted)
+ *  S<byte> Pin status from 0 - 255
+ */
+void gcode_M42() {
+  if (!parser.seenval('S')) return;
+  const byte pin_status = parser.value_byte();
+
+  const int pin_number = parser.intval('P', LED_PIN);
+  if (pin_number < 0) return;
+
+  if (pin_is_protected(pin_number)) {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_PROTECTED_PIN);
+    return;
+  }
+
+  pinMode(pin_number, OUTPUT);
+  digitalWrite(pin_number, pin_status);
+  analogWrite(pin_number, pin_status);
+
+  #if FAN_COUNT > 0
+    switch (pin_number) {
+      #if HAS_FAN0
+        case FAN_PIN: fanSpeeds[0] = pin_status; break;
+      #endif
+      #if HAS_FAN1
+        case FAN1_PIN: fanSpeeds[1] = pin_status; break;
+      #endif
+      #if HAS_FAN2
+        case FAN2_PIN: fanSpeeds[2] = pin_status; break;
+      #endif
+    }
+  #endif
+}

Marlin/src/gcode/control/M605.h

@@ -0,0 +1,76 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  /**
+   * M605: Set dual x-carriage movement mode
+   *
+   *    M605 S0: Full control mode. The slicer has full control over x-carriage movement
+   *    M605 S1: Auto-park mode. The inactive head will auto park/unpark without slicer involvement
+   *    M605 S2 [Xnnn] [Rmmm]: Duplication mode. The second extruder will duplicate the first with nnn
+   *                         units x-offset and an optional differential hotend temperature of
+   *                         mmm degrees. E.g., with "M605 S2 X100 R2" the second extruder will duplicate
+   *                         the first with a spacing of 100mm in the x direction and 2 degrees hotter.
+   *
+   *    Note: the X axis should be homed after changing dual x-carriage mode.
+   */
+  void gcode_M605() {
+    stepper.synchronize();
+    if (parser.seen('S')) dual_x_carriage_mode = (DualXMode)parser.value_byte();
+    switch (dual_x_carriage_mode) {
+      case DXC_FULL_CONTROL_MODE:
+      case DXC_AUTO_PARK_MODE:
+        break;
+      case DXC_DUPLICATION_MODE:
+        if (parser.seen('X')) duplicate_extruder_x_offset = max(parser.value_linear_units(), X2_MIN_POS - x_home_pos(0));
+        if (parser.seen('R')) duplicate_extruder_temp_offset = parser.value_celsius_diff();
+        SERIAL_ECHO_START();
+        SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
+        SERIAL_CHAR(' ');
+        SERIAL_ECHO(hotend_offset[X_AXIS][0]);
+        SERIAL_CHAR(',');
+        SERIAL_ECHO(hotend_offset[Y_AXIS][0]);
+        SERIAL_CHAR(' ');
+        SERIAL_ECHO(duplicate_extruder_x_offset);
+        SERIAL_CHAR(',');
+        SERIAL_ECHOLN(hotend_offset[Y_AXIS][1]);
+        break;
+      default:
+        dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
+        break;
+    }
+    active_extruder_parked = false;
+    extruder_duplication_enabled = false;
+    delayed_move_time = 0;
+  }
+
+#elif ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
+
+  void gcode_M605() {
+    stepper.synchronize();
+    extruder_duplication_enabled = parser.intval('S') == (int)DXC_DUPLICATION_MODE;
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLNPAIR(MSG_DUPLICATION_MODE, extruder_duplication_enabled ? MSG_ON : MSG_OFF);
+  }
+
+#endif // DUAL_NOZZLE_DUPLICATION_MODE

Marlin/src/gcode/control/M80.h

@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M80   : Turn on the Power Supply
+ * M80 S : Report the current state and exit
+ */
+void gcode_M80() {
+
+  // S: Report the current power supply state and exit
+  if (parser.seen('S')) {
+    serialprintPGM(powersupply_on ? PSTR("PS:1\n") : PSTR("PS:0\n"));
+    return;
+  }
+
+  OUT_WRITE(PS_ON_PIN, PS_ON_AWAKE); // GND
+
+  /**
+   * If you have a switch on suicide pin, this is useful
+   * if you want to start another print with suicide feature after
+   * a print without suicide...
+   */
+  #if HAS_SUICIDE
+    OUT_WRITE(SUICIDE_PIN, HIGH);
+  #endif
+
+  #if ENABLED(HAVE_TMC2130)
+    delay(100);
+    tmc2130_init(); // Settings only stick when the driver has power
+  #endif
+
+  powersupply_on = true;
+
+  #if ENABLED(ULTIPANEL)
+    LCD_MESSAGEPGM(WELCOME_MSG);
+  #endif
+}

Marlin/src/gcode/control/M81.h

@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M81: Turn off Power, including Power Supply, if there is one.
+ *
+ *      This code should ALWAYS be available for EMERGENCY SHUTDOWN!
+ */
+void gcode_M81() {
+  thermalManager.disable_all_heaters();
+  stepper.finish_and_disable();
+
+  #if FAN_COUNT > 0
+    for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0;
+    #if ENABLED(PROBING_FANS_OFF)
+      fans_paused = false;
+      ZERO(paused_fanSpeeds);
+    #endif
+  #endif
+
+  safe_delay(1000); // Wait 1 second before switching off
+
+  #if HAS_SUICIDE
+    stepper.synchronize();
+    suicide();
+  #elif HAS_POWER_SWITCH
+    OUT_WRITE(PS_ON_PIN, PS_ON_ASLEEP);
+    powersupply_on = false;
+  #endif
+
+  #if ENABLED(ULTIPANEL)
+    LCD_MESSAGEPGM(MACHINE_NAME " " MSG_OFF ".");
+  #endif
+}

Marlin/src/gcode/control/M85.h

@@ -0,0 +1,31 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M85: Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ */
+void gcode_M85() {
+
+  if (parser.seen('S')) max_inactive_time = parser.value_millis_from_seconds();
+
+}
+

Marlin/src/gcode/control/M999.h

@@ -0,0 +1,41 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M999: Restart after being stopped
+ *
+ * Default behaviour is to flush the serial buffer and request
+ * a resend to the host starting on the last N line received.
+ *
+ * Sending "M999 S1" will resume printing without flushing the
+ * existing command buffer.
+ *
+ */
+void gcode_M999() {
+  Running = true;
+  lcd_reset_alert_level();
+
+  if (parser.boolval('S')) return;
+
+  // gcode_LastN = Stopped_gcode_LastN;
+  FlushSerialRequestResend();
+}

Marlin/src/gcode/control/T.h

@@ -0,0 +1,62 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "../../module/tool_change.h"
+
+/**
+ * T0-T3: Switch tool, usually switching extruders
+ *
+ *   F[units/min] Set the movement feedrate
+ *   S1           Don't move the tool in XY after change
+ */
+void gcode_T(uint8_t tmp_extruder) {
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) {
+      SERIAL_ECHOPAIR(">>> gcode_T(", tmp_extruder);
+      SERIAL_CHAR(')');
+      SERIAL_EOL();
+      DEBUG_POS("BEFORE", current_position);
+    }
+  #endif
+
+  #if HOTENDS == 1 || (ENABLED(MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1)
+
+    tool_change(tmp_extruder);
+
+  #elif HOTENDS > 1
+
+    tool_change(
+      tmp_extruder,
+      MMM_TO_MMS(parser.linearval('F')),
+      (tmp_extruder == active_extruder) || parser.boolval('S')
+    );
+
+  #endif
+
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_POS("AFTER", current_position);
+      SERIAL_ECHOLNPGM("<<< gcode_T");
+    }
+  #endif
+}

Marlin/src/gcode/eeprom/M500.h

@@ -0,0 +1,28 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M500: Store settings in EEPROM
+ */
+void gcode_M500() {
+  (void)settings.save();
+}

Marlin/src/gcode/eeprom/M501.h

@@ -0,0 +1,28 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M501: Read settings from EEPROM
+ */
+void gcode_M501() {
+  (void)settings.load();
+}

Marlin/src/gcode/eeprom/M502.h

@@ -0,0 +1,28 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M502: Revert to default settings
+ */
+void gcode_M502() {
+  (void)settings.reset();
+}

Marlin/src/gcode/eeprom/M503.h

@@ -0,0 +1,28 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M503: print settings currently in memory
+ */
+void gcode_M503() {
+  (void)settings.report(!parser.boolval('S', true));
+}

Marlin/src/gcode/feature/advance/M900.h

@@ -0,0 +1,52 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M900: Set and/or Get advance K factor and WH/D ratio
+ *
+ *  K<factor>                  Set advance K factor
+ *  R<ratio>                   Set ratio directly (overrides WH/D)
+ *  W<width> H<height> D<diam> Set ratio from WH/D
+ */
+void gcode_M900() {
+  stepper.synchronize();
+
+  const float newK = parser.floatval('K', -1);
+  if (newK >= 0) planner.extruder_advance_k = newK;
+
+  float newR = parser.floatval('R', -1);
+  if (newR < 0) {
+    const float newD = parser.floatval('D', -1),
+                newW = parser.floatval('W', -1),
+                newH = parser.floatval('H', -1);
+    if (newD >= 0 && newW >= 0 && newH >= 0)
+      newR = newD ? (newW * newH) / (sq(newD * 0.5) * M_PI) : 0;
+  }
+  if (newR >= 0) planner.advance_ed_ratio = newR;
+
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPAIR("Advance K=", planner.extruder_advance_k);
+  SERIAL_ECHOPGM(" E/D=");
+  const float ratio = planner.advance_ed_ratio;
+  if (ratio) SERIAL_ECHO(ratio); else SERIAL_ECHOPGM("Auto");
+  SERIAL_EOL();
+}

Marlin/src/gcode/feature/baricuda/M126.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M126: Heater 1 valve open
+ */
+void gcode_M126() {
+
+  baricuda_valve_pressure = parser.byteval('S', 255);
+
+}

Marlin/src/gcode/feature/baricuda/M127.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M127: Heater 1 valve close
+ */
+void gcode_M127() {
+
+  baricuda_valve_pressure = 0;
+
+}

Marlin/src/gcode/feature/baricuda/M128.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M128: Heater 2 valve open
+ */
+void gcode_M128() {
+
+  baricuda_e_to_p_pressure = parser.byteval('S', 255);
+
+}

Marlin/src/gcode/feature/baricuda/M129.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M129: Heater 2 valve close
+ */
+void gcode_M129() {
+
+  baricuda_e_to_p_pressure = 0;
+
+}

Marlin/src/gcode/feature/camera/M240.h

@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M240: Trigger a camera by emulating a Canon RC-1
+ *       See http://www.doc-diy.net/photo/rc-1_hacked/
+ */
+void gcode_M240() {
+  #ifdef CHDK
+
+    OUT_WRITE(CHDK, HIGH);
+    chdkHigh = millis();
+    chdkActive = true;
+
+  #elif HAS_PHOTOGRAPH
+
+    const uint8_t NUM_PULSES = 16;
+    const float PULSE_LENGTH = 0.01524;
+    for (int i = 0; i < NUM_PULSES; i++) {
+      WRITE(PHOTOGRAPH_PIN, HIGH);
+      _delay_ms(PULSE_LENGTH);
+      WRITE(PHOTOGRAPH_PIN, LOW);
+      _delay_ms(PULSE_LENGTH);
+    }
+    delay(7.33);
+    for (int i = 0; i < NUM_PULSES; i++) {
+      WRITE(PHOTOGRAPH_PIN, HIGH);
+      _delay_ms(PULSE_LENGTH);
+      WRITE(PHOTOGRAPH_PIN, LOW);
+      _delay_ms(PULSE_LENGTH);
+    }
+
+  #endif
+}

Marlin/src/gcode/feature/caselight/M355.h

@@ -0,0 +1,77 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#if HAS_CASE_LIGHT
+
+  #ifndef INVERT_CASE_LIGHT
+    #define INVERT_CASE_LIGHT false
+  #endif
+  int case_light_brightness;  // LCD routine wants INT
+  bool case_light_on;
+
+  void update_case_light() {
+    pinMode(CASE_LIGHT_PIN, OUTPUT); // digitalWrite doesn't set the port mode
+    uint8_t case_light_bright = (uint8_t)case_light_brightness;
+    if (case_light_on) {
+      if (USEABLE_HARDWARE_PWM(CASE_LIGHT_PIN)) {
+        analogWrite(CASE_LIGHT_PIN, INVERT_CASE_LIGHT ? 255 - case_light_brightness : case_light_brightness );
+      }
+      else WRITE(CASE_LIGHT_PIN, INVERT_CASE_LIGHT ? LOW : HIGH);
+    }
+    else WRITE(CASE_LIGHT_PIN, INVERT_CASE_LIGHT ? HIGH : LOW);
+  }
+
+#endif // HAS_CASE_LIGHT
+
+/**
+ * M355: Turn case light on/off and set brightness
+ *
+ *   P<byte>  Set case light brightness (PWM pin required - ignored otherwise)
+ *
+ *   S<bool>  Set case light on/off
+ *
+ *   When S turns on the light on a PWM pin then the current brightness level is used/restored
+ *
+ *   M355 P200 S0 turns off the light & sets the brightness level
+ *   M355 S1 turns on the light with a brightness of 200 (assuming a PWM pin)
+ */
+void gcode_M355() {
+  #if HAS_CASE_LIGHT
+    uint8_t args = 0;
+    if (parser.seenval('P')) ++args, case_light_brightness = parser.value_byte();
+    if (parser.seenval('S')) ++args, case_light_on = parser.value_bool();
+    if (args) update_case_light();
+
+    // always report case light status
+    SERIAL_ECHO_START();
+    if (!case_light_on) {
+      SERIAL_ECHOLN("Case light: off");
+    }
+    else {
+      if (!USEABLE_HARDWARE_PWM(CASE_LIGHT_PIN)) SERIAL_ECHOLN("Case light: on");
+      else SERIAL_ECHOLNPAIR("Case light: ", case_light_brightness);
+    }
+  #else
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLNPGM(MSG_ERR_M355_NONE);
+  #endif
+}

Marlin/src/gcode/feature/clean/G12.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * G12: Clean the nozzle
+ */
+void gcode_G12() {
+  // Don't allow nozzle cleaning without homing first
+  if (axis_unhomed_error()) return;
+
+  const uint8_t pattern = parser.ushortval('P', 0),
+                strokes = parser.ushortval('S', NOZZLE_CLEAN_STROKES),
+                objects = parser.ushortval('T', NOZZLE_CLEAN_TRIANGLES);
+  const float radius = parser.floatval('R', NOZZLE_CLEAN_CIRCLE_RADIUS);
+
+  Nozzle::clean(pattern, strokes, radius, objects);
+}

Marlin/src/gcode/feature/digipot/M907.h

@@ -0,0 +1,61 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M907: Set digital trimpot motor current using axis codes X, Y, Z, E, B, S
+ */
+void gcode_M907() {
+  #if HAS_DIGIPOTSS
+
+    LOOP_XYZE(i) if (parser.seen(axis_codes[i])) stepper.digipot_current(i, parser.value_int());
+    if (parser.seen('B')) stepper.digipot_current(4, parser.value_int());
+    if (parser.seen('S')) for (uint8_t i = 0; i <= 4; i++) stepper.digipot_current(i, parser.value_int());
+
+  #elif HAS_MOTOR_CURRENT_PWM
+
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
+      if (parser.seen('X')) stepper.digipot_current(0, parser.value_int());
+    #endif
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+      if (parser.seen('Z')) stepper.digipot_current(1, parser.value_int());
+    #endif
+    #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
+      if (parser.seen('E')) stepper.digipot_current(2, parser.value_int());
+    #endif
+
+  #endif
+
+  #if ENABLED(DIGIPOT_I2C)
+    // this one uses actual amps in floating point
+    LOOP_XYZE(i) if (parser.seen(axis_codes[i])) digipot_i2c_set_current(i, parser.value_float());
+    // for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
+    for (uint8_t i = NUM_AXIS; i < DIGIPOT_I2C_NUM_CHANNELS; i++) if (parser.seen('B' + i - (NUM_AXIS))) digipot_i2c_set_current(i, parser.value_float());
+  #endif
+
+  #if ENABLED(DAC_STEPPER_CURRENT)
+    if (parser.seen('S')) {
+      const float dac_percent = parser.value_float();
+      for (uint8_t i = 0; i <= 4; i++) dac_current_percent(i, dac_percent);
+    }
+    LOOP_XYZE(i) if (parser.seen(axis_codes[i])) dac_current_percent(i, parser.value_float());
+  #endif
+}

Marlin/src/gcode/feature/digipot/M908.h

@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M908: Control digital trimpot directly (M908 P<pin> S<current>)
+ */
+void gcode_M908() {
+  #if HAS_DIGIPOTSS
+    stepper.digitalPotWrite(
+      parser.intval('P'),
+      parser.intval('S')
+    );
+  #endif
+  #ifdef DAC_STEPPER_CURRENT
+    dac_current_raw(
+      parser.byteval('P', -1),
+      parser.ushortval('S', 0)
+    );
+  #endif
+}

Marlin/src/gcode/feature/digipot/M909.h

@@ -0,0 +1,27 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void gcode_M909() {
+
+  dac_print_values();
+
+}

Marlin/src/gcode/feature/digipot/M910.h

@@ -0,0 +1,27 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void gcode_M910() {
+
+  dac_commit_eeprom();
+
+}

Marlin/src/gcode/feature/fwretract/G10_G11.h

@@ -0,0 +1,41 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * G10 - Retract filament according to settings of M207
+ */
+void gcode_G10() {
+  #if EXTRUDERS > 1
+    const bool rs = parser.boolval('S');
+    retracted_swap[active_extruder] = rs; // Use 'S' for swap, default to false
+  #endif
+  retract(true
+    #if EXTRUDERS > 1
+      , rs
+    #endif
+  );
+}
+
+/**
+ * G11 - Recover filament according to settings of M208
+ */
+void gcode_G11() { retract(false); }

Marlin/src/gcode/feature/fwretract/M207.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M207: Set firmware retraction values
+ *
+ *   S[+units]    retract_length
+ *   W[+units]    swap_retract_length (multi-extruder)
+ *   F[units/min] retract_feedrate_mm_s
+ *   Z[units]     retract_zlift
+ */
+void gcode_M207() {
+  if (parser.seen('S')) retract_length = parser.value_axis_units(E_AXIS);
+  if (parser.seen('F')) retract_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS));
+  if (parser.seen('Z')) retract_zlift = parser.value_linear_units();
+  if (parser.seen('W')) swap_retract_length = parser.value_axis_units(E_AXIS);
+}

Marlin/src/gcode/feature/fwretract/M208.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M208: Set firmware un-retraction values
+ *
+ *   S[+units]    retract_recover_length (in addition to M207 S*)
+ *   W[+units]    swap_retract_recover_length (multi-extruder)
+ *   F[units/min] retract_recover_feedrate_mm_s
+ *   R[units/min] swap_retract_recover_feedrate_mm_s
+ */
+void gcode_M208() {
+  if (parser.seen('S')) retract_recover_length = parser.value_axis_units(E_AXIS);
+  if (parser.seen('F')) retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS));
+  if (parser.seen('R')) swap_retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS));
+  if (parser.seen('W')) swap_retract_recover_length = parser.value_axis_units(E_AXIS);
+}

Marlin/src/gcode/feature/fwretract/M209.h

@@ -0,0 +1,35 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M209: Enable automatic retract (M209 S1)
+ *   For slicers that don't support G10/11, reversed extrude-only
+ *   moves will be classified as retraction.
+ */
+void gcode_M209() {
+  if (MIN_AUTORETRACT <= MAX_AUTORETRACT) {
+    if (parser.seen('S')) {
+      autoretract_enabled = parser.value_bool();
+      for (uint8_t i = 0; i < EXTRUDERS; i++) retracted[i] = false;
+    }
+  }
+}

Marlin/src/gcode/feature/i2c/M260_M261.h

@@ -0,0 +1,70 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M260: Send data to a I2C slave device
+ *
+ * This is a PoC, the formating and arguments for the GCODE will
+ * change to be more compatible, the current proposal is:
+ *
+ *  M260 A<slave device address base 10> ; Sets the I2C slave address the data will be sent to
+ *
+ *  M260 B<byte-1 value in base 10>
+ *  M260 B<byte-2 value in base 10>
+ *  M260 B<byte-3 value in base 10>
+ *
+ *  M260 S1 ; Send the buffered data and reset the buffer
+ *  M260 R1 ; Reset the buffer without sending data
+ *
+ */
+void gcode_M260() {
+  // Set the target address
+  if (parser.seen('A')) i2c.address(parser.value_byte());
+
+  // Add a new byte to the buffer
+  if (parser.seen('B')) i2c.addbyte(parser.value_byte());
+
+  // Flush the buffer to the bus
+  if (parser.seen('S')) i2c.send();
+
+  // Reset and rewind the buffer
+  else if (parser.seen('R')) i2c.reset();
+}
+
+/**
+ * M261: Request X bytes from I2C slave device
+ *
+ * Usage: M261 A<slave device address base 10> B<number of bytes>
+ */
+void gcode_M261() {
+  if (parser.seen('A')) i2c.address(parser.value_byte());
+
+  uint8_t bytes = parser.byteval('B', 1);
+
+  if (i2c.addr && bytes && bytes <= TWIBUS_BUFFER_SIZE) {
+    i2c.relay(bytes);
+  }
+  else {
+    SERIAL_ERROR_START();
+    SERIAL_ERRORLN("Bad i2c request");
+  }
+}

Marlin/src/gcode/feature/leds/M150.h

@@ -0,0 +1,46 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M150: Set Status LED Color - Use R-U-B-W for R-G-B-W
+ *
+ * Always sets all 3 or 4 components. If a component is left out, set to 0.
+ *
+ * Examples:
+ *
+ *   M150 R255       ; Turn LED red
+ *   M150 R255 U127  ; Turn LED orange (PWM only)
+ *   M150            ; Turn LED off
+ *   M150 R U B      ; Turn LED white
+ *   M150 W          ; Turn LED white using a white LED
+ *
+ */
+void gcode_M150() {
+  set_led_color(
+    parser.seen('R') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('U') ? (parser.has_value() ? parser.value_byte() : 255) : 0,
+    parser.seen('B') ? (parser.has_value() ? parser.value_byte() : 255) : 0
+    #if ENABLED(RGBW_LED) || ENABLED(NEOPIXEL_RGBW_LED)
+      , parser.seen('W') ? (parser.has_value() ? parser.value_byte() : 255) : 0
+    #endif
+  );
+}

Marlin/src/gcode/feature/mixing/M163.h

@@ -0,0 +1,38 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M163: Set a single mix factor for a mixing extruder
+ *       This is called "weight" by some systems.
+ *
+ *   S[index]   The channel index to set
+ *   P[float]   The mix value
+ *
+ */
+void gcode_M163() {
+  const int mix_index = parser.intval('S');
+  if (mix_index < MIXING_STEPPERS) {
+    float mix_value = parser.floatval('P');
+    NOLESS(mix_value, 0.0);
+    mixing_factor[mix_index] = RECIPROCAL(mix_value);
+  }
+}

Marlin/src/gcode/feature/mixing/M164.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M164: Store the current mix factors as a virtual tool.
+ *
+ *   S[index]   The virtual tool to store
+ *
+ */
+void gcode_M164() {
+  const int tool_index = parser.intval('S');
+  if (tool_index < MIXING_VIRTUAL_TOOLS) {
+    normalize_mix();
+    for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
+      mixing_virtual_tool_mix[tool_index][i] = mixing_factor[i];
+  }
+}

Marlin/src/gcode/feature/mixing/M165.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M165: Set multiple mix factors for a mixing extruder.
+ *       Factors that are left out will be set to 0.
+ *       All factors together must add up to 1.0.
+ *
+ *   A[factor] Mix factor for extruder stepper 1
+ *   B[factor] Mix factor for extruder stepper 2
+ *   C[factor] Mix factor for extruder stepper 3
+ *   D[factor] Mix factor for extruder stepper 4
+ *   H[factor] Mix factor for extruder stepper 5
+ *   I[factor] Mix factor for extruder stepper 6
+ *
+ */
+void gcode_M165() { gcode_get_mix(); }

Marlin/src/gcode/feature/pause/G27.h

@@ -0,0 +1,30 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * G27: Park the nozzle
+ */
+void gcode_G27() {
+  // Don't allow nozzle parking without homing first
+  if (axis_unhomed_error()) return;
+  Nozzle::park(parser.ushortval('P'));
+}

Marlin/src/gcode/feature/pause/M125.h

@@ -0,0 +1,89 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "common.h"
+
+/**
+ * M125: Store current position and move to filament change position.
+ *       Called on pause (by M25) to prevent material leaking onto the
+ *       object. On resume (M24) the head will be moved back and the
+ *       print will resume.
+ *
+ *       If Marlin is compiled without SD Card support, M125 can be
+ *       used directly to pause the print and move to park position,
+ *       resuming with a button click or M108.
+ *
+ *    L = override retract length
+ *    X = override X
+ *    Y = override Y
+ *    Z = override Z raise
+ */
+void gcode_M125() {
+
+  // Initial retract before move to filament change position
+  const float retract = parser.seen('L') ? parser.value_axis_units(E_AXIS) : 0
+    #ifdef PAUSE_PARK_RETRACT_LENGTH
+      - (PAUSE_PARK_RETRACT_LENGTH)
+    #endif
+  ;
+
+  // Lift Z axis
+  const float z_lift = parser.linearval('Z')
+    #ifdef PAUSE_PARK_Z_ADD
+      + PAUSE_PARK_Z_ADD
+    #endif
+  ;
+
+  // Move XY axes to filament change position or given position
+  const float x_pos = parser.linearval('X')
+    #ifdef PAUSE_PARK_X_POS
+      + PAUSE_PARK_X_POS
+    #endif
+    #if HOTENDS > 1 && DISABLED(DUAL_X_CARRIAGE)
+      + (active_extruder ? hotend_offset[X_AXIS][active_extruder] : 0)
+    #endif
+  ;
+  const float y_pos = parser.linearval('Y')
+    #ifdef PAUSE_PARK_Y_POS
+      + PAUSE_PARK_Y_POS
+    #endif
+    #if HOTENDS > 1 && DISABLED(DUAL_X_CARRIAGE)
+      + (active_extruder ? hotend_offset[Y_AXIS][active_extruder] : 0)
+    #endif
+  ;
+
+  #if DISABLED(SDSUPPORT)
+    const bool job_running = print_job_timer.isRunning();
+  #endif
+
+  if (pause_print(retract, z_lift, x_pos, y_pos)) {
+    #if DISABLED(SDSUPPORT)
+      // Wait for lcd click or M108
+      wait_for_filament_reload();
+
+      // Return to print position and continue
+      resume_print();
+
+      if (job_running) print_job_timer.start();
+    #endif
+  }
+}

Marlin/src/gcode/feature/pause/M600.h

@@ -0,0 +1,103 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+#include "common.h"
+
+/**
+ * M600: Pause for filament change
+ *
+ *  E[distance] - Retract the filament this far (negative value)
+ *  Z[distance] - Move the Z axis by this distance
+ *  X[position] - Move to this X position, with Y
+ *  Y[position] - Move to this Y position, with X
+ *  U[distance] - Retract distance for removal (negative value) (manual reload)
+ *  L[distance] - Extrude distance for insertion (positive value) (manual reload)
+ *  B[count]    - Number of times to beep, -1 for indefinite (if equipped with a buzzer)
+ *
+ *  Default values are used for omitted arguments.
+ *
+ */
+void gcode_M600() {
+
+  #if ENABLED(HOME_BEFORE_FILAMENT_CHANGE)
+    // Don't allow filament change without homing first
+    if (axis_unhomed_error()) home_all_axes();
+  #endif
+
+  // Initial retract before move to filament change position
+  const float retract = parser.seen('E') ? parser.value_axis_units(E_AXIS) : 0
+    #ifdef PAUSE_PARK_RETRACT_LENGTH
+      - (PAUSE_PARK_RETRACT_LENGTH)
+    #endif
+  ;
+
+  // Lift Z axis
+  const float z_lift = parser.linearval('Z', 0
+    #ifdef PAUSE_PARK_Z_ADD
+      + PAUSE_PARK_Z_ADD
+    #endif
+  );
+
+  // Move XY axes to filament exchange position
+  const float x_pos = parser.linearval('X', 0
+    #ifdef PAUSE_PARK_X_POS
+      + PAUSE_PARK_X_POS
+    #endif
+  );
+  const float y_pos = parser.linearval('Y', 0
+    #ifdef PAUSE_PARK_Y_POS
+      + PAUSE_PARK_Y_POS
+    #endif
+  );
+
+  // Unload filament
+  const float unload_length = parser.seen('U') ? parser.value_axis_units(E_AXIS) : 0
+    #if defined(FILAMENT_CHANGE_UNLOAD_LENGTH) && FILAMENT_CHANGE_UNLOAD_LENGTH > 0
+      - (FILAMENT_CHANGE_UNLOAD_LENGTH)
+    #endif
+  ;
+
+  // Load filament
+  const float load_length = parser.seen('L') ? parser.value_axis_units(E_AXIS) : 0
+    #ifdef FILAMENT_CHANGE_LOAD_LENGTH
+      + FILAMENT_CHANGE_LOAD_LENGTH
+    #endif
+  ;
+
+  const int beep_count = parser.intval('B',
+    #ifdef FILAMENT_CHANGE_NUMBER_OF_ALERT_BEEPS
+      FILAMENT_CHANGE_NUMBER_OF_ALERT_BEEPS
+    #else
+      -1
+    #endif
+  );
+
+  const bool job_running = print_job_timer.isRunning();
+
+  if (pause_print(retract, z_lift, x_pos, y_pos, unload_length, beep_count, true)) {
+    wait_for_filament_reload(beep_count);
+    resume_print(load_length, ADVANCED_PAUSE_EXTRUDE_LENGTH, beep_count);
+  }
+
+  // Resume the print job timer if it was running
+  if (job_running) print_job_timer.start();
+}

Marlin/src/gcode/feature/pause/common.h

@@ -0,0 +1,335 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/pause/common.h - Merge this with its G-codes in the refactor
+ */
+
+#ifndef PAUSE_COMMON_H
+#define PAUSE_COMMON_H
+
+#if IS_KINEMATIC
+  #define RUNPLAN(RATE_MM_S) planner.buffer_line_kinematic(destination, RATE_MM_S, active_extruder)
+#else
+  #define RUNPLAN(RATE_MM_S) line_to_destination(RATE_MM_S)
+#endif
+
+static float resume_position[XYZE];
+static bool move_away_flag = false;
+#if ENABLED(SDSUPPORT)
+  static bool sd_print_paused = false;
+#endif
+
+static void filament_change_beep(const int8_t max_beep_count, const bool init=false) {
+  static millis_t next_buzz = 0;
+  static int8_t runout_beep = 0;
+
+  if (init) next_buzz = runout_beep = 0;
+
+  const millis_t ms = millis();
+  if (ELAPSED(ms, next_buzz)) {
+    if (max_beep_count < 0 || runout_beep < max_beep_count + 5) { // Only beep as long as we're supposed to
+      next_buzz = ms + ((max_beep_count < 0 || runout_beep < max_beep_count) ? 2500 : 400);
+      BUZZ(300, 2000);
+      runout_beep++;
+    }
+  }
+}
+
+static void ensure_safe_temperature() {
+  bool heaters_heating = true;
+
+  wait_for_heatup = true;    // M108 will clear this
+  while (wait_for_heatup && heaters_heating) {
+    idle();
+    heaters_heating = false;
+    HOTEND_LOOP() {
+      if (thermalManager.degTargetHotend(e) && abs(thermalManager.degHotend(e) - thermalManager.degTargetHotend(e)) > TEMP_HYSTERESIS) {
+        heaters_heating = true;
+        #if ENABLED(ULTIPANEL)
+          lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_WAIT_FOR_NOZZLES_TO_HEAT);
+        #endif
+        break;
+      }
+    }
+  }
+}
+
+static bool pause_print(const float &retract, const float &z_lift, const float &x_pos, const float &y_pos,
+                        const float &unload_length = 0 , const int8_t max_beep_count = 0, const bool show_lcd = false
+) {
+  if (move_away_flag) return false; // already paused
+
+  if (!DEBUGGING(DRYRUN) && (unload_length != 0 || retract != 0)) {
+    #if ENABLED(PREVENT_COLD_EXTRUSION)
+      if (!thermalManager.allow_cold_extrude &&
+          thermalManager.degTargetHotend(active_extruder) < thermalManager.extrude_min_temp) {
+        SERIAL_ERROR_START();
+        SERIAL_ERRORLNPGM(MSG_TOO_COLD_FOR_M600);
+        return false;
+      }
+    #endif
+
+    ensure_safe_temperature(); // wait for extruder to heat up before unloading
+  }
+
+  // Indicate that the printer is paused
+  move_away_flag = true;
+
+  // Pause the print job and timer
+  #if ENABLED(SDSUPPORT)
+    if (IS_SD_PRINTING) {
+      card.pauseSDPrint();
+      sd_print_paused = true;
+    }
+  #endif
+  print_job_timer.pause();
+
+  // Show initial message and wait for synchronize steppers
+  if (show_lcd) {
+    #if ENABLED(ULTIPANEL)
+      lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_INIT);
+    #endif
+  }
+
+  // Save current position
+  stepper.synchronize();
+  COPY(resume_position, current_position);
+
+  if (retract) {
+    // Initial retract before move to filament change position
+    set_destination_to_current();
+    destination[E_AXIS] += retract;
+    RUNPLAN(PAUSE_PARK_RETRACT_FEEDRATE);
+    stepper.synchronize();
+  }
+
+  // Lift Z axis
+  if (z_lift > 0)
+    do_blocking_move_to_z(current_position[Z_AXIS] + z_lift, PAUSE_PARK_Z_FEEDRATE);
+
+  // Move XY axes to filament exchange position
+  do_blocking_move_to_xy(x_pos, y_pos, PAUSE_PARK_XY_FEEDRATE);
+
+  if (unload_length != 0) {
+    if (show_lcd) {
+      #if ENABLED(ULTIPANEL)
+        lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_UNLOAD);
+        idle();
+      #endif
+    }
+
+    // Unload filament
+    set_destination_to_current();
+    destination[E_AXIS] += unload_length;
+    RUNPLAN(FILAMENT_CHANGE_UNLOAD_FEEDRATE);
+    stepper.synchronize();
+  }
+
+  if (show_lcd) {
+    #if ENABLED(ULTIPANEL)
+      lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_INSERT);
+    #endif
+  }
+
+  #if HAS_BUZZER
+    filament_change_beep(max_beep_count, true);
+  #endif
+
+  idle();
+
+  // Disable extruders steppers for manual filament changing (only on boards that have separate ENABLE_PINS)
+  #if E0_ENABLE_PIN != X_ENABLE_PIN && E1_ENABLE_PIN != Y_ENABLE_PIN
+    disable_e_steppers();
+    safe_delay(100);
+  #endif
+
+  // Start the heater idle timers
+  const millis_t nozzle_timeout = (millis_t)(PAUSE_PARK_NOZZLE_TIMEOUT) * 1000UL;
+
+  HOTEND_LOOP()
+    thermalManager.start_heater_idle_timer(e, nozzle_timeout);
+
+  return true;
+}
+
+static void wait_for_filament_reload(const int8_t max_beep_count = 0) {
+  bool nozzle_timed_out = false;
+
+  // Wait for filament insert by user and press button
+  KEEPALIVE_STATE(PAUSED_FOR_USER);
+  wait_for_user = true;    // LCD click or M108 will clear this
+  while (wait_for_user) {
+    #if HAS_BUZZER
+      filament_change_beep(max_beep_count);
+    #endif
+
+    // If the nozzle has timed out, wait for the user to press the button to re-heat the nozzle, then
+    // re-heat the nozzle, re-show the insert screen, restart the idle timers, and start over
+    if (!nozzle_timed_out)
+      HOTEND_LOOP()
+        nozzle_timed_out |= thermalManager.is_heater_idle(e);
+
+    if (nozzle_timed_out) {
+      #if ENABLED(ULTIPANEL)
+        lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_CLICK_TO_HEAT_NOZZLE);
+      #endif
+
+      // Wait for LCD click or M108
+      while (wait_for_user) idle(true);
+
+      // Re-enable the heaters if they timed out
+      HOTEND_LOOP() thermalManager.reset_heater_idle_timer(e);
+
+      // Wait for the heaters to reach the target temperatures
+      ensure_safe_temperature();
+
+      #if ENABLED(ULTIPANEL)
+        lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_INSERT);
+      #endif
+
+      // Start the heater idle timers
+      const millis_t nozzle_timeout = (millis_t)(PAUSE_PARK_NOZZLE_TIMEOUT) * 1000UL;
+
+      HOTEND_LOOP()
+        thermalManager.start_heater_idle_timer(e, nozzle_timeout);
+
+      wait_for_user = true; /* Wait for user to load filament */
+      nozzle_timed_out = false;
+
+      #if HAS_BUZZER
+        filament_change_beep(max_beep_count, true);
+      #endif
+    }
+
+    idle(true);
+  }
+  KEEPALIVE_STATE(IN_HANDLER);
+}
+
+static void resume_print(const float &load_length = 0, const float &initial_extrude_length = 0, const int8_t max_beep_count = 0) {
+  bool nozzle_timed_out = false;
+
+  if (!move_away_flag) return;
+
+  // Re-enable the heaters if they timed out
+  HOTEND_LOOP() {
+    nozzle_timed_out |= thermalManager.is_heater_idle(e);
+    thermalManager.reset_heater_idle_timer(e);
+  }
+
+  if (nozzle_timed_out) ensure_safe_temperature();
+
+  #if HAS_BUZZER
+    filament_change_beep(max_beep_count, true);
+  #endif
+
+  if (load_length != 0) {
+    #if ENABLED(ULTIPANEL)
+      // Show "insert filament"
+      if (nozzle_timed_out)
+        lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_INSERT);
+    #endif
+
+    KEEPALIVE_STATE(PAUSED_FOR_USER);
+    wait_for_user = true;    // LCD click or M108 will clear this
+    while (wait_for_user && nozzle_timed_out) {
+      #if HAS_BUZZER
+        filament_change_beep(max_beep_count);
+      #endif
+      idle(true);
+    }
+    KEEPALIVE_STATE(IN_HANDLER);
+
+    #if ENABLED(ULTIPANEL)
+      // Show "load" message
+      lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_LOAD);
+    #endif
+
+    // Load filament
+    destination[E_AXIS] += load_length;
+    RUNPLAN(FILAMENT_CHANGE_LOAD_FEEDRATE);
+    stepper.synchronize();
+  }
+
+  #if ENABLED(ULTIPANEL) && ADVANCED_PAUSE_EXTRUDE_LENGTH > 0
+
+    float extrude_length = initial_extrude_length;
+
+    do {
+      if (extrude_length > 0) {
+        // "Wait for filament extrude"
+        lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_EXTRUDE);
+
+        // Extrude filament to get into hotend
+        destination[E_AXIS] += extrude_length;
+        RUNPLAN(ADVANCED_PAUSE_EXTRUDE_FEEDRATE);
+        stepper.synchronize();
+      }
+
+      // Show "Extrude More" / "Resume" menu and wait for reply
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
+      wait_for_user = false;
+      lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_OPTION);
+      while (advanced_pause_menu_response == ADVANCED_PAUSE_RESPONSE_WAIT_FOR) idle(true);
+      KEEPALIVE_STATE(IN_HANDLER);
+
+      extrude_length = ADVANCED_PAUSE_EXTRUDE_LENGTH;
+
+      // Keep looping if "Extrude More" was selected
+    } while (advanced_pause_menu_response == ADVANCED_PAUSE_RESPONSE_EXTRUDE_MORE);
+
+  #endif
+
+  #if ENABLED(ULTIPANEL)
+    // "Wait for print to resume"
+    lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_RESUME);
+  #endif
+
+  // Set extruder to saved position
+  destination[E_AXIS] = current_position[E_AXIS] = resume_position[E_AXIS];
+  planner.set_e_position_mm(current_position[E_AXIS]);
+
+  // Move XY to starting position, then Z
+  do_blocking_move_to_xy(resume_position[X_AXIS], resume_position[Y_AXIS], PAUSE_PARK_XY_FEEDRATE);
+  do_blocking_move_to_z(resume_position[Z_AXIS], PAUSE_PARK_Z_FEEDRATE);
+
+  #if ENABLED(FILAMENT_RUNOUT_SENSOR)
+    filament_ran_out = false;
+  #endif
+
+  #if ENABLED(ULTIPANEL)
+    // Show status screen
+    lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_STATUS);
+  #endif
+
+  #if ENABLED(SDSUPPORT)
+    if (sd_print_paused) {
+      card.startFileprint();
+      sd_print_paused = false;
+    }
+  #endif
+
+  move_away_flag = false;
+}
+
+#endif // PAUSE_COMMON_H

Marlin/src/gcode/feature/snmm/M702.h

@@ -0,0 +1,50 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+inline void select_multiplexed_stepper(const uint8_t e) {
+  stepper.synchronize();
+  disable_e_steppers();
+  WRITE(E_MUX0_PIN, TEST(e, 0) ? HIGH : LOW);
+  WRITE(E_MUX1_PIN, TEST(e, 1) ? HIGH : LOW);
+  WRITE(E_MUX2_PIN, TEST(e, 2) ? HIGH : LOW);
+  safe_delay(100);
+}
+
+/**
+ * M702: Unload all extruders
+ */
+void gcode_M702() {
+  for (uint8_t s = 0; s < E_STEPPERS; s++) {
+    select_multiplexed_stepper(e);
+    // TODO: standard unload filament function
+    // MK2 firmware behavior:
+    //  - Make sure temperature is high enough
+    //  - Raise Z to at least 15 to make room
+    //  - Extrude 1cm of filament in 1 second
+    //  - Under 230C quickly purge ~12mm, over 230C purge ~10mm
+    //  - Change E max feedrate to 80, eject the filament from the tube. Sync.
+    //  - Restore E max feedrate to 50
+  }
+  // Go back to the last active extruder
+  select_multiplexed_stepper(active_extruder);
+  disable_e_steppers();
+}

Marlin/src/gcode/feature/trinamic/M906.h

@@ -0,0 +1,65 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+static void tmc2130_get_current(TMC2130Stepper &st, const char name) {
+  SERIAL_CHAR(name);
+  SERIAL_ECHOPGM(" axis driver current: ");
+  SERIAL_ECHOLN(st.getCurrent());
+}
+static void tmc2130_set_current(TMC2130Stepper &st, const char name, const int mA) {
+  st.setCurrent(mA, R_SENSE, HOLD_MULTIPLIER);
+  tmc2130_get_current(st, name);
+}
+
+/**
+ * M906: Set motor current in milliamps using axis codes X, Y, Z, E
+ * Report driver currents when no axis specified
+ *
+ * S1: Enable automatic current control
+ * S0: Disable
+ */
+void gcode_M906() {
+  uint16_t values[XYZE];
+  LOOP_XYZE(i)
+    values[i] = parser.intval(axis_codes[i]);
+
+  #if ENABLED(X_IS_TMC2130)
+    if (values[X_AXIS]) tmc2130_set_current(stepperX, 'X', values[X_AXIS]);
+    else tmc2130_get_current(stepperX, 'X');
+  #endif
+  #if ENABLED(Y_IS_TMC2130)
+    if (values[Y_AXIS]) tmc2130_set_current(stepperY, 'Y', values[Y_AXIS]);
+    else tmc2130_get_current(stepperY, 'Y');
+  #endif
+  #if ENABLED(Z_IS_TMC2130)
+    if (values[Z_AXIS]) tmc2130_set_current(stepperZ, 'Z', values[Z_AXIS]);
+    else tmc2130_get_current(stepperZ, 'Z');
+  #endif
+  #if ENABLED(E0_IS_TMC2130)
+    if (values[E_AXIS]) tmc2130_set_current(stepperE0, 'E', values[E_AXIS]);
+    else tmc2130_get_current(stepperE0, 'E');
+  #endif
+
+  #if ENABLED(AUTOMATIC_CURRENT_CONTROL)
+    if (parser.seen('S')) auto_current_control = parser.value_bool();
+  #endif
+}

Marlin/src/gcode/feature/trinamic/M911.h

@@ -0,0 +1,49 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+static void tmc2130_report_otpw(TMC2130Stepper &st, const char name) {
+  SERIAL_CHAR(name);
+  SERIAL_ECHOPGM(" axis temperature prewarn triggered: ");
+  serialprintPGM(st.getOTPW() ? PSTR("true") : PSTR("false"));
+  SERIAL_EOL();
+}
+
+/**
+ * M911: Report TMC2130 stepper driver overtemperature pre-warn flag
+ * The flag is held by the library and persist until manually cleared by M912
+ */
+void gcode_M911() {
+  const bool reportX = parser.seen('X'), reportY = parser.seen('Y'), reportZ = parser.seen('Z'), reportE = parser.seen('E'),
+           reportAll = (!reportX && !reportY && !reportZ && !reportE) || (reportX && reportY && reportZ && reportE);
+  #if ENABLED(X_IS_TMC2130)
+    if (reportX || reportAll) tmc2130_report_otpw(stepperX, 'X');
+  #endif
+  #if ENABLED(Y_IS_TMC2130)
+    if (reportY || reportAll) tmc2130_report_otpw(stepperY, 'Y');
+  #endif
+  #if ENABLED(Z_IS_TMC2130)
+    if (reportZ || reportAll) tmc2130_report_otpw(stepperZ, 'Z');
+  #endif
+  #if ENABLED(E0_IS_TMC2130)
+    if (reportE || reportAll) tmc2130_report_otpw(stepperE0, 'E');
+  #endif
+}

Marlin/src/gcode/feature/trinamic/M912.h

@@ -0,0 +1,47 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+static void tmc2130_clear_otpw(TMC2130Stepper &st, const char name) {
+  st.clear_otpw();
+  SERIAL_CHAR(name);
+  SERIAL_ECHOLNPGM(" prewarn flag cleared");
+}
+
+/**
+ * M912: Clear TMC2130 stepper driver overtemperature pre-warn flag held by the library
+ */
+void gcode_M912() {
+  const bool clearX = parser.seen('X'), clearY = parser.seen('Y'), clearZ = parser.seen('Z'), clearE = parser.seen('E'),
+           clearAll = (!clearX && !clearY && !clearZ && !clearE) || (clearX && clearY && clearZ && clearE);
+  #if ENABLED(X_IS_TMC2130)
+    if (clearX || clearAll) tmc2130_clear_otpw(stepperX, 'X');
+  #endif
+  #if ENABLED(Y_IS_TMC2130)
+    if (clearY || clearAll) tmc2130_clear_otpw(stepperY, 'Y');
+  #endif
+  #if ENABLED(Z_IS_TMC2130)
+    if (clearZ || clearAll) tmc2130_clear_otpw(stepperZ, 'Z');
+  #endif
+  #if ENABLED(E0_IS_TMC2130)
+    if (clearE || clearAll) tmc2130_clear_otpw(stepperE0, 'E');
+  #endif
+}

Marlin/src/gcode/feature/trinamic/M913.h

@@ -0,0 +1,57 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+static void tmc2130_get_pwmthrs(TMC2130Stepper &st, const char name, const uint16_t spmm) {
+  SERIAL_CHAR(name);
+  SERIAL_ECHOPGM(" stealthChop max speed set to ");
+  SERIAL_ECHOLN(12650000UL * st.microsteps() / (256 * st.stealth_max_speed() * spmm));
+}
+static void tmc2130_set_pwmthrs(TMC2130Stepper &st, const char name, const int32_t thrs, const uint32_t spmm) {
+  st.stealth_max_speed(12650000UL * st.microsteps() / (256 * thrs * spmm));
+  tmc2130_get_pwmthrs(st, name, spmm);
+}
+
+/**
+ * M913: Set HYBRID_THRESHOLD speed.
+ */
+void gcode_M913() {
+  uint16_t values[XYZE];
+  LOOP_XYZE(i)
+    values[i] = parser.intval(axis_codes[i]);
+
+  #if ENABLED(X_IS_TMC2130)
+    if (values[X_AXIS]) tmc2130_set_pwmthrs(stepperX, 'X', values[X_AXIS], planner.axis_steps_per_mm[X_AXIS]);
+    else tmc2130_get_pwmthrs(stepperX, 'X', planner.axis_steps_per_mm[X_AXIS]);
+  #endif
+  #if ENABLED(Y_IS_TMC2130)
+    if (values[Y_AXIS]) tmc2130_set_pwmthrs(stepperY, 'Y', values[Y_AXIS], planner.axis_steps_per_mm[Y_AXIS]);
+    else tmc2130_get_pwmthrs(stepperY, 'Y', planner.axis_steps_per_mm[Y_AXIS]);
+  #endif
+  #if ENABLED(Z_IS_TMC2130)
+    if (values[Z_AXIS]) tmc2130_set_pwmthrs(stepperZ, 'Z', values[Z_AXIS], planner.axis_steps_per_mm[Z_AXIS]);
+    else tmc2130_get_pwmthrs(stepperZ, 'Z', planner.axis_steps_per_mm[Z_AXIS]);
+  #endif
+  #if ENABLED(E0_IS_TMC2130)
+    if (values[E_AXIS]) tmc2130_set_pwmthrs(stepperE0, 'E', values[E_AXIS], planner.axis_steps_per_mm[E_AXIS]);
+    else tmc2130_get_pwmthrs(stepperE0, 'E', planner.axis_steps_per_mm[E_AXIS]);
+  #endif
+}

Marlin/src/gcode/feature/trinamic/M914.h

@@ -0,0 +1,45 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+static void tmc2130_get_sgt(TMC2130Stepper &st, const char name) {
+  SERIAL_CHAR(name);
+  SERIAL_ECHOPGM(" driver homing sensitivity set to ");
+  SERIAL_ECHOLN(st.sgt());
+}
+static void tmc2130_set_sgt(TMC2130Stepper &st, const char name, const int8_t sgt_val) {
+  st.sgt(sgt_val);
+  tmc2130_get_sgt(st, name);
+}
+
+/**
+ * M914: Set SENSORLESS_HOMING sensitivity.
+ */
+void gcode_M914() {
+  #if ENABLED(X_IS_TMC2130)
+    if (parser.seen(axis_codes[X_AXIS])) tmc2130_set_sgt(stepperX, 'X', parser.value_int());
+    else tmc2130_get_sgt(stepperX, 'X');
+  #endif
+  #if ENABLED(Y_IS_TMC2130)
+    if (parser.seen(axis_codes[Y_AXIS])) tmc2130_set_sgt(stepperY, 'Y', parser.value_int());
+    else tmc2130_get_sgt(stepperY, 'Y');
+  #endif
+}

Marlin/src/gcode/gcode.h

@@ -0,0 +1,682 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * gcode.h - Temporary container for all gcode handlers
+ */
+
+/**
+ * -----------------
+ * G-Codes in Marlin
+ * -----------------
+ *
+ * Helpful G-code references:
+ *  - http://linuxcnc.org/handbook/gcode/g-code.html
+ *  - http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
+ *
+ * Help to document Marlin's G-codes online:
+ *  - http://reprap.org/wiki/G-code
+ *  - https://github.com/MarlinFirmware/MarlinDocumentation
+ *
+ * -----------------
+ *
+ * "G" Codes
+ *
+ * G0   -> G1
+ * G1   - Coordinated Movement X Y Z E
+ * G2   - CW ARC
+ * G3   - CCW ARC
+ * G4   - Dwell S<seconds> or P<milliseconds>
+ * G5   - Cubic B-spline with XYZE destination and IJPQ offsets
+ * G10  - Retract filament according to settings of M207 (Requires FWRETRACT)
+ * G11  - Retract recover filament according to settings of M208 (Requires FWRETRACT)
+ * G12  - Clean tool (Requires NOZZLE_CLEAN_FEATURE)
+ * G17  - Select Plane XY (Requires CNC_WORKSPACE_PLANES)
+ * G18  - Select Plane ZX (Requires CNC_WORKSPACE_PLANES)
+ * G19  - Select Plane YZ (Requires CNC_WORKSPACE_PLANES)
+ * G20  - Set input units to inches (Requires INCH_MODE_SUPPORT)
+ * G21  - Set input units to millimeters (Requires INCH_MODE_SUPPORT)
+ * G26  - Mesh Validation Pattern (Requires UBL_G26_MESH_VALIDATION)
+ * G27  - Park Nozzle (Requires NOZZLE_PARK_FEATURE)
+ * G28  - Home one or more axes
+ * G29  - Start or continue the bed leveling probe procedure (Requires bed leveling)
+ * G30  - Single Z probe, probes bed at X Y location (defaults to current XY location)
+ * G31  - Dock sled (Z_PROBE_SLED only)
+ * G32  - Undock sled (Z_PROBE_SLED only)
+ * G33  - Delta Auto-Calibration (Requires DELTA_AUTO_CALIBRATION)
+ * G38  - Probe in any direction using the Z_MIN_PROBE (Requires G38_PROBE_TARGET)
+ * G42  - Coordinated move to a mesh point (Requires AUTO_BED_LEVELING_UBL)
+ * G90  - Use Absolute Coordinates
+ * G91  - Use Relative Coordinates
+ * G92  - Set current position to coordinates given
+ *
+ * "M" Codes
+ *
+ * M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
+ * M1   -> M0
+ * M3   - Turn laser/spindle on, set spindle/laser speed/power, set rotation to clockwise
+ * M4   - Turn laser/spindle on, set spindle/laser speed/power, set rotation to counter-clockwise
+ * M5   - Turn laser/spindle off
+ * M17  - Enable/Power all stepper motors
+ * M18  - Disable all stepper motors; same as M84
+ * M20  - List SD card. (Requires SDSUPPORT)
+ * M21  - Init SD card. (Requires SDSUPPORT)
+ * M22  - Release SD card. (Requires SDSUPPORT)
+ * M23  - Select SD file: "M23 /path/file.gco". (Requires SDSUPPORT)
+ * M24  - Start/resume SD print. (Requires SDSUPPORT)
+ * M25  - Pause SD print. (Requires SDSUPPORT)
+ * M26  - Set SD position in bytes: "M26 S12345". (Requires SDSUPPORT)
+ * M27  - Report SD print status. (Requires SDSUPPORT)
+ * M28  - Start SD write: "M28 /path/file.gco". (Requires SDSUPPORT)
+ * M29  - Stop SD write. (Requires SDSUPPORT)
+ * M30  - Delete file from SD: "M30 /path/file.gco"
+ * M31  - Report time since last M109 or SD card start to serial.
+ * M32  - Select file and start SD print: "M32 [S<bytepos>] !/path/file.gco#". (Requires SDSUPPORT)
+ *        Use P to run other files as sub-programs: "M32 P !filename#"
+ *        The '#' is necessary when calling from within sd files, as it stops buffer prereading
+ * M33  - Get the longname version of a path. (Requires LONG_FILENAME_HOST_SUPPORT)
+ * M34  - Set SD Card sorting options. (Requires SDCARD_SORT_ALPHA)
+ * M42  - Change pin status via gcode: M42 P<pin> S<value>. LED pin assumed if P is omitted.
+ * M43  - Display pin status, watch pins for changes, watch endstops & toggle LED, Z servo probe test, toggle pins
+ * M48  - Measure Z Probe repeatability: M48 P<points> X<pos> Y<pos> V<level> E<engage> L<legs>. (Requires Z_MIN_PROBE_REPEATABILITY_TEST)
+ * M75  - Start the print job timer.
+ * M76  - Pause the print job timer.
+ * M77  - Stop the print job timer.
+ * M78  - Show statistical information about the print jobs. (Requires PRINTCOUNTER)
+ * M80  - Turn on Power Supply. (Requires POWER_SUPPLY > 0)
+ * M81  - Turn off Power Supply. (Requires POWER_SUPPLY > 0)
+ * M82  - Set E codes absolute (default).
+ * M83  - Set E codes relative while in Absolute (G90) mode.
+ * M84  - Disable steppers until next move, or use S<seconds> to specify an idle
+ *        duration after which steppers should turn off. S0 disables the timeout.
+ * M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
+ * M92  - Set planner.axis_steps_per_mm for one or more axes.
+ * M100 - Watch Free Memory (for debugging) (Requires M100_FREE_MEMORY_WATCHER)
+ * M104 - Set extruder target temp.
+ * M105 - Report current temperatures.
+ * M106 - Fan on.
+ * M107 - Fan off.
+ * M108 - Break out of heating loops (M109, M190, M303). With no controller, breaks out of M0/M1. (Requires EMERGENCY_PARSER)
+ * M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
+ *        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
+ *        If AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
+ * M110 - Set the current line number. (Used by host printing)
+ * M111 - Set debug flags: "M111 S<flagbits>". See flag bits defined in enum.h.
+ * M112 - Emergency stop.
+ * M113 - Get or set the timeout interval for Host Keepalive "busy" messages. (Requires HOST_KEEPALIVE_FEATURE)
+ * M114 - Report current position.
+ * M115 - Report capabilities. (Extended capabilities requires EXTENDED_CAPABILITIES_REPORT)
+ * M117 - Display a message on the controller screen. (Requires an LCD)
+ * M118 - Display a message in the host console.
+ * M119 - Report endstops status.
+ * M120 - Enable endstops detection.
+ * M121 - Disable endstops detection.
+ * M125 - Save current position and move to filament change position. (Requires PARK_HEAD_ON_PAUSE)
+ * M126 - Solenoid Air Valve Open. (Requires BARICUDA)
+ * M127 - Solenoid Air Valve Closed. (Requires BARICUDA)
+ * M128 - EtoP Open. (Requires BARICUDA)
+ * M129 - EtoP Closed. (Requires BARICUDA)
+ * M140 - Set bed target temp. S<temp>
+ * M145 - Set heatup values for materials on the LCD. H<hotend> B<bed> F<fan speed> for S<material> (0=PLA, 1=ABS)
+ * M149 - Set temperature units. (Requires TEMPERATURE_UNITS_SUPPORT)
+ * M150 - Set Status LED Color as R<red> U<green> B<blue>. Values 0-255. (Requires BLINKM, RGB_LED, RGBW_LED, or PCA9632)
+ * M155 - Auto-report temperatures with interval of S<seconds>. (Requires AUTO_REPORT_TEMPERATURES)
+ * M163 - Set a single proportion for a mixing extruder. (Requires MIXING_EXTRUDER)
+ * M164 - Save the mix as a virtual extruder. (Requires MIXING_EXTRUDER and MIXING_VIRTUAL_TOOLS)
+ * M165 - Set the proportions for a mixing extruder. Use parameters ABCDHI to set the mixing factors. (Requires MIXING_EXTRUDER)
+ * M190 - Sxxx Wait for bed current temp to reach target temp. ** Waits only when heating! **
+ *        Rxxx Wait for bed current temp to reach target temp. ** Waits for heating or cooling. **
+ * M200 - Set filament diameter, D<diameter>, setting E axis units to cubic. (Use S0 to revert to linear units.)
+ * M201 - Set max acceleration in units/s^2 for print moves: "M201 X<accel> Y<accel> Z<accel> E<accel>"
+ * M202 - Set max acceleration in units/s^2 for travel moves: "M202 X<accel> Y<accel> Z<accel> E<accel>" ** UNUSED IN MARLIN! **
+ * M203 - Set maximum feedrate: "M203 X<fr> Y<fr> Z<fr> E<fr>" in units/sec.
+ * M204 - Set default acceleration in units/sec^2: P<printing> R<extruder_only> T<travel>
+ * M205 - Set advanced settings. Current units apply:
+            S<print> T<travel> minimum speeds
+            B<minimum segment time>
+            X<max X jerk>, Y<max Y jerk>, Z<max Z jerk>, E<max E jerk>
+ * M206 - Set additional homing offset. (Disabled by NO_WORKSPACE_OFFSETS or DELTA)
+ * M207 - Set Retract Length: S<length>, Feedrate: F<units/min>, and Z lift: Z<distance>. (Requires FWRETRACT)
+ * M208 - Set Recover (unretract) Additional (!) Length: S<length> and Feedrate: F<units/min>. (Requires FWRETRACT)
+ * M209 - Turn Automatic Retract Detection on/off: S<0|1> (For slicers that don't support G10/11). (Requires FWRETRACT)
+          Every normal extrude-only move will be classified as retract depending on the direction.
+ * M211 - Enable, Disable, and/or Report software endstops: S<0|1> (Requires MIN_SOFTWARE_ENDSTOPS or MAX_SOFTWARE_ENDSTOPS)
+ * M218 - Set a tool offset: "M218 T<index> X<offset> Y<offset>". (Requires 2 or more extruders)
+ * M220 - Set Feedrate Percentage: "M220 S<percent>" (i.e., "FR" on the LCD)
+ * M221 - Set Flow Percentage: "M221 S<percent>"
+ * M226 - Wait until a pin is in a given state: "M226 P<pin> S<state>"
+ * M240 - Trigger a camera to take a photograph. (Requires CHDK or PHOTOGRAPH_PIN)
+ * M250 - Set LCD contrast: "M250 C<contrast>" (0-63). (Requires LCD support)
+ * M260 - i2c Send Data (Requires EXPERIMENTAL_I2CBUS)
+ * M261 - i2c Request Data (Requires EXPERIMENTAL_I2CBUS)
+ * M280 - Set servo position absolute: "M280 P<index> S<angle|µs>". (Requires servos)
+ * M300 - Play beep sound S<frequency Hz> P<duration ms>
+ * M301 - Set PID parameters P I and D. (Requires PIDTEMP)
+ * M302 - Allow cold extrudes, or set the minimum extrude S<temperature>. (Requires PREVENT_COLD_EXTRUSION)
+ * M303 - PID relay autotune S<temperature> sets the target temperature. Default 150C. (Requires PIDTEMP)
+ * M304 - Set bed PID parameters P I and D. (Requires PIDTEMPBED)
+ * M350 - Set microstepping mode. (Requires digital microstepping pins.)
+ * M351 - Toggle MS1 MS2 pins directly. (Requires digital microstepping pins.)
+ * M355 - Set Case Light on/off and set brightness. (Requires CASE_LIGHT_PIN)
+ * M380 - Activate solenoid on active extruder. (Requires EXT_SOLENOID)
+ * M381 - Disable all solenoids. (Requires EXT_SOLENOID)
+ * M400 - Finish all moves.
+ * M401 - Lower Z probe. (Requires a probe)
+ * M402 - Raise Z probe. (Requires a probe)
+ * M404 - Display or set the Nominal Filament Width: "W<diameter>". (Requires FILAMENT_WIDTH_SENSOR)
+ * M405 - Enable Filament Sensor flow control. "M405 D<delay_cm>". (Requires FILAMENT_WIDTH_SENSOR)
+ * M406 - Disable Filament Sensor flow control. (Requires FILAMENT_WIDTH_SENSOR)
+ * M407 - Display measured filament diameter in millimeters. (Requires FILAMENT_WIDTH_SENSOR)
+ * M410 - Quickstop. Abort all planned moves.
+ * M420 - Enable/Disable Leveling (with current values) S1=enable S0=disable (Requires MESH_BED_LEVELING or ABL)
+ * M421 - Set a single Z coordinate in the Mesh Leveling grid. X<units> Y<units> Z<units> (Requires MESH_BED_LEVELING or AUTO_BED_LEVELING_UBL)
+ * M428 - Set the home_offset based on the current_position. Nearest edge applies. (Disabled by NO_WORKSPACE_OFFSETS or DELTA)
+ * M500 - Store parameters in EEPROM. (Requires EEPROM_SETTINGS)
+ * M501 - Restore parameters from EEPROM. (Requires EEPROM_SETTINGS)
+ * M502 - Revert to the default "factory settings". ** Does not write them to EEPROM! **
+ * M503 - Print the current settings (in memory): "M503 S<verbose>". S0 specifies compact output.
+ * M540 - Enable/disable SD card abort on endstop hit: "M540 S<state>". (Requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+ * M600 - Pause for filament change: "M600 X<pos> Y<pos> Z<raise> E<first_retract> L<later_retract>". (Requires ADVANCED_PAUSE_FEATURE)
+ * M665 - Set delta configurations: "M665 L<diagonal rod> R<delta radius> S<segments/s> A<rod A trim mm> B<rod B trim mm> C<rod C trim mm> I<tower A trim angle> J<tower B trim angle> K<tower C trim angle>" (Requires DELTA)
+ * M666 - Set delta endstop adjustment. (Requires DELTA)
+ * M605 - Set dual x-carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
+ * M851 - Set Z probe's Z offset in current units. (Negative = below the nozzle.)
+ * M860 - Report the position of position encoder modules.
+ * M861 - Report the status of position encoder modules.
+ * M862 - Perform an axis continuity test for position encoder modules.
+ * M863 - Perform steps-per-mm calibration for position encoder modules.
+ * M864 - Change position encoder module I2C address.
+ * M865 - Check position encoder module firmware version.
+ * M866 - Report or reset position encoder module error count.
+ * M867 - Enable/disable or toggle error correction for position encoder modules.
+ * M868 - Report or set position encoder module error correction threshold.
+ * M869 - Report position encoder module error.
+ * M900 - Get and/or Set advance K factor and WH/D ratio. (Requires LIN_ADVANCE)
+ * M906 - Set or get motor current in milliamps using axis codes X, Y, Z, E. Report values if no axis codes given. (Requires HAVE_TMC2130)
+ * M907 - Set digital trimpot motor current using axis codes. (Requires a board with digital trimpots)
+ * M908 - Control digital trimpot directly. (Requires DAC_STEPPER_CURRENT or DIGIPOTSS_PIN)
+ * M909 - Print digipot/DAC current value. (Requires DAC_STEPPER_CURRENT)
+ * M910 - Commit digipot/DAC value to external EEPROM via I2C. (Requires DAC_STEPPER_CURRENT)
+ * M911 - Report stepper driver overtemperature pre-warn condition. (Requires HAVE_TMC2130)
+ * M912 - Clear stepper driver overtemperature pre-warn condition flag. (Requires HAVE_TMC2130)
+ * M913 - Set HYBRID_THRESHOLD speed. (Requires HYBRID_THRESHOLD)
+ * M914 - Set SENSORLESS_HOMING sensitivity. (Requires SENSORLESS_HOMING)
+ *
+ * M360 - SCARA calibration: Move to cal-position ThetaA (0 deg calibration)
+ * M361 - SCARA calibration: Move to cal-position ThetaB (90 deg calibration - steps per degree)
+ * M362 - SCARA calibration: Move to cal-position PsiA (0 deg calibration)
+ * M363 - SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
+ * M364 - SCARA calibration: Move to cal-position PSIC (90 deg to Theta calibration position)
+ *
+ * ************ Custom codes - This can change to suit future G-code regulations
+ * M928 - Start SD logging: "M928 filename.gco". Stop with M29. (Requires SDSUPPORT)
+ * M999 - Restart after being stopped by error
+ *
+ * "T" Codes
+ *
+ * T0-T3 - Select an extruder (tool) by index: "T<n> F<units/min>"
+ *
+ */
+
+#ifndef GCODE_H
+#define GCODE_H
+
+#include "../inc/MarlinConfig.h"
+#include "parser.h"
+
+#if ENABLED(I2C_POSITION_ENCODERS)
+  #include "../feature/I2CPositionEncoder.h"
+#endif
+
+class GcodeSuite {
+public:
+
+  GcodeSuite() {}
+
+private:
+
+  static void G0_G1(
+    #if IS_SCARA
+      bool fast_move=false
+    #endif
+  );
+
+  #if ENABLED(ARC_SUPPORT)
+    static void G2_G3(bool clockwise);
+  #endif
+
+  static void G4();
+
+  #if ENABLED(BEZIER_CURVE_SUPPORT)
+    static void G5();
+  #endif
+
+  #if ENABLED(FWRETRACT)
+    static void G10();
+    static void G11();
+  #endif
+
+  #if ENABLED(NOZZLE_CLEAN_FEATURE)
+    static void G12();
+  #endif
+
+  #if ENABLED(CNC_WORKSPACE_PLANES)
+    static void G17();
+    static void G18();
+    static void G19();
+  #endif
+
+  #if ENABLED(INCH_MODE_SUPPORT)
+    static void G20();
+    static void G21();
+  #endif
+
+  #if ENABLED(UBL_G26_MESH_VALIDATION)
+    static void G26();
+  #endif
+
+  #if ENABLED(NOZZLE_PARK_FEATURE)
+    static void G27();
+  #endif
+
+  static void G28(const bool always_home_all);
+
+  #if HAS_LEVELING
+    static void G29();
+  #endif
+
+  #if HAS_BED_PROBE
+    static void G30();
+    #if ENABLED(Z_PROBE_SLED)
+      static void G31();
+      static void G32();
+    #endif
+  #endif
+
+  #if PROBE_SELECTED && ENABLED(DELTA_AUTO_CALIBRATION)
+    static void G33();
+  #endif
+
+  #if ENABLED(G38_PROBE_TARGET)
+    static void G38(bool is_38_2);
+  #endif
+
+  #if HAS_MESH
+    static void G42();
+  #endif
+
+  static void G92();
+
+  #if HAS_RESUME_CONTINUE
+    static void M0_M1();
+  #endif
+
+  #if ENABLED(SPINDLE_LASER_ENABLE)
+    static void M3_M4(bool is_M3);
+    static void M5();
+  #endif
+
+  static void M17();
+
+  static void M18_M84();
+
+  #if ENABLED(SDSUPPORT)
+    static void M20();
+    static void M21();
+    static void M22();
+    static void M23();
+    static void M24();
+    static void M25();
+    static void M26();
+    static void M27();
+    static void M28();
+    static void M29();
+    static void M30();
+  #endif
+
+  static void M31();
+
+  #if ENABLED(SDSUPPORT)
+    static void M32();
+    #if ENABLED(LONG_FILENAME_HOST_SUPPORT)
+      static void M33();
+    #endif
+    #if ENABLED(SDCARD_SORT_ALPHA) && ENABLED(SDSORT_GCODE)
+      static void M34();
+    #endif
+  #endif
+
+  static void M42();
+
+  #if ENABLED(PINS_DEBUGGING)
+    static void M43();
+  #endif
+
+  #if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
+    static void M48();
+  #endif
+
+  #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_G26_MESH_VALIDATION)
+    static void M49();
+  #endif
+
+  static void M75();
+  static void M76();
+  static void M77();
+
+  #if ENABLED(PRINTCOUNTER)
+    static void M78();
+  #endif
+
+  #if HAS_POWER_SWITCH
+    static void M80();
+  #endif
+
+  static void M81();
+  static void M82();
+  static void M83();
+  static void M85();
+  static void M92();
+
+  #if ENABLED(M100_FREE_MEMORY_WATCHER)
+    static void M100();
+  #endif
+
+  static void M104();
+  static void M105();
+  static void M106();
+  static void M107();
+
+  #if DISABLED(EMERGENCY_PARSER)
+    static void M108();
+  #endif
+
+  static void M109();
+
+  static void M110();
+  static void M111();
+
+  #if DISABLED(EMERGENCY_PARSER)
+    static void M112();
+  #endif
+
+  #if ENABLED(HOST_KEEPALIVE_FEATURE)
+    static void M113();
+  #endif
+
+  static void M114();
+  static void M115();
+  static void M117();
+  static void M118();
+  static void M119();
+  static void M120();
+  static void M121();
+
+  #if ENABLED(PARK_HEAD_ON_PAUSE)
+    static void M125();
+  #endif
+
+  #if ENABLED(BARICUDA)
+    #if HAS_HEATER_1
+      static void M126();
+      static void M127();
+    #endif
+    #if HAS_HEATER_2
+      static void M128();
+      static void M129();
+    #endif
+  #endif
+
+  #if HAS_TEMP_BED
+    static void M140();
+  #endif
+
+  #if ENABLED(ULTIPANEL)
+    static void M145();
+  #endif
+
+  #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+    static void M149();
+  #endif
+
+  #if HAS_COLOR_LEDS
+    static void M150();
+  #endif
+
+  #if ENABLED(AUTO_REPORT_TEMPERATURES) && (HAS_TEMP_HOTEND || HAS_TEMP_BED)
+    static void M155();
+  #endif
+
+  #if ENABLED(MIXING_EXTRUDER)
+    static void M163();
+    #if MIXING_VIRTUAL_TOOLS > 1
+      static void M164();
+    #endif
+    #if ENABLED(DIRECT_MIXING_IN_G1)
+      static void M165();
+    #endif
+  #endif
+
+  #if HAS_TEMP_BED
+    static void M190();
+  #endif
+
+  static void M200();
+  static void M201();
+
+  #if 0
+    static void M202(); // Not used for Sprinter/grbl gen6
+  #endif
+
+  static void M203();
+  static void M204();
+  static void M205();
+
+  #if HAS_M206_COMMAND
+    static void M206();
+  #endif
+
+  #if ENABLED(FWRETRACT)
+    static void M207();
+    static void M208();
+    static void M209();
+  #endif
+
+  static void M211();
+
+  #if HOTENDS > 1
+    static void M218();
+  #endif
+
+  static void M220();
+  static void M221();
+  static void M226();
+
+  #if defined(CHDK) || HAS_PHOTOGRAPH
+    static void M240();
+  #endif
+
+  #if HAS_LCD_CONTRAST
+    static void M250();
+  #endif
+
+  #if ENABLED(EXPERIMENTAL_I2CBUS)
+    static void M260();
+    static void M261();
+  #endif
+
+  #if HAS_SERVOS
+    static void M280();
+  #endif
+
+  #if HAS_BUZZER
+    static void M300();
+  #endif
+
+  #if ENABLED(PIDTEMP)
+    static void M301();
+  #endif
+
+  #if ENABLED(PREVENT_COLD_EXTRUSION)
+    static void M302();
+  #endif
+
+  static void M303();
+
+  #if ENABLED(PIDTEMPBED)
+    static void M304();
+  #endif
+
+  #if HAS_MICROSTEPS
+    static void M350();
+    static void M351();
+  #endif
+
+  static void M355();
+
+  #if ENABLED(MORGAN_SCARA)
+    static bool M360();
+    static bool M361();
+    static bool M362();
+    static bool M363();
+    static bool M364();
+  #endif
+
+  #if ENABLED(EXT_SOLENOID)
+    static void M380();
+    static void M381();
+  #endif
+
+  static void M400();
+
+  #if HAS_BED_PROBE
+    static void M401();
+    static void M402();
+  #endif
+
+  #if ENABLED(FILAMENT_WIDTH_SENSOR)
+    static void M404();
+    static void M405();
+    static void M406();
+    static void M407();
+  #endif
+
+  #if DISABLED(EMERGENCY_PARSER)
+    static void M410();
+  #endif
+
+  #if HAS_LEVELING
+    static void M420();
+    static void M421();
+  #endif
+
+  #if HAS_M206_COMMAND
+    static void M428();
+  #endif
+
+  static void M500();
+  static void M501();
+  static void M502();
+  #if DISABLED(DISABLE_M503)
+    static void M503();
+  #endif
+
+  #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+    static void M540();
+  #endif
+
+  #if ENABLED(ADVANCED_PAUSE_FEATURE)
+    static void M600();
+  #endif
+
+  #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
+    static void M605();
+  #endif
+
+  #if IS_KINEMATIC
+    static void M665();
+  #endif
+
+  #if ENABLED(DELTA) || ENABLED(Z_DUAL_ENDSTOPS)
+    static void M666();
+  #endif
+
+  #if ENABLED(MK2_MULTIPLEXER)
+    static void M702();
+  #endif
+
+  #if HAS_BED_PROBE
+    static void M851();
+  #endif
+
+  #if ENABLED(I2C_POSITION_ENCODERS)
+    FORCE_INLINE static void M860() { I2CPEM.M860(); }
+    FORCE_INLINE static void M861() { I2CPEM.M861(); }
+    FORCE_INLINE static void M862() { I2CPEM.M862(); }
+    FORCE_INLINE static void M863() { I2CPEM.M863(); }
+    FORCE_INLINE static void M864() { I2CPEM.M864(); }
+    FORCE_INLINE static void M865() { I2CPEM.M865(); }
+    FORCE_INLINE static void M866() { I2CPEM.M866(); }
+    FORCE_INLINE static void M867() { I2CPEM.M867(); }
+    FORCE_INLINE static void M868() { I2CPEM.M868(); }
+    FORCE_INLINE static void M869() { I2CPEM.M869(); }
+  #endif
+
+  #if ENABLED(LIN_ADVANCE)
+    static void M900();
+  #endif
+
+  #if ENABLED(HAVE_TMC2130)
+    static void M906();
+    static void M911();
+    static void M912();
+    #if ENABLED(HYBRID_THRESHOLD)
+      static void M913();
+    #endif
+    #if ENABLED(SENSORLESS_HOMING)
+      static void M914();
+    #endif
+  #endif
+
+  static void M907();
+
+  #if HAS_DIGIPOTSS || ENABLED(DAC_STEPPER_CURRENT)
+    static void M908();
+    #if ENABLED(DAC_STEPPER_CURRENT)
+      static void M909();
+      static void M910();
+    #endif
+  #endif
+
+  #if ENABLED(SDSUPPORT)
+    static void M928();
+  #endif
+
+  static void M999();
+
+  static void T(uint8_t tmp_extruder);
+
+};
+
+extern GcodeSuite gcode;
+
+#endif // GCODE_H

Marlin/src/gcode/geometry/G17-G19.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+void report_workspace_plane() {
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPGM("Workspace Plane ");
+  serialprintPGM(workspace_plane == PLANE_YZ ? PSTR("YZ\n") : workspace_plane == PLANE_ZX ? PSTR("ZX\n") : PSTR("XY\n"));
+}
+
+/**
+ * G17: Select Plane XY
+ * G18: Select Plane ZX
+ * G19: Select Plane YZ
+ */
+void gcode_G17() { workspace_plane = PLANE_XY; }
+void gcode_G18() { workspace_plane = PLANE_ZX; }
+void gcode_G19() { workspace_plane = PLANE_YZ; }

Marlin/src/gcode/geometry/G92.h

@@ -0,0 +1,66 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * G92: Set current position to given X Y Z E
+ */
+void gcode_G92() {
+  bool didXYZ = false,
+       didE = parser.seenval('E');
+
+  if (!didE) stepper.synchronize();
+
+  LOOP_XYZE(i) {
+    if (parser.seenval(axis_codes[i])) {
+      #if IS_SCARA
+        current_position[i] = parser.value_axis_units((AxisEnum)i);
+        if (i != E_AXIS) didXYZ = true;
+      #else
+        #if HAS_POSITION_SHIFT
+          const float p = current_position[i];
+        #endif
+        const float v = parser.value_axis_units((AxisEnum)i);
+
+        current_position[i] = v;
+
+        if (i != E_AXIS) {
+          didXYZ = true;
+          #if HAS_POSITION_SHIFT
+            position_shift[i] += v - p; // Offset the coordinate space
+            update_software_endstops((AxisEnum)i);
+
+            #if ENABLED(I2C_POSITION_ENCODERS)
+              I2CPEM.encoders[I2CPEM.idx_from_axis((AxisEnum)i)].set_axis_offset(position_shift[i]);
+            #endif
+
+          #endif
+        }
+      #endif
+    }
+  }
+  if (didXYZ)
+    SYNC_PLAN_POSITION_KINEMATIC();
+  else if (didE)
+    sync_plan_position_e();
+
+  report_current_position();
+}

Marlin/src/gcode/geometry/M206.h

@@ -0,0 +1,42 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y
+ *
+ * *** @thinkyhead: I recommend deprecating M206 for SCARA in favor of M665.
+ * ***              M206 for SCARA will remain enabled in 1.1.x for compatibility.
+ * ***              In the 2.0 release, it will simply be disabled by default.
+ */
+void gcode_M206() {
+  LOOP_XYZ(i)
+    if (parser.seen(axis_codes[i]))
+      set_home_offset((AxisEnum)i, parser.value_linear_units());
+
+  #if ENABLED(MORGAN_SCARA)
+    if (parser.seen('T')) set_home_offset(A_AXIS, parser.value_linear_units()); // Theta
+    if (parser.seen('P')) set_home_offset(B_AXIS, parser.value_linear_units()); // Psi
+  #endif
+
+  SYNC_PLAN_POSITION_KINEMATIC();
+  report_current_position();
+}

Marlin/src/gcode/geometry/M428.h

@@ -0,0 +1,61 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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/>.
+ *
+ */
+
+/**
+ * M428: Set home_offset based on the distance between the
+ *       current_position and the nearest "reference point."
+ *       If an axis is past center its endstop position
+ *       is the reference-point. Otherwise it uses 0. This allows
+ *       the Z offset to be set near the bed when using a max endstop.
+ *
+ *       M428 can't be used more than 2cm away from 0 or an endstop.
+ *
+ *       Use M206 to set these values directly.
+ */
+void gcode_M428() {
+  bool err = false;
+  LOOP_XYZ(i) {
+    if (axis_homed[i]) {
+      const float base = (current_position[i] > (soft_endstop_min[i] + soft_endstop_max[i]) * 0.5) ? base_home_pos((AxisEnum)i) : 0,
+                  diff = base - RAW_POSITION(current_position[i], i);
+      if (WITHIN(diff, -20, 20)) {
+        set_home_offset((AxisEnum)i, diff);
+      }
+      else {
+        SERIAL_ERROR_START();
+        SERIAL_ERRORLNPGM(MSG_ERR_M428_TOO_FAR);
+        LCD_ALERTMESSAGEPGM("Err: Too far!");
+        BUZZ(200, 40);
+        err = true;
+        break;
+      }
+    }
+  }
+
+  if (!err) {
+    SYNC_PLAN_POSITION_KINEMATIC();
+    report_current_position();
+    LCD_MESSAGEPGM(MSG_HOME_OFFSETS_APPLIED);
+    BUZZ(100, 659);
+    BUZZ(100, 698);
+  }
+}