fix G68, G69, add G50, G51 (#3)

Author: DerAndere1

Marlin/src/gcode/gcode.cpp

@@ -100,6 +100,21 @@ relative_t GcodeSuite::axis_relative; // Init in constructor
   xyz_pos_t GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS];
 #endif
 
+#if ENABLED(SCALE_WORKSPACE)
+  float GcodeSuite::scaling_center_x = 0.0f;
+  float GcodeSuite::scaling_center_y = 0.0f;
+  float GcodeSuite::scaling_center_z = 0.0f;
+  float GcodeSuite::scaling_factor_x = 1.0f;
+  float GcodeSuite::scaling_factor_y = 1.0f;
+  float GcodeSuite::scaling_factor_z = 1.0f;
+#endif
+
+#if ENABLED(ROTATE_WORKSPACE)
+  float GcodeSuite::rotation_center_x = 0.0f;
+  float GcodeSuite::rotation_center_y = 0.0f;
+  float GcodeSuite::rotation_angle = 0.0f;
+#endif
+
 void GcodeSuite::report_echo_start(const bool forReplay) { if (!forReplay) SERIAL_ECHO_START(); }
 void GcodeSuite::report_heading(const bool forReplay, FSTR_P const fstr, const bool eol/*=true*/) {
   if (forReplay) return;
@@ -171,19 +186,58 @@ void GcodeSuite::get_destination_from_command() {
     constexpr bool skip_move = false;
   #endif
 
+  const float angle_rad = RADIANS(rotation_angle);
+
   // Get new XYZ position, whether absolute or relative
   LOOP_NUM_AXES(i) {
     if ( (seen[i] = parser.seenval(AXIS_CHAR(i))) ) {
       const float v = parser.value_axis_units((AxisEnum)i);
-      if (skip_move)
+      if (skip_move) {
+        #if ANY(SCALE_WORKSPACE, ROTATE_WORKSPACE)
+          raw_destination[i] = current_position[i];
+        #else
+          destination[i] = current_position[i];
+        #endif
+      }
+      else {
+        #if ANY(SCALE_WORKSPACE, ROTATE_WORKSPACE)
+          raw_destination[i] = axis_is_relative(AxisEnum(i)) ? raw_destination[i] + v : LOGICAL_TO_NATIVE(v, i);
+        #else
+          destination[i] = axis_is_relative(AxisEnum(i)) ? current_position[i] + v : LOGICAL_TO_NATIVE(v, i);
+        #endif
+      }
+    }
+    else {
+      #if NONE(SCALE_WORKSPACE, ROTATE_WORKSPACE)
         destination[i] = current_position[i];
-      else
-        destination[i] = axis_is_relative(AxisEnum(i)) ? current_position[i] + v : LOGICAL_TO_NATIVE(v, i);
+      #endif
     }
-    else
-      destination[i] = current_position[i];
   }
 
+  #if ANY(SCALE_WORKSPACE, ROTATE_WORKPLACE)
+    destination = raw_destination;
+  #endif
+
+  #if ENABLED(SCALE_WORKSPACE)
+    if (!(NEAR(scaling_factor_x, 1.0f) || NEAR(scaling_factor_y, 1.0f) || NEAR(scaling_factor_z, 1.0f))) {
+      destination.x = (raw_destination.x - scaling_center_x) * scaling_factor_x + scaling_center_x;
+      TERN_(HAS_Y_AXIS, destination.y = (raw_destination.y - scaling_center_y) * scaling_factor_y + scaling_center_y);
+      TERN_(HAS_Z_AXIS, destination.z = (raw_destination.z - scaling_center_z) * scaling_factor_z + scaling_center_z);
+    }
+  #endif
+
+  #if ENABLED(ROTATE_WORKSPACE)
+    if (!NEAR_ZERO(rotation_angle)) {
+      const float cos_angle = cos(angle_rad);
+      const float sin_angle = sin(angle_rad);
+      // Apply rotation
+      const float temp_x = destination.x - rotation_center_x;
+      const float temp_y = destination.y - rotation_center_y;
+      destination.x = temp_x * cos_angle - temp_y * sin_angle + rotation_center_x;
+      destination.y = temp_x * sin_angle + temp_y * cos_angle + rotation_center_y;
+    }
+  #endif
+
   #if HAS_EXTRUDERS
     // Get new E position, whether absolute or relative
     if ( (seen.e = parser.seenval('E')) ) {
@@ -435,6 +489,11 @@ void GcodeSuite::process_parsed_command(bool no_ok/*=false*/) {
         case 42: G42(); break;                                    // G42: Coordinated move to a mesh point
       #endif
 
+      #if ENABLED(SCALE_WORKSPACE)
+        case 50: G50(); break;                                    // G50: Cancel Workspace Scaling
+        case 51: G51(); break;                                    // G51: Set Workspace Scaling
+      #endif
+
       #if ENABLED(CNC_COORDINATE_SYSTEMS)
         case 53: G53(); break;                                    // G53: (prefix) Apply native workspace
         case 54: G54(); break;                                    // G54: Switch to Workspace 1
@@ -452,6 +511,7 @@ void GcodeSuite::process_parsed_command(bool no_ok/*=false*/) {
 
       #if ENABLED(ROTATE_WORKSPACE)
         case 68: G68(); break;                                    // G68: Set Workspace Rotation
+        case 69: G69(); break;                                    // G69: Cancel Workspace Rotation
       #endif
 
       #if ALL(PTC_PROBE, PTC_BED)

Marlin/src/gcode/gcode.h

@@ -64,9 +64,12 @@
  * G35  - Read bed corners to help adjust bed screws: T<screw_thread> (Requires ASSISTED_TRAMMING)
  * G38  - Probe in any direction using the Z_MIN_PROBE (Requires G38_PROBE_TARGET)
  * G42  - Coordinated move to a mesh point (Requires MESH_BED_LEVELING, AUTO_BED_LEVELING_BLINEAR, or AUTO_BED_LEVELING_UBL)
+ * G50  - Cancel workspace scaling (Requires SCALE_WORKSPACE)
+ * G51  - Set workspace scaling (Requires SCALE_WORKSPACE)
  * G60  - Save current position. (Requires SAVED_POSITIONS)
  * G61  - Apply/Restore saved coordinates. (Requires SAVED_POSITIONS)
  * G68  - Set Workspace Rotation
+ * G69  - Cancel Workspace Rotation
  * G76  - Calibrate first layer temperature offsets. (Requires PTC_PROBE and PTC_BED)
  * G80  - Cancel current motion mode (Requires GCODE_MOTION_MODES)
  * G90  - Use Absolute Coordinates
@@ -432,12 +435,20 @@ class GcodeSuite {
     static bool select_coordinate_system(const int8_t _new);
   #endif
 
+
+  #if ENABLED(SCALE_WORKSPACE)
+    static float scaling_center_x;
+    static float scaling_center_y;
+    static float scaling_center_z;
+    static float scaling_factor_x;
+    static float scaling_factor_y;
+    static float scaling_factor_z;
+  #endif
+
   #if ENABLED(ROTATE_WORKSPACE)
-    static uint8_t active_workspace;
-    static float rotation_angle[MAX_COORDINATE_SYSTEMS]; // Store rotation for each workspace
+    static float rotation_angle;
     static float rotation_center_x;
     static float rotation_center_y;
-    static void apply_workspace_rotation();
   #endif
 
   static millis_t previous_move_ms, max_inactive_time;
@@ -618,7 +629,11 @@ class GcodeSuite {
     static void G42();
   #endif
 
-  #if ENABLED(CNC_COORDINATE_SYSTEMS)
+  #if ENABLED(SCALE_WORKSPACE)
+    static void G50();
+    static void G51();  
+  #endif
+
     static void G53();
     static void G54();
     static void G55();
@@ -639,6 +654,7 @@ class GcodeSuite {
 
   #if ENABLED(ROTATE_WORKSPACE)
     static void G68();
+    static void G69();
   #endif
 
   #if ENABLED(GCODE_MOTION_MODES)

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

@@ -37,8 +37,13 @@ inline void report_workspace_plane() {
 }
 
 inline void set_workspace_plane(const GcodeSuite::WorkspacePlane plane) {
-  gcode.workspace_plane = plane;
-  if (DEBUGGING(INFO)) report_workspace_plane();
+  if (!NEAR_ZERO(gcode.rotation_angle)) {
+    SERIAL_ECHOLNPGM("Error: Workspace plane cannnot change while using workspace rotation.");
+  }
+  else {
+    gcode.workspace_plane = plane;
+    if (DEBUGGING(INFO)) report_workspace_plane();
+  }
 }
 
 /**

Marlin/src/gcode/geometry/G50-G51.cpp

@@ -0,0 +1,86 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(SCALE_WORKSPACE)
+
+  #include "../gcode.h"
+  #include "../../module/motion.h"
+  
+  /**
+   * G51: Set Workspace Scaling
+   *
+   * Scale the current workspace coordinate system.
+   *
+   * Parameters:
+   *   X<axis units> x coordinate of the scaling center
+   *   Y<axis units> y coordinate of the scaling center
+   *   Z<axis units> z coordinate of the scaling center
+   *   I<scaling factor> scaling factor for X axis
+   *   J<scaling factor> scaling factor for Y axis
+   *   K<scaling factor> scaling factor for Z axis
+   *   P<scaling factor> scaling factor
+   */
+
+  void GcodeSuite::G51() {
+  
+    if (parser.seenval('P')) {
+      const float scaling_factor = parser.value_float();
+      scaling_factor_x = scaling_factor;
+      TERN_(HAS_Y_AXIS, scaling_factor_y = scaling_factor);
+      TERN_(HAS_Z_AXIS, scaling_factor_z = scaling_factor);
+    }
+    else {
+      if (parser.seenval('I'))
+        scaling_factor_x = parser.value_float();
+      #if HAS_Y_AXIS
+        if (parser.seenval('J'))
+          scaling_factor_y = parser.value_float();
+      #endif
+      #if HAS_Z_AXIS
+        if (parser.seenval('K'))
+          scaling_factor_z = parser.value_float();
+      #endif
+    }
+
+    scaling_center_x = parser.seenval('X') ? LOGICAL_TO_NATIVE(parser.value_axis_units(X_AXIS), X_AXIS) : current_position.x;
+    TERN_(HAS_Y_AXIS, scaling_center_y = parser.seenval('Y') ? LOGICAL_TO_NATIVE(parser.value_axis_units(Y_AXIS), Y_AXIS) : current_position.y);
+    TERN_(HAS_Z_AXIS, scaling_center_z = parser.seenval('Z') ? LOGICAL_TO_NATIVE(parser.value_axis_units(Z_AXIS), Z_AXIS) : current_position.z);
+
+    SERIAL_ECHOLNPGM("Workspace scaling set");
+  }
+
+  void GcodeSuite::G50() {
+    scaling_factor_x = 1.0f;
+    scaling_center_x = 0.0f;
+    scaling_factor_y = 1.0f;
+    scaling_center_y = 0.0f;
+    #if HAS_Z_AXIS
+      scaling_factor_z = 1.0f;
+      scaling_center_z = 0.0f;
+    #endif
+    SERIAL_ECHOLNPGM("Workspace scaling canceled");
+  }
+
+
+#endif // SCALE_WORKSPACE

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

@@ -33,6 +33,10 @@
  */
 bool GcodeSuite::select_coordinate_system(const int8_t _new) {
   if (active_coordinate_system == _new) return false;
+  if (!NEAR_ZERO(gcode.rotation_angle)) {
+    SERIAL_ECHOLNPGM("Cannot change workspace while workspace rotation is active");
+    return false;
+  }
   active_coordinate_system = _new;
   xyz_float_t new_offset{0};
   if (WITHIN(_new, 0, MAX_COORDINATE_SYSTEMS - 1))

Marlin/src/gcode/geometry/G68-G69.cpp

@@ -0,0 +1,74 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 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 <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(ROTATE_WORKSPACE)
+
+  #include "../gcode.h"
+  #include "../../module/motion.h"
+  
+  /**
+   * G68: Set Workspace Rotation
+   *
+   * Set the rotation (about Z axis) for the current workspace (begins at 0).
+   *
+   * Parameters:
+   *   X<axis units> x coordinate of the rotation center for the current workspace
+   *   Y<axis units> y coordinate of the rotation center for the current workspace
+   *   R<deg>        Rotation angle in degrees (Required)
+   *
+   * Example:
+   *   G68 R45       ; Rotate active workspace by 45° counter-clockwise (when viewed from positive Z) 
+   *                 ; around current position
+   *   G68 R-30      ; Rotate active workspace by -30° around current position
+   *   G68 X0 Y0 R45 ; Rotate active  workspace by 45°C around X0 Y0 (X and Y are specified in the current workspace)
+   *
+   * NOTES:
+   *   - Only rotation is set. No translation/offset is changed.
+   *   - All subsequent moves are rotated by the specified angle.
+   *   - It is an error to change workspace or working plane while workspace rotation is active
+   *     (https://forums.autodesk.com/t5/fusion-manufacture-forum/probing-and-updating-wcs-for-angle/td-p/9487027 , 
+   *      https://www.machsupport.com/forum/index.php?topic=43012)
+   */
+
+  void GcodeSuite::G68() {
+  
+    if (!parser.seenval('R')) {
+      SERIAL_ECHOLNPGM("Missing R parameter (rotation angle).");
+      return;
+    }
+    else {
+      rotation_angle = parser.value_float();
+    }
+    TERN_(HAS_X_AXIS, rotation_center_x = parser.seenval('X') ? LOGICAL_TO_NATIVE(parser.value_axis_units(X_AXIS), X_AXIS) : current_position.x);
+    TERN_(HAS_Y_AXIS, rotation_center_y = parser.seenval('Y') ? LOGICAL_TO_NATIVE(parser.value_axis_units(Y_AXIS), Y_AXIS) : current_position.y);
+    SERIAL_ECHOLNPGM("Workspace rotation set");
+  }
+
+  void GcodeSuite::G69() {
+    rotation_angle = 0.0f;
+    SERIAL_ECHOLNPGM("Workspace rotation canceled");
+  }
+
+
+#endif // ROTATE_WORKSPACE

Marlin/src/gcode/motion/G0_G1.cpp

@@ -41,6 +41,10 @@
 
 extern xyze_pos_t destination;
 
+#if defined(ROTATE_WORKSPACE)
+  extern xyz_pos_t raw_destination;
+#endif
+
 #if ENABLED(VARIABLE_G0_FEEDRATE)
   feedRate_t fast_move_feedrate = MMM_TO_MMS(G0_FEEDRATE);
 #endif