G68-G69 Coordinate transform G-codes

Marlin/Configuration_adv.h

@@ -3864,6 +3864,13 @@
  */
 //#define CNC_COORDINATE_SYSTEMS
 
+/**
+ * CNC Coordinate Rotation
+ *
+ * Enables 'G68 X Y R' to set rotation around a point, and G69 to reset it.
+ */
+//#define CNC_COORDINATE_ROTATION
+
 // @section security
 
 /**

Marlin/src/gcode/gcode.cpp

@@ -100,6 +100,10 @@ relative_t GcodeSuite::axis_relative; // Init in constructor
   xyz_pos_t GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS];
 #endif
 
+#if ENABLED(CNC_COORDINATE_ROTATION)
+  rotation_t GcodeSuite::rotation;
+#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;
@@ -170,6 +174,14 @@ void GcodeSuite::get_destination_from_command() {
     constexpr bool skip_move = false;
   #endif
 
+  // TODO: Slow and introduces errors, so we actually need to
+  // cache the unrotated current_position and destination, then
+  // ensure both copies are updated in sync throughout Marlin.
+  #if ENABLED(CNC_COORDINATE_ROTATION)
+    rotation.unrotate(current_position);
+    rotation.unrotate(destination);
+  #endif
+
   // Get new XYZ position, whether absolute or relative
   LOOP_NUM_AXES(i) {
     if ( (seen[i] = parser.seenval(AXIS_CHAR(i))) ) {
@@ -183,6 +195,12 @@ void GcodeSuite::get_destination_from_command() {
       destination[i] = current_position[i];
   }
 
+  // Re-rotate the current position and destination
+  #if ENABLED(CNC_COORDINATE_ROTATION)
+    rotation.rotate(current_position);
+    rotation.rotate(destination);
+  #endif
+
   #if HAS_EXTRUDERS
     // Get new E position, whether absolute or relative
     if ( (seen.e = parser.seenval('E')) ) {
@@ -446,7 +464,12 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
 
       #if SAVED_POSITIONS
         case 60: G60(); break;                                    // G60:  save current position
-        case 61: G61(); break;                                    // G61:  Apply/restore saved coordinates.
+        case 61: G61(); break;                                    // G61:  Apply/restore saved coordinates
+      #endif
+
+      #if ENABLED(CNC_COORDINATE_ROTATION)
+        case 68: G68(); break;                                    // G68  - Set coordinate rotation center and angle
+        case 69: G69(); break;                                    // G69  - Reset coordinate rotation
       #endif
 
       #if ALL(PTC_PROBE, PTC_BED)

Marlin/src/gcode/gcode.h

@@ -66,6 +66,8 @@
  * G42  - Coordinated move to a mesh point (Requires MESH_BED_LEVELING, AUTO_BED_LEVELING_BLINEAR, or AUTO_BED_LEVELING_UBL)
  * G60  - Save current position. (Requires SAVED_POSITIONS)
  * G61  - Apply/restore saved coordinates. (Requires SAVED_POSITIONS)
+ * G68  - Set coordinate rotation center and angle. (Requires CNC_COORDINATE_ROTATION)
+ * G69  - Reset coordinate rotation. (Requires CNC_COORDINATE_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
@@ -373,6 +375,26 @@ enum AxisRelative : uint8_t {
 };
 typedef bits_t(NUM_REL_MODES) relative_t;
 
+#if ENABLED(CNC_COORDINATE_ROTATION)
+  typedef struct {
+    float x, y, rad, s, c;
+    void set_angle(const_float_t r) { rad = r; s = sin(r); c = cos(r); }
+    void reset() { x = y = rad = s = c = 0.0f; }
+    void rotate(xy_pos_t &point) {
+      if (!rad) return;
+      const xy_pos_t p = point;
+      point.x = x + (p.x - x) * c - (p.y - y) * s;
+      point.y = y + (p.x - x) * s + (p.y - y) * c;
+    }
+    void unrotate(xy_pos_t &point) {
+      if (!rad) return;
+      const xy_pos_t p = point;
+      point.x = x + (p.x - x) * c - (p.y - y) * s;
+      point.y = y + (p.x - x) * s + (p.y - y) * c;
+    }
+  } rotation_t;
+#endif
+
 extern const char G28_STR[];
 
 class GcodeSuite {
@@ -428,6 +450,10 @@ class GcodeSuite {
     static bool select_coordinate_system(const int8_t _new);
   #endif
 
+  #if ENABLED(CNC_COORDINATE_ROTATION)
+    static rotation_t rotation;
+  #endif
+
   static millis_t previous_move_ms, max_inactive_time;
   FORCE_INLINE static bool stepper_max_timed_out(const millis_t ms=millis()) {
     return max_inactive_time && ELAPSED(ms, previous_move_ms + max_inactive_time);
@@ -625,6 +651,12 @@ class GcodeSuite {
     static void G61(int8_t slot=-1);
   #endif
 
+  #if ENABLED(CNC_COORDINATE_ROTATION)
+    static void G68();
+    static void G68_report();
+    static void G69();
+  #endif
+
   #if ENABLED(GCODE_MOTION_MODES)
     static void G80();
   #endif

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

@@ -0,0 +1,56 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2023 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(CNC_COORDINATE_ROTATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+
+void GcodeSuite::G68_report() {
+  SERIAL_ECHOLNPGM("Rotation: R", DEGREES(rotation.rad), " X", rotation.x, " Y", rotation.y);
+}
+
+/**
+ * G68: Set a center and angle for coordinate rotation
+ *   X<linear> - X center for rotation, workspace-relative
+ *   Y<linear> - Y center for rotation, workspace-relative
+ *   R<angle>  - Coordinate Rotation in Degrees
+ */
+void GcodeSuite::G68() {
+  if (!parser.seen_any()) return G68_report();
+  if (parser.seenval('X') || parser.seenval('A')) rotation.x = RAW_X_POSITION(parser.value_linear_units());
+  if (parser.seenval('Y') || parser.seenval('B')) rotation.y = RAW_Y_POSITION(parser.value_linear_units());
+  if (parser.seenval('R')) rotation.set_angle(RADIANS(parser.value_float()));
+}
+
+/**
+ * G69: Cancel coordinate rotation
+ */
+void GcodeSuite::G69() {
+
+  rotation.reset();
+
+}
+
+#endif // CNC_COORDINATE_ROTATION

Marlin/src/module/kinematics/g50_g51.h

@@ -0,0 +1,41 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2023 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 "transform_gcode.h"
+
+/**
+ * G50 takes a center and a scale, and scales subsequent G-code coordinates by that scale centered on that center
+ *
+ * G51 cancels scaling
+ */
+class G50G51 : TransformGCode {
+
+  // This is currently just a skeleton to give an idea as to how the kinematics refactoring might be implemented
+
+  matrix_3x3 *get_forward_kinematics();
+  matrix_3x3 *get_inverse_kinematics();
+  vector_3 *get_offset();
+
+  void execute_g50();
+  void execute_g51(xyz_pos_t &center, xyz_float_t &scale);
+
+};

Marlin/src/module/kinematics/kinematics.h

@@ -0,0 +1,36 @@
+/**
+ * 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 "../../core/types.h"
+
+class Kinematics {
+
+  public:
+    static xyz_pos_t mechanical_to_machine(xyz_pos_t &mechanical);
+    static xyz_pos_t machine_to_gcode(xyz_pos_t &machine);
+    static xyz_pos_t gcode_to_machine(xyz_pos_t &gcode);
+    static xyz_pos_t machine_to_mechanical(xyz_pos_t &machine);
+    static xyz_pos_t gcode_to_mechanical(xyz_pos_t &gcode);
+    static xyz_pos_t mechanical_to_gcode(xyz_pos_t &mechanical);
+    static void set_cache_invalid(); // Called when a gcode has been executed which invalidates the previously-cached aggregrate transforms
+
+};

Marlin/src/module/kinematics/transform_gcode.h

@@ -0,0 +1,45 @@
+/**
+ * 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 "../../libs/vector_3.h"
+
+/**
+ * A transform gcode is a gcode which changes how subsequent gcode coordinates are mapped onto machine coordinates
+ */
+class TransformGCode {
+
+  /**
+   * Forward kinematics go in the direction of mechanical -> machine -> gcode
+   */
+  virtual matrix_3x3 *get_forward_kinematics() = 0;
+
+  /**
+   * Inverse kinematics go in the direction gcode -> machine -> mechanical
+   */
+  virtual matrix_3x3 *get_inverse_kinematics() = 0;
+
+  /**
+   * The offset is added before applying forward kinematics, or subtracted after applying inverse kinematics
+   */
+  virtual vector_3 *get_offset() = 0;
+
+};

buildroot/tests/DUE

@@ -25,7 +25,7 @@ opt_enable S_CURVE_ACCELERATION EEPROM_SETTINGS GCODE_MACROS \
            AUTO_BED_LEVELING_BILINEAR Z_MIN_PROBE_REPEATABILITY_TEST DEBUG_LEVELING_FEATURE \
            SKEW_CORRECTION SKEW_CORRECTION_FOR_Z SKEW_CORRECTION_GCODE CALIBRATION_GCODE \
            BACKLASH_COMPENSATION BACKLASH_GCODE BAUD_RATE_GCODE BEZIER_CURVE_SUPPORT \
-           FWRETRACT ARC_SUPPORT ARC_P_CIRCLES CNC_WORKSPACE_PLANES CNC_COORDINATE_SYSTEMS \
+           FWRETRACT ARC_SUPPORT ARC_P_CIRCLES CNC_WORKSPACE_PLANES CNC_COORDINATE_SYSTEMS CNC_COORDINATE_ROTATION \
            PSU_CONTROL AUTO_POWER_CONTROL E_DUAL_STEPPER_DRIVERS \
            PIDTEMPBED SLOW_PWM_HEATERS THERMAL_PROTECTION_CHAMBER \
            PINS_DEBUGGING MAX7219_DEBUG M114_DETAIL MAX7219_REINIT_ON_POWERUP \
@@ -39,7 +39,7 @@ exec_test $1 $2 "RAMPS4DUE_EFB with ABL (Bilinear), ExtUI, S-Curve, many options
 restore_configs
 opt_set MOTHERBOARD BOARD_RADDS Z_DRIVER_TYPE A4988 Z2_DRIVER_TYPE A4988 Z3_DRIVER_TYPE A4988 \
         X_MAX_PIN -1 Y_MAX_PIN -1
-opt_enable ENDSTOPPULLUPS BLTOUCH AUTO_BED_LEVELING_BILINEAR \
+opt_enable ENDSTOPPULLUPS BLTOUCH AUTO_BED_LEVELING_BILINEAR CNC_COORDINATE_ROTATION \
            Z_STEPPER_AUTO_ALIGN Z_STEPPER_ALIGN_STEPPER_XY Z_SAFE_HOMING
 exec_test $1 $2 "RADDS with ABL (Bilinear), Triple Z Axis, Z_STEPPER_AUTO_ALIGN, E_DUAL_STEPPER_DRIVERS" "$3"
 

ini/features.ini

@@ -319,6 +319,7 @@ FILAMENT_LOAD_UNLOAD_GCODES            = build_src_filter=+<src/gcode/feature/pa
 HAS_STEALTHCHOP                        = build_src_filter=+<src/gcode/feature/trinamic/M569.cpp>
 CNC_WORKSPACE_PLANES                   = build_src_filter=+<src/gcode/geometry/G17-G19.cpp>
 CNC_COORDINATE_SYSTEMS                 = build_src_filter=+<src/gcode/geometry/G53-G59.cpp>
+CNC_COORDINATE_ROTATION                = build_src_filter=+<src/gcode/geometry/G68-G69.cpp>
 HAS_HOME_OFFSET                        = build_src_filter=+<src/gcode/geometry/M206_M428.cpp>
 EXPECTED_PRINTER_CHECK                 = build_src_filter=+<src/gcode/host/M16.cpp>
 HOST_KEEPALIVE_FEATURE                 = build_src_filter=+<src/gcode/host/M113.cpp>