Add gcode.cpp, motion.*, queue.* - Apply to some G-codes.

Marlin/src/Marlin.h

@@ -167,9 +167,6 @@ void enable_all_steppers();
 void disable_e_steppers();
 void disable_all_steppers();
 
-void FlushSerialRequestResend();
-void ok_to_send();
-
 void kill(const char*);
 
 void quickstop_stepper();
@@ -182,13 +179,6 @@ extern bool Running;
 inline bool IsRunning() { return  Running; }
 inline bool IsStopped() { return !Running; }
 
-bool enqueue_and_echo_command(const char* cmd, bool say_ok=false); // Add a single command to the end of the buffer. Return false on failure.
-void enqueue_and_echo_commands_P(const char * const cmd);          // Set one or more commands to be prioritized over the next Serial/SD command.
-void clear_command_queue();
-
-extern millis_t previous_cmd_ms;
-inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
-
 /**
  * Feedrate scaling and conversion
  */
@@ -196,11 +186,11 @@ extern int16_t feedrate_percentage;
 
 #define MMS_SCALED(MM_S) ((MM_S)*feedrate_percentage*0.01)
 
-extern bool axis_relative_modes[];
 extern bool volumetric_enabled;
 extern int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder
 extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
 extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
+
 extern bool axis_known_position[XYZ];
 extern bool axis_homed[XYZ];
 extern volatile bool wait_for_heatup;
@@ -209,48 +199,6 @@ extern volatile bool wait_for_heatup;
   extern volatile bool wait_for_user;
 #endif
 
-extern float current_position[NUM_AXIS];
-
-// Workspace offsets
-#if HAS_WORKSPACE_OFFSET
-  #if HAS_HOME_OFFSET
-    extern float home_offset[XYZ];
-  #endif
-  #if HAS_POSITION_SHIFT
-    extern float position_shift[XYZ];
-  #endif
-#endif
-
-#if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
-  extern float workspace_offset[XYZ];
-  #define WORKSPACE_OFFSET(AXIS) workspace_offset[AXIS]
-#elif HAS_HOME_OFFSET
-  #define WORKSPACE_OFFSET(AXIS) home_offset[AXIS]
-#elif HAS_POSITION_SHIFT
-  #define WORKSPACE_OFFSET(AXIS) position_shift[AXIS]
-#else
-  #define WORKSPACE_OFFSET(AXIS) 0
-#endif
-
-#define LOGICAL_POSITION(POS, AXIS) ((POS) + WORKSPACE_OFFSET(AXIS))
-#define RAW_POSITION(POS, AXIS)     ((POS) - WORKSPACE_OFFSET(AXIS))
-
-#if HAS_POSITION_SHIFT || DISABLED(DELTA)
-  #define LOGICAL_X_POSITION(POS)   LOGICAL_POSITION(POS, X_AXIS)
-  #define LOGICAL_Y_POSITION(POS)   LOGICAL_POSITION(POS, Y_AXIS)
-  #define RAW_X_POSITION(POS)       RAW_POSITION(POS, X_AXIS)
-  #define RAW_Y_POSITION(POS)       RAW_POSITION(POS, Y_AXIS)
-#else
-  #define LOGICAL_X_POSITION(POS)   (POS)
-  #define LOGICAL_Y_POSITION(POS)   (POS)
-  #define RAW_X_POSITION(POS)       (POS)
-  #define RAW_Y_POSITION(POS)       (POS)
-#endif
-
-#define LOGICAL_Z_POSITION(POS)     LOGICAL_POSITION(POS, Z_AXIS)
-#define RAW_Z_POSITION(POS)         RAW_POSITION(POS, Z_AXIS)
-#define RAW_CURRENT_POSITION(A)     RAW_##A##_POSITION(current_position[A##_AXIS])
-
 // Hotend Offsets
 #if HOTENDS > 1
   extern float hotend_offset[XYZ][HOTENDS];
@@ -259,14 +207,6 @@ extern float current_position[NUM_AXIS];
 // Software Endstops
 extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ];
 
-#if HAS_SOFTWARE_ENDSTOPS
-  extern bool soft_endstops_enabled;
-  void clamp_to_software_endstops(float target[XYZ]);
-#else
-  #define soft_endstops_enabled false
-  #define clamp_to_software_endstops(x) NOOP
-#endif
-
 #if HAS_WORKSPACE_OFFSET || ENABLED(DUAL_X_CARRIAGE)
   void update_software_endstops(const AxisEnum axis);
 #endif
@@ -381,15 +321,15 @@ extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ];
   extern Stopwatch print_job_timer;
 #endif
 
-// Handling multiple extruders pins
-extern uint8_t active_extruder;
-
 #if HAS_TEMP_HOTEND || HAS_TEMP_BED
   void print_heaterstates();
 #endif
 
 #if ENABLED(MIXING_EXTRUDER)
   extern float mixing_factor[MIXING_STEPPERS];
+  #if MIXING_VIRTUAL_TOOLS > 1
+    extern float mixing_virtual_tool_mix[MIXING_VIRTUAL_TOOLS][MIXING_STEPPERS];
+  #endif
 #endif
 
 void calculate_volumetric_multipliers();
@@ -406,62 +346,4 @@ void do_blocking_move_to_xy(const float &x, const float &y, const float &fr_mm_s
   bool axis_unhomed_error(const bool x=true, const bool y=true, const bool z=true);
 #endif
 
-/**
- * position_is_reachable family of functions
- */
-
-#if IS_KINEMATIC // (DELTA or SCARA)
-
-  #if IS_SCARA
-    extern const float L1, L2;
-  #endif
-
-  inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) {
-    #if ENABLED(DELTA)
-      return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS);
-    #elif IS_SCARA
-      #if MIDDLE_DEAD_ZONE_R > 0
-        const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y);
-        return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2);
-      #else
-        return HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y) <= sq(L1 + L2);
-      #endif
-    #else // CARTESIAN
-      // To be migrated from MakerArm branch in future
-    #endif
-  }
-
-  inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) {
-
-    // Both the nozzle and the probe must be able to reach the point.
-    // This won't work on SCARA since the probe offset rotates with the arm.
-
-    return position_is_reachable_raw_xy(rx, ry)
-        && position_is_reachable_raw_xy(rx - X_PROBE_OFFSET_FROM_EXTRUDER, ry - Y_PROBE_OFFSET_FROM_EXTRUDER);
-  }
-
-#else // CARTESIAN
-
-  inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) {
-      // Add 0.001 margin to deal with float imprecision
-      return WITHIN(rx, X_MIN_POS - 0.001, X_MAX_POS + 0.001)
-          && WITHIN(ry, Y_MIN_POS - 0.001, Y_MAX_POS + 0.001);
-  }
-
-  inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) {
-      // Add 0.001 margin to deal with float imprecision
-      return WITHIN(rx, MIN_PROBE_X - 0.001, MAX_PROBE_X + 0.001)
-          && WITHIN(ry, MIN_PROBE_Y - 0.001, MAX_PROBE_Y + 0.001);
-  }
-
-#endif // CARTESIAN
-
-FORCE_INLINE bool position_is_reachable_by_probe_xy(const float &lx, const float &ly) {
-  return position_is_reachable_by_probe_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
-}
-
-FORCE_INLINE bool position_is_reachable_xy(const float &lx, const float &ly) {
-  return position_is_reachable_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
-}
-
 #endif // __MARLIN_H__

Marlin/src/core/utility.h

@@ -25,6 +25,8 @@
 
 #include "../inc/MarlinConfig.h"
 
+constexpr char axis_codes[XYZE] = { 'X', 'Y', 'Z', 'E' };
+
 void safe_delay(millis_t ms);
 
 #if ENABLED(EEPROM_SETTINGS)

Marlin/src/feature/mbl/mesh_bed_leveling.cpp

@@ -26,6 +26,8 @@
 
   #include "mesh_bed_leveling.h"
 
+  #include "../../module/motion.h"
+
   mesh_bed_leveling mbl;
 
   uint8_t mesh_bed_leveling::status;
@@ -49,4 +51,63 @@
     ZERO(z_values);
   }
 
+  /**
+   * Prepare a mesh-leveled linear move in a Cartesian setup,
+   * splitting the move where it crosses mesh borders.
+   */
+  void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
+    int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X)),
+        cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y)),
+        cx2 = mbl.cell_index_x(RAW_X_POSITION(destination[X_AXIS])),
+        cy2 = mbl.cell_index_y(RAW_Y_POSITION(destination[Y_AXIS]));
+    NOMORE(cx1, GRID_MAX_POINTS_X - 2);
+    NOMORE(cy1, GRID_MAX_POINTS_Y - 2);
+    NOMORE(cx2, GRID_MAX_POINTS_X - 2);
+    NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
+
+    if (cx1 == cx2 && cy1 == cy2) {
+      // Start and end on same mesh square
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
+
+    float normalized_dist, end[XYZE];
+
+    // Split at the left/front border of the right/top square
+    const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+    if (cx2 != cx1 && TEST(x_splits, gcx)) {
+      COPY(end, destination);
+      destination[X_AXIS] = LOGICAL_X_POSITION(mbl.index_to_xpos[gcx]);
+      normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
+      destination[Y_AXIS] = MBL_SEGMENT_END(Y);
+      CBI(x_splits, gcx);
+    }
+    else if (cy2 != cy1 && TEST(y_splits, gcy)) {
+      COPY(end, destination);
+      destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.index_to_ypos[gcy]);
+      normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
+      destination[X_AXIS] = MBL_SEGMENT_END(X);
+      CBI(y_splits, gcy);
+    }
+    else {
+      // Already split on a border
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    destination[Z_AXIS] = MBL_SEGMENT_END(Z);
+    destination[E_AXIS] = MBL_SEGMENT_END(E);
+
+    // Do the split and look for more borders
+    mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
+
+    // Restore destination from stack
+    COPY(destination, end);
+    mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
+  }
+
 #endif // MESH_BED_LEVELING

Marlin/src/feature/mbl/mesh_bed_leveling.h

@@ -120,4 +120,6 @@ public:
 
 extern mesh_bed_leveling mbl;
 
+void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF);
+
 #endif // _MESH_BED_LEVELING_H_

Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp

@@ -33,6 +33,7 @@
   #include "../../Marlin.h"
   #include "../../module/planner.h"
   #include "../../module/stepper.h"
+  #include "../../module/motion.h"
   #include "../../module/temperature.h"
   #include "../../lcd/ultralcd.h"
   #include "../../gcode/parser.h"
@@ -129,7 +130,6 @@
 
   // External references
 
-  extern float feedrate_mm_s; // must set before calling prepare_move_to_destination
   extern Planner planner;
   #if ENABLED(ULTRA_LCD)
     extern char lcd_status_message[];

Marlin/src/feature/ubl/ubl.cpp

@@ -30,6 +30,7 @@
   #include "../../module/configuration_store.h"
   #include "../../core/serial.h"
   #include "../../module/planner.h"
+  #include "../../module/motion.h"
 
   #include "math.h"
 

Marlin/src/feature/ubl/ubl_motion.cpp

@@ -28,6 +28,7 @@
   #include "../../Marlin.h"
   #include "../../module/planner.h"
   #include "../../module/stepper.h"
+  #include "../../module/motion.h"
 
   #include <math.h>
 

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

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../queue.h"
+
 #include "../../libs/buzzer.h"
 #include "../../lcd/ultralcd.h"
 

Marlin/src/gcode/config/M200.h → Marlin/src/gcode/config/M200.cpp

@@ -20,15 +20,18 @@
  *
  */
 
+#include "../gcode.h"
+#include "../../Marlin.h"
+
 /**
  * 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() {
+void GcodeSuite::M200() {
 
-  if (get_target_extruder_from_command(200)) return;
+  if (get_target_extruder_from_command()) return;
 
   if (parser.seen('D')) {
     // setting any extruder filament size disables volumetric on the assumption that

Marlin/src/gcode/config/M201.h

@@ -27,7 +27,7 @@
  */
 void gcode_M201() {
 
-  GET_TARGET_EXTRUDER(201);
+  GET_TARGET_EXTRUDER();
 
   LOOP_XYZE(i) {
     if (parser.seen(axis_codes[i])) {

Marlin/src/gcode/config/M203.h

@@ -27,7 +27,7 @@
  */
 void gcode_M203() {
 
-  GET_TARGET_EXTRUDER(203);
+  GET_TARGET_EXTRUDER();
 
   LOOP_XYZE(i)
     if (parser.seen(axis_codes[i])) {

Marlin/src/gcode/config/M218.h → Marlin/src/gcode/config/M218.cpp

@@ -20,6 +20,13 @@
  *
  */
 
+#include "../../inc/MarlinConfig.h"
+
+#if HOTENDS > 1
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
 /**
  * M218 - set hotend offset (in linear units)
  *
@@ -28,8 +35,8 @@
  *   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;
+void GcodeSuite::M218() {
+  if (get_target_extruder_from_command() || 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();
@@ -52,3 +59,5 @@ void gcode_M218() {
   }
   SERIAL_EOL();
 }
+
+#endif // HOTENDS > 1

Marlin/src/gcode/config/M221.h → Marlin/src/gcode/config/M221.cpp

@@ -20,11 +20,14 @@
  *
  */
 
+#include "../gcode.h"
+#include "../../Marlin.h"
+
 /**
  * M221: Set extrusion percentage (M221 T0 S95)
  */
-void gcode_M221() {
-  if (get_target_extruder_from_command(221)) return;
+void GcodeSuite::M221() {
+  if (get_target_extruder_from_command()) return;
   if (parser.seenval('S'))
     flow_percentage[target_extruder] = parser.value_int();
 }

Marlin/src/gcode/config/M43.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../gcode.h"
+
 #include "../../pins/pinsDebug.h"
 
 inline void toggle_pins() {
@@ -141,7 +143,7 @@ inline void servo_probe_test() {
     }
     if (probe_inverting != deploy_state) SERIAL_PROTOCOLLNPGM("WARNING - INVERTING setting probably backwards");
 
-    refresh_cmd_timeout();
+    gcode.refresh_cmd_timeout();
 
     if (deploy_state != stow_state) {
       SERIAL_PROTOCOLLNPGM("BLTouch clone detected");
@@ -170,7 +172,7 @@ inline void servo_probe_test() {
         safe_delay(2);
 
         if (0 == j % (500 * 1)) // keep cmd_timeout happy
-          refresh_cmd_timeout();
+          gcode.refresh_cmd_timeout();
 
         if (deploy_state != READ(PROBE_TEST_PIN)) { // probe triggered
 

Marlin/src/gcode/config/M92.h

@@ -28,7 +28,7 @@
  */
 void gcode_M92() {
 
-  GET_TARGET_EXTRUDER(92);
+  GET_TARGET_EXTRUDER();
 
   LOOP_XYZE(i) {
     if (parser.seen(axis_codes[i])) {

Marlin/src/gcode/control/M999.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../queue.h"
+
 /**
  * M999: Restart after being stopped
  *
@@ -37,5 +39,5 @@ void gcode_M999() {
   if (parser.boolval('S')) return;
 
   // gcode_LastN = Stopped_gcode_LastN;
-  FlushSerialRequestResend();
+  flush_and_request_resend();
 }

Marlin/src/gcode/process_next_command.h → Marlin/src/gcode/gcode.cpp

@@ -21,10 +21,242 @@
  */
 
 /**
+ * gcode.cpp - Temporary container for all gcode handlers
+ *             Most will migrate to classes, by feature.
+ */
+
+#include "gcode.h"
+GcodeSuite gcode;
+
+#include "parser.h"
+#include "queue.h"
+#include "../module/motion.h"
+
+#if ENABLED(PRINTCOUNTER)
+  #include "../module/printcounter.h"
+#endif
+
+uint8_t GcodeSuite::target_extruder;
+millis_t GcodeSuite::previous_cmd_ms;
+
+bool GcodeSuite::axis_relative_modes[] = AXIS_RELATIVE_MODES;
+
+/**
+ * Set target_extruder from the T parameter or the active_extruder
+ *
+ * Returns TRUE if the target is invalid
+ */
+bool GcodeSuite::get_target_extruder_from_command() {
+  if (parser.seenval('T')) {
+    const int8_t e = parser.value_byte();
+    if (e >= EXTRUDERS) {
+      SERIAL_ECHO_START();
+      SERIAL_CHAR('M');
+      SERIAL_ECHO(parser.codenum);
+      SERIAL_ECHOLNPAIR(" " MSG_INVALID_EXTRUDER " ", e);
+      return true;
+    }
+    target_extruder = e;
+  }
+  else
+    target_extruder = active_extruder;
+
+  return false;
+}
+
+/**
+ * Set XYZE destination and feedrate from the current GCode command
+ *
+ *  - Set destination from included axis codes
+ *  - Set to current for missing axis codes
+ *  - Set the feedrate, if included
+ */
+void GcodeSuite::get_destination_from_command() {
+  LOOP_XYZE(i) {
+    if (parser.seen(axis_codes[i]))
+      destination[i] = parser.value_axis_units((AxisEnum)i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0);
+    else
+      destination[i] = current_position[i];
+  }
+
+  if (parser.linearval('F') > 0.0)
+    feedrate_mm_s = MMM_TO_MMS(parser.value_feedrate());
+
+  #if ENABLED(PRINTCOUNTER)
+    if (!DEBUGGING(DRYRUN))
+      print_job_timer.incFilamentUsed(destination[E_AXIS] - current_position[E_AXIS]);
+  #endif
+
+  // Get ABCDHI mixing factors
+  #if ENABLED(MIXING_EXTRUDER) && ENABLED(DIRECT_MIXING_IN_G1)
+    gcode_get_mix();
+  #endif
+}
+
+//
+// Placeholders for non-migrated codes
+//
+extern void gcode_G0_G1(
+  #if IS_SCARA
+    bool fast_move=false
+  #endif
+);
+extern void gcode_G2_G3(bool clockwise);
+extern void gcode_G4();
+extern void gcode_G5();
+extern void gcode_G12();
+extern void gcode_G17();
+extern void gcode_G18();
+extern void gcode_G19();
+extern void gcode_G20();
+extern void gcode_G21();
+extern void gcode_G26();
+extern void gcode_G27();
+extern void gcode_G28(const bool always_home_all);
+extern void gcode_G29();
+extern void gcode_G30();
+extern void gcode_G31();
+extern void gcode_G32();
+extern void gcode_G33();
+extern void gcode_G38(bool is_38_2);
+extern void gcode_G42();
+extern void gcode_G92();
+extern void gcode_M0_M1();
+extern void gcode_M3_M4(bool is_M3);
+extern void gcode_M5();
+extern void gcode_M17();
+extern void gcode_M18_M84();
+extern void gcode_M20();
+extern void gcode_M21();
+extern void gcode_M22();
+extern void gcode_M23();
+extern void gcode_M24();
+extern void gcode_M25();
+extern void gcode_M26();
+extern void gcode_M27();
+extern void gcode_M28();
+extern void gcode_M29();
+extern void gcode_M30();
+extern void gcode_M31();
+extern void gcode_M32();
+extern void gcode_M33();
+extern void gcode_M34();
+extern void gcode_M42();
+extern void gcode_M43();
+extern void gcode_M48();
+extern void gcode_M49();
+extern void gcode_M75();
+extern void gcode_M76();
+extern void gcode_M77();
+extern void gcode_M78();
+extern void gcode_M80();
+extern void gcode_M81();
+extern void gcode_M82();
+extern void gcode_M83();
+extern void gcode_M85();
+extern void gcode_M92();
+extern void gcode_M100();
+extern void gcode_M105();
+extern void gcode_M106();
+extern void gcode_M107();
+extern void gcode_M108();
+extern void gcode_M110();
+extern void gcode_M111();
+extern void gcode_M112();
+extern void gcode_M113();
+extern void gcode_M114();
+extern void gcode_M115();
+extern void gcode_M117();
+extern void gcode_M118();
+extern void gcode_M119();
+extern void gcode_M120();
+extern void gcode_M121();
+extern void gcode_M125();
+extern void gcode_M126();
+extern void gcode_M127();
+extern void gcode_M128();
+extern void gcode_M129();
+extern void gcode_M140();
+extern void gcode_M145();
+extern void gcode_M149();
+extern void gcode_M150();
+extern void gcode_M155();
+extern void gcode_M163();
+extern void gcode_M164();
+extern void gcode_M165();
+extern void gcode_M190();
+extern void gcode_M201();
+extern void gcode_M203();
+extern void gcode_M204();
+extern void gcode_M205();
+extern void gcode_M206();
+extern void gcode_M211();
+extern void gcode_M220();
+extern void gcode_M226();
+extern void gcode_M240();
+extern void gcode_M250();
+extern void gcode_M260();
+extern void gcode_M261();
+extern void gcode_M280();
+extern void gcode_M300();
+extern void gcode_M301();
+extern void gcode_M302();
+extern void gcode_M304();
+extern void gcode_M350();
+extern void gcode_M351();
+extern void gcode_M355();
+extern bool gcode_M360();
+extern bool gcode_M361();
+extern bool gcode_M362();
+extern bool gcode_M363();
+extern bool gcode_M364();
+extern void gcode_M380();
+extern void gcode_M381();
+extern void gcode_M400();
+extern void gcode_M401();
+extern void gcode_M402();
+extern void gcode_M404();
+extern void gcode_M405();
+extern void gcode_M406();
+extern void gcode_M407();
+extern void gcode_M410();
+extern void gcode_M420();
+extern void gcode_M421();
+extern void gcode_M428();
+extern void gcode_M500();
+extern void gcode_M501();
+extern void gcode_M502();
+extern void gcode_M503();
+extern void gcode_M540();
+extern void gcode_M600();
+extern void gcode_M605();
+extern void gcode_M665();
+extern void gcode_M666();
+extern void gcode_M702();
+extern void gcode_M851();
+extern void gcode_M900();
+extern void gcode_M906();
+extern void gcode_M911();
+extern void gcode_M912();
+extern void gcode_M913();
+extern void gcode_M914();
+extern void gcode_M907();
+extern void gcode_M908();
+extern void gcode_M909();
+extern void gcode_M910();
+extern void gcode_M928();
+extern void gcode_M999();
+extern void gcode_T(uint8_t tmp_extruder);
+
+#if ENABLED(M100_FREE_MEMORY_WATCHER)
+  extern void M100_dump_routine(const char * const title, const char *start, const char *end);
+#endif
+
+/**
  * Process a single command and dispatch it to its handler
  * This is called from the main loop()
  */
-void process_next_command() {
+void GcodeSuite::process_next_command() {
   char * const current_command = command_queue[cmd_queue_index_r];
 
   if (DEBUGGING(ECHO)) {
@@ -49,9 +281,9 @@ void process_next_command() {
       case 0:
       case 1:
         #if IS_SCARA
-          gcode_G0_G1(parser.codenum == 0);
+          G0_G1(parser.codenum == 0);
         #else
-          gcode_G0_G1();
+          G0_G1();
         #endif
         break;
 
@@ -76,10 +308,10 @@ void process_next_command() {
 
       #if ENABLED(FWRETRACT)
         case 10: // G10: retract
-          gcode_G10();
+          G10();
           break;
         case 11: // G11: retract_recover
-          gcode_G11();
+          G11();
           break;
       #endif // FWRETRACT
 
@@ -303,9 +535,8 @@ void process_next_command() {
           break;
       #endif
 
-      case 104: // M104: Set hot end temperature
-        gcode_M104();
-        break;
+      case 104: M104(); break;    // M104: Set hot end temperature
+      case 109: M109(); break;    // M109: Wait for hotend temperature to reach target
 
       case 110: // M110: Set Current Line Number
         gcode_M110();
@@ -353,10 +584,6 @@ void process_next_command() {
           break;
       #endif
 
-      case 109: // M109: Wait for hotend temperature to reach target
-        gcode_M109();
-        break;
-
       #if HAS_TEMP_BED
         case 190: // M190: Wait for bed temperature to reach target
           gcode_M190();
@@ -488,7 +715,7 @@ void process_next_command() {
       #endif
 
       case 200: // M200: Set filament diameter, E to cubic units
-        gcode_M200();
+        M200();
         break;
       case 201: // M201: Set max acceleration for print moves (units/s^2)
         gcode_M201();
@@ -528,13 +755,13 @@ void process_next_command() {
 
       #if ENABLED(FWRETRACT)
         case 207: // M207: Set Retract Length, Feedrate, and Z lift
-          gcode_M207();
+          M207();
           break;
         case 208: // M208: Set Recover (unretract) Additional Length and Feedrate
-          gcode_M208();
+          M208();
           break;
         case 209: // M209: Turn Automatic Retract Detection on/off
-          if (MIN_AUTORETRACT <= MAX_AUTORETRACT) gcode_M209();
+          if (MIN_AUTORETRACT <= MAX_AUTORETRACT) M209();
           break;
       #endif // FWRETRACT
 
@@ -544,7 +771,7 @@ void process_next_command() {
 
       #if HOTENDS > 1
         case 218: // M218: Set a tool offset
-          gcode_M218();
+          M218();
           break;
       #endif
 
@@ -553,7 +780,7 @@ void process_next_command() {
         break;
 
       case 221: // M221: Set Flow Percentage
-        gcode_M221();
+        M221();
         break;
 
       case 226: // M226: Wait until a pin reaches a state
@@ -615,7 +842,7 @@ void process_next_command() {
       #endif // PREVENT_COLD_EXTRUSION
 
       case 303: // M303: PID autotune
-        gcode_M303();
+        M303();
         break;
 
       #if ENABLED(MORGAN_SCARA)
@@ -636,6 +863,15 @@ void process_next_command() {
           break;
       #endif // SCARA
 
+      #if ENABLED(EXT_SOLENOID)
+        case 380: // M380: Activate solenoid on active extruder
+          gcode_M380();
+          break;
+        case 381: // M381: Disable all solenoids
+          gcode_M381();
+          break;
+      #endif
+
       case 400: // M400: Finish all moves
         gcode_M400();
         break;
@@ -809,48 +1045,17 @@ void process_next_command() {
       #endif
 
       #if ENABLED(I2C_POSITION_ENCODERS)
-
-        case 860: // M860 Report encoder module position
-          gcode_M860();
-          break;
-
-        case 861: // M861 Report encoder module status
-          gcode_M861();
-          break;
-
-        case 862: // M862 Perform axis test
-          gcode_M862();
-          break;
-
-        case 863: // M863 Calibrate steps/mm
-          gcode_M863();
-          break;
-
-        case 864: // M864 Change module address
-          gcode_M864();
-          break;
-
-        case 865: // M865 Check module firmware version
-          gcode_M865();
-          break;
-
-        case 866: // M866 Report axis error count
-          gcode_M866();
-          break;
-
-        case 867: // M867 Toggle error correction
-          gcode_M867();
-          break;
-
-        case 868: // M868 Set error correction threshold
-          gcode_M868();
-          break;
-
-        case 869: // M869 Report axis error
-          gcode_M869();
-          break;
-
-      #endif // I2C_POSITION_ENCODERS
+        case 860: M860(); break; // M860: Report encoder module position
+        case 861: M861(); break; // M861: Report encoder module status
+        case 862: M862(); break; // M862: Perform axis test
+        case 863: M863(); break; // M863: Calibrate steps/mm
+        case 864: M864(); break; // M864: Change module address
+        case 865: M865(); break; // M865: Check module firmware version
+        case 866: M866(); break; // M866: Report axis error count
+        case 867: M867(); break; // M867: Toggle error correction
+        case 868: M868(); break; // M868: Set error correction threshold
+        case 869: M869(); break; // M869: Report axis error
+      #endif
 
       case 999: // M999: Restart after being Stopped
         gcode_M999();
@@ -868,4 +1073,4 @@ void process_next_command() {
   KEEPALIVE_STATE(NOT_BUSY);
 
   ok_to_send();
-}
+}

Marlin/src/gcode/gcode.h

@@ -237,8 +237,8 @@
  *
  */
 
-#ifndef GCODE_H
-#define GCODE_H
+#ifndef _GCODE_H_
+#define _GCODE_H_
 
 #include "../inc/MarlinConfig.h"
 #include "parser.h"
@@ -252,6 +252,30 @@ public:
 
   GcodeSuite() {}
 
+  static uint8_t target_extruder;
+
+  static bool axis_relative_modes[];
+
+  static millis_t previous_cmd_ms;
+  FORCE_INLINE static void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
+
+  static bool get_target_extruder_from_command();
+  static void get_destination_from_command();
+  static void process_next_command();
+
+  /**
+   * Multi-stepper support for M92, M201, M203
+   */
+  #if ENABLED(DISTINCT_E_FACTORS)
+    #define GET_TARGET_EXTRUDER() if (gcode.get_target_extruder_from_command()) return
+    #define TARGET_EXTRUDER gcode.target_extruder
+  #else
+    #define GET_TARGET_EXTRUDER() NOOP
+    #define TARGET_EXTRUDER 0
+  #endif
+
+  static FORCE_INLINE void home_all_axes() { G28(true); }
+
 private:
 
   static void G0_G1(
@@ -375,7 +399,7 @@ private:
     static void M48();
   #endif
 
-  #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_G26_MESH_VALIDATION)
+  #if ENABLED(UBL_G26_MESH_VALIDATION)
     static void M49();
   #endif
 
@@ -679,4 +703,4 @@ private:
 
 extern GcodeSuite gcode;
 
-#endif // GCODE_H
+#endif // _GCODE_H_

Marlin/src/gcode/host/M110.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../queue.h"
+
 /**
  * M110: Set Current Line Number
  */

Marlin/src/gcode/lcd/M0_M1.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../gcode.h"
+
 /**
  * M0: Unconditional stop - Wait for user button press on LCD
  * M1: Conditional stop   - Wait for user button press on LCD
@@ -62,7 +64,7 @@ void gcode_M0_M1() {
   wait_for_user = true;
 
   stepper.synchronize();
-  refresh_cmd_timeout();
+  gcode.refresh_cmd_timeout();
 
   if (ms > 0) {
     ms += previous_cmd_ms;  // wait until this time for a click

Marlin/src/gcode/motion/G0_G1.h → Marlin/src/gcode/motion/G0_G1.cpp

@@ -20,16 +20,25 @@
  *
  */
 
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+#include "../../Marlin.h"
+
+#include "../../sd/cardreader.h"
+
+extern float destination[XYZE];
+
 /**
  * G0, G1: Coordinated movement of X Y Z E axes
  */
-void gcode_G0_G1(
+void GcodeSuite::G0_G1(
   #if IS_SCARA
-    bool fast_move=false
+    bool fast_move/*=false*/
   #endif
 ) {
   if (IsRunning()) {
-    gcode_get_destination(); // For X Y Z E F
+    get_destination_from_command(); // For X Y Z E F
 
     #if ENABLED(FWRETRACT)
       if (MIN_AUTORETRACT <= MAX_AUTORETRACT) {

Marlin/src/gcode/motion/G2_G3.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../gcode.h"
+
 #if N_ARC_CORRECTION < 1
   #undef N_ARC_CORRECTION
   #define N_ARC_CORRECTION 1
@@ -209,7 +211,7 @@ void gcode_G2_G3(bool clockwise) {
       relative_mode = true;
     #endif
 
-    gcode_get_destination();
+    gcode.get_destination_from_command();
 
     #if ENABLED(SF_ARC_FIX)
       relative_mode = relative_mode_backup;
@@ -252,7 +254,7 @@ void gcode_G2_G3(bool clockwise) {
 
       // Send the arc to the planner
       plan_arc(destination, arc_offset, clockwise);
-      refresh_cmd_timeout();
+      gcode.refresh_cmd_timeout();
     }
     else {
       // Bad arguments

Marlin/src/gcode/motion/G5.h

@@ -21,6 +21,7 @@
  */
 
 #include "../../module/planner_bezier.h"
+#include "../../gcode/gcode.h"
 
 void plan_cubic_move(const float offset[4]) {
   cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
@@ -52,7 +53,7 @@ void gcode_G5() {
       }
     #endif
 
-    gcode_get_destination();
+    gcode.get_destination_from_command();
 
     const float offset[] = {
       parser.linearval('I'),

Marlin/src/gcode/parser.h

@@ -242,7 +242,7 @@ public:
       FORCE_INLINE static char temp_units_code() {
         return input_temp_units == TEMPUNIT_K ? 'K' : input_temp_units == TEMPUNIT_F ? 'F' : 'C';
       }
-      FORCE_INLINE static char* temp_units_name() {
+      FORCE_INLINE static const char* temp_units_name() {
         return input_temp_units == TEMPUNIT_K ? PSTR("Kelvin") : input_temp_units == TEMPUNIT_F ? PSTR("Fahrenheit") : PSTR("Celsius");
       }
       inline static float to_temp_units(const float &f) {

Marlin/src/gcode/probe/G38.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../gcode.h"
+
 static bool G38_run_probe() {
 
   bool G38_pass_fail = false;
@@ -88,7 +90,7 @@ static bool G38_run_probe() {
  */
 void gcode_G38(bool is_38_2) {
   // Get X Y Z E F
-  gcode_get_destination();
+  gcode.get_destination_from_command();
 
   setup_for_endstop_or_probe_move();
 

Marlin/src/gcode/queue.cpp

@@ -0,0 +1,473 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * queue.cpp - The G-code command queue
+ */
+
+#include "queue.h"
+#include "gcode.h"
+
+#include "../lcd/ultralcd.h"
+#include "../sd/cardreader.h"
+#include "../module/planner.h"
+#include "../Marlin.h"
+
+/**
+ * GCode line number handling. Hosts may opt to include line numbers when
+ * sending commands to Marlin, and lines will be checked for sequentiality.
+ * M110 N<int> sets the current line number.
+ */
+long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
+
+/**
+ * GCode Command Queue
+ * A simple ring buffer of BUFSIZE command strings.
+ *
+ * Commands are copied into this buffer by the command injectors
+ * (immediate, serial, sd card) and they are processed sequentially by
+ * the main loop. The gcode.process_next_command method parses the next
+ * command and hands off execution to individual handler functions.
+ */
+uint8_t commands_in_queue = 0, // Count of commands in the queue
+        cmd_queue_index_r = 0, // Ring buffer read position
+        cmd_queue_index_w = 0; // Ring buffer write position
+
+char command_queue[BUFSIZE][MAX_CMD_SIZE];
+
+/**
+ * Serial command injection
+ */
+
+// Number of characters read in the current line of serial input
+static int serial_count = 0;
+
+bool send_ok[BUFSIZE];
+
+/**
+ * Next Injected Command pointer. NULL if no commands are being injected.
+ * Used by Marlin internally to ensure that commands initiated from within
+ * are enqueued ahead of any pending serial or sd card commands.
+ */
+static const char *injected_commands_P = NULL;
+
+void queue_setup() {
+  // Send "ok" after commands by default
+  for (uint8_t i = 0; i < COUNT(send_ok); i++) send_ok[i] = true;
+}
+
+/**
+ * Clear the Marlin command queue
+ */
+void clear_command_queue() {
+  cmd_queue_index_r = cmd_queue_index_w;
+  commands_in_queue = 0;
+}
+
+/**
+ * Once a new command is in the ring buffer, call this to commit it
+ */
+inline void _commit_command(bool say_ok) {
+  send_ok[cmd_queue_index_w] = say_ok;
+  if (++cmd_queue_index_w >= BUFSIZE) cmd_queue_index_w = 0;
+  commands_in_queue++;
+}
+
+/**
+ * Copy a command from RAM into the main command buffer.
+ * Return true if the command was successfully added.
+ * Return false for a full buffer, or if the 'command' is a comment.
+ */
+inline bool _enqueuecommand(const char* cmd, bool say_ok/*=false*/) {
+  if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
+  strcpy(command_queue[cmd_queue_index_w], cmd);
+  _commit_command(say_ok);
+  return true;
+}
+
+/**
+ * Enqueue with Serial Echo
+ */
+bool enqueue_and_echo_command(const char* cmd, bool say_ok/*=false*/) {
+  if (_enqueuecommand(cmd, say_ok)) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPAIR(MSG_ENQUEUEING, cmd);
+    SERIAL_CHAR('"');
+    SERIAL_EOL();
+    return true;
+  }
+  return false;
+}
+
+/**
+ * Inject the next "immediate" command, when possible, onto the front of the queue.
+ * Return true if any immediate commands remain to inject.
+ */
+static bool drain_injected_commands_P() {
+  if (injected_commands_P != NULL) {
+    size_t i = 0;
+    char c, cmd[30];
+    strncpy_P(cmd, injected_commands_P, sizeof(cmd) - 1);
+    cmd[sizeof(cmd) - 1] = '\0';
+    while ((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
+    cmd[i] = '\0';
+    if (enqueue_and_echo_command(cmd))     // success?
+      injected_commands_P = c ? injected_commands_P + i + 1 : NULL; // next command or done
+  }
+  return (injected_commands_P != NULL);    // return whether any more remain
+}
+
+/**
+ * Record one or many commands to run from program memory.
+ * Aborts the current queue, if any.
+ * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
+ */
+void enqueue_and_echo_commands_P(const char * const pgcode) {
+  injected_commands_P = pgcode;
+  drain_injected_commands_P(); // first command executed asap (when possible)
+}
+
+/**
+ * Send an "ok" message to the host, indicating
+ * that a command was successfully processed.
+ *
+ * If ADVANCED_OK is enabled also include:
+ *   N<int>  Line number of the command, if any
+ *   P<int>  Planner space remaining
+ *   B<int>  Block queue space remaining
+ */
+void ok_to_send() {
+  gcode.refresh_cmd_timeout();
+  if (!send_ok[cmd_queue_index_r]) return;
+  SERIAL_PROTOCOLPGM(MSG_OK);
+  #if ENABLED(ADVANCED_OK)
+    char* p = command_queue[cmd_queue_index_r];
+    if (*p == 'N') {
+      SERIAL_PROTOCOL(' ');
+      SERIAL_ECHO(*p++);
+      while (NUMERIC_SIGNED(*p))
+        SERIAL_ECHO(*p++);
+    }
+    SERIAL_PROTOCOLPGM(" P"); SERIAL_PROTOCOL(int(BLOCK_BUFFER_SIZE - planner.movesplanned() - 1));
+    SERIAL_PROTOCOLPGM(" B"); SERIAL_PROTOCOL(BUFSIZE - commands_in_queue);
+  #endif
+  SERIAL_EOL();
+}
+
+/**
+ * Send a "Resend: nnn" message to the host to
+ * indicate that a command needs to be re-sent.
+ */
+void flush_and_request_resend() {
+  //char command_queue[cmd_queue_index_r][100]="Resend:";
+  MYSERIAL.flush();
+  SERIAL_PROTOCOLPGM(MSG_RESEND);
+  SERIAL_PROTOCOLLN(gcode_LastN + 1);
+  ok_to_send();
+}
+
+void gcode_line_error(const char* err, bool doFlush = true) {
+  SERIAL_ERROR_START();
+  serialprintPGM(err);
+  SERIAL_ERRORLN(gcode_LastN);
+  //Serial.println(gcode_N);
+  if (doFlush) flush_and_request_resend();
+  serial_count = 0;
+}
+
+/**
+ * Get all commands waiting on the serial port and queue them.
+ * Exit when the buffer is full or when no more characters are
+ * left on the serial port.
+ */
+inline void get_serial_commands() {
+  static char serial_line_buffer[MAX_CMD_SIZE];
+  static bool serial_comment_mode = false;
+
+  // If the command buffer is empty for too long,
+  // send "wait" to indicate Marlin is still waiting.
+  #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
+    static millis_t last_command_time = 0;
+    const millis_t ms = millis();
+    if (commands_in_queue == 0 && !MYSERIAL.available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
+      SERIAL_ECHOLNPGM(MSG_WAIT);
+      last_command_time = ms;
+    }
+  #endif
+
+  /**
+   * Loop while serial characters are incoming and the queue is not full
+   */
+  while (commands_in_queue < BUFSIZE && MYSERIAL.available() > 0) {
+
+    char serial_char = MYSERIAL.read();
+
+    /**
+     * If the character ends the line
+     */
+    if (serial_char == '\n' || serial_char == '\r') {
+
+      serial_comment_mode = false; // end of line == end of comment
+
+      if (!serial_count) continue; // skip empty lines
+
+      serial_line_buffer[serial_count] = 0; // terminate string
+      serial_count = 0; //reset buffer
+
+      char* command = serial_line_buffer;
+
+      while (*command == ' ') command++; // skip any leading spaces
+      char *npos = (*command == 'N') ? command : NULL, // Require the N parameter to start the line
+           *apos = strchr(command, '*');
+
+      if (npos) {
+
+        bool M110 = strstr_P(command, PSTR("M110")) != NULL;
+
+        if (M110) {
+          char* n2pos = strchr(command + 4, 'N');
+          if (n2pos) npos = n2pos;
+        }
+
+        gcode_N = strtol(npos + 1, NULL, 10);
+
+        if (gcode_N != gcode_LastN + 1 && !M110) {
+          gcode_line_error(PSTR(MSG_ERR_LINE_NO));
+          return;
+        }
+
+        if (apos) {
+          byte checksum = 0, count = 0;
+          while (command[count] != '*') checksum ^= command[count++];
+
+          if (strtol(apos + 1, NULL, 10) != checksum) {
+            gcode_line_error(PSTR(MSG_ERR_CHECKSUM_MISMATCH));
+            return;
+          }
+          // if no errors, continue parsing
+        }
+        else {
+          gcode_line_error(PSTR(MSG_ERR_NO_CHECKSUM));
+          return;
+        }
+
+        gcode_LastN = gcode_N;
+        // if no errors, continue parsing
+      }
+      else if (apos) { // No '*' without 'N'
+        gcode_line_error(PSTR(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM), false);
+        return;
+      }
+
+      // Movement commands alert when stopped
+      if (IsStopped()) {
+        char* gpos = strchr(command, 'G');
+        if (gpos) {
+          const int codenum = strtol(gpos + 1, NULL, 10);
+          switch (codenum) {
+            case 0:
+            case 1:
+            case 2:
+            case 3:
+              SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
+              LCD_MESSAGEPGM(MSG_STOPPED);
+              break;
+          }
+        }
+      }
+
+      #if DISABLED(EMERGENCY_PARSER)
+        // If command was e-stop process now
+        if (strcmp(command, "M108") == 0) {
+          wait_for_heatup = false;
+          #if ENABLED(ULTIPANEL)
+            wait_for_user = false;
+          #endif
+        }
+        if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED));
+        if (strcmp(command, "M410") == 0) { quickstop_stepper(); }
+      #endif
+
+      #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
+        last_command_time = ms;
+      #endif
+
+      // Add the command to the queue
+      _enqueuecommand(serial_line_buffer, true);
+    }
+    else if (serial_count >= MAX_CMD_SIZE - 1) {
+      // Keep fetching, but ignore normal characters beyond the max length
+      // The command will be injected when EOL is reached
+    }
+    else if (serial_char == '\\') {  // Handle escapes
+      if (MYSERIAL.available() > 0) {
+        // if we have one more character, copy it over
+        serial_char = MYSERIAL.read();
+        if (!serial_comment_mode) serial_line_buffer[serial_count++] = serial_char;
+      }
+      // otherwise do nothing
+    }
+    else { // it's not a newline, carriage return or escape char
+      if (serial_char == ';') serial_comment_mode = true;
+      if (!serial_comment_mode) serial_line_buffer[serial_count++] = serial_char;
+    }
+
+  } // queue has space, serial has data
+}
+
+#if ENABLED(SDSUPPORT)
+
+  /**
+   * Get commands from the SD Card until the command buffer is full
+   * or until the end of the file is reached. The special character '#'
+   * can also interrupt buffering.
+   */
+  inline void get_sdcard_commands() {
+    static bool stop_buffering = false,
+                sd_comment_mode = false;
+
+    if (!IS_SD_PRINTING) return;
+
+    /**
+     * '#' stops reading from SD to the buffer prematurely, so procedural
+     * macro calls are possible. If it occurs, stop_buffering is triggered
+     * and the buffer is run dry; this character _can_ occur in serial com
+     * due to checksums, however, no checksums are used in SD printing.
+     */
+
+    if (commands_in_queue == 0) stop_buffering = false;
+
+    uint16_t sd_count = 0;
+    bool card_eof = card.eof();
+    while (commands_in_queue < BUFSIZE && !card_eof && !stop_buffering) {
+      const int16_t n = card.get();
+      char sd_char = (char)n;
+      card_eof = card.eof();
+      if (card_eof || n == -1
+          || sd_char == '\n' || sd_char == '\r'
+          || ((sd_char == '#' || sd_char == ':') && !sd_comment_mode)
+      ) {
+        if (card_eof) {
+          SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
+          card.printingHasFinished();
+          #if ENABLED(PRINTER_EVENT_LEDS)
+            LCD_MESSAGEPGM(MSG_INFO_COMPLETED_PRINTS);
+            set_led_color(0, 255, 0); // Green
+            #if HAS_RESUME_CONTINUE
+              enqueue_and_echo_commands_P(PSTR("M0")); // end of the queue!
+            #else
+              safe_delay(1000);
+            #endif
+            set_led_color(0, 0, 0);   // OFF
+          #endif
+          card.checkautostart(true);
+        }
+        else if (n == -1) {
+          SERIAL_ERROR_START();
+          SERIAL_ECHOLNPGM(MSG_SD_ERR_READ);
+        }
+        if (sd_char == '#') stop_buffering = true;
+
+        sd_comment_mode = false; // for new command
+
+        if (!sd_count) continue; // skip empty lines (and comment lines)
+
+        command_queue[cmd_queue_index_w][sd_count] = '\0'; // terminate string
+        sd_count = 0; // clear sd line buffer
+
+        _commit_command(false);
+      }
+      else if (sd_count >= MAX_CMD_SIZE - 1) {
+        /**
+         * Keep fetching, but ignore normal characters beyond the max length
+         * The command will be injected when EOL is reached
+         */
+      }
+      else {
+        if (sd_char == ';') sd_comment_mode = true;
+        if (!sd_comment_mode) command_queue[cmd_queue_index_w][sd_count++] = sd_char;
+      }
+    }
+  }
+
+#endif // SDSUPPORT
+
+/**
+ * Add to the circular command queue the next command from:
+ *  - The command-injection queue (injected_commands_P)
+ *  - The active serial input (usually USB)
+ *  - The SD card file being actively printed
+ */
+void get_available_commands() {
+
+  // if any immediate commands remain, don't get other commands yet
+  if (drain_injected_commands_P()) return;
+
+  get_serial_commands();
+
+  #if ENABLED(SDSUPPORT)
+    get_sdcard_commands();
+  #endif
+}
+
+/**
+ * Get the next command in the queue, optionally log it to SD, then dispatch it
+ */
+void advance_command_queue() {
+
+  if (!commands_in_queue) return;
+
+  #if ENABLED(SDSUPPORT)
+
+    if (card.saving) {
+      char* command = command_queue[cmd_queue_index_r];
+      if (strstr_P(command, PSTR("M29"))) {
+        // M29 closes the file
+        card.closefile();
+        SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
+        ok_to_send();
+      }
+      else {
+        // Write the string from the read buffer to SD
+        card.write_command(command);
+        if (card.logging)
+          gcode.process_next_command(); // The card is saving because it's logging
+        else
+          ok_to_send();
+      }
+    }
+    else
+      gcode.process_next_command();
+
+  #else
+
+    gcode.process_next_command();
+
+  #endif // SDSUPPORT
+
+  // The queue may be reset by a command handler or by code invoked by idle() within a handler
+  if (commands_in_queue) {
+    --commands_in_queue;
+    if (++cmd_queue_index_r >= BUFSIZE) cmd_queue_index_r = 0;
+  }
+
+}

Marlin/src/gcode/queue.h

@@ -0,0 +1,106 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * queue.h - The G-code command queue, which holds commands before they
+ *           go to the parser and dispatcher.
+ */
+
+#ifndef GCODE_QUEUE_H
+#define GCODE_QUEUE_H
+
+#include "../inc/MarlinConfig.h"
+
+/**
+ * GCode line number handling. Hosts may include line numbers when sending
+ * commands to Marlin, and lines will be checked for sequentiality.
+ * M110 N<int> sets the current line number.
+ */
+extern long gcode_LastN, Stopped_gcode_LastN;
+
+/**
+ * GCode Command Queue
+ * A simple ring buffer of BUFSIZE command strings.
+ *
+ * Commands are copied into this buffer by the command injectors
+ * (immediate, serial, sd card) and they are processed sequentially by
+ * the main loop. The gcode.process_next_command method parses the next
+ * command and hands off execution to individual handler functions.
+ */
+extern uint8_t commands_in_queue, // Count of commands in the queue
+               cmd_queue_index_r; // Ring buffer read position
+
+extern char command_queue[BUFSIZE][MAX_CMD_SIZE];
+
+/**
+ * Initialization of queue for setup()
+ */
+void queue_setup();
+
+/**
+ * Clear the Marlin command queue
+ */
+void clear_command_queue();
+
+/**
+ * Clear the serial line and request a resend of
+ * the next expected line number.
+ */
+void flush_and_request_resend();
+
+/**
+ * Send an "ok" message to the host, indicating
+ * that a command was successfully processed.
+ *
+ * If ADVANCED_OK is enabled also include:
+ *   N<int>  Line number of the command, if any
+ *   P<int>  Planner space remaining
+ *   B<int>  Block queue space remaining
+ */
+void ok_to_send();
+
+/**
+ * Record one or many commands to run from program memory.
+ * Aborts the current queue, if any.
+ * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
+ */
+void enqueue_and_echo_commands_P(const char * const pgcode);
+
+/**
+ * Enqueue with Serial Echo
+ */
+bool enqueue_and_echo_command(const char* cmd, bool say_ok=false);
+
+/**
+ * Add to the circular command queue the next command from:
+ *  - The command-injection queue (injected_commands_P)
+ *  - The active serial input (usually USB)
+ *  - The SD card file being actively printed
+ */
+void get_available_commands();
+
+/**
+ * Get the next command in the queue, optionally log it to SD, then dispatch it
+ */
+void advance_command_queue();
+
+#endif // GCODE_QUEUE_H

Marlin/src/gcode/temperature/M104.h → Marlin/src/gcode/temperature/M104.cpp

@@ -20,23 +20,35 @@
  *
  */
 
+#include "../gcode.h"
+#include "../../module/temperature.h"
+#include "../../module/motion.h"
+#include "../../module/planner.h"
+#include "../../lcd/ultralcd.h"
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+#endif
+
 /**
  * M104: Set hot end temperature
  */
-void gcode_M104() {
-  if (get_target_extruder_from_command(104)) return;
+void GcodeSuite::M104() {
+  if (get_target_extruder_from_command()) return;
   if (DEBUGGING(DRYRUN)) return;
 
+  const uint8_t e = target_extruder;
+
   #if ENABLED(SINGLENOZZLE)
-    if (target_extruder != active_extruder) return;
+    if (e != active_extruder) return;
   #endif
 
   if (parser.seenval('S')) {
     const int16_t temp = parser.value_celsius();
-    thermalManager.setTargetHotend(temp, target_extruder);
+    thermalManager.setTargetHotend(temp, e);
 
     #if ENABLED(DUAL_X_CARRIAGE)
-      if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && target_extruder == 0)
+      if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && e == 0)
         thermalManager.setTargetHotend(temp ? temp + duplicate_extruder_temp_offset : 0, 1);
     #endif
 
@@ -53,8 +65,8 @@ void gcode_M104() {
       }
     #endif
 
-    if (parser.value_celsius() > thermalManager.degHotend(target_extruder))
-      lcd_status_printf_P(0, PSTR("E%i %s"), target_extruder + 1, MSG_HEATING);
+    if (parser.value_celsius() > thermalManager.degHotend(e))
+      lcd_status_printf_P(0, PSTR("E%i %s"), e + 1, MSG_HEATING);
   }
 
   #if ENABLED(AUTOTEMP)

Marlin/src/gcode/temperature/M105.h

@@ -24,7 +24,7 @@
  * M105: Read hot end and bed temperature
  */
 void gcode_M105() {
-  if (get_target_extruder_from_command(105)) return;
+  if (gcode.get_target_extruder_from_command()) return;
 
   #if HAS_TEMP_HOTEND || HAS_TEMP_BED
     SERIAL_PROTOCOLPGM(MSG_OK);

Marlin/src/gcode/temperature/M109.h → Marlin/src/gcode/temperature/M109.cpp

@@ -20,6 +20,20 @@
  *
  */
 
+#include "../gcode.h"
+#include "../../module/temperature.h"
+#include "../../module/planner.h"
+#include "../../lcd/ultralcd.h"
+#include "../../Marlin.h"
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+  #include "../../module/printcounter.h"
+#endif
+
+#if ENABLED(DUAL_X_CARRIAGE)
+  #include "../../module/motion.h"
+#endif
+
 /**
  * M109: Sxxx Wait for extruder(s) to reach temperature. Waits only when heating.
  *       Rxxx Wait for extruder(s) to reach temperature. Waits when heating and cooling.
@@ -32,9 +46,9 @@
   #define MIN_COOLING_SLOPE_TIME 60
 #endif
 
-void gcode_M109() {
+void GcodeSuite::M109() {
 
-  if (get_target_extruder_from_command(109)) return;
+  if (get_target_extruder_from_command()) return;
   if (DEBUGGING(DRYRUN)) return;
 
   #if ENABLED(SINGLENOZZLE)

Marlin/src/gcode/temperature/M190.h

@@ -20,6 +20,8 @@
  *
  */
 
+#include "../gcode.h"
+
 #ifndef MIN_COOLING_SLOPE_DEG_BED
   #define MIN_COOLING_SLOPE_DEG_BED 1.50
 #endif
@@ -63,7 +65,7 @@ void gcode_M190() {
     KEEPALIVE_STATE(NOT_BUSY);
   #endif
 
-  target_extruder = active_extruder; // for print_heaterstates
+  gcode.target_extruder = active_extruder; // for print_heaterstates
 
   #if ENABLED(PRINTER_EVENT_LEDS)
     const float start_temp = thermalManager.degBed();
@@ -95,7 +97,7 @@ void gcode_M190() {
     }
 
     idle();
-    refresh_cmd_timeout(); // to prevent stepper_inactive_time from running out
+    gcode.refresh_cmd_timeout(); // to prevent stepper_inactive_time from running out
 
     const float temp = thermalManager.degBed();
 

Marlin/src/gcode/temperature/M303.h → Marlin/src/gcode/temperature/M303.cpp

@@ -20,6 +20,9 @@
  *
  */
 
+#include "../gcode.h"
+#include "../../module/temperature.h"
+
 /**
  * M303: PID relay autotune
  *
@@ -28,7 +31,7 @@
  *       C<cycles>
  *       U<bool> with a non-zero value will apply the result to current settings
  */
-void gcode_M303() {
+void GcodeSuite::M303() {
   #if HAS_PID_HEATING
     const int e = parser.intval('E'), c = parser.intval('C', 5);
     const bool u = parser.boolval('U');

Marlin/src/gcode/units/M82_M83.h

@@ -23,9 +23,9 @@
 /**
  * M82: Set E codes absolute (default)
  */
-void gcode_M82() { axis_relative_modes[E_AXIS] = false; }
+void gcode_M82() { gcode.axis_relative_modes[E_AXIS] = false; }
 
 /**
  * M83: Set E codes relative while in Absolute Coordinates (G90) mode
  */
-void gcode_M83() { axis_relative_modes[E_AXIS] = true; }
+void gcode_M83() { gcode.axis_relative_modes[E_AXIS] = true; }

Marlin/src/lcd/ultralcd.cpp

@@ -30,6 +30,9 @@
 #include "../module/temperature.h"
 #include "../module/planner.h"
 #include "../module/stepper.h"
+#include "../module/motion.h"
+#include "../gcode/gcode.h"
+#include "../gcode/queue.h"
 #include "../module/configuration_store.h"
 
 #include "../Marlin.h"
@@ -1734,7 +1737,7 @@ void kill_screen(const char* lcd_msg) {
       // Encoder knob or keypad buttons adjust the Z position
       //
       if (encoderPosition) {
-        refresh_cmd_timeout();
+        gcode.refresh_cmd_timeout();
         const float z = current_position[Z_AXIS] + float((int32_t)encoderPosition) * (MBL_Z_STEP);
         line_to_z(constrain(z, -(LCD_PROBE_Z_RANGE) * 0.5, (LCD_PROBE_Z_RANGE) * 0.5));
         lcdDrawUpdate = LCDVIEW_CALL_REDRAW_NEXT;
@@ -2280,7 +2283,7 @@ void kill_screen(const char* lcd_msg) {
         x_plot += step_scaler / (ENCODER_STEPS_PER_MENU_ITEM);
         if (abs(step_scaler) >= ENCODER_STEPS_PER_MENU_ITEM)
           step_scaler = 0;
-        refresh_cmd_timeout();
+        gcode.refresh_cmd_timeout();
 
         encoderPosition = 0;
         lcdDrawUpdate = LCDVIEW_REDRAW_NOW;
@@ -2317,7 +2320,7 @@ void kill_screen(const char* lcd_msg) {
           set_current_from_steppers_for_axis(ALL_AXES);
           sync_plan_position();
           ubl_map_move_to_xy(); // Move to new location
-          refresh_cmd_timeout();
+          gcode.refresh_cmd_timeout();
         }
       }
     }
@@ -2702,7 +2705,7 @@ void kill_screen(const char* lcd_msg) {
     if (lcd_clicked) { return lcd_goto_previous_menu(); }
     ENCODER_DIRECTION_NORMAL();
     if (encoderPosition) {
-      refresh_cmd_timeout();
+      gcode.refresh_cmd_timeout();
 
       float min = current_position[axis] - 1000,
             max = current_position[axis] + 1000;

Marlin/src/libs/nozzle.cpp

@@ -23,6 +23,7 @@
 #include "nozzle.h"
 
 #include "../Marlin.h"
+#include "../module/motion.h"
 #include "point_t.h"
 
 /**

Marlin/src/module/motion.cpp

@@ -0,0 +1,574 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * motion.cpp
+ */
+
+#include "motion.h"
+
+#include "../gcode/gcode.h"
+// #include "../module/planner.h"
+// #include "../Marlin.h"
+// #include "../inc/MarlinConfig.h"
+
+#include "../core/serial.h"
+#include "../module/stepper.h"
+#include "../module/temperature.h"
+
+#if IS_SCARA
+  #include "../libs/buzzer.h"
+  #include "../lcd/ultralcd.h"
+#endif
+
+#if ENABLED(AUTO_BED_LEVELING_UBL)
+  #include "../feature/ubl/ubl.h"
+#endif
+
+#define XYZ_CONSTS(type, array, CONFIG) const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }
+
+XYZ_CONSTS(float, base_min_pos,   MIN_POS);
+XYZ_CONSTS(float, base_max_pos,   MAX_POS);
+XYZ_CONSTS(float, base_home_pos,  HOME_POS);
+XYZ_CONSTS(float, max_length,     MAX_LENGTH);
+XYZ_CONSTS(float, home_bump_mm,   HOME_BUMP_MM);
+XYZ_CONSTS(signed char, home_dir, HOME_DIR);
+
+// Relative Mode. Enable with G91, disable with G90.
+bool relative_mode = false;
+
+/**
+ * Cartesian Current Position
+ *   Used to track the logical position as moves are queued.
+ *   Used by 'line_to_current_position' to do a move after changing it.
+ *   Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
+ */
+float current_position[XYZE] = { 0.0 };
+
+/**
+ * Cartesian Destination
+ *   A temporary position, usually applied to 'current_position'.
+ *   Set with 'get_destination_from_command' or 'set_destination_to_current'.
+ *   'line_to_destination' sets 'current_position' to 'destination'.
+ */
+float destination[XYZE] = { 0.0 };
+
+// The active extruder (tool). Set with T<extruder> command.
+uint8_t active_extruder = 0;
+
+// The feedrate for the current move, often used as the default if
+// no other feedrate is specified. Overridden for special moves.
+// Set by the last G0 through G5 command's "F" parameter.
+// Functions that override this for custom moves *must always* restore it!
+float feedrate_mm_s = MMM_TO_MMS(1500.0);
+
+/**
+ * sync_plan_position
+ *
+ * Set the planner/stepper positions directly from current_position with
+ * no kinematic translation. Used for homing axes and cartesian/core syncing.
+ */
+void sync_plan_position() {
+  #if ENABLED(DEBUG_LEVELING_FEATURE)
+    if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
+  #endif
+  planner.set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+}
+
+void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
+
+/**
+ * Move the planner to the current position from wherever it last moved
+ * (or from wherever it has been told it is located).
+ */
+void line_to_current_position() {
+  planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate_mm_s, active_extruder);
+}
+
+/**
+ * Move the planner to the position stored in the destination array, which is
+ * used by G0/G1/G2/G3/G5 and many other functions to set a destination.
+ */
+void line_to_destination(const float fr_mm_s) {
+  planner.buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], fr_mm_s, active_extruder);
+}
+
+#if IS_KINEMATIC
+
+  void sync_plan_position_kinematic() {
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
+    #endif
+    planner.set_position_mm_kinematic(current_position);
+  }
+
+  /**
+   * Calculate delta, start a line, and set current_position to destination
+   */
+  void prepare_uninterpolated_move_to_destination(const float fr_mm_s/*=0.0*/) {
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
+    #endif
+
+    gcode.refresh_cmd_timeout();
+
+    #if UBL_DELTA
+      // ubl segmented line will do z-only moves in single segment
+      ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
+    #else
+      if ( current_position[X_AXIS] == destination[X_AXIS]
+        && current_position[Y_AXIS] == destination[Y_AXIS]
+        && current_position[Z_AXIS] == destination[Z_AXIS]
+        && current_position[E_AXIS] == destination[E_AXIS]
+      ) return;
+
+      planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
+    #endif
+
+    set_current_to_destination();
+  }
+
+#endif // IS_KINEMATIC
+
+// Software Endstops are based on the configured limits.
+float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
+      soft_endstop_max[XYZ] = { X_MAX_BED, Y_MAX_BED, Z_MAX_POS };
+
+#if HAS_SOFTWARE_ENDSTOPS
+
+  // Software Endstops are based on the configured limits.
+  bool soft_endstops_enabled = true;
+
+  /**
+   * Constrain the given coordinates to the software endstops.
+   */
+
+  // NOTE: This makes no sense for delta beds other than Z-axis.
+  //       For delta the X/Y would need to be clamped at
+  //       DELTA_PRINTABLE_RADIUS from center of bed, but delta
+  //       now enforces is_position_reachable for X/Y regardless
+  //       of HAS_SOFTWARE_ENDSTOPS, so that enforcement would be
+  //       redundant here.
+
+  void clamp_to_software_endstops(float target[XYZ]) {
+    if (!soft_endstops_enabled) return;
+    #if ENABLED(MIN_SOFTWARE_ENDSTOPS)
+      #if DISABLED(DELTA)
+        NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
+        NOLESS(target[Y_AXIS], soft_endstop_min[Y_AXIS]);
+      #endif
+      NOLESS(target[Z_AXIS], soft_endstop_min[Z_AXIS]);
+    #endif
+    #if ENABLED(MAX_SOFTWARE_ENDSTOPS)
+      #if DISABLED(DELTA)
+        NOMORE(target[X_AXIS], soft_endstop_max[X_AXIS]);
+        NOMORE(target[Y_AXIS], soft_endstop_max[Y_AXIS]);
+      #endif
+      NOMORE(target[Z_AXIS], soft_endstop_max[Z_AXIS]);
+    #endif
+  }
+
+#endif
+
+#if ENABLED(AUTO_BED_LEVELING_BILINEAR) && !IS_KINEMATIC
+
+  #define CELL_INDEX(A,V) ((RAW_##A##_POSITION(V) - bilinear_start[A##_AXIS]) * ABL_BG_FACTOR(A##_AXIS))
+
+  /**
+   * Prepare a bilinear-leveled linear move on Cartesian,
+   * splitting the move where it crosses grid borders.
+   */
+  void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF);
+    int cx1 = CELL_INDEX(X, current_position[X_AXIS]),
+        cy1 = CELL_INDEX(Y, current_position[Y_AXIS]),
+        cx2 = CELL_INDEX(X, destination[X_AXIS]),
+        cy2 = CELL_INDEX(Y, destination[Y_AXIS]);
+    cx1 = constrain(cx1, 0, ABL_BG_POINTS_X - 2);
+    cy1 = constrain(cy1, 0, ABL_BG_POINTS_Y - 2);
+    cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2);
+    cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 2);
+
+    if (cx1 == cx2 && cy1 == cy2) {
+      // Start and end on same mesh square
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    #define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
+
+    float normalized_dist, end[XYZE];
+
+    // Split at the left/front border of the right/top square
+    const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+    if (cx2 != cx1 && TEST(x_splits, gcx)) {
+      COPY(end, destination);
+      destination[X_AXIS] = LOGICAL_X_POSITION(bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx);
+      normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
+      destination[Y_AXIS] = LINE_SEGMENT_END(Y);
+      CBI(x_splits, gcx);
+    }
+    else if (cy2 != cy1 && TEST(y_splits, gcy)) {
+      COPY(end, destination);
+      destination[Y_AXIS] = LOGICAL_Y_POSITION(bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy);
+      normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
+      destination[X_AXIS] = LINE_SEGMENT_END(X);
+      CBI(y_splits, gcy);
+    }
+    else {
+      // Already split on a border
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    destination[Z_AXIS] = LINE_SEGMENT_END(Z);
+    destination[E_AXIS] = LINE_SEGMENT_END(E);
+
+    // Do the split and look for more borders
+    bilinear_line_to_destination(fr_mm_s, x_splits, y_splits);
+
+    // Restore destination from stack
+    COPY(destination, end);
+    bilinear_line_to_destination(fr_mm_s, x_splits, y_splits);
+  }
+
+#endif // AUTO_BED_LEVELING_BILINEAR
+
+#if IS_KINEMATIC && !UBL_DELTA
+
+  /**
+   * Prepare a linear move in a DELTA or SCARA setup.
+   *
+   * This calls planner.buffer_line several times, adding
+   * small incremental moves for DELTA or SCARA.
+   */
+  inline bool prepare_kinematic_move_to(float ltarget[XYZE]) {
+
+    // Get the top feedrate of the move in the XY plane
+    const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
+
+    // If the move is only in Z/E don't split up the move
+    if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
+      planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
+      return false;
+    }
+
+    // Fail if attempting move outside printable radius
+    if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true;
+
+    // Get the cartesian distances moved in XYZE
+    const float difference[XYZE] = {
+      ltarget[X_AXIS] - current_position[X_AXIS],
+      ltarget[Y_AXIS] - current_position[Y_AXIS],
+      ltarget[Z_AXIS] - current_position[Z_AXIS],
+      ltarget[E_AXIS] - current_position[E_AXIS]
+    };
+
+    // Get the linear distance in XYZ
+    float cartesian_mm = SQRT(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
+
+    // If the move is very short, check the E move distance
+    if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(difference[E_AXIS]);
+
+    // No E move either? Game over.
+    if (UNEAR_ZERO(cartesian_mm)) return true;
+
+    // Minimum number of seconds to move the given distance
+    const float seconds = cartesian_mm / _feedrate_mm_s;
+
+    // The number of segments-per-second times the duration
+    // gives the number of segments
+    uint16_t segments = delta_segments_per_second * seconds;
+
+    // For SCARA minimum segment size is 0.25mm
+    #if IS_SCARA
+      NOMORE(segments, cartesian_mm * 4);
+    #endif
+
+    // At least one segment is required
+    NOLESS(segments, 1);
+
+    // The approximate length of each segment
+    const float inv_segments = 1.0 / float(segments),
+                segment_distance[XYZE] = {
+                  difference[X_AXIS] * inv_segments,
+                  difference[Y_AXIS] * inv_segments,
+                  difference[Z_AXIS] * inv_segments,
+                  difference[E_AXIS] * inv_segments
+                };
+
+    // SERIAL_ECHOPAIR("mm=", cartesian_mm);
+    // SERIAL_ECHOPAIR(" seconds=", seconds);
+    // SERIAL_ECHOLNPAIR(" segments=", segments);
+
+    #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
+      // SCARA needs to scale the feed rate from mm/s to degrees/s
+      const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
+                  feed_factor = inv_segment_length * _feedrate_mm_s;
+      float oldA = stepper.get_axis_position_degrees(A_AXIS),
+            oldB = stepper.get_axis_position_degrees(B_AXIS);
+    #endif
+
+    // Get the logical current position as starting point
+    float logical[XYZE];
+    COPY(logical, current_position);
+
+    // Drop one segment so the last move is to the exact target.
+    // If there's only 1 segment, loops will be skipped entirely.
+    --segments;
+
+    // Calculate and execute the segments
+    for (uint16_t s = segments + 1; --s;) {
+      LOOP_XYZE(i) logical[i] += segment_distance[i];
+      #if ENABLED(DELTA)
+        DELTA_LOGICAL_IK(); // Delta can inline its kinematics
+      #else
+        inverse_kinematics(logical);
+      #endif
+
+      ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
+
+      #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
+        // For SCARA scale the feed rate from mm/s to degrees/s
+        // Use ratio between the length of the move and the larger angle change
+        const float adiff = abs(delta[A_AXIS] - oldA),
+                    bdiff = abs(delta[B_AXIS] - oldB);
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
+        oldA = delta[A_AXIS];
+        oldB = delta[B_AXIS];
+      #else
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
+      #endif
+    }
+
+    // Since segment_distance is only approximate,
+    // the final move must be to the exact destination.
+
+    #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
+      // For SCARA scale the feed rate from mm/s to degrees/s
+      // With segments > 1 length is 1 segment, otherwise total length
+      inverse_kinematics(ltarget);
+      ADJUST_DELTA(ltarget);
+      const float adiff = abs(delta[A_AXIS] - oldA),
+                  bdiff = abs(delta[B_AXIS] - oldB);
+      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
+    #else
+      planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
+    #endif
+
+    return false;
+  }
+
+#else // !IS_KINEMATIC || UBL_DELTA
+
+  /**
+   * Prepare a linear move in a Cartesian setup.
+   * If Mesh Bed Leveling is enabled, perform a mesh move.
+   *
+   * Returns true if the caller didn't update current_position.
+   */
+  inline bool prepare_move_to_destination_cartesian() {
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      const float fr_scaled = MMS_SCALED(feedrate_mm_s);
+      if (ubl.state.active) { // direct use of ubl.state.active for speed
+        ubl.line_to_destination_cartesian(fr_scaled, active_extruder);
+        return true;
+      }
+      else
+        line_to_destination(fr_scaled);
+    #else
+      // Do not use feedrate_percentage for E or Z only moves
+      if (current_position[X_AXIS] == destination[X_AXIS] && current_position[Y_AXIS] == destination[Y_AXIS])
+        line_to_destination();
+      else {
+        const float fr_scaled = MMS_SCALED(feedrate_mm_s);
+        #if ENABLED(MESH_BED_LEVELING)
+          if (mbl.active()) { // direct used of mbl.active() for speed
+            mesh_line_to_destination(fr_scaled);
+            return true;
+          }
+          else
+        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          if (planner.abl_enabled) { // direct use of abl_enabled for speed
+            bilinear_line_to_destination(fr_scaled);
+            return true;
+          }
+          else
+        #endif
+            line_to_destination(fr_scaled);
+      }
+    #endif
+    return false;
+  }
+
+#endif // !IS_KINEMATIC || UBL_DELTA
+
+#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
+  bool extruder_duplication_enabled = false;                              // Used in Dual X mode 2
+#endif
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  DualXMode dual_x_carriage_mode         = DEFAULT_DUAL_X_CARRIAGE_MODE;
+  float inactive_extruder_x_pos          = X2_MAX_POS,                    // used in mode 0 & 1
+        raised_parked_position[XYZE],                                     // used in mode 1
+        duplicate_extruder_x_offset      = DEFAULT_DUPLICATION_X_OFFSET;  // used in mode 2
+  bool active_extruder_parked            = false;                         // used in mode 1 & 2
+  millis_t delayed_move_time             = 0;                             // used in mode 1
+  int16_t duplicate_extruder_temp_offset = 0;                             // used in mode 2
+
+  float x_home_pos(const int extruder) {
+    if (extruder == 0)
+      return LOGICAL_X_POSITION(base_home_pos(X_AXIS));
+    else
+      /**
+       * In dual carriage mode the extruder offset provides an override of the
+       * second X-carriage position when homed - otherwise X2_HOME_POS is used.
+       * This allows soft recalibration of the second extruder home position
+       * without firmware reflash (through the M218 command).
+       */
+      return LOGICAL_X_POSITION(hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS);
+  }
+
+  /**
+   * Prepare a linear move in a dual X axis setup
+   */
+  inline bool prepare_move_to_destination_dualx() {
+    if (active_extruder_parked) {
+      switch (dual_x_carriage_mode) {
+        case DXC_FULL_CONTROL_MODE:
+          break;
+        case DXC_AUTO_PARK_MODE:
+          if (current_position[E_AXIS] == destination[E_AXIS]) {
+            // This is a travel move (with no extrusion)
+            // Skip it, but keep track of the current position
+            // (so it can be used as the start of the next non-travel move)
+            if (delayed_move_time != 0xFFFFFFFFUL) {
+              set_current_to_destination();
+              NOLESS(raised_parked_position[Z_AXIS], destination[Z_AXIS]);
+              delayed_move_time = millis();
+              return true;
+            }
+          }
+          // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
+          for (uint8_t i = 0; i < 3; i++)
+            planner.buffer_line(
+              i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
+              i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
+              i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
+              current_position[E_AXIS],
+              i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
+              active_extruder
+            );
+          delayed_move_time = 0;
+          active_extruder_parked = false;
+          #if ENABLED(DEBUG_LEVELING_FEATURE)
+            if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Clear active_extruder_parked");
+          #endif
+          break;
+        case DXC_DUPLICATION_MODE:
+          if (active_extruder == 0) {
+            #if ENABLED(DEBUG_LEVELING_FEATURE)
+              if (DEBUGGING(LEVELING)) {
+                SERIAL_ECHOPAIR("Set planner X", LOGICAL_X_POSITION(inactive_extruder_x_pos));
+                SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
+              }
+            #endif
+            // move duplicate extruder into correct duplication position.
+            planner.set_position_mm(
+              LOGICAL_X_POSITION(inactive_extruder_x_pos),
+              current_position[Y_AXIS],
+              current_position[Z_AXIS],
+              current_position[E_AXIS]
+            );
+            planner.buffer_line(
+              current_position[X_AXIS] + duplicate_extruder_x_offset,
+              current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
+              planner.max_feedrate_mm_s[X_AXIS], 1
+            );
+            SYNC_PLAN_POSITION_KINEMATIC();
+            stepper.synchronize();
+            extruder_duplication_enabled = true;
+            active_extruder_parked = false;
+            #if ENABLED(DEBUG_LEVELING_FEATURE)
+              if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Set extruder_duplication_enabled\nClear active_extruder_parked");
+            #endif
+          }
+          else {
+            #if ENABLED(DEBUG_LEVELING_FEATURE)
+              if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Active extruder not 0");
+            #endif
+          }
+          break;
+      }
+    }
+    return false;
+  }
+
+#endif // DUAL_X_CARRIAGE
+
+/**
+ * Prepare a single move and get ready for the next one
+ *
+ * This may result in several calls to planner.buffer_line to
+ * do smaller moves for DELTA, SCARA, mesh moves, etc.
+ */
+void prepare_move_to_destination() {
+  clamp_to_software_endstops(destination);
+  gcode.refresh_cmd_timeout();
+
+  #if ENABLED(PREVENT_COLD_EXTRUSION)
+
+    if (!DEBUGGING(DRYRUN)) {
+      if (destination[E_AXIS] != current_position[E_AXIS]) {
+        if (thermalManager.tooColdToExtrude(active_extruder)) {
+          current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
+          SERIAL_ECHO_START();
+          SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
+        }
+        #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+          if (destination[E_AXIS] - current_position[E_AXIS] > EXTRUDE_MAXLENGTH) {
+            current_position[E_AXIS] = destination[E_AXIS]; // Behave as if the move really took place, but ignore E part
+            SERIAL_ECHO_START();
+            SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
+          }
+        #endif
+      }
+    }
+
+  #endif
+
+  if (
+    #if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely)
+      ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
+    #elif IS_KINEMATIC
+      prepare_kinematic_move_to(destination)
+    #elif ENABLED(DUAL_X_CARRIAGE)
+      prepare_move_to_destination_dualx() || prepare_move_to_destination_cartesian()
+    #else
+      prepare_move_to_destination_cartesian()
+    #endif
+  ) return;
+
+  set_current_to_destination();
+}

Marlin/src/module/motion.h

@@ -0,0 +1,237 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * motion.h
+ *
+ * High-level motion commands to feed the planner
+ * Some of these methods may migrate to the planner class.
+ */
+
+#ifndef MOTION_H
+#define MOTION_H
+
+#include "../inc/MarlinConfig.h"
+
+//#include "../HAL/HAL.h"
+
+// #if ENABLED(DELTA)
+//   #include "../module/delta.h"
+// #endif
+
+extern bool relative_mode;
+
+extern float current_position[XYZE], destination[XYZE];
+
+extern float feedrate_mm_s;
+
+extern uint8_t active_extruder;
+
+extern float soft_endstop_min[XYZ], soft_endstop_max[XYZ];
+
+FORCE_INLINE float pgm_read_any(const float *p) { return pgm_read_float_near(p); }
+FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_byte_near(p); }
+
+#define XYZ_DEFS(type, array, CONFIG) \
+  extern const type array##_P[XYZ]; \
+  FORCE_INLINE type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
+  typedef void __void_##CONFIG##__
+
+XYZ_DEFS(float, base_min_pos,   MIN_POS);
+XYZ_DEFS(float, base_max_pos,   MAX_POS);
+XYZ_DEFS(float, base_home_pos,  HOME_POS);
+XYZ_DEFS(float, max_length,     MAX_LENGTH);
+XYZ_DEFS(float, home_bump_mm,   HOME_BUMP_MM);
+XYZ_DEFS(signed char, home_dir, HOME_DIR);
+
+#if HAS_SOFTWARE_ENDSTOPS
+  extern bool soft_endstops_enabled;
+  void clamp_to_software_endstops(float target[XYZ]);
+#else
+  #define soft_endstops_enabled false
+  #define clamp_to_software_endstops(x) NOOP
+#endif
+
+inline void set_current_to_destination() { COPY(current_position, destination); }
+inline void set_destination_to_current() { COPY(destination, current_position); }
+
+/**
+ * sync_plan_position
+ *
+ * Set the planner/stepper positions directly from current_position with
+ * no kinematic translation. Used for homing axes and cartesian/core syncing.
+ */
+void sync_plan_position();
+void sync_plan_position_e();
+
+#if IS_KINEMATIC
+  void sync_plan_position_kinematic();
+  #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
+#else
+  #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position()
+#endif
+
+/**
+ * Move the planner to the current position from wherever it last moved
+ * (or from wherever it has been told it is located).
+ */
+void line_to_current_position();
+
+/**
+ * Move the planner to the position stored in the destination array, which is
+ * used by G0/G1/G2/G3/G5 and many other functions to set a destination.
+ */
+void line_to_destination(const float fr_mm_s);
+
+inline void line_to_destination() { line_to_destination(feedrate_mm_s); }
+
+#if IS_KINEMATIC
+  void prepare_uninterpolated_move_to_destination(const float fr_mm_s=0.0);
+#endif
+
+void prepare_move_to_destination();
+
+void clamp_to_software_endstops(float target[XYZ]);
+
+//
+// Macros
+//
+
+// Workspace offsets
+#if HAS_WORKSPACE_OFFSET
+  #if HAS_HOME_OFFSET
+    extern float home_offset[XYZ];
+  #endif
+  #if HAS_POSITION_SHIFT
+    extern float position_shift[XYZ];
+  #endif
+#endif
+
+#if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
+  extern float workspace_offset[XYZ];
+  #define WORKSPACE_OFFSET(AXIS) workspace_offset[AXIS]
+#elif HAS_HOME_OFFSET
+  #define WORKSPACE_OFFSET(AXIS) home_offset[AXIS]
+#elif HAS_POSITION_SHIFT
+  #define WORKSPACE_OFFSET(AXIS) position_shift[AXIS]
+#else
+  #define WORKSPACE_OFFSET(AXIS) 0
+#endif
+
+#define LOGICAL_POSITION(POS, AXIS) ((POS) + WORKSPACE_OFFSET(AXIS))
+#define RAW_POSITION(POS, AXIS)     ((POS) - WORKSPACE_OFFSET(AXIS))
+
+#if HAS_POSITION_SHIFT || DISABLED(DELTA)
+  #define LOGICAL_X_POSITION(POS)   LOGICAL_POSITION(POS, X_AXIS)
+  #define LOGICAL_Y_POSITION(POS)   LOGICAL_POSITION(POS, Y_AXIS)
+  #define RAW_X_POSITION(POS)       RAW_POSITION(POS, X_AXIS)
+  #define RAW_Y_POSITION(POS)       RAW_POSITION(POS, Y_AXIS)
+#else
+  #define LOGICAL_X_POSITION(POS)   (POS)
+  #define LOGICAL_Y_POSITION(POS)   (POS)
+  #define RAW_X_POSITION(POS)       (POS)
+  #define RAW_Y_POSITION(POS)       (POS)
+#endif
+
+#define LOGICAL_Z_POSITION(POS)     LOGICAL_POSITION(POS, Z_AXIS)
+#define RAW_Z_POSITION(POS)         RAW_POSITION(POS, Z_AXIS)
+#define RAW_CURRENT_POSITION(A)     RAW_##A##_POSITION(current_position[A##_AXIS])
+
+/**
+ * position_is_reachable family of functions
+ */
+
+#if IS_KINEMATIC // (DELTA or SCARA)
+
+  #if IS_SCARA
+    extern const float L1, L2;
+  #endif
+
+  inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) {
+    #if ENABLED(DELTA)
+      return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS);
+    #elif IS_SCARA
+      #if MIDDLE_DEAD_ZONE_R > 0
+        const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y);
+        return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2);
+      #else
+        return HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y) <= sq(L1 + L2);
+      #endif
+    #else // CARTESIAN
+      // To be migrated from MakerArm branch in future
+    #endif
+  }
+
+  inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) {
+
+    // Both the nozzle and the probe must be able to reach the point.
+    // This won't work on SCARA since the probe offset rotates with the arm.
+
+    return position_is_reachable_raw_xy(rx, ry)
+        && position_is_reachable_raw_xy(rx - X_PROBE_OFFSET_FROM_EXTRUDER, ry - Y_PROBE_OFFSET_FROM_EXTRUDER);
+  }
+
+#else // CARTESIAN
+
+  inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) {
+      // Add 0.001 margin to deal with float imprecision
+      return WITHIN(rx, X_MIN_POS - 0.001, X_MAX_POS + 0.001)
+          && WITHIN(ry, Y_MIN_POS - 0.001, Y_MAX_POS + 0.001);
+  }
+
+  inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) {
+      // Add 0.001 margin to deal with float imprecision
+      return WITHIN(rx, MIN_PROBE_X - 0.001, MAX_PROBE_X + 0.001)
+          && WITHIN(ry, MIN_PROBE_Y - 0.001, MAX_PROBE_Y + 0.001);
+  }
+
+#endif // CARTESIAN
+
+FORCE_INLINE bool position_is_reachable_by_probe_xy(const float &lx, const float &ly) {
+  return position_is_reachable_by_probe_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
+}
+
+FORCE_INLINE bool position_is_reachable_xy(const float &lx, const float &ly) {
+  return position_is_reachable_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
+}
+
+#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
+  extern bool extruder_duplication_enabled;       // Used in Dual X mode 2
+#endif
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  extern DualXMode dual_x_carriage_mode;
+  extern float inactive_extruder_x_pos,           // used in mode 0 & 1
+               raised_parked_position[XYZE],      // used in mode 1
+               duplicate_extruder_x_offset;       // used in mode 2
+  extern bool active_extruder_parked;             // used in mode 1 & 2
+  extern millis_t delayed_move_time;              // used in mode 1
+  extern int16_t duplicate_extruder_temp_offset;  // used in mode 2
+
+  float x_home_pos(const int extruder);
+
+  FORCE_INLINE int x_home_dir(const uint8_t extruder) { return extruder ? X2_HOME_DIR : X_HOME_DIR; }
+
+#endif // DUAL_X_CARRIAGE
+
+#endif // MOTION_H

Marlin/src/module/planner.cpp

@@ -60,6 +60,7 @@
 
 #include "planner.h"
 #include "stepper.h"
+#include "motion.h"
 #include "../module/temperature.h"
 #include "../lcd/ultralcd.h"
 #include "../core/language.h"

Marlin/src/module/planner_bezier.cpp

@@ -31,10 +31,13 @@
 
 #if ENABLED(BEZIER_CURVE_SUPPORT)
 
+#include "planner.h"
+#include "motion.h"
+#include "temperature.h"
+
 #include "../Marlin.h"
-#include "../module/planner.h"
 #include "../core/language.h"
-#include "../module/temperature.h"
+#include "../gcode/queue.h"
 
 // See the meaning in the documentation of cubic_b_spline().
 #define MIN_STEP 0.002

Marlin/src/module/stepper.cpp

@@ -57,6 +57,7 @@
 #include "../module/temperature.h"
 #include "../lcd/ultralcd.h"
 #include "../core/language.h"
+#include "../gcode/queue.h"
 #include "../sd/cardreader.h"
 
 #if MB(ALLIGATOR)

Marlin/src/sd/cardreader.cpp

@@ -31,6 +31,7 @@
 #include "../module/stepper.h"
 #include "../module/printcounter.h"
 #include "../core/language.h"
+#include "../gcode/queue.h"
 
 #include <ctype.h>