diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 39cc1d5689c7..8bdd7dee4cd6 100644 --- a/Marlin/Configuration_adv.h +++ b/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 /** diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index 9fed4dcada3f..97444d601efd 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/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) diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index 589cd2bc486c..972cf7fe0ddf 100644 --- a/Marlin/src/gcode/gcode.h +++ b/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 diff --git a/Marlin/src/gcode/geometry/G68-G69.cpp b/Marlin/src/gcode/geometry/G68-G69.cpp new file mode 100644 index 000000000000..ab456a8c9e01 --- /dev/null +++ b/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 . + * + */ + +#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 - X center for rotation, workspace-relative + * Y - Y center for rotation, workspace-relative + * R - 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 diff --git a/Marlin/src/module/kinematics/g50_g51.h b/Marlin/src/module/kinematics/g50_g51.h new file mode 100644 index 000000000000..29a3cff2ce94 --- /dev/null +++ b/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 . + * + */ + +#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 ¢er, xyz_float_t &scale); + +}; diff --git a/Marlin/src/module/kinematics/kinematics.h b/Marlin/src/module/kinematics/kinematics.h new file mode 100644 index 000000000000..ea509d1ea7fb --- /dev/null +++ b/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 . + * + */ + +#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 + +}; diff --git a/Marlin/src/module/kinematics/transform_gcode.h b/Marlin/src/module/kinematics/transform_gcode.h new file mode 100644 index 000000000000..44c900234645 --- /dev/null +++ b/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 . + * + */ + +#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; + +}; diff --git a/buildroot/tests/DUE b/buildroot/tests/DUE index 33f6bd511a65..3e031627db84 100755 --- a/buildroot/tests/DUE +++ b/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" diff --git a/docs/Skew Mathematics.odt b/docs/Skew Mathematics.odt new file mode 100644 index 000000000000..9e41c3c6b0cf Binary files /dev/null and b/docs/Skew Mathematics.odt differ diff --git a/ini/features.ini b/ini/features.ini index b2e9d6884e75..57301f7eca6f 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -319,6 +319,7 @@ FILAMENT_LOAD_UNLOAD_GCODES = build_src_filter=+ CNC_WORKSPACE_PLANES = build_src_filter=+ CNC_COORDINATE_SYSTEMS = build_src_filter=+ +CNC_COORDINATE_ROTATION = build_src_filter=+ HAS_HOME_OFFSET = build_src_filter=+ EXPECTED_PRINTER_CHECK = build_src_filter=+ HOST_KEEPALIVE_FEATURE = build_src_filter=+