From c656a62d41e4a573051f90dde2176cc86540c2fa Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 24 Sep 2020 18:16:47 -0500 Subject: [PATCH] Homing cycle settings (#613) * Initial Tests Complete * Update Grbl.h * Update variables Co-authored-by: Mitch Bradley --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 75 +++++++------------------- Grbl_Esp32/src/SettingsDefinitions.cpp | 7 +++ Grbl_Esp32/src/SettingsDefinitions.h | 1 + 4 files changed, 29 insertions(+), 56 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 332a4f62..dfe8b8b0 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -23,7 +23,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20200919"; +const char* const GRBL_VERSION_BUILD = "20200924"; //#include #include diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 3e93f8e1..474ad502 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -118,7 +118,7 @@ void mc_arc(float* target, #ifdef USE_KINEMATICS float previous_position[MAX_N_AXIS]; uint16_t n; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (n = 0; n < n_axis; n++) { previous_position[n] = position[n]; } @@ -313,59 +313,24 @@ void mc_homing_cycle(uint8_t cycle_mask) { else #endif { - // Search to engage all axes limit switches at faster homing seek rate. - if (!axis_is_squared(HOMING_CYCLE_0)) { - limits_go_home(HOMING_CYCLE_0); // Homing cycle 0 - } else { - ganged_mode = SquaringMode::Dual; - n_homing_locate_cycle = 0; // don't do a second touch cycle - limits_go_home(HOMING_CYCLE_0); - ganged_mode = SquaringMode::A; - n_homing_locate_cycle = NHomingLocateCycle; // restore to default value - limits_go_home(HOMING_CYCLE_0); - ganged_mode = SquaringMode::B; - limits_go_home(HOMING_CYCLE_0); - ganged_mode = SquaringMode::Dual; // always return to dual + for (int cycle = 0; cycle < MAX_N_AXIS; cycle++) { + auto homing_mask = homing_cycle[cycle]->get(); + if (homing_mask) { // if there are some axes in this cycle + if (!axis_is_squared(homing_mask)) { + limits_go_home(homing_mask); // Homing cycle 0 + } else { + ganged_mode = SquaringMode::Dual; + n_homing_locate_cycle = 0; // don't do a second touch cycle + limits_go_home(homing_mask); + ganged_mode = SquaringMode::A; + n_homing_locate_cycle = NHomingLocateCycle; // restore to default value + limits_go_home(homing_mask); + ganged_mode = SquaringMode::B; + limits_go_home(homing_mask); + ganged_mode = SquaringMode::Dual; // always return to dual + } + } } -#ifdef HOMING_CYCLE_1 - if (!axis_is_squared(HOMING_CYCLE_1)) { - limits_go_home(HOMING_CYCLE_1); - } else { - ganged_mode = SquaringMode::Dual; - n_homing_locate_cycle = 0; // don't do a second touch cycle - limits_go_home(HOMING_CYCLE_1); - ganged_mode = SquaringMode::A; - n_homing_locate_cycle = NHomingLocateCycle; // restore to default value - limits_go_home(HOMING_CYCLE_1); - ganged_mode = SquaringMode::B; - limits_go_home(HOMING_CYCLE_1); - ganged_mode = SquaringMode::Dual; // always return to dual - } -#endif -#ifdef HOMING_CYCLE_2 - if (!axis_is_squared(HOMING_CYCLE_2)) { - limits_go_home(HOMING_CYCLE_2); - } else { - ganged_mode = SquaringMode::Dual; - n_homing_locate_cycle = 0; // don't do a second touch cycle - limits_go_home(HOMING_CYCLE_2); - ganged_mode = SquaringMode::A; - n_homing_locate_cycle = NHomingLocateCycle; // restore to default value - limits_go_home(HOMING_CYCLE_2); - ganged_mode = SquaringMode::B; - limits_go_home(HOMING_CYCLE_2); - ganged_mode = SquaringMode::Dual; // always return to dual - } -#endif -#ifdef HOMING_CYCLE_3 - limits_go_home(HOMING_CYCLE_3); // Homing cycle 3 -#endif -#ifdef HOMING_CYCLE_4 - limits_go_home(HOMING_CYCLE_4); // Homing cycle 4 -#endif -#ifdef HOMING_CYCLE_5 - limits_go_home(HOMING_CYCLE_5); // Homing cycle 5 -#endif } protocol_execute_realtime(); // Check for reset and set system abort. if (sys.abort) { @@ -508,8 +473,8 @@ void mc_reset() { sys_io_control(0xFF, LOW, false); sys_pwm_control(0xFF, 0, false); #ifdef ENABLE_SD_CARD - // do we need to stop a running SD job? - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + // do we need to stop a running SD job? + if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { //Report print stopped report_feedback_message(Message::SdFileQuit); closeFile(); diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index caeb5621..c6e37b4c 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -39,6 +39,7 @@ FloatSetting* homing_feed_rate; FloatSetting* homing_seek_rate; FloatSetting* homing_debounce; FloatSetting* homing_pulloff; +AxisMaskSetting* homing_cycle[MAX_N_AXIS]; FloatSetting* spindle_pwm_freq; FloatSetting* rpm_max; FloatSetting* rpm_min; @@ -344,4 +345,10 @@ void make_settings() { pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); spindle_type = new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", static_cast(SPINDLE_TYPE), &spindleTypes); stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, checkStallguardDebugMask); + + const char* homing_names[] = { "Homing/Cycle0", "Homing/Cycle1", "Homing/Cycle2", + "Homing/Cycle3", "Homing/Cycle4", "Homing/Cycle5" }; + for (uint8_t i = 0; i < MAX_N_AXIS; i++) { + homing_cycle[i] = new AxisMaskSetting(EXTENDED, WG, NULL, homing_names[i], 0); + } } diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index c3fe8310..0fd68b33 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -23,6 +23,7 @@ extern AxisMaskSetting* step_invert_mask; extern AxisMaskSetting* dir_invert_mask; extern AxisMaskSetting* homing_dir_mask; extern AxisMaskSetting* homing_squared_axes; +extern AxisMaskSetting* homing_cycle[MAX_N_AXIS]; extern FlagSetting* step_enable_invert; extern FlagSetting* limit_invert;