Implement CNC_COORDINATE_SYSTEMS

Marlin/src/gcode/gcode.cpp

@@ -56,6 +56,11 @@ bool GcodeSuite::axis_relative_modes[] = AXIS_RELATIVE_MODES;
   GcodeSuite::WorkspacePlane GcodeSuite::workspace_plane = PLANE_XY;
 #endif
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+  int8_t GcodeSuite::active_coordinate_system = -1; // machine space
+  float GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
+#endif
+
 /**
  * Set target_extruder from the T parameter or the active_extruder
  *
@@ -125,26 +130,11 @@ void GcodeSuite::dwell(millis_t time) {
 #endif
 
 /**
- * Process a single command and dispatch it to its handler
- * This is called from the main loop()
+ * Process the parsed command and dispatch it to its handler
  */
-void GcodeSuite::process_next_command() {
-  char * const current_command = command_queue[cmd_queue_index_r];
-
-  if (DEBUGGING(ECHO)) {
-    SERIAL_ECHO_START();
-    SERIAL_ECHOLN(current_command);
-    #if ENABLED(M100_FREE_MEMORY_WATCHER)
-      SERIAL_ECHOPAIR("slot:", cmd_queue_index_r);
-      M100_dump_routine("   Command Queue:", (const char*)command_queue, (const char*)(command_queue + sizeof(command_queue)));
-    #endif
-  }
-
+void GcodeSuite::process_parsed_command() {
   KEEPALIVE_STATE(IN_HANDLER);
 
-  // Parse the next command in the queue
-  parser.parse(current_command);
-
   // Handle a known G, M, or T
   switch (parser.command_letter) {
     case 'G': switch (parser.codenum) {
@@ -711,6 +701,27 @@ void GcodeSuite::process_next_command() {
   ok_to_send();
 }
 
+/**
+ * Process a single command and dispatch it to its handler
+ * This is called from the main loop()
+ */
+void GcodeSuite::process_next_command() {
+  char * const current_command = command_queue[cmd_queue_index_r];
+
+  if (DEBUGGING(ECHO)) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLN(current_command);
+    #if ENABLED(M100_FREE_MEMORY_WATCHER)
+      SERIAL_ECHOPAIR("slot:", cmd_queue_index_r);
+      M100_dump_routine("   Command Queue:", (const char*)command_queue, (const char*)(command_queue + sizeof(command_queue)));
+    #endif
+  }
+
+  // Parse the next command in the queue
+  parser.parse(current_command);
+  process_parsed_command();
+}
+
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
   /**

Marlin/src/gcode/gcode.h

@@ -266,11 +266,19 @@ public:
     static WorkspacePlane workspace_plane;
   #endif
 
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    #define MAX_COORDINATE_SYSTEMS 9
+    static int8_t active_coordinate_system;
+    static float coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
+    static bool select_coordinate_system(const int8_t _new);
+  #endif
+
   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_parsed_command();
   static void process_next_command();
 
   static FORCE_INLINE void home_all_axes() { G28(true); }
@@ -383,6 +391,17 @@ private:
     static void G42();
   #endif
 
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    bool select_coordinate_system(const int8_t _new);
+    static void G53();
+    static void G54();
+    static void G55();
+    static void G56();
+    static void G57();
+    static void G58();
+    static void G59();
+  #endif
+
   static void G92();
 
   #if HAS_RESUME_CONTINUE

Marlin/src/gcode/geometry/G53-G59.cpp

@@ -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/>.
+ *
+ */
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+//#include "../../module/stepper.h"
+
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+
+  /**
+   * Select a coordinate system and update the current position.
+   * System index -1 is used to specify machine-native.
+   */
+  bool GCodeSuite::select_coordinate_system(const int8_t _new) {
+    if (active_coordinate_system == _new) return false;
+    stepper.synchronize();
+    float old_offset[XYZ] = { 0 }, new_offset[XYZ] = { 0 };
+    if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1))
+      COPY(old_offset, coordinate_system[active_coordinate_system]);
+    if (WITHIN(_new, 0, MAX_COORDINATE_SYSTEMS - 1))
+      COPY(new_offset, coordinate_system[_new]);
+    active_coordinate_system = _new;
+    bool didXYZ = false;
+    LOOP_XYZ(i) {
+      const float diff = new_offset[i] - old_offset[i];
+      if (diff) {
+        position_shift[i] += diff;
+        update_software_endstops((AxisEnum)i);
+        didXYZ = true;
+      }
+    }
+    if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
+    return true;
+  }
+
+  /**
+   * In CNC G-code G53 is like a modifier
+   * It precedes a movement command (or other modifiers) on the same line.
+   * This is the first command to use parser.chain() to make this possible.
+   */
+  void GCodeSuite::G53() {
+    // If this command has more following...
+    if (parser.chain()) {
+      const int8_t _system = active_coordinate_system;
+      active_coordinate_system = -1;
+      process_parsed_command();
+      active_coordinate_system = _system;
+    }
+  }
+
+  /**
+   * G54-G59.3: Select a new workspace
+   *
+   * A workspace is an XYZ offset to the machine native space.
+   * All workspaces default to 0,0,0 at start, or with EEPROM
+   * support they may be restored from a previous session.
+   *
+   * G92 is used to set the current workspace's offset.
+   */
+  void G54_59(uint8_t subcode=0) {
+    const int8_t _space = parser.codenum - 54 + subcode;
+    if (gcode.select_coordinate_system(_space)) {
+      SERIAL_PROTOCOLLNPAIR("Select workspace ", _space);
+      report_current_position();
+    }
+  }
+  void GCodeSuite::G54() { G54_59(); }
+  void GCodeSuite::G55() { G54_59(); }
+  void GCodeSuite::G56() { G54_59(); }
+  void GCodeSuite::G57() { G54_59(); }
+  void GCodeSuite::G58() { G54_59(); }
+  void GCodeSuite::G59() { G54_59(parser.subcode); }
+
+#endif // CNC_COORDINATE_SYSTEMS

Marlin/src/gcode/geometry/G92.cpp

@@ -37,7 +37,30 @@ void GcodeSuite::G92() {
 
   if (!didE) stepper.synchronize();
 
-  LOOP_XYZE(i) {
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    switch (parser.subcode) {
+      case 1:
+        // Zero the G92 values and restore current position
+        #if !IS_SCARA
+          LOOP_XYZ(i) {
+            const float v = position_shift[i];
+            if (v) {
+              position_shift[i] = 0;
+              update_software_endstops((AxisEnum)i);
+            }
+          }
+        #endif // Not SCARA
+        return;
+    }
+  #endif
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    #define IS_G92_0 (parser.subcode == 0)
+  #else
+    #define IS_G92_0 true
+  #endif
+
+  if (IS_G92_0) LOOP_XYZE(i) {
     if (parser.seenval(axis_codes[i])) {
       #if IS_SCARA
         current_position[i] = parser.value_axis_units((AxisEnum)i);
@@ -60,6 +83,13 @@ void GcodeSuite::G92() {
       #endif
     }
   }
+
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    // Apply workspace offset to the active coordinate system
+    if (WITHIN(active_coordinate_system, 0, MAX_COORDINATE_SYSTEMS - 1))
+      COPY(coordinate_system[active_coordinate_system], position_shift);
+  #endif
+
   if (didXYZ)
     SYNC_PLAN_POSITION_KINEMATIC();
   else if (didE)

Marlin/src/gcode/parser.cpp

@@ -233,6 +233,26 @@ void GCodeParser::parse(char *p) {
   }
 }
 
+#if ENABLED(CNC_COORDINATE_SYSTEMS)
+
+  // Parse the next parameter as a new command
+  bool GCodeParser::chain() {
+    #if ENABLED(FASTER_GCODE_PARSER)
+      char *next_command = command_ptr;
+      if (next_command) {
+        while (*next_command && *next_command != ' ') ++next_command;
+        while (*next_command == ' ') ++next_command;
+        if (!*next_command) next_command = NULL;
+      }
+    #else
+      const char *next_command = command_args;
+    #endif
+    if (next_command) parse(next_command);
+    return !!next_command;
+  }
+
+#endif // CNC_COORDINATE_SYSTEMS
+
 void GCodeParser::unknown_command_error() {
   SERIAL_ECHO_START();
   SERIAL_ECHOPAIR(MSG_UNKNOWN_COMMAND, command_ptr);

Marlin/src/gcode/parser.h

@@ -156,6 +156,11 @@ public:
   // This uses 54 bytes of SRAM to speed up seen/value
   static void parse(char * p);
 
+  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+    // Parse the next parameter as a new command
+    static bool chain();
+  #endif
+
   // The code value pointer was set
   FORCE_INLINE static bool has_value() { return value_ptr != NULL; }