From 8e6646f0e8d90bf0a85a25b8c88cf09637a953a3 Mon Sep 17 00:00:00 2001 From: Stefan de Bruijn Date: Sat, 19 Sep 2020 22:20:47 +0200 Subject: [PATCH 01/82] Fixed various small bugs (#605) * Fixed various small bugs * Fixed potential cast bug * Fixed double reporting of errors Co-authored-by: Stefan de Bruijn --- Grbl_Esp32/src/Eeprom.cpp | 9 +++++---- Grbl_Esp32/src/GCode.cpp | 17 +++++++++++++++++ Grbl_Esp32/src/Motors/Dynamixel2.cpp | 3 ++- Grbl_Esp32/src/Report.cpp | 23 ++++++++++++----------- 4 files changed, 36 insertions(+), 16 deletions(-) diff --git a/Grbl_Esp32/src/Eeprom.cpp b/Grbl_Esp32/src/Eeprom.cpp index 7096d24f..6654289a 100644 --- a/Grbl_Esp32/src/Eeprom.cpp +++ b/Grbl_Esp32/src/Eeprom.cpp @@ -23,9 +23,10 @@ void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size) { unsigned char checksum = 0; for (; size > 0; size--) { - checksum = (checksum << 1) || (checksum >> 7); - checksum += *source; - EEPROM.write(destination++, *(source++)); + unsigned char data = static_cast(*source++); + checksum = (checksum << 1) | (checksum >> 7); + checksum += data; + EEPROM.write(destination++, data); } EEPROM.write(destination, checksum); EEPROM.commit(); @@ -35,7 +36,7 @@ int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, uns unsigned char data, checksum = 0; for (; size > 0; size--) { data = EEPROM.read(source++); - checksum = (checksum << 1) || (checksum >> 7); + checksum = (checksum << 1) | (checksum >> 7); checksum += data; *(destination++) = data; } diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 2c5bef76..3f2c00c0 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -37,6 +37,23 @@ parser_block_t gc_block; #define FAIL(status) return (status); void gc_init() { + // First thing we do here is iterate through the coord systems and read them all, so that + // we get all our coord system errors here, and not while we're busy: + float coord_system[MAX_N_AXIS]; + + // g54 - g59 is 6 coordinate systems, plus 2 for G28 and G30 reference positions + bool reported_error = false; + const int MAX_COORD_SYSTEMS = 8; + for (uint8_t i = 0; i < MAX_COORD_SYSTEMS; ++i) { + if (!(settings_read_coord_data(i, coord_system))) { + if (!reported_error) { + reported_error = true; + report_status_message(Error::SettingReadFail, CLIENT_SERIAL); + } + } + } + + // Reset parser state: memset(&gc_state, 0, sizeof(parser_state_t)); // Load default G54 coordinate system. if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system))) { diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp index 73057b4b..fb0314f4 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp +++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp @@ -365,7 +365,8 @@ namespace Motors { tx_message[++msg_index] = 4; // low order data length tx_message[++msg_index] = 0; // high order data length - for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { + auto n_axis = number_axis->get(); + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { current_id = ids[axis][gang_index]; if (current_id != 0) { diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 38d160b8..e93b578a 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -167,6 +167,7 @@ void grbl_notifyf(const char* title, const char* format, ...) { } // formats axis values into a string and returns that string in rpt +// NOTE: rpt should have at least size: 20 * MAX_N_AXIS static void report_util_axis_values(float* axis_value, char* rpt) { uint8_t idx; char axisVal[20]; @@ -178,9 +179,9 @@ static void report_util_axis_values(float* axis_value, char* rpt) { auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { if (report_inches->get()) { - sprintf(axisVal, "%4.4f", axis_value[idx] * unit_conv); // Report inches to 4 decimals + snprintf(axisVal, 19, "%4.4f", axis_value[idx] * unit_conv); // Report inches to 4 decimals } else { - sprintf(axisVal, "%4.3f", axis_value[idx] * unit_conv); // Report mm to 3 decimals + snprintf(axisVal, 19, "%4.3f", axis_value[idx] * unit_conv); // Report mm to 3 decimals } strcat(rpt, axisVal); if (idx < (number_axis->get() - 1)) { @@ -283,8 +284,8 @@ void report_grbl_help(uint8_t client) { void report_probe_parameters(uint8_t client) { // Report in terms of machine position. float print_position[MAX_N_AXIS]; - char probe_rpt[100]; // the probe report we are building here - char temp[60]; + char probe_rpt[(MAX_N_AXIS * 20 + 13 + 6 + 1)]; // the probe report we are building here + char temp[MAX_N_AXIS * 20]; strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters // get the machine position and put them into a string and append to the probe report system_convert_array_steps_to_mpos(print_position, sys_probe_position); @@ -300,8 +301,8 @@ void report_probe_parameters(uint8_t client) { void report_ngc_parameters(uint8_t client) { float coord_data[MAX_N_AXIS]; uint8_t coord_select; - char temp[60]; - char ngc_rpt[500]; + char temp[MAX_N_AXIS * 20]; + char ngc_rpt[((8 + (MAX_N_AXIS * 20)) * SETTING_INDEX_NCOORD + 4 + MAX_N_AXIS * 20 + 8 + 2 * 20)]; ngc_rpt[0] = '\0'; for (coord_select = 0; coord_select <= SETTING_INDEX_NCOORD; coord_select++) { if (!(settings_read_coord_data(coord_select, coord_data))) { @@ -332,9 +333,9 @@ void report_ngc_parameters(uint8_t client) { strcat(ngc_rpt, "]\r\n"); strcat(ngc_rpt, "[TLO:"); // Print tool length offset value if (report_inches->get()) { - sprintf(temp, "%4.3f]\r\n", gc_state.tool_length_offset * INCH_PER_MM); + snprintf(temp, 20, "%4.3f]\r\n", gc_state.tool_length_offset * INCH_PER_MM); } else { - sprintf(temp, "%4.3f]\r\n", gc_state.tool_length_offset); + snprintf(temp, 20, "%4.3f]\r\n", gc_state.tool_length_offset); } strcat(ngc_rpt, temp); grbl_send(client, ngc_rpt); @@ -598,7 +599,7 @@ void report_realtime_status(uint8_t client) { memcpy(current_position, sys_position, sizeof(sys_position)); float print_position[MAX_N_AXIS]; char status[200]; - char temp[80]; + char temp[MAX_N_AXIS * 20]; system_convert_array_steps_to_mpos(print_position, current_position); // Report current machine state and sub-states strcpy(status, "<"); @@ -670,7 +671,7 @@ void report_realtime_status(uint8_t client) { if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) { strcat(status, "|MPos:"); } else { -#ifdef USE_FWD_KINEMATICS +#ifdef USE_FWD_KINEMATICS forward_kinematics(print_position); #endif strcat(status, "|WPos:"); @@ -855,7 +856,7 @@ void report_realtime_status(uint8_t client) { void report_realtime_steps() { uint8_t idx; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { grbl_sendf(CLIENT_ALL, "%ld\n", sys_position[idx]); // OK to send to all ... debug stuff } From cd8c3cb89b000e65ad75c7e7e7490cc60337cd73 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 20 Sep 2020 08:28:02 -0500 Subject: [PATCH 02/82] Stallguard tuning (#607) * Devt (#571) * Handles Tranimic drivers errors better - If an unsupported driver is specified, it will give a message and not crash. * Cleaned up unused files Got rid of old unipolar files Got rid of servo axis feature - it is a motor class now Got rid of solenoid pen feature - never really used and it should be a motor class if it is. * Fix ENABLE_AUTHENTICATION (#569) * Fixed authentication code. * Removed another const cast Co-authored-by: Stefan de Bruijn * Fix step leakage with inverted steps (#570) * Fix step leakage with inverted steps * Update build date for merge Co-authored-by: Bart Dring Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring * Update platformio.ini Per PR 583 * Created an enum for mode * Removing some unused machine defs * Added test machine definition * Clean up for PR * Remove test machine def. Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Machines/6_pack_trinamic_V1.h | 140 ------------------ .../src/Machines/6_pack_trinamic_stallguard.h | 19 +-- .../src/Machines/i2s_out_xyzabc_trinamic.h | 4 +- .../src/Machines/spi_daisy_4axis_xyyz.h | 4 +- Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h | 4 +- .../src/Machines/spi_daisy_4axis_xyza.h | 2 +- Grbl_Esp32/src/Machines/tmc2130_pen.h | 4 +- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 14 +- Grbl_Esp32/src/Motors/TrinamicDriver.h | 23 ++- 10 files changed, 43 insertions(+), 173 deletions(-) delete mode 100644 Grbl_Esp32/src/Machines/6_pack_trinamic_V1.h diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 1fb409dd..332a4f62 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 = "20200910"; +const char* const GRBL_VERSION_BUILD = "20200919"; //#include #include diff --git a/Grbl_Esp32/src/Machines/6_pack_trinamic_V1.h b/Grbl_Esp32/src/Machines/6_pack_trinamic_V1.h deleted file mode 100644 index 9959dd1e..00000000 --- a/Grbl_Esp32/src/Machines/6_pack_trinamic_V1.h +++ /dev/null @@ -1,140 +0,0 @@ -#pragma once -// clang-format off - -/* - 6_pack_trinamic_V1.h - Part of Grbl_ESP32 - Pin assignments for the ESP32 SPI 6-axis board - 2018 - Bart Dring - 2020 - Mitch Bradley - 2020 - Michiyasu Odaki - Grbl_ESP32 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. - Grbl 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 Grbl_ESP32. If not, see . -*/ -#define MACHINE_NAME "6 Pack Controller V1 (Trinamic)" - -#define N_AXIS 6 - -// I2S (steppers & other output-only pins) -#define USE_I2S_OUT -#define USE_I2S_STEPS -//#define DEFAULT_STEPPER ST_I2S_STATIC - -#define I2S_OUT_BCK GPIO_NUM_22 -#define I2S_OUT_WS GPIO_NUM_17 -#define I2S_OUT_DATA GPIO_NUM_21 - - -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP -#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP - - -#define X_TRINAMIC_DRIVER 2130 -#define X_DISABLE_PIN I2SO(0) -#define X_DIRECTION_PIN I2SO(1) -#define X_STEP_PIN I2SO(2) -#define X_CS_PIN I2SO(3) -#define X_RSENSE TMC2130_RSENSE_DEFAULT - -#define Y_TRINAMIC_DRIVER X_TRINAMIC_DRIVER -#define Y_DIRECTION_PIN I2SO(4) -#define Y_STEP_PIN I2SO(5) -#define Y_DISABLE_PIN I2SO(7) -#define Y_CS_PIN I2SO(6) -#define Y_RSENSE X_RSENSE - -#define Z_TRINAMIC_DRIVER X_TRINAMIC_DRIVER -#define Z_DISABLE_PIN I2SO(8) -#define Z_DIRECTION_PIN I2SO(9) -#define Z_STEP_PIN I2SO(10) -#define Z_CS_PIN I2SO(11) -#define Z_RSENSE X_RSENSE - -#define A_TRINAMIC_DRIVER X_TRINAMIC_DRIVER -#define A_DIRECTION_PIN I2SO(12) -#define A_STEP_PIN I2SO(13) -#define A_DISABLE_PIN I2SO(15) -#define A_CS_PIN I2SO(14) -#define A_RSENSE X_RSENSE - -#define B_TRINAMIC_DRIVER X_TRINAMIC_DRIVER -#define B_DISABLE_PIN I2SO(16) -#define B_DIRECTION_PIN I2SO(17) -#define B_STEP_PIN I2SO(18) -#define B_CS_PIN I2SO(19) -#define B_RSENSE X_RSENSE - -#define C_TRINAMIC_DRIVER X_TRINAMIC_DRIVER -#define C_DIRECTION_PIN I2SO(20) -#define C_STEP_PIN I2SO(21) -#define C_DISABLE_PIN I2SO(23) -#define C_CS_PIN I2SO(22) -#define C_RSENSE X_RSENSE - -/* - Socket I/O reference - The list of modules is here... - https://github.com/bdring/6-Pack_CNC_Controller/wiki/CNC-I-O-Module-List - Click on each module to get example for using the modules in the sockets - -Socket #1 -#1 GPIO_NUM_33 -#2 GPIO_NUM_32 -#3 GPIO_NUM_35 (input only) -#4 GPIO_NUM_34 (input only) - -Socket #2 -#1 GPIO_NUM_2 -#2 GPIO_NUM_25 -#3 GPIO_NUM_39 (input only) -#4 GPIO_NUM_36 (input only) - -Socket #3 -#1 GPIO_NUM_26 -#2 GPIO_NUM_4 -#3 GPIO_NUM_16 -#4 GPIO_NUM_27 - -Socket #4 -#1 GPIO_NUM_14 -#2 GPIO_NUM_13 -#3 GPIO_NUM_15 -#4 GPIO_NUM_12 - -Socket #5 -#1 I2SO(24) (output only) -#2 I2SO(25) (output only) -#3 I2SO26) (output only) - -*/ - -// 4x Input Module in Socket #1 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -#define X_LIMIT_PIN GPIO_NUM_33 -#define Y_LIMIT_PIN GPIO_NUM_32 -#define Z_LIMIT_PIN GPIO_NUM_35 -#define A_LIMIT_PIN GPIO_NUM_34 - -// 4x Input Module in Socket #2 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -#define B_LIMIT_PIN GPIO_NUM_2 -#define C_LIMIT_PIN GPIO_NUM_25 -#define PROBE_PIN GPIO_NUM_39 - -// 0-10v CNC Module in Socket #3 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/0-10V-Output-Module -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_26 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 -#define SPINDLE_DIR_PIN GPIO_NUM_16 - -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE diff --git a/Grbl_Esp32/src/Machines/6_pack_trinamic_stallguard.h b/Grbl_Esp32/src/Machines/6_pack_trinamic_stallguard.h index f5a9be34..83518936 100644 --- a/Grbl_Esp32/src/Machines/6_pack_trinamic_stallguard.h +++ b/Grbl_Esp32/src/Machines/6_pack_trinamic_stallguard.h @@ -32,9 +32,8 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 - -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP -#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_STALLGUARD +#define TRINAMIC_RUN_MODE Motors::TrinamicMode::CoolStep +#define TRINAMIC_HOMING_MODE Motors::TrinamicMode::StallGuard // Motor Socket #1 #define X_TRINAMIC_DRIVER 2130 @@ -91,16 +90,16 @@ Click on each module to get example for using the modules in the sockets Socket #1 -#1 GPIO_NUM_33 -#2 GPIO_NUM_32 -#3 GPIO_NUM_35 (input only) -#4 GPIO_NUM_34 (input only) +#1 GPIO_NUM_33 (Sg1) +#2 GPIO_NUM_32 (Sg2) +#3 GPIO_NUM_35 (Sg3) (input only) +#4 GPIO_NUM_34 (Sg4) (input only) Socket #2 #1 GPIO_NUM_2 #2 GPIO_NUM_25 -#3 GPIO_NUM_39 (input only) -#4 GPIO_NUM_36 (input only) +#3 GPIO_NUM_39 (Sg5) (input only) +#4 GPIO_NUM_36 (Sg6) (input only) Socket #3 #1 GPIO_NUM_26 @@ -122,12 +121,14 @@ Socket #5 */ // Socket #1 (Empty) +// Install StallGuard Jumpers #define X_LIMIT_PIN GPIO_NUM_33 // Sg1 #define Y_LIMIT_PIN GPIO_NUM_32 // Sg2 #define Z_LIMIT_PIN GPIO_NUM_35 // Sg3 #define A_LIMIT_PIN GPIO_NUM_34 // Sg4 // Socket #2 (Empty) +// Install StallGuard Jumpers #define B_LIMIT_PIN GPIO_NUM_39 // Sg5 #define C_LIMIT_PIN GPIO_NUM_36 // Sg6 diff --git a/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h b/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h index cce3fa6c..3d5cffb7 100644 --- a/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h +++ b/Grbl_Esp32/src/Machines/i2s_out_xyzabc_trinamic.h @@ -37,8 +37,8 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP -#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP +#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep +#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep #define X_TRINAMIC_DRIVER 2130 #define X_DISABLE_PIN I2SO(0) diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h index 1286b702..1bd15efd 100644 --- a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h +++ b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h @@ -35,8 +35,8 @@ #define TRINAMIC_DAISY_CHAIN -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP -#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP +#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep +#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep // Use SPI enable instead of the enable pin // The hardware enable pin is tied to ground diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h index 28ef76ea..fc9b45f6 100644 --- a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h +++ b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyz.h @@ -35,8 +35,8 @@ #define TRINAMIC_DAISY_CHAIN -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP -#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP +#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep +#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep // Use SPI enable instead of the enable pin // The hardware enable pin is tied to ground diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyza.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyza.h index 62baebd5..5ca27c61 100644 --- a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyza.h +++ b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyza.h @@ -35,7 +35,7 @@ #define TRINAMIC_DAISY_CHAIN -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_STEALTHCHOP +#define TRINAMIC_RUN_MODE TrinamicMode::StealthChop // Use SPI enable instead of the enable pin // The hardware enable pin is tied to ground diff --git a/Grbl_Esp32/src/Machines/tmc2130_pen.h b/Grbl_Esp32/src/Machines/tmc2130_pen.h index c1477abb..8c938015 100644 --- a/Grbl_Esp32/src/Machines/tmc2130_pen.h +++ b/Grbl_Esp32/src/Machines/tmc2130_pen.h @@ -38,8 +38,8 @@ #define X_LIMIT_PIN GPIO_NUM_32 #endif -#define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP -#define TRINAMIC_HOMING_MODE TRINAMIC_MODE_COOLSTEP +#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep +#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep #define X_STEP_PIN GPIO_NUM_12 #define X_DIRECTION_PIN GPIO_NUM_26 diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index e8ed5d93..f785862c 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -205,21 +205,21 @@ namespace Motors { _lastMode = _mode; switch (_mode) { - case TRINAMIC_MODE_STEALTHCHOP: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_STEALTHCHOP"); + case TrinamicMode ::StealthChop: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); tmcstepper->en_pwm_mode(true); tmcstepper->pwm_autoscale(true); tmcstepper->diag1_stall(false); break; - case TRINAMIC_MODE_COOLSTEP: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_COOLSTEP"); + case TrinamicMode :: CoolStep: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); tmcstepper->en_pwm_mode(false); tmcstepper->pwm_autoscale(false); tmcstepper->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep tmcstepper->THIGH(NORMAL_THIGH); break; - case TRINAMIC_MODE_STALLGUARD: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_STALLGUARD"); + case TrinamicMode ::StallGuard: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); tmcstepper->en_pwm_mode(false); tmcstepper->pwm_autoscale(false); tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); @@ -286,7 +286,7 @@ namespace Motors { if (_disabled) { tmcstepper->toff(TRINAMIC_TOFF_DISABLE); } else { - if (_mode == TRINAMIC_MODE_STEALTHCHOP) { + if (_mode == TrinamicMode::StealthChop) { tmcstepper->toff(TRINAMIC_TOFF_STEALTHCHOP); } else { tmcstepper->toff(TRINAMIC_TOFF_COOLSTEP); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h index 46475c61..d7e7b681 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h @@ -24,9 +24,9 @@ #include // https://github.com/teemuatlut/TMCStepper -#define TRINAMIC_MODE_STEALTHCHOP 0 // very quiet -#define TRINAMIC_MODE_COOLSTEP 1 // everything runs cooler so higher current possible -#define TRINAMIC_MODE_STALLGUARD 2 // coolstep plus generates stall indication +//#define TRINAMIC_MODE_STEALTHCHOP 0 // very quiet +//#define TRINAMIC_MODE_COOLSTEP 1 // everything runs cooler so higher current possible +//#define TRINAMIC_MODE_STALLGUARD 2 // coolstep plus generates stall indication const float TMC2130_RSENSE_DEFAULT = 0.11f; const float TMC5160_RSENSE_DEFAULT = 0.075f; @@ -39,8 +39,9 @@ const int TRINAMIC_SPI_FREQ = 100000; const double TRINAMIC_FCLK = 12700000.0; // Internal clock Approx (Hz) used to calculate TSTEP from homing rate // ==== defaults OK to define them in your machine definition ==== + #ifndef TRINAMIC_RUN_MODE -# define TRINAMIC_RUN_MODE TRINAMIC_MODE_COOLSTEP +# define TRINAMIC_RUN_MODE TrinamicMode ::StealthChop #endif #ifndef TRINAMIC_HOMING_MODE @@ -60,6 +61,14 @@ const double TRINAMIC_FCLK = 12700000.0; // Internal clock Approx (Hz) used to #endif namespace Motors { + + enum class TrinamicMode : uint8_t { + None = 0, // not for machine defs! + StealthChop = 1, + CoolStep = 2, + StallGuard = 3, + }; + class TrinamicDriver : public StandardStepper { public: TrinamicDriver(uint8_t axis_index, @@ -86,14 +95,14 @@ namespace Motors { uint32_t calc_tstep(float speed, float percent); TMC2130Stepper* tmcstepper; // all other driver types are subclasses of this one - uint8_t _homing_mode; + TrinamicMode _homing_mode; uint8_t cs_pin = UNDEFINED_PIN; // The chip select pin (can be the same for daisy chain) uint16_t _driver_part_number; // example: use 2130 for TMC2130 float _r_sense; int8_t spi_index; protected: - uint8_t _mode; - uint8_t _lastMode = 255; + TrinamicMode _mode; + TrinamicMode _lastMode = TrinamicMode::None; }; } From 286ed2a1046b4471af27fa05c4f38dd0df0af8f3 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 22 Sep 2020 17:42:59 -0500 Subject: [PATCH 03/82] Basic testing Complete --- Grbl_Esp32/src/Grbl.cpp | 1 + Grbl_Esp32/src/Limits.cpp | 8 ++++---- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 18 ++++++++++++++++- Grbl_Esp32/src/Protocol.cpp | 8 ++++---- Grbl_Esp32/src/Stepper.cpp | 20 +++++++++---------- Grbl_Esp32/src/System.cpp | 1 + Grbl_Esp32/src/System.h | 5 +++-- 7 files changed, 39 insertions(+), 22 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index 8aaeebdc..737bb63e 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -86,6 +86,7 @@ static void reset_variables() { memset(sys_probe_position, 0, sizeof(sys_probe_position)); // Clear probe position. sys_probe_state = 0; sys_rt_exec_state = 0; + cycle_stop = false; sys_rt_exec_motion_override = 0; sys_rt_exec_accessory_override = 0; system_clear_exec_alarm(); diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 5d1bdb4e..8c5e252c 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -202,7 +202,7 @@ void limits_go_home(uint8_t cycle_mask) { } st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. // Exit routines: No time to run protocol_execute_realtime() in this loop. - if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) { + if ((sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET)) || cycle_stop) { uint8_t rt_exec = sys_rt_exec_state; // Homing failure condition: Reset issued during cycle. if (rt_exec & EXEC_RESET) { @@ -217,7 +217,7 @@ void limits_go_home(uint8_t cycle_mask) { system_set_exec_alarm(ExecAlarm::HomingFailPulloff); } // Homing failure condition: Limit switch not found during approach. - if (approach && (rt_exec & EXEC_CYCLE_STOP)) { + if (approach && cycle_stop) { system_set_exec_alarm(ExecAlarm::HomingFailApproach); } @@ -228,7 +228,7 @@ void limits_go_home(uint8_t cycle_mask) { return; } else { // Pull-off motion complete. Disable CYCLE_STOP from executing. - system_clear_exec_state_flag(EXEC_CYCLE_STOP); + cycle_stop = false; break; } } @@ -371,7 +371,7 @@ void limits_disable() { // number in bit position, i.e. Z_AXIS is bit(2), and Y_AXIS is bit(1). uint8_t limits_get_state() { uint8_t pinMask = 0; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (int axis = 0; axis < n_axis; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { uint8_t pin = limit_pins[axis][gang_index]; diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index f18e3ec4..b8b70c3f 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -110,11 +110,27 @@ Socket #5 */ - // 4x Input Module in Socket #1 +/* +// 4x Input Module in Socket #1 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module #define X_LIMIT_PIN GPIO_NUM_33 #define Y_LIMIT_PIN GPIO_NUM_32 #define Z_LIMIT_PIN GPIO_NUM_35 +*/ + +// 4x Input Module in Socket #2 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +#define X_LIMIT_PIN GPIO_NUM_2 +#define Y_LIMIT_PIN GPIO_NUM_25 +#define Z_LIMIT_PIN GPIO_NUM_39 + +// 4x Input Module in Socket #3 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +#define CONTROL_CYCLE_START_PIN GPIO_NUM_26 +#define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 +#define CONTROL_RESET_PIN GPIO_NUM_16 +#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_27 +//#define INVERT_CONTROL_PIN_MASK B0000 // ================= Setting Defaults ========================== diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 106d90a5..d2d3d0cb 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -271,7 +271,7 @@ void protocol_exec_rt_system() { system_clear_exec_alarm(); // Clear alarm } uint8_t rt_exec = sys_rt_exec_state; // Copy volatile sys_rt_exec_state. - if (rt_exec) { + if (rt_exec || cycle_stop) { // Execute system abort. if (rt_exec & EXEC_RESET) { sys.abort = true; // Only place this is set true. @@ -399,12 +399,12 @@ void protocol_exec_rt_system() { } system_clear_exec_state_flag(EXEC_CYCLE_START); } - if (rt_exec & EXEC_CYCLE_STOP) { + if (cycle_stop) { // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by // realtime command execution in the main program, ensuring that the planner re-plans safely. // NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. - // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. + // NOTE: cycle_stop is set by the stepper subsystem when a cycle or feed hold completes. if ((sys.state == State::Hold || sys.state == State::SafetyDoor || sys.state == State::Sleep) && !(sys.soft_limit) && !(sys.suspend & SUSPEND_JOG_CANCEL)) { // Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user @@ -433,7 +433,7 @@ void protocol_exec_rt_system() { sys.state = State::Idle; } } - system_clear_exec_state_flag(EXEC_CYCLE_STOP); + cycle_stop = false; } } // Execute overrides. diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 639abc06..858e15c2 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -296,9 +296,8 @@ static void stepper_pulse_func() { spindle->set_rpm(0); } } - - system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. + cycle_stop = true; + return; // Nothing to do but exit. } } // Check probing state. @@ -362,8 +361,7 @@ static void stepper_pulse_func() { st.counter_a -= st.exec_block->step_event_count; if (st.exec_block->direction_bits & bit(A_AXIS)) { sys_position[A_AXIS]--; - } - else { + } else { sys_position[A_AXIS]++; } } @@ -379,8 +377,7 @@ static void stepper_pulse_func() { st.counter_b -= st.exec_block->step_event_count; if (st.exec_block->direction_bits & bit(B_AXIS)) { sys_position[B_AXIS]--; - } - else { + } else { sys_position[B_AXIS]++; } } @@ -396,8 +393,7 @@ static void stepper_pulse_func() { st.counter_c -= st.exec_block->step_event_count; if (st.exec_block->direction_bits & bit(C_AXIS)) { sys_position[C_AXIS]--; - } - else { + } else { sys_position[C_AXIS]++; } } @@ -1229,8 +1225,10 @@ float st_get_realtime_rate() { case State::Homing: case State::Hold: case State::Jog: - case State::SafetyDoor: return prep.current_speed; - default: return 0.0f; + case State::SafetyDoor: + return prep.current_speed; + default: + return 0.0f; } } diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 2aa6ad67..7c434ec3 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -30,6 +30,7 @@ volatile uint8_t sys_rt_exec_state; // Global realtime executor volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides. volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. +volatile bool cycle_stop; // For state transitions, instead of bitflag #ifdef DEBUG volatile uint8_t sys_rt_exec_debug; #endif diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 361bb804..e631d734 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -68,7 +68,7 @@ extern system_t sys; // know when there is a realtime command to execute. #define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 #define EXEC_CYCLE_START bit(1) // bitmask 00000010 -#define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 +// #define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 moved to cycle_stop #define EXEC_FEED_HOLD bit(3) // bitmask 00001000 #define EXEC_RESET bit(4) // bitmask 00010000 #define EXEC_SAFETY_DOOR bit(5) // bitmask 00100000 @@ -163,6 +163,7 @@ extern volatile uint8_t sys_rt_exec_state; // Global realtime executor bitfla extern volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. extern volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides. extern volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. +extern volatile bool cycle_stop; #ifdef DEBUG # define EXEC_DEBUG_REPORT bit(0) @@ -228,5 +229,5 @@ bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized); int8_t sys_get_next_RMT_chan_num(); -int8_t sys_get_next_PWM_chan_num(); +int8_t sys_get_next_PWM_chan_num(); uint8_t sys_calc_pwm_precision(uint32_t freq); From cf21de847aa39795b0244679a885752b083e6302 Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 23 Sep 2020 12:59:58 -0500 Subject: [PATCH 04/82] Made state variable volatile. --- Grbl_Esp32/src/System.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index e631d734..4568980d 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -39,7 +39,7 @@ enum class State : uint8_t { // Define global system variables typedef struct { - State state; // Tracks the current system state of Grbl. + volatile State state; // Tracks the current system state of Grbl. uint8_t abort; // System abort flag. Forces exit back to main loop for reset. uint8_t suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. uint8_t soft_limit; // Tracks soft limit errors for the state machine. (boolean) From c656a62d41e4a573051f90dde2176cc86540c2fa Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 24 Sep 2020 18:16:47 -0500 Subject: [PATCH 05/82] 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; From 1c1210686920e637dec27006003757c89f759d6d Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 24 Sep 2020 18:22:36 -0500 Subject: [PATCH 06/82] fixed dual switches when inverted (#614) * fixed dual switches when inverted * Removed debug message --- Grbl_Esp32/src/Limits.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 8c5e252c..382b02e1 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -376,7 +376,10 @@ uint8_t limits_get_state() { for (int gang_index = 0; gang_index < 2; gang_index++) { uint8_t pin = limit_pins[axis][gang_index]; if (pin != UNDEFINED_PIN) { - pinMask |= (digitalRead(pin) << axis); + if (limit_invert->get()) + pinMask |= (!digitalRead(pin) << axis); + else + pinMask |= (digitalRead(pin) << axis); } } } @@ -384,9 +387,6 @@ uint8_t limits_get_state() { #ifdef INVERT_LIMIT_PIN_MASK // not normally used..unless you have both normal and inverted switches pinMask ^= INVERT_LIMIT_PIN_MASK; #endif - if (limit_invert->get()) { - pinMask ^= limit_mask; - } return pinMask; } From b40fd88d16a35e8270a0c2d72f75191b716170af Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 25 Sep 2020 08:42:57 -0500 Subject: [PATCH 07/82] Cleaning up the machine defs Removed unused #defines. --- Grbl_Esp32/src/Config.h | 28 +------------------------ Grbl_Esp32/src/Machines/3axis_rs485.h | 14 +------------ Grbl_Esp32/src/Machines/3axis_v4.h | 13 ------------ Grbl_Esp32/src/Machines/atari_1020.h | 12 ++--------- Grbl_Esp32/src/Machines/midtbot.h | 18 +--------------- Grbl_Esp32/src/Machines/polar_coaster.h | 14 +++---------- Grbl_Esp32/src/Machines/template.h | 17 +-------------- 7 files changed, 9 insertions(+), 107 deletions(-) diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index ad29943b..205db46e 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -46,31 +46,7 @@ Some features should not be changed. See notes below. // machine.h is #included below, after some definitions // that the machine file might choose to undefine. -// Define the homing cycle patterns with bitmasks. The homing cycle first performs a search mode -// to quickly engage the limit switches, followed by a slower locate mode, and finished by a short -// pull-off motion to disengage the limit switches. The following HOMING_CYCLE_x defines are executed -// in order starting with suffix 0 and completes the homing routine for the specified-axes only. If -// an axis is omitted from the defines, it will not home, nor will the system update its position. -// Meaning that this allows for users with non-standard Cartesian machines, such as a lathe (x then z, -// with no y), to configure the homing cycle behavior to their needs. -// NOTE: The homing cycle is designed to allow sharing of limit pins, if the axes are not in the same -// cycle, but this requires some pin settings changes in the machine definition file. For example, the default homing -// cycle can share the Z limit pin with either X or Y limit pins, since they are on different cycles. -// By sharing a pin, this frees up a precious IO pin for other purposes. In theory, all axes limit pins -// may be reduced to one pin, if all axes are homed with separate cycles, or vice versa, all three axes -// on separate pin, but homed in one cycle. Also, it should be noted that the function of hard limits -// will not be affected by pin sharing. - -// NOTE: Defaults are set for a traditional 3-axis CNC machine. Z-axis first to clear, followed by X & Y. -// These homing cycle definitions precede the machine.h file so that the machine -// definition can undefine them if necessary. -#define HOMING_CYCLE_0 bit(Z_AXIS) // TYPICALLY REQUIRED: First move Z to clear workspace. -#define HOMING_CYCLE_1 bit(X_AXIS) -#define HOMING_CYCLE_2 bit(Y_AXIS) - -// NOTE: The following is for for homing X and Y at the same time -// #define HOMING_CYCLE_0 bit(Z_AXIS) // first home z by itself -// #define HOMING_CYCLE_1 (bit(X_AXIS)|bit(Y_AXIS)) // Homes both X-Y in one cycle. NOT COMPATIBLE WITH COREXY!!! +// Note: HOMING_CYCLES are now settings // Inverts pin logic of the control command pins based on a mask. This essentially means you can use // normally-closed switches on the specified pins, rather than the default normally-open switches. @@ -291,8 +267,6 @@ const double SAFETY_DOOR_SPINDLE_DELAY = 4.0; // Float (seconds) const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds) // Enable CoreXY kinematics. Use ONLY with CoreXY machines. -// IMPORTANT: If homing is enabled, you must reconfigure the homing cycle #defines above to -// #define HOMING_CYCLE_0 bit(X_AXIS) and #define HOMING_CYCLE_1 bit(Y_AXIS) // NOTE: This configuration option alters the motion of the X and Y axes to principle of operation // defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as // described, if not, motions may move in strange directions. Grbl requires the CoreXY A and B motors diff --git a/Grbl_Esp32/src/Machines/3axis_rs485.h b/Grbl_Esp32/src/Machines/3axis_rs485.h index 0dd02d5e..39a565cb 100644 --- a/Grbl_Esp32/src/Machines/3axis_rs485.h +++ b/Grbl_Esp32/src/Machines/3axis_rs485.h @@ -49,19 +49,7 @@ #define Y2_LIMIT_PIN GPIO_NUM_35 #define Z_LIMIT_PIN GPIO_NUM_34 -#ifdef HOMING_CYCLE_0 - #undef HOMING_CYCLE_0 -#endif -#define HOMING_CYCLE_0 bit(Z_AXIS) // Z first - -#ifdef HOMING_CYCLE_1 - #undef HOMING_CYCLE_1 -#endif -#define HOMING_CYCLE_1 (bit(X_AXIS)|bit(Y_AXIS)) - -#ifdef HOMING_CYCLE_2 - #undef HOMING_CYCLE_2 -#endif +// Set $Homing/Cycle0=X and $Homing/Cycle=XY #define PROBE_PIN GPIO_NUM_14 // labeled Probe #define CONTROL_RESET_PIN GPIO_NUM_27 // labeled Reset diff --git a/Grbl_Esp32/src/Machines/3axis_v4.h b/Grbl_Esp32/src/Machines/3axis_v4.h index 31308420..7b817180 100644 --- a/Grbl_Esp32/src/Machines/3axis_v4.h +++ b/Grbl_Esp32/src/Machines/3axis_v4.h @@ -39,19 +39,6 @@ #define Y_LIMIT_PIN GPIO_NUM_4 #define Z_LIMIT_PIN GPIO_NUM_16 -#ifdef HOMING_CYCLE_0 - #undef HOMING_CYCLE_0 -#endif -#define HOMING_CYCLE_0 bit(Z_AXIS) // Z first - -#ifdef HOMING_CYCLE_1 - #undef HOMING_CYCLE_1 -#endif -#define HOMING_CYCLE_1 (bit(X_AXIS)|bit(Y_AXIS)) - -#ifdef HOMING_CYCLE_2 - #undef HOMING_CYCLE_2 -#endif // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 diff --git a/Grbl_Esp32/src/Machines/atari_1020.h b/Grbl_Esp32/src/Machines/atari_1020.h index 61d8556b..eecc2146 100644 --- a/Grbl_Esp32/src/Machines/atari_1020.h +++ b/Grbl_Esp32/src/Machines/atari_1020.h @@ -54,16 +54,8 @@ #define SOLENOID_DIRECTION_PIN GPIO_NUM_4 #define SOLENOID_PEN_PIN GPIO_NUM_2 -#ifdef HOMING_CYCLE_0 - #undef HOMING_CYCLE_0 -#endif -#define HOMING_CYCLE_0 bit(X_AXIS) // this 'bot only homes the X axis -#ifdef HOMING_CYCLE_1 - #undef HOMING_CYCLE_1 -#endif -#ifdef HOMING_CYCLE_2 - #undef HOMING_CYCLE_2 -#endif +// this 'bot only homes the X axis +// Set $Homing/Cycle0=0 #define REED_SW_PIN GPIO_NUM_17 diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index be77d54d..800faf5b 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -48,24 +48,8 @@ #define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty #define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty -// redefine some stuff from config.h -#ifdef HOMING_CYCLE_0 - #undef HOMING_CYCLE_0 -#endif -#define HOMING_CYCLE_0 bit(Y_AXIS) - -#ifdef HOMING_CYCLE_1 - #undef HOMING_CYCLE_1 -#endif - -#define HOMING_CYCLE_1 bit(X_AXIS) - -#ifdef HOMING_CYCLE_2 - #undef HOMING_CYCLE_2 -#endif - -#define SERVO_PEN_PIN GPIO_NUM_27 +// Set $Homing/Cycle0=Y and $Homing/Cycle1=X #define SPINDLE_TYPE SpindleType::NONE diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h index 91526fcb..5e600d76 100644 --- a/Grbl_Esp32/src/Machines/polar_coaster.h +++ b/Grbl_Esp32/src/Machines/polar_coaster.h @@ -74,17 +74,9 @@ #define MACRO_BUTTON_1_PIN GPIO_NUM_12 #define MACRO_BUTTON_2_PIN GPIO_NUM_14 -// redefine some stuff from config.h -#ifdef HOMING_CYCLE_0 - #undef HOMING_CYCLE_0 -#endif -#define HOMING_CYCLE_0 bit(X_AXIS) // this 'bot only homes the X axis -#ifdef HOMING_CYCLE_1 - #undef HOMING_CYCLE_1 -#endif -#ifdef HOMING_CYCLE_2 - #undef HOMING_CYCLE_2 -#endif + +// this 'bot only homes the X axis +// Set $Homing/Cycle0=X // ============= End CPU MAP ================== diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h index c8967c70..fc321a11 100644 --- a/Grbl_Esp32/src/Machines/template.h +++ b/Grbl_Esp32/src/Machines/template.h @@ -136,22 +136,7 @@ // #define Z_SERVO_CAL_MIN 1.0 // calibration factor for the maximum PWM duty // === Homing cycles -// The default homing order is Z first (HOMING_CYCLE_0), -// then X (HOMING_CYCLE_1), and finally Y (HOMING_CYCLE_2) -// For machines that need different homing order, you can -// undefine HOMING_CYCLE_n and redefine it accordingly. -// For example, the following would first home X and Y -// simultaneously, then A and B simultaneously, and Z -// not at all. - -// #undef HOMING_CYCLE_0 -// #define HOMING_CYCLE_0 (bit(X_AXIS)|bit(Y_AXIS)) - -// #undef HOMING_CYCLE_1 -// #define HOMING_CYCLE_1 (bit(A_AXIS)|bit(B_AXIS)) - -// #undef HOMING_CYCLE_2 -// #endif +// Set them using $Homing/Cycle0= optionally up to $Homing/Cycle5= // === Default settings // Grbl has many run-time settings that the user can changed by From c0c3c2054818d5988be20f3c6bc0cdb2336783f2 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 25 Sep 2020 08:54:35 -1000 Subject: [PATCH 08/82] Store coordinate offsets in NVS (#611) * Store coordinate offsets in NVS * Handle both old Eeprom formats --- Grbl_Esp32/src/Eeprom.cpp | 19 +++- Grbl_Esp32/src/Eeprom.h | 1 + Grbl_Esp32/src/GCode.cpp | 124 ++++++++++++------------- Grbl_Esp32/src/GCode.h | 4 +- Grbl_Esp32/src/ProcessSettings.cpp | 7 +- Grbl_Esp32/src/Report.cpp | 95 ++++++++++--------- Grbl_Esp32/src/Settings.cpp | 29 ++++++ Grbl_Esp32/src/Settings.h | 24 ++++- Grbl_Esp32/src/SettingsDefinitions.cpp | 29 ++++++ Grbl_Esp32/src/SettingsStorage.cpp | 27 +++--- Grbl_Esp32/src/SettingsStorage.h | 39 +++++--- 11 files changed, 251 insertions(+), 147 deletions(-) diff --git a/Grbl_Esp32/src/Eeprom.cpp b/Grbl_Esp32/src/Eeprom.cpp index 6654289a..d88d9bc2 100644 --- a/Grbl_Esp32/src/Eeprom.cpp +++ b/Grbl_Esp32/src/Eeprom.cpp @@ -24,14 +24,29 @@ void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source unsigned char checksum = 0; for (; size > 0; size--) { unsigned char data = static_cast(*source++); - checksum = (checksum << 1) | (checksum >> 7); + // Note: This checksum calculation is broken as described below. + checksum = (checksum << 1) || (checksum >> 7); checksum += data; - EEPROM.write(destination++, data); + EEPROM.write(destination++, *(source++)); } EEPROM.write(destination, checksum); EEPROM.commit(); } +int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size) { + unsigned char data, checksum = 0; + for (; size > 0; size--) { + data = EEPROM.read(source++); + // Note: This checksum calculation is broken - the || should be just | - + // thus making the checksum very weak. + // We leave it as-is so we can read old data after a firmware upgrade. + // The new storage format uses the tagged NVS mechanism, avoiding this bug. + checksum = (checksum << 1) || (checksum >> 7); + checksum += data; + *(destination++) = data; + } + return (checksum == EEPROM.read(source)); +} int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size) { unsigned char data, checksum = 0; for (; size > 0; size--) { diff --git a/Grbl_Esp32/src/Eeprom.h b/Grbl_Esp32/src/Eeprom.h index a9c1d3ab..0a69ec3e 100644 --- a/Grbl_Esp32/src/Eeprom.h +++ b/Grbl_Esp32/src/Eeprom.h @@ -26,3 +26,4 @@ //void eeprom_put_char(unsigned int addr, unsigned char new_value); void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size); int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size); +int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size); diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 3f2c00c0..8d5391e2 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -37,28 +37,11 @@ parser_block_t gc_block; #define FAIL(status) return (status); void gc_init() { - // First thing we do here is iterate through the coord systems and read them all, so that - // we get all our coord system errors here, and not while we're busy: - float coord_system[MAX_N_AXIS]; - - // g54 - g59 is 6 coordinate systems, plus 2 for G28 and G30 reference positions - bool reported_error = false; - const int MAX_COORD_SYSTEMS = 8; - for (uint8_t i = 0; i < MAX_COORD_SYSTEMS; ++i) { - if (!(settings_read_coord_data(i, coord_system))) { - if (!reported_error) { - reported_error = true; - report_status_message(Error::SettingReadFail, CLIENT_SERIAL); - } - } - } - // Reset parser state: memset(&gc_state, 0, sizeof(parser_state_t)); // Load default G54 coordinate system. - if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system))) { - report_status_message(Error::SettingReadFail, CLIENT_SERIAL); - } + gc_state.modal.coord_select = CoordIndex::G54; + coords[gc_state.modal.coord_select]->get(gc_state.coord_system); } // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard @@ -149,7 +132,7 @@ Error gc_execute_line(char* line, uint8_t client) { memcpy(&gc_block.modal, &gc_state.modal, sizeof(gc_modal_t)); // Copy current modes AxisCommand axis_command = AxisCommand::None; uint8_t axis_0, axis_1, axis_linear; - uint8_t coord_select = 0; // Tracks G10 P coordinate selection for execution + CoordIndex coord_select = CoordIndex::G54; // Tracks G10 P coordinate selection for execution // Initialize bitflag tracking variables for axis indices compatible operations. uint8_t axis_words = 0; // XYZ tracking uint8_t ijk_words = 0; // IJK tracking @@ -158,6 +141,8 @@ Error gc_execute_line(char* line, uint8_t client) { uint32_t value_words = 0; // Tracks value words. uint8_t gc_parser_flags = GCParserNone; auto n_axis = number_axis->get(); + float coord_data[MAX_N_AXIS]; // Used by WCO-related commands + uint8_t pValue; // Integer value of P word // Determine if the line is a jogging motion or a normal g-code block. if (line[0] == '$') { // NOTE: `$J=` already parsed when passed to this function. @@ -409,15 +394,30 @@ Error gc_execute_line(char* line, uint8_t client) { mg_word_bit = ModalGroup::MG8; break; case 54: - case 55: - case 56: - case 57: - case 58: - case 59: - // NOTE: G59.x are not supported. (But their int_values would be 60, 61, and 62.) - gc_block.modal.coord_select = int_value - 54; // Shift to array indexing. + gc_block.modal.coord_select = CoordIndex::G54; mg_word_bit = ModalGroup::MG12; break; + case 55: + gc_block.modal.coord_select = CoordIndex::G55; + mg_word_bit = ModalGroup::MG12; + break; + case 56: + gc_block.modal.coord_select = CoordIndex::G56; + mg_word_bit = ModalGroup::MG12; + break; + case 57: + gc_block.modal.coord_select = CoordIndex::G57; + mg_word_bit = ModalGroup::MG12; + break; + case 58: + gc_block.modal.coord_select = CoordIndex::G58; + mg_word_bit = ModalGroup::MG12; + break; + case 59: + gc_block.modal.coord_select = CoordIndex::G59; + mg_word_bit = ModalGroup::MG12; + break; + // NOTE: G59.x are not supported. case 61: if (mantissa != 0) { FAIL(Error::GcodeUnsupportedCommand); // [G61.1 not supported] @@ -879,13 +879,13 @@ Error gc_execute_line(char* line, uint8_t client) { float block_coord_system[MAX_N_AXIS]; memcpy(block_coord_system, gc_state.coord_system, sizeof(gc_state.coord_system)); if (bit_istrue(command_words, bit(ModalGroup::MG12))) { // Check if called in block - if (gc_block.modal.coord_select > N_COORDINATE_SYSTEM) { + // This error probably cannot happen because preceding code sets + // gc_block.modal.coord_select only to specific supported values + if (gc_block.modal.coord_select >= CoordIndex::NWCSystems) { FAIL(Error::GcodeUnsupportedCoordSys); // [Greater than N sys] } if (gc_state.modal.coord_select != gc_block.modal.coord_select) { - if (!(settings_read_coord_data(gc_block.modal.coord_select, block_coord_system))) { - FAIL(Error::SettingReadFail); - } + coords[gc_block.modal.coord_select]->get(block_coord_system); } } // [16. Set path control mode ]: N/A. Only G61. G61.1 and G64 NOT SUPPORTED. @@ -907,10 +907,6 @@ Error gc_execute_line(char* line, uint8_t client) { if (bit_isfalse(value_words, (bit(GCodeWord::P) | bit(GCodeWord::L)))) { FAIL(Error::GcodeValueWordMissing); // [P/L word missing] } - coord_select = trunc(gc_block.values.p); // Convert p value to int. - if (coord_select > N_COORDINATE_SYSTEM) { - FAIL(Error::GcodeUnsupportedCoordSys); // [Greater than N sys] - } if (gc_block.values.l != 20) { if (gc_block.values.l == 2) { if (bit_istrue(value_words, bit(GCodeWord::R))) { @@ -920,18 +916,21 @@ Error gc_execute_line(char* line, uint8_t client) { FAIL(Error::GcodeUnsupportedCommand); // [Unsupported L] } } - bit_false(value_words, (bit(GCodeWord::L) | bit(GCodeWord::P))); - // Determine coordinate system to change and try to load from EEPROM. - if (coord_select > 0) { - coord_select--; // Adjust P1-P6 index to EEPROM coordinate data indexing. + // Select the coordinate system based on the P word + pValue = trunc(gc_block.values.p); // Convert p value to integer + if (pValue > 0) { + // P1 means G54, P2 means G55, etc. + coord_select = static_cast(pValue - 1 + CoordIndex::G54); } else { - coord_select = gc_block.modal.coord_select; // Index P0 as the active coordinate system + // P0 means use currently-selected system + coord_select = gc_block.modal.coord_select; } - // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command. - // FIXME: Instead of IJK, we'd better use: float vector[MAX_N_AXIS]; // [DG] - if (!settings_read_coord_data(coord_select, gc_block.values.ijk)) { - FAIL(Error::SettingReadFail); // [EEPROM read fail] + if (coord_select >= CoordIndex::NWCSystems) { + FAIL(Error::GcodeUnsupportedCoordSys); // [Greater than N sys] } + bit_false(value_words, (bit(GCodeWord::L) | bit(GCodeWord::P))); + coords[coord_select]->get(coord_data); + // Pre-calculate the coordinate data changes. for (idx = 0; idx < n_axis; idx++) { // Axes indices are consistent, so loop may be used. // Update axes defined only in block. Always in machine coordinates. Can change non-active system. @@ -939,13 +938,13 @@ Error gc_execute_line(char* line, uint8_t client) { if (gc_block.values.l == 20) { // L20: Update coordinate system axis at current position (with modifiers) with programmed value // WPos = MPos - WCS - G92 - TLO -> WCS = MPos - G92 - TLO - WPos - gc_block.values.ijk[idx] = gc_state.position[idx] - gc_state.coord_offset[idx] - gc_block.values.xyz[idx]; + coord_data[idx] = gc_state.position[idx] - gc_state.coord_offset[idx] - gc_block.values.xyz[idx]; if (idx == TOOL_LENGTH_OFFSET_AXIS) { - gc_block.values.ijk[idx] -= gc_state.tool_length_offset; + coord_data[idx] -= gc_state.tool_length_offset; } } else { // L2: Update coordinate system axis to programmed value. - gc_block.values.ijk[idx] = gc_block.values.xyz[idx]; + coord_data[idx] = gc_block.values.xyz[idx]; } } // Else, keep current stored value. } @@ -1003,21 +1002,16 @@ Error gc_execute_line(char* line, uint8_t client) { case NonModal::GoHome1: // G30 // [G28/30 Errors]: Cutter compensation is enabled. // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM - // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command. if (gc_block.non_modal_command == NonModal::GoHome0) { - if (!settings_read_coord_data(SETTING_INDEX_G28, gc_block.values.ijk)) { - FAIL(Error::SettingReadFail); - } + coords[CoordIndex::G28]->get(coord_data); } else { // == NonModal::GoHome1 - if (!settings_read_coord_data(SETTING_INDEX_G30, gc_block.values.ijk)) { - FAIL(Error::SettingReadFail); - } + coords[CoordIndex::G30]->get(coord_data); } if (axis_words) { // Move only the axes specified in secondary move. for (idx = 0; idx < n_axis; idx++) { if (!(axis_words & bit(idx))) { - gc_block.values.ijk[idx] = gc_state.position[idx]; + coord_data[idx] = gc_state.position[idx]; } } } else { @@ -1454,7 +1448,7 @@ Error gc_execute_line(char* line, uint8_t client) { // [15. Coordinate system selection ]: if (gc_state.modal.coord_select != gc_block.modal.coord_select) { gc_state.modal.coord_select = gc_block.modal.coord_select; - memcpy(gc_state.coord_system, block_coord_system, MAX_N_AXIS * sizeof(float)); + memcpy(gc_state.coord_system, block_coord_system, sizeof(gc_state.coord_system)); system_flag_wco_change(); } // [16. Set path control mode ]: G61.1/G64 NOT SUPPORTED @@ -1465,10 +1459,10 @@ Error gc_execute_line(char* line, uint8_t client) { // [19. Go to predefined position, Set G10, or Set axis offsets ]: switch (gc_block.non_modal_command) { case NonModal::SetCoordinateData: - settings_write_coord_data(coord_select, gc_block.values.ijk); + coords[coord_select]->set(coord_data); // Update system coordinate system if currently active. if (gc_state.modal.coord_select == coord_select) { - memcpy(gc_state.coord_system, gc_block.values.ijk, MAX_N_AXIS * sizeof(float)); + memcpy(gc_state.coord_system, coord_data, sizeof(gc_state.coord_system)); system_flag_wco_change(); } break; @@ -1480,14 +1474,14 @@ Error gc_execute_line(char* line, uint8_t client) { if (axis_command != AxisCommand::None) { mc_line(gc_block.values.xyz, pl_data); // kinematics kinematics not used for homing righ now } - mc_line(gc_block.values.ijk, pl_data); - memcpy(gc_state.position, gc_block.values.ijk, MAX_N_AXIS * sizeof(float)); + mc_line(coord_data, pl_data); + memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: - settings_write_coord_data(SETTING_INDEX_G28, gc_state.position); + coords[CoordIndex::G28]->set(coord_data); break; case NonModal::SetHome1: - settings_write_coord_data(SETTING_INDEX_G30, gc_state.position); + coords[CoordIndex::G30]->set(coord_data); break; case NonModal::SetCoordinateOffset: memcpy(gc_state.coord_offset, gc_block.values.xyz, sizeof(gc_block.values.xyz)); @@ -1587,7 +1581,7 @@ Error gc_execute_line(char* line, uint8_t client) { gc_state.modal.distance = Distance::Absolute; gc_state.modal.feed_rate = FeedRate::UnitsPerMin; // gc_state.modal.cutter_comp = CutterComp::Disable; // Not supported. - gc_state.modal.coord_select = 0; // G54 + gc_state.modal.coord_select = CoordIndex::G54; gc_state.modal.spindle = SpindleState::Disable; gc_state.modal.coolant = {}; #ifdef ENABLE_PARKING_OVERRIDE_CONTROL @@ -1605,9 +1599,7 @@ Error gc_execute_line(char* line, uint8_t client) { #endif // Execute coordinate change and spindle/coolant stop. if (sys.state != State::CheckMode) { - if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system))) { - FAIL(Error::SettingReadFail); - } + coords[gc_state.modal.coord_select]->get(gc_state.coord_system); system_flag_wco_change(); // Set to refresh immediately just in case something altered. spindle->set_state(SpindleState::Disable, 0); coolant_off(); diff --git a/Grbl_Esp32/src/GCode.h b/Grbl_Esp32/src/GCode.h index d272a096..1f594bfe 100644 --- a/Grbl_Esp32/src/GCode.h +++ b/Grbl_Esp32/src/GCode.h @@ -251,7 +251,7 @@ typedef struct { Plane plane_select; // {G17,G18,G19} // CutterCompensation cutter_comp; // {G40} NOTE: Don't track. Only default supported. ToolLengthOffset tool_length; // {G43.1,G49} - uint8_t coord_select; // {G54,G55,G56,G57,G58,G59} + CoordIndex coord_select; // {G54,G55,G56,G57,G58,G59} // uint8_t control; // {G61} NOTE: Don't track. Only default supported. ProgramFlow program_flow; // {M0,M1,M2,M30} CoolantState coolant; // {M7,M8,M9} @@ -264,7 +264,7 @@ typedef struct { typedef struct { uint8_t e; // M67 float f; // Feed - float ijk[MAX_N_AXIS]; // I,J,K Axis arc offsets + float ijk[3]; // I,J,K Axis arc offsets - only 3 are possible uint8_t l; // G10 or canned cycles parameters int32_t n; // Line number float p; // G10 or dwell parameters diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 7a052381..f4eca17b 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -56,11 +56,8 @@ void settings_restore(uint8_t restore_flag) { } } if (restore_flag & SETTINGS_RESTORE_PARAMETERS) { - uint8_t idx; - float coord_data[MAX_N_AXIS]; - memset(&coord_data, 0, sizeof(coord_data)); - for (idx = 0; idx <= SETTING_INDEX_NCOORD; idx++) { - settings_write_coord_data(idx, coord_data); + for (auto idx = CoordIndex::Begin; idx < CoordIndex::End; ++idx) { + coords[idx]->setDefault(); } } if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) { diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index e93b578a..e307b05d 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -166,23 +166,24 @@ void grbl_notifyf(const char* title, const char* format, ...) { } } +static const int coordStringLen = 20; +static const int axesStringLen = coordStringLen * MAX_N_AXIS; + // formats axis values into a string and returns that string in rpt -// NOTE: rpt should have at least size: 20 * MAX_N_AXIS +// NOTE: rpt should have at least size: axesStringLen static void report_util_axis_values(float* axis_value, char* rpt) { uint8_t idx; - char axisVal[20]; + char axisVal[coordStringLen]; float unit_conv = 1.0; // unit conversion multiplier..default is mm + const char* format = "%4.3f"; // Default - report mm to 3 decimal places rpt[0] = '\0'; if (report_inches->get()) { unit_conv = 1.0 / MM_PER_INCH; + format = "%4.4f"; // Report inches to 4 decimal places } auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { - if (report_inches->get()) { - snprintf(axisVal, 19, "%4.4f", axis_value[idx] * unit_conv); // Report inches to 4 decimals - } else { - snprintf(axisVal, 19, "%4.3f", axis_value[idx] * unit_conv); // Report mm to 3 decimals - } + snprintf(axisVal, coordStringLen-1, format, axis_value[idx] * unit_conv); strcat(rpt, axisVal); if (idx < (number_axis->get() - 1)) { strcat(rpt, ","); @@ -190,6 +191,27 @@ static void report_util_axis_values(float* axis_value, char* rpt) { } } +// This version returns the axis values as a String +static String report_util_axis_values(const float* axis_value) { + String rpt = ""; + uint8_t idx; + char axisVal[coordStringLen]; + float unit_conv = 1.0; // unit conversion multiplier..default is mm + int decimals = 3; // Default - report mm to 3 decimal places + if (report_inches->get()) { + unit_conv = 1.0 / MM_PER_INCH; + decimals = 4; // Report inches to 4 decimal places + } + auto n_axis = number_axis->get(); + for (idx = 0; idx < n_axis; idx++) { + rpt += String(axis_value[idx] * unit_conv, decimals); + if (idx < (number_axis->get() - 1)) { + rpt += ","; + } + } + return rpt; +} + // Handles the primary confirmation protocol response for streaming interfaces and human-feedback. // For every incoming line, this method responds with an 'ok' for a successful command or an // 'error:' to indicate some error event with the line or some critical system error during @@ -284,8 +306,8 @@ void report_grbl_help(uint8_t client) { void report_probe_parameters(uint8_t client) { // Report in terms of machine position. float print_position[MAX_N_AXIS]; - char probe_rpt[(MAX_N_AXIS * 20 + 13 + 6 + 1)]; // the probe report we are building here - char temp[MAX_N_AXIS * 20]; + char probe_rpt[(axesStringLen + 13 + 6 + 1)]; // the probe report we are building here + char temp[axesStringLen]; strcpy(probe_rpt, "[PRB:"); // initialize the string with the first characters // get the machine position and put them into a string and append to the probe report system_convert_array_steps_to_mpos(print_position, sys_probe_position); @@ -299,46 +321,27 @@ void report_probe_parameters(uint8_t client) { // Prints Grbl NGC parameters (coordinate offsets, probing) void report_ngc_parameters(uint8_t client) { - float coord_data[MAX_N_AXIS]; - uint8_t coord_select; - char temp[MAX_N_AXIS * 20]; - char ngc_rpt[((8 + (MAX_N_AXIS * 20)) * SETTING_INDEX_NCOORD + 4 + MAX_N_AXIS * 20 + 8 + 2 * 20)]; - ngc_rpt[0] = '\0'; - for (coord_select = 0; coord_select <= SETTING_INDEX_NCOORD; coord_select++) { - if (!(settings_read_coord_data(coord_select, coord_data))) { - report_status_message(Error::SettingReadFail, CLIENT_SERIAL); - return; - } - strcat(ngc_rpt, "[G"); - switch (coord_select) { - case 6: - strcat(ngc_rpt, "28"); - break; - case 7: - strcat(ngc_rpt, "30"); - break; - default: - sprintf(temp, "%d", coord_select + 54); - strcat(ngc_rpt, temp); - break; // G54-G59 - } - strcat(ngc_rpt, ":"); - report_util_axis_values(coord_data, temp); - strcat(ngc_rpt, temp); - strcat(ngc_rpt, "]\r\n"); + String ngc_rpt = ""; + + // Print persistent offsets G54 - G59, G28, and G30 + for (auto coord_select = CoordIndex::Begin; coord_select < CoordIndex::End; ++coord_select) { + ngc_rpt += "["; + ngc_rpt += coords[coord_select]->getName(); + ngc_rpt += ":"; + ngc_rpt += report_util_axis_values(coords[coord_select]->get()); + ngc_rpt += "]\r\n"; } - strcat(ngc_rpt, "[G92:"); // Print G92,G92.1 which are not persistent in memory - report_util_axis_values(gc_state.coord_offset, temp); - strcat(ngc_rpt, temp); - strcat(ngc_rpt, "]\r\n"); - strcat(ngc_rpt, "[TLO:"); // Print tool length offset value + ngc_rpt += "[G92:"; // Print non-persistent G92,G92.1 + ngc_rpt += report_util_axis_values(gc_state.coord_offset); + ngc_rpt += "]\r\n"; + ngc_rpt += "[TLO:"; // Print tool length offset + float tlo = gc_state.tool_length_offset; if (report_inches->get()) { - snprintf(temp, 20, "%4.3f]\r\n", gc_state.tool_length_offset * INCH_PER_MM); - } else { - snprintf(temp, 20, "%4.3f]\r\n", gc_state.tool_length_offset); + tlo *= INCH_PER_MM; } - strcat(ngc_rpt, temp); - grbl_send(client, ngc_rpt); + ngc_rpt += String(tlo, 3);; + ngc_rpt += "]\r\n"; + grbl_send(client, ngc_rpt.c_str()); report_probe_parameters(client); } diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 701037f6..f1be2536 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -653,3 +653,32 @@ Error GrblCommand::action(char* value, WebUI::AuthenticationLevel auth_level, We } return _action((const char*)value, auth_level, out); }; +Coordinates* coords[CoordIndex::End]; + +bool Coordinates::load() { + size_t len; + switch (nvs_get_blob(Setting::_handle, _name, _currentValue, &len)) { + case ESP_OK: + return true; + case ESP_ERR_NVS_INVALID_LENGTH: + // This could happen if the stored value is longer than the buffer. + // That is highly unlikely since we always store MAX_N_AXIS coordinates. + // It would indicate that we have decreased MAX_N_AXIS since the + // value was stored. We don't flag it as an error, but rather + // accept the initial coordinates and ignore the residue. + // We could issue a warning message if we were so inclined. + return true; + case ESP_ERR_NVS_INVALID_NAME: + case ESP_ERR_NVS_INVALID_HANDLE: + default: + return false; + } +}; + +void Coordinates::set(float value[MAX_N_AXIS]) { + memcpy(&_currentValue, value, sizeof(_currentValue)); +#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE + protocol_buffer_synchronize(); +#endif + nvs_set_blob(Setting::_handle, _name, _currentValue, sizeof(_currentValue)); +} diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 233e8571..5b91c80e 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -76,7 +76,6 @@ public: class Setting : public Word { private: protected: - static nvs_handle _handle; // group_t _group; axis_t _axis = NO_AXIS; Setting* link; // linked list of setting objects @@ -85,6 +84,7 @@ protected: const char* _keyName; public: + static nvs_handle _handle; static void init(); static Setting* List; Setting* next() { return link; } @@ -206,6 +206,28 @@ public: int32_t get() { return _currentValue; } }; +class Coordinates { +private: + float _currentValue[MAX_N_AXIS]; + const char* _name; +public: + Coordinates(const char* name) : _name(name) {} + + const char* getName() { return _name; } + bool load(); + void setDefault() { + float zeros[MAX_N_AXIS] = { 0.0, }; + set(zeros); + }; + // Copy the value to an array + void get(float* value) { memcpy(value, _currentValue, sizeof(_currentValue)); } + // Return a pointer to the array + const float* get() { return _currentValue; } + void set(float *value); +}; + +extern Coordinates* coords[CoordIndex::End]; + class FloatSetting : public Setting { private: float _defaultValue; diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index c6e37b4c..2f649dcb 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -205,9 +205,38 @@ static const char* makeGrblName(int axisNum, int base) { return strcpy(retval, buf); } +void make_coordinate(CoordIndex index, const char* name) { + float coord_data[MAX_N_AXIS] = { 0.0 }; + auto coord = new Coordinates(name); + coords[index] = coord; + if (!coord->load()) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Propagating %s data to NVS format", coord->getName()); + // If coord->load() returns false it means that no entry + // was present in non-volatile storage. In that case we + // first look for an old-format entry in the EEPROM section. + // If an entry is present some number of float values at + // the beginning of coord_data will be overwritten with + // the EEPROM data, and the rest will remain at 0.0. + // If no old-format entry is present, all will remain 0.0 + // Regardless, we create a new entry with that data. + (void)old_settings_read_coord_data(index, coord_data); + coords[index]->set(coord_data); + } +} void make_settings() { Setting::init(); + // Propagate old coordinate system data to the new format if necessary. + // G54 - G59 work coordinate systems, G28, G30 reference positions, etc + make_coordinate(CoordIndex::G54, "G54"); + make_coordinate(CoordIndex::G55, "G55"); + make_coordinate(CoordIndex::G56, "G56"); + make_coordinate(CoordIndex::G57, "G57"); + make_coordinate(CoordIndex::G58, "G58"); + make_coordinate(CoordIndex::G59, "G59"); + make_coordinate(CoordIndex::G28, "G28"); + make_coordinate(CoordIndex::G30, "G30"); + // number_axis = new IntSetting(EXTENDED, WG, NULL, "NumberAxis", N_AXIS, 0, 6, NULL, true); number_axis = new FakeSetting(N_AXIS); diff --git a/Grbl_Esp32/src/SettingsStorage.cpp b/Grbl_Esp32/src/SettingsStorage.cpp index b45f040c..5c17d8ba 100644 --- a/Grbl_Esp32/src/SettingsStorage.cpp +++ b/Grbl_Esp32/src/SettingsStorage.cpp @@ -25,26 +25,21 @@ #include "Grbl.h" // Read selected coordinate data from EEPROM. Updates pointed coord_data value. -uint8_t settings_read_coord_data(uint8_t coord_select, float* coord_data) { - uint32_t addr = coord_select * (sizeof(float) * MAX_N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; - if (!(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) { +// This is now a compatibility routine that is used to propagate coordinate data +// in the old EEPROM format to the new tagged NVS format. +bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) { + uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; + if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) && + !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) { // Reset with default zero vector clear_vector_float(coord_data); - settings_write_coord_data(coord_select, coord_data); + // The old code used to rewrite the zeroed data but now that is done + // elsewhere, in a different format. return false; } return true; } -// Method to store coord data parameters into EEPROM -void settings_write_coord_data(uint8_t coord_select, float* coord_data) { -#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE - protocol_buffer_synchronize(); -#endif - uint32_t addr = coord_select * (sizeof(float) * MAX_N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; - memcpy_to_eeprom_with_checksum(addr, (char*)coord_data, sizeof(float) * MAX_N_AXIS); -} - // Method to store build info into EEPROM // NOTE: This function can only be called in IDLE state. void settings_store_build_info(const char* line) { @@ -73,3 +68,9 @@ uint8_t get_step_pin_mask(uint8_t axis_idx) { uint8_t get_direction_pin_mask(uint8_t axis_idx) { return bit(axis_idx); } + +// Allow iteration over CoordIndex values +CoordIndex& operator ++ (CoordIndex& i) { + i = static_cast(static_cast(i) + 1); + return i; +} diff --git a/Grbl_Esp32/src/SettingsStorage.h b/Grbl_Esp32/src/SettingsStorage.h index 02566d15..4df9870a 100644 --- a/Grbl_Esp32/src/SettingsStorage.h +++ b/Grbl_Esp32/src/SettingsStorage.h @@ -48,14 +48,6 @@ const int EEPROM_SIZE = 1024U; const int EEPROM_ADDR_PARAMETERS = 512U; const int EEPROM_ADDR_BUILD_INFO = 942U; -// Define EEPROM address indexing for coordinate parameters -const int N_COORDINATE_SYSTEM = 6; // Number of supported work coordinate systems (from index 1) -const int SETTING_INDEX_NCOORD = N_COORDINATE_SYSTEM + 1; // Total number of system stored (from index 0) -// NOTE: Work coordinate indices are (0=G54, 1=G55, ... , 6=G59) -const int SETTING_INDEX_G28 = N_COORDINATE_SYSTEM; // Home position 1 -const int SETTING_INDEX_G30 = N_COORDINATE_SYSTEM + 1; // Home position 2 -// const int SETTING_INDEX_G92 = N_COORDINATE_SYSTEM+2; // Coordinate offset (G92.2,G92.3 not supported) - // Initialize the configuration subsystem (load settings from EEPROM) void settings_init(); void settings_restore(uint8_t restore_flag); @@ -64,14 +56,37 @@ void write_global_settings(); uint8_t settings_read_build_info(char* line); void settings_store_build_info(const char* line); -// Writes selected coordinate data to EEPROM -void settings_write_coord_data(uint8_t coord_select, float* coord_data); - // Reads selected coordinate data from EEPROM -uint8_t settings_read_coord_data(uint8_t coord_select, float* coord_data); +bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data); // Returns the step pin mask according to Grbl's internal axis numbering uint8_t get_step_pin_mask(uint8_t i); // Returns the direction pin mask according to Grbl's internal axis numbering uint8_t get_direction_pin_mask(uint8_t i); + +// Various places in the code access saved coordinate system data +// by a small integer index according to the values below. +enum CoordIndex : uint8_t{ + Begin = 0, + G54 = Begin, + G55, + G56, + G57, + G58, + G59, + // To support 9 work coordinate systems it would be necessary to define + // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 + // G59_1, + // G59_2, + // G59_3, + NWCSystems, + G28 = NWCSystems, + G30, + // G92_2, + // G92_3, + End, +}; +// Allow iteration over CoordIndex values +CoordIndex& operator ++ (CoordIndex& i); + From 675b339f6ad6060bbf24f710ea3aa58489eb2f44 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 27 Sep 2020 20:55:40 -0500 Subject: [PATCH 09/82] Implementing fixes (#616) - Stop creating additional tasks when limit_init() gets called again from homing and resets - Explicitly delete an object that was causing a memory loss. --- Grbl_Esp32/src/Limits.cpp | 34 ++++++++++++++++-------------- Grbl_Esp32/src/ProcessSettings.cpp | 6 +++++- 2 files changed, 23 insertions(+), 17 deletions(-) diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 382b02e1..e3539fee 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -330,27 +330,29 @@ void limits_init() { } else { detachInterrupt(pin); } - /* - // Change to do this once. limits_init() happens often - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "%c%s Axis limit switch on pin %s", - report_get_axis_letter(axis), - gang_index ? "2" : " ", - pinName(pin).c_str()); - */ + + if (limit_sw_queue == NULL) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%c%s Axis limit switch on pin %s", + report_get_axis_letter(axis), + gang_index ? "2" : " ", + pinName(pin).c_str()); + } } } } // setup task used for debouncing - limit_sw_queue = xQueueCreate(10, sizeof(int)); - xTaskCreate(limitCheckTask, - "limitCheckTask", - 2048, - NULL, - 5, // priority - NULL); + if (limit_sw_queue == NULL) { + limit_sw_queue = xQueueCreate(10, sizeof(int)); + xTaskCreate(limitCheckTask, + "limitCheckTask", + 2048, + NULL, + 5, // priority + NULL); + } } // Disables hard limits. diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index f4eca17b..4c698fcc 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -556,8 +556,12 @@ Error system_execute_line(char* line, WebUI::ESPResponseStream* out, WebUI::Auth // non-empty string - [ESPxxx]yyy or $xxx=yyy return do_command_or_setting(key, value, auth_level, out); } + Error system_execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_level) { - return system_execute_line(line, new WebUI::ESPResponseStream(client, true), auth_level); + auto resp = new WebUI::ESPResponseStream(client, true); + auto ret = system_execute_line(line, resp, auth_level); + delete resp; + return ret; } void system_execute_startup(char* line) { From f4607719316c335d64f2f24ec47613a66eac132b Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 28 Sep 2020 10:34:17 -0500 Subject: [PATCH 10/82] Update Grbl.h --- Grbl_Esp32/src/Grbl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index dfe8b8b0..ad5c50f1 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 = "20200924"; +const char* const GRBL_VERSION_BUILD = "20200927"; //#include #include From 7a872874ad07164450742e94498b5ba8c61cc7d3 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 28 Sep 2020 14:35:47 -0500 Subject: [PATCH 11/82] Tweak memory fix and add $H check for $Homing/Cycles --- Grbl_Esp32/src/Error.cpp | 1 + Grbl_Esp32/src/Error.h | 1 + Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 6 ++++++ Grbl_Esp32/src/ProcessSettings.cpp | 6 ++---- 5 files changed, 11 insertions(+), 5 deletions(-) diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index 8f232e99..e7fa5de6 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -40,6 +40,7 @@ std::map ErrorCodes = { { Error::TravelExceeded, "Max travel exceeded during jog" }, { Error::InvalidJogCommand, "Invalid jog command" }, { Error::SettingDisabledLaser, "Laser mode requires PWM output" }, + { Error::HomingNoCycles, "No Homing/Cycle defined in settings" }, { Error::GcodeUnsupportedCommand, "Unsupported GCode command" }, { Error::GcodeModalGroupViolation, "Gcode modal group violation" }, { Error::GcodeUndefinedFeedRate, "Gcode undefined feed rate" }, diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index da7d04d2..cfc2b379 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -43,6 +43,7 @@ enum class Error : uint8_t { TravelExceeded = 15, InvalidJogCommand = 16, SettingDisabledLaser = 17, + HomingNoCycles = 18, GcodeUnsupportedCommand = 20, GcodeModalGroupViolation = 21, GcodeUndefinedFeedRate = 22, diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index ad5c50f1..fcaf9890 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 = "20200927"; +const char* const GRBL_VERSION_BUILD = "20200928"; //#include #include diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 474ad502..d122529f 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -264,6 +264,7 @@ static bool axis_is_squared(uint8_t axis_mask) { // NOTE: There should be no motions in the buffer and Grbl must be in an idle state before // executing the homing cycle. This prevents incorrect buffered plans after homing. void mc_homing_cycle(uint8_t cycle_mask) { + bool no_cycles_defined = true; #ifdef USE_CUSTOM_HOMING if (user_defined_homing()) { return; @@ -316,6 +317,7 @@ void mc_homing_cycle(uint8_t cycle_mask) { 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 + no_cycles_defined = false; if (!axis_is_squared(homing_mask)) { limits_go_home(homing_mask); // Homing cycle 0 } else { @@ -331,6 +333,10 @@ void mc_homing_cycle(uint8_t cycle_mask) { } } } + + if (no_cycles_defined) { + report_status_message(Error::HomingNoCycles, CLIENT_ALL); + } } protocol_execute_realtime(); // Check for reset and set system abort. if (sys.abort) { diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 4c698fcc..ddae1b22 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -558,10 +558,8 @@ Error system_execute_line(char* line, WebUI::ESPResponseStream* out, WebUI::Auth } Error system_execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_level) { - auto resp = new WebUI::ESPResponseStream(client, true); - auto ret = system_execute_line(line, resp, auth_level); - delete resp; - return ret; + WebUI::ESPResponseStream stream(client, true); + return system_execute_line(line, &stream, auth_level); } void system_execute_startup(char* line) { From e0588ee377ce82fcabfed45bcf3a7ab6ad4a5058 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 29 Sep 2020 13:48:13 -0500 Subject: [PATCH 12/82] Fix G28.1 and G30.1 --- Grbl_Esp32/src/GCode.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 8d5391e2..8031c95e 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -142,7 +142,7 @@ Error gc_execute_line(char* line, uint8_t client) { uint8_t gc_parser_flags = GCParserNone; auto n_axis = number_axis->get(); float coord_data[MAX_N_AXIS]; // Used by WCO-related commands - uint8_t pValue; // Integer value of P word + uint8_t pValue; // Integer value of P word // Determine if the line is a jogging motion or a normal g-code block. if (line[0] == '$') { // NOTE: `$J=` already parsed when passed to this function. @@ -1478,10 +1478,10 @@ Error gc_execute_line(char* line, uint8_t client) { memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: - coords[CoordIndex::G28]->set(coord_data); + coords[CoordIndex::G28]->set(gc_state.position); break; case NonModal::SetHome1: - coords[CoordIndex::G30]->set(coord_data); + coords[CoordIndex::G30]->set(gc_state.position); break; case NonModal::SetCoordinateOffset: memcpy(gc_state.coord_offset, gc_block.values.xyz, sizeof(gc_block.values.xyz)); From d9a8cff12a1a13f08a9b99fa6d0dfd0702005462 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 29 Sep 2020 13:52:41 -0500 Subject: [PATCH 13/82] Update Grbl.h --- Grbl_Esp32/src/Grbl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index fcaf9890..60159039 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 = "20200928"; +const char* const GRBL_VERSION_BUILD = "20200929"; //#include #include From 9a5cd491874a5735546888332e33a2d044d53bd6 Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 3 Oct 2020 12:41:40 -0500 Subject: [PATCH 14/82] Homing cycle defaults (#624) * Changed to add homing cycle defaults There needs to be a way to set the homing cycle defaults in a machine definition. There will likely be a better way to do this in the future. * Update 10vSpindle.cpp Had wrong error message --- Grbl_Esp32/src/Defaults.h | 24 ++++++++++++ Grbl_Esp32/src/Grbl.h | 2 +- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 38 +++++++------------ Grbl_Esp32/src/Machines/midtbot.h | 3 ++ Grbl_Esp32/src/Machines/test_drive.h | 9 +++++ Grbl_Esp32/src/SettingsDefinitions.cpp | 11 +++--- Grbl_Esp32/src/Spindles/10vSpindle.cpp | 2 +- 7 files changed, 58 insertions(+), 31 deletions(-) diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 4594002e..1fd2241b 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -118,6 +118,30 @@ # define DEFAULT_HOMING_SQUARED_AXES 0 #endif +#ifndef DEFAULT_HOMING_CYCLE_0 +# define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS) +#endif + +#ifndef DEFAULT_HOMING_CYCLE_1 +# define DEFAULT_HOMING_CYCLE_1 (bit(X_AXIS) | bit(Y_AXIS)) +#endif + +#ifndef DEFAULT_HOMING_CYCLE_2 +# define DEFAULT_HOMING_CYCLE_2 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_3 +# define DEFAULT_HOMING_CYCLE_3 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_4 +# define DEFAULT_HOMING_CYCLE_4 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_5 +# define DEFAULT_HOMING_CYCLE_5 0 +#endif + // ======== SPINDLE STUFF ==================== #ifndef SPINDLE_TYPE # define SPINDLE_TYPE SpindleType::NONE diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 60159039..860c2021 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 = "20200929"; +const char* const GRBL_VERSION_BUILD = "20201001"; //#include #include diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index b8b70c3f..ea00a45e 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -26,16 +26,6 @@ #define N_AXIS 3 -#ifdef HOMING_CYCLE_0 - #undef HOMING_CYCLE_0 -#endif -#define HOMING_CYCLE_0 bit(Z_AXIS) // Z first - -#ifdef HOMING_CYCLE_1 - #undef HOMING_CYCLE_1 -#endif -#define HOMING_CYCLE_1 (bit(X_AXIS)|bit(Y_AXIS)) - // === Special Features // I2S (steppers & other output-only pins) @@ -110,27 +100,27 @@ Socket #5 */ -/* + // 4x Input Module in Socket #1 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module #define X_LIMIT_PIN GPIO_NUM_33 #define Y_LIMIT_PIN GPIO_NUM_32 #define Z_LIMIT_PIN GPIO_NUM_35 -*/ -// 4x Input Module in Socket #2 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -#define X_LIMIT_PIN GPIO_NUM_2 -#define Y_LIMIT_PIN GPIO_NUM_25 -#define Z_LIMIT_PIN GPIO_NUM_39 -// 4x Input Module in Socket #3 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -#define CONTROL_CYCLE_START_PIN GPIO_NUM_26 -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 -#define CONTROL_RESET_PIN GPIO_NUM_16 -#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_27 -//#define INVERT_CONTROL_PIN_MASK B0000 +// // 4x Input Module in Socket #2 +// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +// #define X_LIMIT_PIN GPIO_NUM_2 +// #define Y_LIMIT_PIN GPIO_NUM_25 +// #define Z_LIMIT_PIN GPIO_NUM_39 + +// // 4x Input Module in Socket #3 +// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +// #define CONTROL_CYCLE_START_PIN GPIO_NUM_26 +// #define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 +// #define CONTROL_RESET_PIN GPIO_NUM_16 +// #define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_27 +// //#define INVERT_CONTROL_PIN_MASK B0000 // ================= Setting Defaults ========================== diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index 800faf5b..39e9c721 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -54,6 +54,9 @@ #define SPINDLE_TYPE SpindleType::NONE // defaults +#define DEFAULT_HOMING_CYCLE_0 bit(Y_AXIS) +#define DEFAULT_HOMING_CYCLE_1 bit(X_AXIS) + #define DEFAULT_STEP_PULSE_MICROSECONDS 3 #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on diff --git a/Grbl_Esp32/src/Machines/test_drive.h b/Grbl_Esp32/src/Machines/test_drive.h index 330b9ca0..d92e6d50 100644 --- a/Grbl_Esp32/src/Machines/test_drive.h +++ b/Grbl_Esp32/src/Machines/test_drive.h @@ -37,6 +37,15 @@ #define MACHINE_NAME "Test Drive - Demo Only No I/O!" +// This cannot use homing because there are no switches +#ifdef DEFAULT_HOMING_CYCLE_0 + #undef DEFAULT_HOMING_CYCLE_0 +#endif + +#ifdef DEFAULT_HOMING_CYCLE_1 + #undef DEFAULT_HOMING_CYCLE_1 +#endif + #define SPINDLE_TYPE SpindleType::NONE #ifdef USE_RMT_STEPS diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 2f649dcb..5c74e2f2 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -375,9 +375,10 @@ void make_settings() { 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); - } + homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); + homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); + homing_cycle[2] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle2", DEFAULT_HOMING_CYCLE_2); + homing_cycle[3] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle3", DEFAULT_HOMING_CYCLE_3); + homing_cycle[4] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle4", DEFAULT_HOMING_CYCLE_4); + homing_cycle[5] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle5", DEFAULT_HOMING_CYCLE_5); } diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.cpp b/Grbl_Esp32/src/Spindles/10vSpindle.cpp index 88066434..c2f050da 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/10vSpindle.cpp @@ -44,7 +44,7 @@ namespace Spindles { #endif if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: BESC output pin not defined"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } From 5df1e5eb5992161b4904f50a0e8401aba8ba8823 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 4 Oct 2020 11:25:40 -0500 Subject: [PATCH 15/82] Fixed typos and removed obsolete #defines --- .../src/Machines/6_pack_Lowrider_stepstick_v1.h | 12 +++++------- Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h | 12 +++++------- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h index f1ad2dbb..0107cd94 100644 --- a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h @@ -47,29 +47,27 @@ #define STEPPER_RESET GPIO_NUM_19 -// Mosor Socket #1 +// Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) -#define X_AXIS_SQUARING -// Mosor Socket #2 +// Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) #define Y_DISABLE_PIN I2SO(7) -#define Y_AXIS_SQUARING -// Mosor Socket #3 +// Motor Socket #3 #define Y2_DISABLE_PIN I2SO(8) #define Y2_DIRECTION_PIN I2SO(9) #define Y2_STEP_PIN I2SO(10) -// Mosor Socket #4 +// Motor Socket #4 #define Z_DIRECTION_PIN I2SO(12) #define Z_STEP_PIN I2SO(13) #define Z_DISABLE_PIN I2SO(15) -// Mosor Socket #5 +// Motor Socket #5 #define Z2_DISABLE_PIN I2SO(16) #define Z2_DIRECTION_PIN I2SO(17) #define Z2_STEP_PIN I2SO(18) diff --git a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h index a8c9507f..d1f36e54 100644 --- a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h @@ -47,29 +47,27 @@ #define STEPPER_RESET GPIO_NUM_19 -// Mosor Socket #1 +// Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) -#define X_AXIS_SQUARING -// Mosor Socket #2 +// Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) #define Y_DISABLE_PIN I2SO(7) -#define Y_AXIS_SQUARING -// Mosor Socket #3 +// Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) -// Mosor Socket #4 +// Motor Socket #4 #define X2_DIRECTION_PIN I2SO(12) #define X2_STEP_PIN I2SO(13) #define X2_DISABLE_PIN I2SO(15) -// Mosor Socket #5 +// Motor Socket #5 #define Y2_DISABLE_PIN I2SO(16) #define Y2_DIRECTION_PIN I2SO(17) #define Y2_STEP_PIN I2SO(18) From 885f57e9701fabca0f001e1583cf9e4d729d2ba5 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 4 Oct 2020 17:22:30 -0500 Subject: [PATCH 16/82] Probe cleanup (#625) * Cleanup probing code * Update Grbl.h * Update after review --- Grbl_Esp32/src/Defaults.h | 4 +++ Grbl_Esp32/src/GCode.cpp | 9 ++++--- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 12 ++++----- Grbl_Esp32/src/Probe.cpp | 45 ++++++++++++++------------------ Grbl_Esp32/src/Probe.h | 8 +++--- Grbl_Esp32/src/Settings.cpp | 4 +++ 7 files changed, 41 insertions(+), 43 deletions(-) diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 1fd2241b..f75812a0 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -599,6 +599,10 @@ # define USER_ANALOG_PIN_3 UNDEFINED_PIN #endif +#ifndef PROBE_PIN +# define PROBE_PIN UNDEFINED_PIN +#endif + #ifndef USER_ANALOG_PIN_0_FREQ # define USER_ANALOG_PIN_0_FREQ 5000 #endif diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 8031c95e..0761eafd 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -271,10 +271,11 @@ Error gc_execute_line(char* line, uint8_t client) { mg_word_bit = ModalGroup::MG1; break; case 38: // G38 - probe -#ifndef PROBE_PIN //only allow G38 "Probe" commands if a probe pin is defined. - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No probe pin defined"); - FAIL(Error::GcodeUnsupportedCommand); // [Unsupported G command] -#endif + //only allow G38 "Probe" commands if a probe pin is defined. + if (PROBE_PIN == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No probe pin defined"); + FAIL(Error::GcodeUnsupportedCommand); // [Unsupported G command] + } // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. // * G43.1 is also an axis command but is not explicitly defined this way. if (axis_command != AxisCommand::None) { diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 860c2021..2f3cbf92 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 = "20201001"; +const char* const GRBL_VERSION_BUILD = "20201004"; //#include #include diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index d122529f..33238736 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -375,14 +375,13 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par uint8_t is_probe_away = bit_istrue(parser_flags, GCParserProbeIsAway); uint8_t is_no_error = bit_istrue(parser_flags, GCParserProbeIsNoError); sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle. - probe_configure_invert_mask(is_probe_away); + set_probe_direction(is_probe_away); // After syncing, check if probe is already triggered. If so, halt and issue alarm. // NOTE: This probe initialization error applies to all probing cycles. - if (probe_get_state()) { // Check probe pin state. + if (probe_get_state() ^ is_probe_away) { // Check probe pin state. system_set_exec_alarm(ExecAlarm::ProbeFailInitial); protocol_execute_realtime(); - probe_configure_invert_mask(false); // Re-initialize invert mask before returning. - return GCUpdatePos::None; // Nothing else to do but bail. + return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. mc_line(target, pl_data); @@ -407,9 +406,8 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par } else { sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully. } - sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled. - probe_configure_invert_mask(false); // Re-initialize invert mask. - protocol_execute_realtime(); // Check and execute run-time commands + sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled. + protocol_execute_realtime(); // Check and execute run-time commands // Reset the stepper and planner buffers to remove the remainder of the probe motion. st_reset(); // Reset step segment buffer. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared. diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp index 681b8821..058b936e 100644 --- a/Grbl_Esp32/src/Probe.cpp +++ b/Grbl_Esp32/src/Probe.cpp @@ -24,47 +24,40 @@ #include "Grbl.h" // Inverts the probe pin state depending on user settings and probing cycle mode. -uint8_t probe_invert_mask; +static bool is_probe_away; // Probe pin initialization routine. void probe_init() { -#ifdef PROBE_PIN -# ifdef DISABLE_PROBE_PIN_PULL_UP - pinMode(PROBE_PIN, INPUT); -# else - pinMode(PROBE_PIN, INPUT_PULLUP); // Enable internal pull-up resistors. Normal high operation. -# endif - probe_configure_invert_mask(false); // Initialize invert mask. + static bool show_init_msg = true; // used to show message only once. + + if (PROBE_PIN != UNDEFINED_PIN) { +#ifdef DISABLE_PROBE_PIN_PULL_UP + pinMode(PROBE_PIN, INPUT); +#else + pinMode(PROBE_PIN, INPUT_PULLUP); // Enable internal pull-up resistors. Normal high operation. #endif + + if (show_init_msg) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Probe on pin %s", pinName(PROBE_PIN).c_str()); + show_init_msg = false; + } + } } -// Called by probe_init() and the mc_probe() routines. Sets up the probe pin invert mask to -// appropriately set the pin logic according to setting for normal-high/normal-low operation -// and the probing cycle modes for toward-workpiece/away-from-workpiece. -void probe_configure_invert_mask(uint8_t is_probe_away) { - probe_invert_mask = 0; // Initialize as zero. - if (probe_invert->get()) { - probe_invert_mask ^= PROBE_MASK; - } - if (is_probe_away) { - probe_invert_mask ^= PROBE_MASK; - } +void set_probe_direction(bool is_away) { + is_probe_away = is_away; } // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor. -uint8_t probe_get_state() { -#ifdef PROBE_PIN - return digitalRead(PROBE_PIN) ^ probe_invert_mask; -#else - return false; -#endif +bool probe_get_state() { + return digitalRead(PROBE_PIN) ^ probe_invert->get(); } // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. // NOTE: This function must be extremely efficient as to not bog down the stepper ISR. void probe_state_monitor() { - if (probe_get_state()) { + if (probe_get_state() ^ is_probe_away) { sys_probe_state = PROBE_OFF; memcpy(sys_probe_position, sys_position, sizeof(sys_position)); bit_true(sys_rt_exec_state, EXEC_MOTION_CANCEL); diff --git a/Grbl_Esp32/src/Probe.h b/Grbl_Esp32/src/Probe.h index 2cc5bdf2..f1612c93 100644 --- a/Grbl_Esp32/src/Probe.h +++ b/Grbl_Esp32/src/Probe.h @@ -30,13 +30,11 @@ const int PROBE_ACTIVE = 1; // Actively watching the input pin. // Probe pin initialization routine. void probe_init(); -// Called by probe_init() and the mc_probe() routines. Sets up the probe pin invert mask to -// appropriately set the pin logic according to setting for normal-high/normal-low operation -// and the probing cycle modes for toward-workpiece/away-from-workpiece. -void probe_configure_invert_mask(uint8_t is_probe_away); +// setup probing direction G38.2 vs. G38.4 +void set_probe_direction(bool is_away); // Returns probe pin state. Triggered = true. Called by gcode parser and probe state monitor. -uint8_t probe_get_state(); +bool probe_get_state(); // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index f1be2536..e53c313c 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -537,6 +537,10 @@ void FlagSetting::setDefault() { Error FlagSetting::setStringValue(char* s) { s = trim(s); + Error err = check(s); + if (err != Error::Ok) { + return err; + } _currentValue = (strcasecmp(s, "on") == 0) || (strcasecmp(s, "true") == 0) || (strcasecmp(s, "enabled") == 0) || (strcasecmp(s, "yes") == 0) || (strcasecmp(s, "1") == 0); // _storedValue is -1, 0, or 1 From e04c289e6b4c3a80683616c43717c1ded348ad7e Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 6 Oct 2020 08:14:16 -0500 Subject: [PATCH 17/82] Update error_codes_en_US.csv --- doc/csv/error_codes_en_US.csv | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/csv/error_codes_en_US.csv b/doc/csv/error_codes_en_US.csv index b64832bf..dd4780fe 100644 --- a/doc/csv/error_codes_en_US.csv +++ b/doc/csv/error_codes_en_US.csv @@ -16,6 +16,7 @@ "15","Travel exceeded","Jog target exceeds machine travel. Jog command has been ignored." "16","Invalid jog command","Jog command has no '=' or contains prohibited g-code." "17","Setting disabled","Laser mode requires PWM output." +"18","No Homing/Cycle defined in settings","Set some $homing/CycleX= Settings" "20","Unsupported command","Unsupported or invalid g-code command found in block." "21","Modal group violation","More than one g-code command from same modal group found in block." "22","Undefined feed rate","Feed rate has not yet been set or is undefined." From 1ff44ae50923ef34b3e688cce46c768938962f25 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 6 Oct 2020 15:59:11 -1000 Subject: [PATCH 18/82] More sd_close() to free memory (#622) --- Grbl_Esp32/src/WebUI/WebServer.cpp | 3 +++ Grbl_Esp32/src/WebUI/WebSettings.cpp | 1 + 2 files changed, 4 insertions(+) diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 495d5017..f0afc64f 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -1348,6 +1348,7 @@ namespace WebUI { s += path; s += " does not exist on SD Card\"}"; _webserver->send(200, "application/json", s); + sd_close(); return; } if (list_files) { @@ -1426,6 +1427,7 @@ namespace WebUI { _webserver->send(200, "application/json", jsonfile); _upload_status = UploadStatusType::NONE; set_sd_state(SDCARD_IDLE); + sd_close(); } //SD File upload with direct access to SD/////////////////////////////// @@ -1543,6 +1545,7 @@ namespace WebUI { if (sdUploadFile) { sdUploadFile.close(); } + sd_close(); return; } } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index d795660a..9aa7a004 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -737,6 +737,7 @@ namespace WebUI { ssd += " Total:" + ESPResponseStream::formatBytes(SD.totalBytes()); ssd += "]"; webPrintln(ssd); + sd_close(); return Error::Ok; } #endif From f0ea7c2c440aa69391ab41e097755b35c26ac6fd Mon Sep 17 00:00:00 2001 From: Stefan de Bruijn Date: Wed, 7 Oct 2020 04:12:28 +0200 Subject: [PATCH 19/82] Changed buffer sizes to 256 throughout various parts of the program. (#626) This is a patch necessary for F360 personal users, because they decided to add a very lengthy comment... Co-authored-by: Stefan de Bruijn --- Grbl_Esp32/src/Protocol.h | 2 +- Grbl_Esp32/src/Serial.cpp | 4 +++- Grbl_Esp32/src/Serial.h | 2 +- Grbl_Esp32/src/WebUI/InputBuffer.h | 2 +- Grbl_Esp32/src/WebUI/Serial2Socket.h | 2 +- 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/Grbl_Esp32/src/Protocol.h b/Grbl_Esp32/src/Protocol.h index 6b91553d..83033064 100644 --- a/Grbl_Esp32/src/Protocol.h +++ b/Grbl_Esp32/src/Protocol.h @@ -31,7 +31,7 @@ // memory space we can invest into here or we re-write the g-code parser not to have this // buffer. #ifndef LINE_BUFFER_SIZE -# define LINE_BUFFER_SIZE 80 +# define LINE_BUFFER_SIZE 256 #endif // Starts Grbl main loop. It handles all incoming characters from the serial port and executes diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index e79516c3..ae559c4b 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -70,6 +70,7 @@ uint8_t serial_get_rx_buffer_available(uint8_t client) { void serial_init() { Serial.begin(BAUD_RATE); + Serial.setRxBufferSize(256); // reset all buffers serial_reset_read_buffer(CLIENT_ALL); grbl_send(CLIENT_SERIAL, "\r\n"); // create some white space after ESP32 boot info @@ -104,7 +105,8 @@ void serialCheckTask(void* pvParameters) { if (WebUI::SerialBT.hasClient() && WebUI::SerialBT.available()) { client = CLIENT_BT; data = WebUI::SerialBT.read(); - //Serial.write(data); // echo all data to serial + + // Serial.write(data); // echo all data to serial. } else { #endif #if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN) diff --git a/Grbl_Esp32/src/Serial.h b/Grbl_Esp32/src/Serial.h index 598c6431..8d3f4b93 100644 --- a/Grbl_Esp32/src/Serial.h +++ b/Grbl_Esp32/src/Serial.h @@ -23,7 +23,7 @@ #include "Grbl.h" #ifndef RX_BUFFER_SIZE -# define RX_BUFFER_SIZE 128 +# define RX_BUFFER_SIZE 256 #endif #ifndef TX_BUFFER_SIZE # ifdef USE_LINE_NUMBERS diff --git a/Grbl_Esp32/src/WebUI/InputBuffer.h b/Grbl_Esp32/src/WebUI/InputBuffer.h index 4eec0ca6..da25b6c5 100644 --- a/Grbl_Esp32/src/WebUI/InputBuffer.h +++ b/Grbl_Esp32/src/WebUI/InputBuffer.h @@ -49,7 +49,7 @@ namespace WebUI { ~InputBuffer(); private: - static const int RXBUFFERSIZE = 128; + static const int RXBUFFERSIZE = 256; uint8_t _RXbuffer[RXBUFFERSIZE]; uint16_t _RXbufferSize; diff --git a/Grbl_Esp32/src/WebUI/Serial2Socket.h b/Grbl_Esp32/src/WebUI/Serial2Socket.h index 7d337520..9babdf95 100644 --- a/Grbl_Esp32/src/WebUI/Serial2Socket.h +++ b/Grbl_Esp32/src/WebUI/Serial2Socket.h @@ -28,7 +28,7 @@ class WebSocketsServer; namespace WebUI { class Serial_2_Socket : public Print { static const int TXBUFFERSIZE = 1200; - static const int RXBUFFERSIZE = 128; + static const int RXBUFFERSIZE = 256; static const int FLUSHTIMEOUT = 500; public: From 78dc79aa4f5402696482c5f13707a089ed751ab9 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 6 Oct 2020 16:33:51 -1000 Subject: [PATCH 20/82] $sd/show and handle settings in SD files (#629) * $sd/show and handle settings in SD files * Added $LocalFs/Show and fixed $LocalFs/Run output * Infer / at beginning of SD path name The LocalFS path processing code already inserts a / at the beginning of the path is one isn't present. This patch does the same for SD files. * Show $ command responses in WebUI console * Added $Settings/ListChanged AKA $SC This is useful for saving settings in a compact form that leaves defaults unchanged. * $sd/show works in idle or alarm state * Apply idle/alarm checks to SPIFFS files too --- Grbl_Esp32/src/ProcessSettings.cpp | 11 ++++ Grbl_Esp32/src/Protocol.cpp | 2 +- Grbl_Esp32/src/SDCard.cpp | 1 + Grbl_Esp32/src/SDCard.h | 1 + Grbl_Esp32/src/Settings.cpp | 69 ++++++++++++++++----- Grbl_Esp32/src/Settings.h | 9 +++ Grbl_Esp32/src/WebUI/ESPResponse.cpp | 3 - Grbl_Esp32/src/WebUI/WebSettings.cpp | 89 +++++++++++++++++++++++----- 8 files changed, 151 insertions(+), 34 deletions(-) diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index ddae1b22..11c5f768 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -159,6 +159,16 @@ Error list_settings(const char* value, WebUI::AuthenticationLevel auth_level, We } return Error::Ok; } +Error list_changed_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + for (Setting* s = Setting::List; s; s = s->next()) { + const char* value = s->getStringValue(); + if (!auth_failed(s, value, auth_level) && strcmp(value, s->getDefaultString())) { + show_setting(s->getName(), value, NULL, out); + } + } + grbl_sendf(out->client(), "(Passwords not shown)\r\n"); + return Error::Ok; +} Error list_commands(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { for (Command* cp = Command::List; cp; cp = cp->next()) { const char* name = cp->getName(); @@ -383,6 +393,7 @@ void make_grbl_commands() { new GrblCommand("+", "ExtendedSettings/List", report_extended_settings, notCycleOrHold); new GrblCommand("L", "GrblNames/List", list_grbl_names, notCycleOrHold); new GrblCommand("S", "Settings/List", list_settings, notCycleOrHold); + new GrblCommand("SC","Settings/ListChanged", list_changed_settings, notCycleOrHold); new GrblCommand("CMD", "Commands/List", list_commands, notCycleOrHold); new GrblCommand("E", "ErrorCodes/List", listErrorCodes, anyState); new GrblCommand("G", "GCode/Modes", report_gcode, anyState); diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index d2d3d0cb..abf37c12 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -142,7 +142,7 @@ void protocol_main_loop() { char fileLine[255]; if (readFileLine(fileLine, 255)) { SD_ready_next = false; - report_status_message(gc_execute_line(fileLine, SD_client), SD_client); + report_status_message(execute_line(fileLine, SD_client, SD_auth_level), SD_client); } else { char temp[50]; sd_get_current_filename(temp); diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index 4178cc91..f675c870 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -23,6 +23,7 @@ File myFile; bool SD_ready_next = false; // Grbl has processed a line and is waiting for another uint8_t SD_client = CLIENT_SERIAL; +WebUI::AuthenticationLevel SD_auth_level = WebUI::AuthenticationLevel::LEVEL_GUEST; uint32_t sd_current_line_number; // stores the most recent line number read from the SD static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. diff --git a/Grbl_Esp32/src/SDCard.h b/Grbl_Esp32/src/SDCard.h index 4f19f8aa..a64a4e7d 100644 --- a/Grbl_Esp32/src/SDCard.h +++ b/Grbl_Esp32/src/SDCard.h @@ -31,6 +31,7 @@ const int SDCARD_BUSY_PARSING = 8; extern bool SD_ready_next; // Grbl has processed a line and is waiting for another extern uint8_t SD_client; +extern WebUI::AuthenticationLevel SD_auth_level; //bool sd_mount(); uint8_t get_sd_state(bool refresh); diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index e53c313c..3c63ed91 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -129,6 +129,12 @@ Error IntSetting::setStringValue(char* s) { return Error::Ok; } +const char* IntSetting::getDefaultString() { + static char strval[32]; + sprintf(strval, "%d", _defaultValue); + return strval; +} + const char* IntSetting::getStringValue() { static char strval[32]; @@ -226,10 +232,8 @@ const char* AxisMaskSetting::getCompatibleValue() { return strval; } -const char* AxisMaskSetting::getStringValue() { - static char strval[32]; +static char* maskToString(uint32_t mask, char* strval) { char* s = strval; - uint32_t mask = get(); for (int i = 0; i < MAX_N_AXIS; i++) { if (mask & bit(i)) { *s++ = "XYZABC"[i]; @@ -239,6 +243,16 @@ const char* AxisMaskSetting::getStringValue() { return strval; } +const char* AxisMaskSetting::getDefaultString() { + static char strval[32]; + return maskToString(_defaultValue, strval); +} + +const char* AxisMaskSetting::getStringValue() { + static char strval[32]; + return maskToString(get(), strval); +} + void AxisMaskSetting::addWebui(WebUI::JSONencoder* j) { if (getDescription()) { j->begin_webui(getName(), getDescription(), "I", getStringValue(), 0, (1 << MAX_N_AXIS) - 1); @@ -312,6 +326,12 @@ Error FloatSetting::setStringValue(char* s) { return Error::Ok; } +const char* FloatSetting::getDefaultString() { + static char strval[32]; + (void)sprintf(strval, "%.3f", _defaultValue); + return strval; +} + const char* FloatSetting::getStringValue() { static char strval[32]; (void)sprintf(strval, "%.3f", get()); @@ -395,17 +415,21 @@ Error StringSetting::setStringValue(char* s) { return Error::Ok; } -const char* StringSetting::getStringValue() { - // If the string is a password do not display it - if (_checker && ( +static bool isPassword(bool (*_checker)(char*)) { #ifdef ENABLE_WIFI - _checker == (bool (*)(char*))WebUI::WiFiConfig::isPasswordValid || -#endif - _checker == (bool (*)(char*))WebUI::COMMANDS::isLocalPasswordValid)) { - return "******"; - } else { - return _currentValue.c_str(); + if (_checker == (bool (*)(char*))WebUI::WiFiConfig::isPasswordValid) { + return true; } +#endif + return _checker == (bool (*)(char*))WebUI::COMMANDS::isLocalPasswordValid; +} + +const char* StringSetting::getDefaultString() { + // If the string is a password do not display it + return (_checker && isPassword(_checker)) ? "******" : _defaultValue.c_str(); +} +const char* StringSetting::getStringValue() { + return (_checker && isPassword(_checker)) ? "******" : get(); } void StringSetting::addWebui(WebUI::JSONencoder* j) { @@ -485,14 +509,20 @@ Error EnumSetting::setStringValue(char* s) { return Error::Ok; } -const char* EnumSetting::getStringValue() { +const char* EnumSetting::enumToString(int8_t value) { for (enum_opt_t::iterator it = _options->begin(); it != _options->end(); it++) { - if (it->second == _currentValue) { + if (it->second == value) { return it->first; } } return "???"; } +const char* EnumSetting::getDefaultString() { + return enumToString(_defaultValue); +} +const char* EnumSetting::getStringValue() { + return enumToString(get()); +} void EnumSetting::addWebui(WebUI::JSONencoder* j) { if (!getDescription()) { @@ -557,6 +587,9 @@ Error FlagSetting::setStringValue(char* s) { } return Error::Ok; } +const char* FlagSetting::getDefaultString() { + return _defaultValue ? "On" : "Off"; +} const char* FlagSetting::getStringValue() { return get() ? "On" : "Off"; } @@ -635,10 +668,14 @@ Error IPaddrSetting::setStringValue(char* s) { return Error::Ok; } +const char* IPaddrSetting::getDefaultString() { + static String s; + s = IPAddress(_defaultValue).toString(); + return s.c_str(); +} const char* IPaddrSetting::getStringValue() { static String s; - IPAddress ipaddr(get()); - s = ipaddr.toString(); + s = IPAddress(get()).toString(); return s.c_str(); } diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 5b91c80e..6504c3dc 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -134,6 +134,7 @@ public: Error setStringValue(String s) { return setStringValue(s.c_str()); } virtual const char* getStringValue() = 0; virtual const char* getCompatibleValue() { return getStringValue(); } + virtual const char* getDefaultString() = 0; }; class IntSetting : public Setting { @@ -173,6 +174,7 @@ public: void addWebui(WebUI::JSONencoder*); Error setStringValue(char* value); const char* getStringValue(); + const char* getDefaultString(); int32_t get() { return _currentValue; } }; @@ -202,6 +204,7 @@ public: Error setStringValue(char* value); const char* getCompatibleValue(); const char* getStringValue(); + const char* getDefaultString(); int32_t get() { return _currentValue; } }; @@ -263,6 +266,7 @@ public: void addWebui(WebUI::JSONencoder*) {} Error setStringValue(char* value); const char* getStringValue(); + const char* getDefaultString(); float get() { return _currentValue; } }; @@ -296,6 +300,7 @@ public: void addWebui(WebUI::JSONencoder*); Error setStringValue(char* value); const char* getStringValue(); + const char* getDefaultString(); const char* get() { return _currentValue.c_str(); } }; @@ -310,6 +315,7 @@ private: int8_t _storedValue; int8_t _currentValue; std::map* _options; + const char* enumToString(int8_t value); public: EnumSetting(const char* description, @@ -328,6 +334,7 @@ public: void addWebui(WebUI::JSONencoder*); Error setStringValue(char* value); const char* getStringValue(); + const char* getDefaultString(); int8_t get() { return _currentValue; } }; @@ -357,6 +364,7 @@ public: Error setStringValue(char* value); const char* getCompatibleValue(); const char* getStringValue(); + const char* getDefaultString(); bool get() { return _currentValue; } }; @@ -388,6 +396,7 @@ public: void addWebui(WebUI::JSONencoder*); Error setStringValue(char* value); const char* getStringValue(); + const char* getDefaultString(); uint32_t get() { return _currentValue; } }; diff --git a/Grbl_Esp32/src/WebUI/ESPResponse.cpp b/Grbl_Esp32/src/WebUI/ESPResponse.cpp index 483b0252..ce128152 100644 --- a/Grbl_Esp32/src/WebUI/ESPResponse.cpp +++ b/Grbl_Esp32/src/WebUI/ESPResponse.cpp @@ -96,9 +96,6 @@ namespace WebUI { return; } #endif - if (_client == CLIENT_WEBUI) { - return; //this is sanity check - } grbl_send(_client, data); } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 9aa7a004..c6ae8bec 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -288,7 +288,11 @@ namespace WebUI { return Error::Ok; } - static Error runFile(char* parameter, AuthenticationLevel auth_level) { // ESP700 + static Error runLocalFile(char* parameter, AuthenticationLevel auth_level) { // ESP700 + if (sys.state != State::Idle) { + webPrintln("Busy"); + return Error::IdleError; + } String path = trim(parameter); if ((path.length() > 0) && (path[0] != '/')) { path = "/" + path; @@ -304,14 +308,13 @@ namespace WebUI { //until no line in file Error err; Error accumErr = Error::Ok; + uint8_t client = (espresponse) ? espresponse->client() : CLIENT_ALL; while (currentfile.available()) { String currentline = currentfile.readStringUntil('\n'); if (currentline.length() > 0) { byte line[256]; currentline.getBytes(line, 255); - // TODO Settings - feed into command interpreter - // while accumulating error codes - err = execute_line((char*)line, CLIENT_WEBUI, auth_level); + err = execute_line((char*)line, client, auth_level); if (err != Error::Ok) { accumErr = err; } @@ -322,6 +325,33 @@ namespace WebUI { return accumErr; } + static Error showLocalFile(char* parameter, AuthenticationLevel auth_level) { // ESP701 + if (sys.state != State::Idle && sys.state != State::Alarm) { + return Error::IdleError; + } + String path = trim(parameter); + if ((path.length() > 0) && (path[0] != '/')) { + path = "/" + path; + } + if (!SPIFFS.exists(path)) { + webPrintln("Error: No such file!"); + return Error::SdFileNotFound; + } + File currentfile = SPIFFS.open(path, FILE_READ); + if (!currentfile) { + return Error::SdFailedOpenFile; + } + while (currentfile.available()) { + // String currentline = currentfile.readStringUntil('\n'); + // if (currentline.length() > 0) { + // webPrintln(currentline); + // } + webPrintln(currentfile.readStringUntil('\n')); + } + currentfile.close(); + return Error::Ok; + } + #ifdef ENABLE_NOTIFICATIONS static Error showSetNotification(char* parameter, AuthenticationLevel auth_level) { // ESP610 if (*parameter == '\0') { @@ -642,12 +672,15 @@ namespace WebUI { } #ifdef ENABLE_SD_CARD - static Error runSDFile(char* parameter, AuthenticationLevel auth_level) { // ESP220 - parameter = trim(parameter); + static Error openSDFile(char* parameter) { if (*parameter == '\0') { webPrintln("Missing file name!"); return Error::InvalidValue; } + String path = trim(parameter); + if (path[0] != '/') { + path = "/" + path; + } int8_t state = get_sd_state(true); if (state != SDCARD_IDLE) { if (state == SDCARD_NOT_PRESENT) { @@ -658,14 +691,39 @@ namespace WebUI { return Error::SdFailedBusy; } } + if (!openFile(SD, path.c_str())) { + report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL); + webPrintln(""); + return Error::SdFailedOpenFile; + } + return Error::Ok; + } + static Error showSDFile(char* parameter, AuthenticationLevel auth_level) { // ESP221 + if (sys.state != State::Idle && sys.state != State::Alarm) { + return Error::IdleError; + } + Error err; + if ((err = openSDFile(parameter)) != Error::Ok) { + return err; + } + SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; + char fileLine[255]; + while (readFileLine(fileLine, 255)) { + webPrintln(fileLine); + } + webPrintln(""); + closeFile(); + return Error::Ok; + } + + static Error runSDFile(char* parameter, AuthenticationLevel auth_level) { // ESP220 + Error err; if (sys.state != State::Idle) { webPrintln("Busy"); return Error::IdleError; } - if (!openFile(SD, parameter)) { - report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL); - webPrintln(""); - return Error::Ok; + if ((err = openSDFile(parameter)) != Error::Ok) { + return err; } char fileLine[255]; if (!readFileLine(fileLine, 255)) { @@ -675,9 +733,10 @@ namespace WebUI { return Error::Ok; } SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; - report_status_message(gc_execute_line(fileLine, (espresponse) ? espresponse->client() : CLIENT_ALL), - (espresponse) ? espresponse->client() : CLIENT_ALL); // execute the first line - report_realtime_status((espresponse) ? espresponse->client() : CLIENT_ALL); + SD_auth_level = auth_level; + // execute the first line now; Protocol.cpp handles later ones when SD_ready_next + report_status_message(execute_line(fileLine, SD_client, SD_auth_level), SD_client); + report_realtime_status(SD_client); webPrintln(""); return Error::Ok; } @@ -962,7 +1021,8 @@ namespace WebUI { new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo); new WebCommand(NULL, WEBCMD, WU, "ESP720", "LocalFS/Size", SPIFFSSize); new WebCommand("FORMAT", WEBCMD, WA, "ESP710", "LocalFS/Format", formatSpiffs); - new WebCommand("path", WEBCMD, WU, "ESP700", "LocalFS/Run", runFile); + new WebCommand("path", WEBCMD, WU, "ESP701", "LocalFS/Show", showLocalFile); + new WebCommand("path", WEBCMD, WU, "ESP700", "LocalFS/Run", runLocalFile); new WebCommand("path", WEBCMD, WU, NULL, "LocalFS/List", listLocalFiles); new WebCommand("path", WEBCMD, WU, NULL, "LocalFS/ListJSON", listLocalFilesJSON); #endif @@ -986,6 +1046,7 @@ namespace WebUI { new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings); #endif #ifdef ENABLE_SD_CARD + new WebCommand("path", WEBCMD, WU, "ESP221", "SD/Show", showSDFile); new WebCommand("path", WEBCMD, WU, "ESP220", "SD/Run", runSDFile); new WebCommand("file_or_directory_path", WEBCMD, WU, "ESP215", "SD/Delete", deleteSDObject); new WebCommand(NULL, WEBCMD, WU, "ESP210", "SD/List", listSDFiles); From 895eded6fc160c137e37fc7bcea33aa3531996ba Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 7 Oct 2020 07:37:30 -0500 Subject: [PATCH 21/82] Changed sd_close to SD.end() sd_close was a temporary function to check for memory usage --- Grbl_Esp32/src/WebUI/WebServer.cpp | 6 +++--- Grbl_Esp32/src/WebUI/WebSettings.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index f0afc64f..6fc21ab9 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -1348,7 +1348,7 @@ namespace WebUI { s += path; s += " does not exist on SD Card\"}"; _webserver->send(200, "application/json", s); - sd_close(); + SD.end(); return; } if (list_files) { @@ -1427,7 +1427,7 @@ namespace WebUI { _webserver->send(200, "application/json", jsonfile); _upload_status = UploadStatusType::NONE; set_sd_state(SDCARD_IDLE); - sd_close(); + SD.end(); } //SD File upload with direct access to SD/////////////////////////////// @@ -1545,7 +1545,7 @@ namespace WebUI { if (sdUploadFile) { sdUploadFile.close(); } - sd_close(); + SD.end(); return; } } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index c6ae8bec..6bd60972 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -796,7 +796,7 @@ namespace WebUI { ssd += " Total:" + ESPResponseStream::formatBytes(SD.totalBytes()); ssd += "]"; webPrintln(ssd); - sd_close(); + SD.end(); return Error::Ok; } #endif From 8c10709f21375d9d3c28a1567489b848bd7cf84c Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 7 Oct 2020 13:17:21 -1000 Subject: [PATCH 22/82] Big BUILD_INFO fix (#632) -- Changes that affect behavior Fixed the bugs with report_build_info() Build info is no longer stored in the fixed "EEPROM" section; instead it is a proper Setting named $Firmware/Build . You can change it in the usual way with $Firmware/Build= $I without the = still works. -- Changes that affect configurability for developers Converted a couple more #defines into enums - SETTINGS_RESTORE_* and BITFLAG_RT_STATUS_* . A side effect of this is that it is no longer possible to configure the behavior of $RST=* by defining SETTINGS_RESTORE_ALL to include only a subset. I think it is a bad idea from a customer support perspective to have the meaning of this command be different for different builds. Changed some of the #define ENABLE_ names to eliminate "EEPROM" -- Changes that are purely cosmetic Clarified descriptions in Config.h, to eliminate spurious/incorrect mentions of "EEPROM" Eliminated all mentions of the name "EEPROM" except the ones that truly mean the EEPROM section, as opposed to generalized non-volatile storage. The contents of SettingsStorage.h and SettingsStorage.cpp, which were really related to coordinate storage in Eeprom, not proper settings, were moved to Eeprom.h and Eeprom.cpp. The SettingsStorage files are gone. Got rid of get_step_pin_mask() and get_direction_pin_mask() because they were just aliases for bit(). That eliminated some junk from the SettingsStorage/Eeprom files. Those files now tightly contain only the residual stuff related to the storage of coordinate data in EEPROM. --- Grbl_Esp32/src/Config.h | 77 ++++++++------------- Grbl_Esp32/src/Eeprom.cpp | 24 ++++++- Grbl_Esp32/src/Eeprom.h | 32 +++++++++ Grbl_Esp32/src/GCode.cpp | 6 +- Grbl_Esp32/src/GCode.h | 2 +- Grbl_Esp32/src/Grbl.cpp | 2 +- Grbl_Esp32/src/Grbl.h | 3 +- Grbl_Esp32/src/Limits.cpp | 4 +- Grbl_Esp32/src/Machines/template.h | 4 +- Grbl_Esp32/src/Planner.cpp | 2 +- Grbl_Esp32/src/ProcessSettings.cpp | 36 ++++------ Grbl_Esp32/src/Report.cpp | 65 ++++++++---------- Grbl_Esp32/src/Report.h | 8 ++- Grbl_Esp32/src/Settings.cpp | 2 +- Grbl_Esp32/src/Settings.h | 16 +++++ Grbl_Esp32/src/SettingsDefinitions.cpp | 1 + Grbl_Esp32/src/SettingsStorage.cpp | 76 --------------------- Grbl_Esp32/src/SettingsStorage.h | 92 -------------------------- Grbl_Esp32/src/Spindles/H2ASpindle.md | 3 +- Grbl_Esp32/src/System.h | 2 +- 20 files changed, 159 insertions(+), 298 deletions(-) delete mode 100644 Grbl_Esp32/src/SettingsStorage.cpp delete mode 100644 Grbl_Esp32/src/SettingsStorage.h diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 205db46e..f69dcaf6 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -162,9 +162,9 @@ const int DEFAULT_RADIO_MODE = ESP_RADIO_OFF; // Define realtime command special characters. These characters are 'picked-off' directly from the // serial read data stream and are not passed to the grbl line execution parser. Select characters -// that do not and must not exist in the streamed g-code program. ASCII control characters may be +// that do not and must not exist in the streamed GCode program. ASCII control characters may be // used, if they are available per user setup. Also, extended ASCII codes (>127), which are never in -// g-code programs, maybe selected for interface programs. +// GCode programs, maybe selected for interface programs. // NOTE: If changed, manually update help message in report.c. const uint8_t CMD_RESET = 0x18; // ctrl-x. @@ -218,9 +218,9 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128) // If you have a two-axis machine, DON'T USE THIS. Instead, just alter the homing cycle for two-axes. #define HOMING_SINGLE_AXIS_COMMANDS // Default disabled. Uncomment to enable. -// Number of blocks Grbl executes upon startup. These blocks are stored in EEPROM, where the size +// Number of blocks Grbl executes upon startup. These blocks are stored in non-volatile storage. // and addresses are defined in settings.h. With the current settings, up to 2 startup blocks may -// be stored and executed in order. These startup blocks would typically be used to set the g-code +// be stored and executed in order. These startup blocks would typically be used to set the GCode // parser state depending on user preferences. #define N_STARTUP_LINE 2 // Integer (1-2) @@ -248,7 +248,7 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128) // coordinates through Grbl '$#' print parameters. #define MESSAGE_PROBE_COORDINATES // Enabled by default. Comment to disable. -// Enables a second coolant control pin via the mist coolant g-code command M7 on the Arduino Uno +// Enables a second coolant control pin via the mist coolant GCode command M7 on the Arduino Uno // analog pin 4. Only use this option if you require a second coolant control pin. // NOTE: The M8 flood coolant control pin on analog pin 3 will still be functional regardless. // ESP32 NOTE! This is here for reference only. You enable both M7 and M8 by assigning them a GPIO Pin @@ -326,7 +326,7 @@ const int MIN_SPINDLE_SPEED_OVERRIDE = 10; // Percent of programmed spi const int SPINDLE_OVERRIDE_COARSE_INCREMENT = 10; // (1-99). Usually 10%. const int SPINDLE_OVERRIDE_FINE_INCREMENT = 1; // (1-99). Usually 1%. -// When a M2 or M30 program end command is executed, most g-code states are restored to their defaults. +// When a M2 or M30 program end command is executed, most GCode states are restored to their defaults. // This compile-time option includes the restoring of the feed, rapid, and spindle speed override values // to their default values at program end. #define RESTORE_OVERRIDES_AFTER_PROGRAM_END // Default enabled. Comment to disable. @@ -438,7 +438,7 @@ const double MINIMUM_FEED_RATE = 1.0; // (mm/min) // bogged down by too many trig calculations. const int N_ARC_CORRECTION = 12; // Integer (1-255) -// The arc G2/3 g-code standard is problematic by definition. Radius-based arcs have horrible numerical +// The arc G2/3 GCode standard is problematic by definition. Radius-based arcs have horrible numerical // errors when arc at semi-circles(pi) or full-circles(2*pi). Offset-based arcs are much more accurate // but still have a problem when arcs are full-circles (2*pi). This define accounts for the floating // point issues when offset-based arcs are commanded as full circles, but get interpreted as extremely @@ -491,13 +491,10 @@ const int DWELL_TIME_STEP = 50; // Integer (1-255) (milliseconds) // #define SEGMENT_BUFFER_SIZE 6 // Uncomment to override default in stepper.h. // Line buffer size from the serial input stream to be executed. Also, governs the size of -// each of the startup blocks, as they are each stored as a string of this size. Make sure -// to account for the available EEPROM at the defined memory address in settings.h and for -// the number of desired startup blocks. +// each of the startup blocks, as they are each stored as a string of this size. // NOTE: 80 characters is not a problem except for extreme cases, but the line buffer size -// can be too small and g-code blocks can get truncated. Officially, the g-code standards -// support up to 256 characters. In future versions, this default will be increased, when -// we know how much extra memory space we can re-invest into this. +// can be too small and GCode blocks can get truncated. Officially, the GCode standards +// support up to 256 characters. // #define LINE_BUFFER_SIZE 80 // Uncomment to override default in protocol.h // Serial send and receive buffer size. The receive buffer is often used as another streaming @@ -545,19 +542,9 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds // Enable the '$RST=*', '$RST=$', and '$RST=#' eeprom restore commands. There are cases where // these commands may be undesirable. Simply comment the desired macro to disable it. -// NOTE: See SETTINGS_RESTORE_ALL macro for customizing the `$RST=*` command. -#define ENABLE_RESTORE_EEPROM_WIPE_ALL // '$RST=*' Default enabled. Comment to disable. -#define ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // '$RST=$' Default enabled. Comment to disable. -#define ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS // '$RST=#' Default enabled. Comment to disable. - -// Defines the EEPROM data restored upon a settings version change and `$RST=*` command. Whenever the -// the settings or other EEPROM data structure changes between Grbl versions, Grbl will automatically -// wipe and restore the EEPROM. This macro controls what data is wiped and restored. This is useful -// particularily for OEMs that need to retain certain data. For example, the BUILD_INFO string can be -// written into the Arduino EEPROM via a seperate .INO sketch to contain product data. Altering this -// macro to not restore the build info EEPROM will ensure this data is retained after firmware upgrades. -// NOTE: Uncomment to override defaults in settings.h -// #define SETTINGS_RESTORE_ALL (SETTINGS_RESTORE_DEFAULTS | SETTINGS_RESTORE_PARAMETERS | SETTINGS_RESTORE_STARTUP_LINES | SETTINGS_RESTORE_BUILD_INFO) +#define ENABLE_RESTORE_WIPE_ALL // '$RST=*' Default enabled. Comment to disable. +#define ENABLE_RESTORE_DEFAULT_SETTINGS // '$RST=$' Default enabled. Comment to disable. +#define ENABLE_RESTORE_PARAMETERS // '$RST=#' Default enabled. Comment to disable. // Additional settings have been added to the original set that you see with the $$ command // Some senders may not be able to parse anything different from the original set @@ -565,30 +552,20 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds // Default is off to limit support issues...you can enable here or in your machine definition file // #define SHOW_EXTENDED_SETTINGS -// Enable the '$I=(string)' build info write command. If disabled, any existing build info data must -// be placed into EEPROM via external means with a valid checksum value. This macro option is useful -// to prevent this data from being over-written by a user, when used to store OEM product data. -// NOTE: If disabled and to ensure Grbl can never alter the build info line, you'll also need to enable -// the SETTING_RESTORE_ALL macro above and remove SETTINGS_RESTORE_BUILD_INFO from the mask. -// NOTE: See the included grblWrite_BuildInfo.ino example file to write this string seperately. -#define ENABLE_BUILD_INFO_WRITE_COMMAND // '$I=' Default enabled. Comment to disable. - -// AVR processors require all interrupts to be disabled during an EEPROM write. This includes both -// the stepper ISRs and serial comm ISRs. In the event of a long EEPROM write, this ISR pause can -// cause active stepping to lose position and serial receive data to be lost. This configuration -// option forces the planner buffer to completely empty whenever the EEPROM is written to prevent -// any chance of lost steps. -// However, this doesn't prevent issues with lost serial RX data during an EEPROM write, especially -// if a GUI is premptively filling up the serial RX buffer simultaneously. It's highly advised for -// GUIs to flag these gcodes (G10,G28.1,G30.1) to always wait for an 'ok' after a block containing -// one of these commands before sending more data to eliminate this issue. -// NOTE: Most EEPROM write commands are implicitly blocked during a job (all '$' commands). However, -// coordinate set g-code commands (G10,G28/30.1) are not, since they are part of an active streaming -// job. At this time, this option only forces a planner buffer sync with these g-code commands. -#define FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE // Default enabled. Comment to disable. +// Writing to non-volatile storage (NVS) can take a long time and interfere with timely instruction +// execution, causing problems for the stepper ISRs and serial comm ISRs and subsequent loss of +// stepper position and serial data. This configuration option forces the planner buffer to completely +// empty whenever the NVS is written, to prevent any chance of lost steps. +// It doesn't prevent loss of serial Rx data, especially if a GUI is premptively filling up the +// serial Rx buffer. GUIs should detect GCodes that write to NVS - notably G10,G28.1,G30.1 - +// and wait for an 'ok' before sending more data. +// NOTE: Most setting changes - $ commands - are blocked when a job is running. Coordinate setting +// GCode commands (G10,G28/30.1) are not blocked, since they are part of an active streaming job. +// This option forces a planner buffer sync only with such GCode commands. +#define FORCE_BUFFER_SYNC_DURING_NVS_WRITE // Default enabled. Comment to disable. // In Grbl v0.9 and prior, there is an old outstanding bug where the `WPos:` work position reported -// may not correlate to what is executing, because `WPos:` is based on the g-code parser state, which +// may not correlate to what is executing, because `WPos:` is based on the GCode parser state, which // can be several motions behind. This option forces the planner buffer to empty, sync, and stop // motion whenever there is a command that alters the work coordinate offsets `G10,G43.1,G92,G54-59`. // This is the simplest way to ensure `WPos:` is always correct. Fortunately, it's exceedingly rare @@ -624,8 +601,8 @@ const double PARKING_PULLOUT_INCREMENT = 5.0; // Spindle pull-out and plunge // Enables a special set of M-code commands that enables and disables the parking motion. // These are controlled by `M56`, `M56 P1`, or `M56 Px` to enable and `M56 P0` to disable. -// The command is modal and will be set after a planner sync. Since it is g-code, it is -// executed in sync with g-code commands. It is not a real-time command. +// The command is modal and will be set after a planner sync. Since it is GCode, it is +// executed in sync with GCode commands. It is not a real-time command. // NOTE: PARKING_ENABLE is required. By default, M56 is active upon initialization. Use // DEACTIVATE_PARKING_UPON_INIT to set M56 P0 as the power-up default. // #define ENABLE_PARKING_OVERRIDE_CONTROL // Default disabled. Uncomment to enable diff --git a/Grbl_Esp32/src/Eeprom.cpp b/Grbl_Esp32/src/Eeprom.cpp index d88d9bc2..083570c1 100644 --- a/Grbl_Esp32/src/Eeprom.cpp +++ b/Grbl_Esp32/src/Eeprom.cpp @@ -1,5 +1,5 @@ /* - Eeprom.cpp - Header for system level commands and real-time processes + Eeprom.cpp - Coordinate data stored in EEPROM Part of Grbl Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC @@ -57,3 +57,25 @@ int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, uns } return (checksum == EEPROM.read(source)); } + +// Read selected coordinate data from EEPROM. Updates pointed coord_data value. +// This is now a compatibility routine that is used to propagate coordinate data +// in the old EEPROM format to the new tagged NVS format. +bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) { + uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; + if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) && + !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) { + // Reset with default zero vector + clear_vector_float(coord_data); + // The old code used to rewrite the zeroed data but now that is done + // elsewhere, in a different format. + return false; + } + return true; +} + +// Allow iteration over CoordIndex values +CoordIndex& operator ++ (CoordIndex& i) { + i = static_cast(static_cast(i) + 1); + return i; +} diff --git a/Grbl_Esp32/src/Eeprom.h b/Grbl_Esp32/src/Eeprom.h index 0a69ec3e..32d66c65 100644 --- a/Grbl_Esp32/src/Eeprom.h +++ b/Grbl_Esp32/src/Eeprom.h @@ -22,8 +22,40 @@ #include "Grbl.h" +// Define EEPROM memory address location values for saved coordinate data. +const int EEPROM_SIZE = 1024U; +const int EEPROM_ADDR_PARAMETERS = 512U; + //unsigned char eeprom_get_char(unsigned int addr); //void eeprom_put_char(unsigned int addr, unsigned char new_value); void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size); int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size); int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size); + +// Reads selected coordinate data from EEPROM +bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data); + +// Various places in the code access saved coordinate system data +// by a small integer index according to the values below. +enum CoordIndex : uint8_t{ + Begin = 0, + G54 = Begin, + G55, + G56, + G57, + G58, + G59, + // To support 9 work coordinate systems it would be necessary to define + // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 + // G59_1, + // G59_2, + // G59_3, + NWCSystems, + G28 = NWCSystems, + G30, + // G92_2, + // G92_3, + End, +}; +// Allow iteration over CoordIndex values +CoordIndex& operator ++ (CoordIndex& i); diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 0761eafd..0118bb31 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -873,10 +873,10 @@ Error gc_execute_line(char* line, uint8_t client) { } } // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active. - // TODO: An EEPROM read of the coordinate data may require a buffer sync when the cycle + // TODO: Reading the coordinate data may require a buffer sync when the cycle // is active. The read pauses the processor temporarily and may cause a rare crash. For // future versions on processors with enough memory, all coordinate data should be stored - // in memory and written to EEPROM only when there is not a cycle active. + // in memory and written to non-volatile storage only when there is not a cycle active. float block_coord_system[MAX_N_AXIS]; memcpy(block_coord_system, gc_state.coord_system, sizeof(gc_state.coord_system)); if (bit_istrue(command_words, bit(ModalGroup::MG12))) { // Check if called in block @@ -1002,7 +1002,7 @@ Error gc_execute_line(char* line, uint8_t client) { case NonModal::GoHome0: // G28 case NonModal::GoHome1: // G30 // [G28/30 Errors]: Cutter compensation is enabled. - // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM + // Retreive G28/30 go-home position data (in machine coordinates) from non-volatile storage if (gc_block.non_modal_command == NonModal::GoHome0) { coords[CoordIndex::G28]->get(coord_data); } else { // == NonModal::GoHome1 diff --git a/Grbl_Esp32/src/GCode.h b/Grbl_Esp32/src/GCode.h index 1f594bfe..f602a8f7 100644 --- a/Grbl_Esp32/src/GCode.h +++ b/Grbl_Esp32/src/GCode.h @@ -286,7 +286,7 @@ typedef struct { float position[MAX_N_AXIS]; // Where the interpreter considers the tool to be at this point in the code float coord_system[MAX_N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine - // position in mm. Loaded from EEPROM when called. + // position in mm. Loaded from non-volatile storage when called. float coord_offset[MAX_N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to // machine zero in mm. Non-persistent. Cleared upon reset and boot. float tool_length_offset; // Tracks tool length offset value when enabled. diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index 737bb63e..9743d06b 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -37,7 +37,7 @@ void grbl_init() { #ifdef MACHINE_NAME report_machine_type(CLIENT_SERIAL); #endif - settings_init(); // Load Grbl settings from EEPROM + settings_init(); // Load Grbl settings from non-volatile storage stepper_init(); // Configure stepper pins and interrupt timers init_motors(); system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 2f3cbf92..77513749 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -41,14 +41,13 @@ const char* const GRBL_VERSION_BUILD = "20201004"; #include "Defaults.h" #include "Error.h" -#include "SettingsStorage.h" +#include "Eeprom.h" #include "WebUI/Authentication.h" #include "WebUI/Commands.h" #include "System.h" #include "GCode.h" #include "Planner.h" -#include "Eeprom.h" #include "CoolantControl.h" #include "Limits.h" #include "MotionControl.h" diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index e3539fee..707d5775 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -113,10 +113,10 @@ void limits_go_home(uint8_t cycle_mask) { for (uint8_t idx = 0; idx < n_axis; idx++) { // Initialize step pin masks - step_pin[idx] = get_step_pin_mask(idx); + step_pin[idx] = bit(idx); #ifdef COREXY if ((idx == A_MOTOR) || (idx == B_MOTOR)) { - step_pin[idx] = (get_step_pin_mask(X_AXIS) | get_step_pin_mask(Y_AXIS)); + step_pin[idx] = (bit(X_AXIS) | bit(Y_AXIS)); } #endif if (bit_istrue(cycle_mask, bit(idx))) { diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h index fc321a11..284adde6 100644 --- a/Grbl_Esp32/src/Machines/template.h +++ b/Grbl_Esp32/src/Machines/template.h @@ -140,8 +140,8 @@ // === Default settings // Grbl has many run-time settings that the user can changed by -// commands like $110=2000 . Their values are stored in EEPROM -// so they persist after the controller has been powered down. +// commands like $110=2000 . Their values are stored in non-volatile +// storage so they persist after the controller has been powered down. // Those settings have default values that are used if the user // has not altered them, or if the settings are explicitly reset // to the default values wth $RST=$. diff --git a/Grbl_Esp32/src/Planner.cpp b/Grbl_Esp32/src/Planner.cpp index cf3cc927..47920960 100644 --- a/Grbl_Esp32/src/Planner.cpp +++ b/Grbl_Esp32/src/Planner.cpp @@ -358,7 +358,7 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { unit_vec[idx] = delta_mm; // Store unit vector numerator // Set direction bits. Bit enabled always means direction is negative. if (delta_mm < 0.0) { - block->direction_bits |= get_direction_pin_mask(idx); + block->direction_bits |= bit(idx); } } // Bail if this is a zero-length block. Highly unlikely to occur. diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 11c5f768..67ce2b90 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -33,7 +33,7 @@ void show_setting(const char* name, const char* value, const char* description, void settings_restore(uint8_t restore_flag) { #ifdef WIFI_OR_BLUETOOTH - if (restore_flag & SETTINGS_RESTORE_WIFI_SETTINGS) { + if (restore_flag & SettingsRestore::Wifi) { # ifdef ENABLE_WIFI WebUI::wifi_config.reset_settings(); # endif @@ -42,8 +42,8 @@ void settings_restore(uint8_t restore_flag) { # endif } #endif - if (restore_flag & SETTINGS_RESTORE_DEFAULTS) { - bool restore_startup = restore_flag & SETTINGS_RESTORE_STARTUP_LINES; + if (restore_flag & SettingsRestore::Defaults) { + bool restore_startup = restore_flag & SettingsRestore::StartupLines; for (Setting* s = Setting::List; s; s = s->next()) { if (!s->getDescription()) { const char* name = s->getName(); @@ -55,16 +55,11 @@ void settings_restore(uint8_t restore_flag) { } } } - if (restore_flag & SETTINGS_RESTORE_PARAMETERS) { + if (restore_flag & SettingsRestore::Parameters) { for (auto idx = CoordIndex::Begin; idx < CoordIndex::End; ++idx) { coords[idx]->setDefault(); } } - if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) { - EEPROM.write(EEPROM_ADDR_BUILD_INFO, 0); - EEPROM.write(EEPROM_ADDR_BUILD_INFO + 1, 0); // Checksum - EEPROM.commit(); - } } // Get settings values from non volatile storage into memory @@ -277,17 +272,10 @@ Error sleep_grbl(const char* value, WebUI::AuthenticationLevel auth_level, WebUI } Error get_report_build_info(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { if (!value) { - char line[128]; - settings_read_build_info(line); - report_build_info(line, out->client()); + report_build_info(build_info->get(), out->client()); return Error::Ok; } -#ifdef ENABLE_BUILD_INFO_WRITE_COMMAND - settings_store_build_info(value); - return Error::Ok; -#else return Error::InvalidStatement; -#endif } Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { report_startup_line(0, startup_line_0->get(), out->client()); @@ -296,16 +284,16 @@ Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_le } std::map restoreCommands = { -#ifdef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS - { "$", SETTINGS_RESTORE_DEFAULTS }, { "settings", SETTINGS_RESTORE_DEFAULTS }, +#ifdef ENABLE_RESTORE_DEFAULT_SETTINGS + { "$", SettingsRestore::Defaults }, { "settings", SettingsRestore::Defaults }, #endif -#ifdef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS - { "#", SETTINGS_RESTORE_PARAMETERS }, { "gcode", SETTINGS_RESTORE_PARAMETERS }, +#ifdef ENABLE_RESTORE_CLEAR_PARAMETERS + { "#", SettingsRestore::Parameters }, { "gcode", SettingsRestore::Parameters }, #endif -#ifdef ENABLE_RESTORE_EEPROM_WIPE_ALL - { "*", SETTINGS_RESTORE_ALL }, { "all", SETTINGS_RESTORE_ALL }, +#ifdef ENABLE_RESTORE_WIPE_ALL + { "*", SettingsRestore::All }, { "all", SettingsRestore::All }, #endif - { "@", SETTINGS_RESTORE_WIFI_SETTINGS }, { "wifi", SETTINGS_RESTORE_WIFI_SETTINGS }, + { "@", SettingsRestore::Wifi }, { "wifi", SettingsRestore::Wifi }, }; Error restore_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { if (!value) { diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index e307b05d..077f67d9 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -513,69 +513,56 @@ void report_execute_startup_message(const char* line, Error status_code, uint8_t } // Prints build info line -void report_build_info(char* line, uint8_t client) { - char build_info[50]; - strcpy(build_info, "[VER:"); - strcat(build_info, GRBL_VERSION); - strcat(build_info, "."); - strcat(build_info, GRBL_VERSION_BUILD); - strcat(build_info, ":"); - strcat(build_info, line); - strcat(build_info, "]\r\n[OPT:"); - strcat(build_info, "V"); // variable spindle..always on now - strcat(build_info, "N"); +void report_build_info(const char* line, uint8_t client) { + grbl_sendf(client, "[VER:%s.%s:%s]\r\n[OPT:", GRBL_VERSION, GRBL_VERSION_BUILD, line); #ifdef COOLANT_MIST_PIN - strcat(build_info, "M"); // TODO Need to deal with M8...it could be disabled + grbl_send(client, "M"); // TODO Need to deal with M8...it could be disabled #endif #ifdef COREXY - strcat(build_info, "C"); + grbl_send(client, "C"); #endif #ifdef PARKING_ENABLE - strcat(build_info, "P"); + grbl_send(client, "P"); #endif #ifdef HOMING_SINGLE_AXIS_COMMANDS - strcat(build_info, "H"); + grbl_send(client, "H"); #endif #ifdef LIMITS_TWO_SWITCHES_ON_AXES - strcat(build_info, "L"); + grbl_send(client, "L"); #endif #ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES - strcat(build_info, "A"); + grbl_send(client, "A"); #endif #ifdef ENABLE_BLUETOOTH - strcat(build_info, "B"); + grbl_send(client, "B"); #endif #ifdef ENABLE_SD_CARD - strcat(build_info, "S"); + grbl_send(client, "S"); #endif #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - serial_write('R'); + grbl_send(client, "R"); #endif #if defined(ENABLE_WIFI) - strcat(build_info, "W"); + grbl_send(client, "W"); #endif -#ifndef ENABLE_RESTORE_EEPROM_WIPE_ALL // NOTE: Shown when disabled. - strcat(build_info, "*"); +#ifndef ENABLE_RESTORE_WIPE_ALL // NOTE: Shown when disabled. + grbl_send(client, "*"); #endif -#ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // NOTE: Shown when disabled. - strcat(build_info, "$"); +#ifndef ENABLE_RESTORE_DEFAULT_SETTINGS // NOTE: Shown when disabled. + grbl_send(client, "$"); #endif -#ifndef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS // NOTE: Shown when disabled. - strcat(build_info, "#"); +#ifndef ENABLE_RESTORE_CLEAR_PARAMETERS // NOTE: Shown when disabled. + grbl_send(client, "#"); #endif -#ifndef ENABLE_BUILD_INFO_WRITE_COMMAND // NOTE: Shown when disabled. - strcat(build_info, "I"); -#endif -#ifndef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE // NOTE: Shown when disabled. - strcat(build_info, "E"); +#ifndef FORCE_BUFFER_SYNC_DURING_NVS_WRITE // NOTE: Shown when disabled. + grbl_send(client, "E"); #endif #ifndef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE // NOTE: Shown when disabled. - strcat(build_info, "W"); + grbl_send(client, "W"); #endif // NOTE: Compiled values, like override increments/max/min values, may be added at some point later. // These will likely have a comma delimiter to separate them. - strcat(build_info, "]\r\n"); - grbl_send(client, build_info); // ok to send to all + grbl_send(client, "]\r\n"); report_machine_type(client); #if defined(ENABLE_WIFI) grbl_send(client, (char*)WebUI::wifi_config.info()); @@ -657,7 +644,7 @@ void report_realtime_status(uint8_t client) { break; } float wco[MAX_N_AXIS]; - if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE) || (sys.report_wco_counter == 0)) { + if (bit_isfalse(status_mask->get(), RtStatus::Position) || (sys.report_wco_counter == 0)) { auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { // Apply work coordinate offsets and tool length offset to current position. @@ -665,13 +652,13 @@ void report_realtime_status(uint8_t client) { if (idx == TOOL_LENGTH_OFFSET_AXIS) { wco[idx] += gc_state.tool_length_offset; } - if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) { + if (bit_isfalse(status_mask->get(), RtStatus::Position)) { print_position[idx] -= wco[idx]; } } } // Report machine position - if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) { + if (bit_istrue(status_mask->get(), RtStatus::Position)) { strcat(status, "|MPos:"); } else { #ifdef USE_FWD_KINEMATICS @@ -683,7 +670,7 @@ void report_realtime_status(uint8_t client) { strcat(status, temp); // Returns planner and serial read buffer states. #ifdef REPORT_FIELD_BUFFER_STATE - if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_BUFFER_STATE)) { + if (bit_istrue(status_mask->get(), RtStatus::Buffer)) { int bufsize = DEFAULTBUFFERSIZE; # if defined(ENABLE_WIFI) && defined(ENABLE_TELNET) if (client == CLIENT_TELNET) { diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index 624cb1d9..efe9ed3c 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -20,6 +20,12 @@ along with Grbl. If not, see . */ +// Define status reporting boolean enable bit flags in status_report_mask +enum RtStatus { + Position = bit(0), + Buffer = bit(1), +}; + const char* errorString(Error errorNumber); // Define Grbl feedback message codes. Valid values (0-255). @@ -103,7 +109,7 @@ void report_startup_line(uint8_t n, const char* line, uint8_t client); void report_execute_startup_message(const char* line, Error status_code, uint8_t client); // Prints build info and user info -void report_build_info(char* line, uint8_t client); +void report_build_info(const char* line, uint8_t client); void report_gcode_comment(char* comment); diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 3c63ed91..5e356d99 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -718,7 +718,7 @@ bool Coordinates::load() { void Coordinates::set(float value[MAX_N_AXIS]) { memcpy(&_currentValue, value, sizeof(_currentValue)); -#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE +#ifdef FORCE_BUFFER_SYNC_DURING_NVS_WRITE protocol_buffer_synchronize(); #endif nvs_set_blob(Setting::_handle, _name, _currentValue, sizeof(_currentValue)); diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 6504c3dc..c513b8b4 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -5,6 +5,22 @@ #include #include "WebUI/ESPResponse.h" +// Initialize the configuration subsystem +void settings_init(); + +// Define settings restore bitflags. +enum SettingsRestore { + Defaults = bit(0), + Parameters = bit(1), + StartupLines = bit(2), + // BuildInfo = bit(3), // Obsolete + Wifi = bit(4), + All = 0xff, +}; + +// Restore subsets of settings to default values +void settings_restore(uint8_t restore_flag); + // Command::List is a linked list of all settings, // so common code can enumerate them. class Command; diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 5c74e2f2..ef5f0f5a 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -359,6 +359,7 @@ void make_settings() { hard_limits = new FlagSetting(GRBL, WG, "21", "Limits/Hard", DEFAULT_HARD_LIMIT_ENABLE); soft_limits = new FlagSetting(GRBL, WG, "20", "Limits/Soft", DEFAULT_SOFT_LIMIT_ENABLE, NULL); + build_info = new StringSetting(EXTENDED, WG, NULL, "Firmware/Build", ""); report_inches = new FlagSetting(GRBL, WG, "13", "Report/Inches", DEFAULT_REPORT_INCHES); // TODO Settings - also need to clear, but not set, soft_limits arc_tolerance = new FloatSetting(GRBL, WG, "12", "GCode/ArcTolerance", DEFAULT_ARC_TOLERANCE, 0, 1); diff --git a/Grbl_Esp32/src/SettingsStorage.cpp b/Grbl_Esp32/src/SettingsStorage.cpp deleted file mode 100644 index 5c17d8ba..00000000 --- a/Grbl_Esp32/src/SettingsStorage.cpp +++ /dev/null @@ -1,76 +0,0 @@ -/* - SettingsStorage.cpp - EEPROM configuration handling - Part of Grbl - - Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P - - Grbl 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. - - Grbl 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 Grbl. If not, see . -*/ - -#include "Grbl.h" - -// Read selected coordinate data from EEPROM. Updates pointed coord_data value. -// This is now a compatibility routine that is used to propagate coordinate data -// in the old EEPROM format to the new tagged NVS format. -bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) { - uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; - if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) && - !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) { - // Reset with default zero vector - clear_vector_float(coord_data); - // The old code used to rewrite the zeroed data but now that is done - // elsewhere, in a different format. - return false; - } - return true; -} - -// Method to store build info into EEPROM -// NOTE: This function can only be called in IDLE state. -void settings_store_build_info(const char* line) { - // Build info can only be stored when state is IDLE. - memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO, (char*)line, LINE_BUFFER_SIZE); -} - -// Reads startup line from EEPROM. Updated pointed line string data. -uint8_t settings_read_build_info(char* line) { - if (!(memcpy_from_eeprom_with_checksum((char*)line, EEPROM_ADDR_BUILD_INFO, LINE_BUFFER_SIZE))) { - // Reset line with default value - line[0] = 0; // Empty line - settings_store_build_info(line); - return false; - } - return true; -} - -// Returns step pin mask according to Grbl internal axis indexing. -uint8_t get_step_pin_mask(uint8_t axis_idx) { - // todo clean this up further up stream - return bit(axis_idx); -} - -// Returns direction pin mask according to Grbl internal axis indexing. -uint8_t get_direction_pin_mask(uint8_t axis_idx) { - return bit(axis_idx); -} - -// Allow iteration over CoordIndex values -CoordIndex& operator ++ (CoordIndex& i) { - i = static_cast(static_cast(i) + 1); - return i; -} diff --git a/Grbl_Esp32/src/SettingsStorage.h b/Grbl_Esp32/src/SettingsStorage.h deleted file mode 100644 index 4df9870a..00000000 --- a/Grbl_Esp32/src/SettingsStorage.h +++ /dev/null @@ -1,92 +0,0 @@ -#pragma once - -/* - SettingsStorage.h - eeprom configuration handling - Part of Grbl - - Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC - Copyright (c) 2009-2011 Simen Svale Skogsrud - - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P - - Grbl 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. - - Grbl 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 Grbl. If not, see . -*/ - -#include "Grbl.h" - -// Define status reporting boolean enable bit flags in status_report_mask -#define BITFLAG_RT_STATUS_POSITION_TYPE bit(0) -#define BITFLAG_RT_STATUS_BUFFER_STATE bit(1) - -// Define settings restore bitflags. -#define SETTINGS_RESTORE_DEFAULTS bit(0) -#define SETTINGS_RESTORE_PARAMETERS bit(1) -#define SETTINGS_RESTORE_STARTUP_LINES bit(2) -#define SETTINGS_RESTORE_BUILD_INFO bit(3) -#define SETTINGS_RESTORE_WIFI_SETTINGS bit(4) -#ifndef SETTINGS_RESTORE_ALL -# define SETTINGS_RESTORE_ALL 0xFF // All bitflags -#endif - -// Define EEPROM memory address location values for Grbl settings and parameters -// NOTE: The Atmega328p has 1KB EEPROM. The upper half is reserved for parameters and -// the startup script. The lower half contains the global settings and space for future -// developments. -const int EEPROM_SIZE = 1024U; -const int EEPROM_ADDR_PARAMETERS = 512U; -const int EEPROM_ADDR_BUILD_INFO = 942U; - -// Initialize the configuration subsystem (load settings from EEPROM) -void settings_init(); -void settings_restore(uint8_t restore_flag); -void write_global_settings(); - -uint8_t settings_read_build_info(char* line); -void settings_store_build_info(const char* line); - -// Reads selected coordinate data from EEPROM -bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data); - -// Returns the step pin mask according to Grbl's internal axis numbering -uint8_t get_step_pin_mask(uint8_t i); - -// Returns the direction pin mask according to Grbl's internal axis numbering -uint8_t get_direction_pin_mask(uint8_t i); - -// Various places in the code access saved coordinate system data -// by a small integer index according to the values below. -enum CoordIndex : uint8_t{ - Begin = 0, - G54 = Begin, - G55, - G56, - G57, - G58, - G59, - // To support 9 work coordinate systems it would be necessary to define - // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 - // G59_1, - // G59_2, - // G59_3, - NWCSystems, - G28 = NWCSystems, - G30, - // G92_2, - // G92_3, - End, -}; -// Allow iteration over CoordIndex values -CoordIndex& operator ++ (CoordIndex& i); - diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.md b/Grbl_Esp32/src/Spindles/H2ASpindle.md index ce5d07da..8edefebe 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.md +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.md @@ -75,7 +75,8 @@ like to have. For example: If you want to query the current [running] status, that's command 0x3000, and the status is 1 byte, so you might as well add `0001` as parameter. There are exceptions here, -obviously when writing data to the EEPROM or the speed. +such as when writing data to non-volatile storage or +changing the speed. I hereby list the most important command sequences, and how they work: diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 4568980d..6ea38625 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -190,7 +190,7 @@ void system_set_exec_accessory_override_flag(uint8_t mask); void system_clear_exec_motion_overrides(); void system_clear_exec_accessory_overrides(); -// Execute the startup script lines stored in EEPROM upon initialization +// Execute the startup script lines stored in non-volatile storage upon initialization void system_execute_startup(char* line); Error execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_level); Error system_execute_line(char* line, WebUI::ESPResponseStream*, WebUI::AuthenticationLevel); From 994de40724a6173f70715d2cc64f154acc6ef443 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 7 Oct 2020 18:43:31 -1000 Subject: [PATCH 23/82] Most #defines are gone (#595) * Many more #defines bite the dust * Fixed botch in rt accessory logic * Update Probe.cpp * Update System.cpp * Typo --- Grbl_Esp32/Custom/atari_1020.cpp | 16 +- Grbl_Esp32/Custom/polar_coaster.cpp | 16 +- Grbl_Esp32/src/Config.h | 90 ++++--- Grbl_Esp32/src/GCode.cpp | 10 +- Grbl_Esp32/src/GCode.h | 9 - Grbl_Esp32/src/Grbl.cpp | 22 +- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Limits.cpp | 63 ++--- Grbl_Esp32/src/Limits.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 36 +-- Grbl_Esp32/src/NutsBolts.cpp | 2 +- Grbl_Esp32/src/Probe.cpp | 4 +- Grbl_Esp32/src/Probe.h | 6 +- Grbl_Esp32/src/ProcessSettings.cpp | 2 +- Grbl_Esp32/src/Protocol.cpp | 357 +++++++++++-------------- Grbl_Esp32/src/Report.cpp | 52 ++-- Grbl_Esp32/src/Serial.cpp | 136 ++++++---- Grbl_Esp32/src/Serial.h | 2 +- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 2 +- Grbl_Esp32/src/Stepper.cpp | 28 +- Grbl_Esp32/src/System.cpp | 184 +++++-------- Grbl_Esp32/src/System.h | 238 ++++++++--------- Grbl_Esp32/src/WebUI/WebServer.cpp | 30 +-- 23 files changed, 592 insertions(+), 717 deletions(-) diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 64aa5d68..278e052a 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -179,7 +179,7 @@ void atari_home_task(void* pvParameters) { void calc_solenoid(float penZ) { bool isPenUp; static bool previousPenState = false; - uint32_t solenoid_pen_pulse_len; // duty cycle of solenoid + uint32_t solenoid_pen_pulse_len; // duty cycle of solenoid isPenUp = ((penZ > 0) || (sys.state == State::Alarm)); // is pen above Z0 or is there an alarm // if the state has not change, we only count down to the pull time if (previousPenState == isPenUp) { @@ -247,23 +247,17 @@ void atari_next_pen() { void user_defined_macro(uint8_t index) { char gcode_line[20]; switch (index) { -#ifdef MACRO_BUTTON_0_PIN - case CONTROL_PIN_INDEX_MACRO_0: + case 0: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Pen switch"); WebUI::inputBuffer.push("$H\r"); break; -#endif -#ifdef MACRO_BUTTON_1_PIN - case CONTROL_PIN_INDEX_MACRO_1: + case 1: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Color switch"); atari_next_pen(); sprintf(gcode_line, "G90G0X%3.2f\r", ATARI_PAPER_WIDTH); // alway return to right side to reduce home travel stalls WebUI::inputBuffer.push(gcode_line); break; -#endif -#ifdef MACRO_BUTTON_2_PIN - case CONTROL_PIN_INDEX_MACRO_2: - // feed out some paper and reset the Y 0 + case 2: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Paper switch"); WebUI::inputBuffer.push("G0Y-25\r"); WebUI::inputBuffer.push("G4P0.1\r"); // sync...forces wait for planner to clear @@ -271,9 +265,7 @@ void user_defined_macro(uint8_t index) { gc_sync_position(); plan_sync_position(); break; -#endif default: - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Switch %d", index); break; } } diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp index ba279da0..85cf3f8a 100644 --- a/Grbl_Esp32/Custom/polar_coaster.cpp +++ b/Grbl_Esp32/Custom/polar_coaster.cpp @@ -223,25 +223,15 @@ float abs_angle(float ang) { // Polar coaster has macro buttons, this handles those button pushes. void user_defined_macro(uint8_t index) { switch (index) { -#ifdef MACRO_BUTTON_0_PIN - case CONTROL_PIN_INDEX_MACRO_0: + case 0: WebUI::inputBuffer.push("$H\r"); // home machine break; -#endif -#ifdef MACRO_BUTTON_1_PIN - case CONTROL_PIN_INDEX_MACRO_1: + case 1: WebUI::inputBuffer.push("[ESP220]/1.nc\r"); // run SD card file 1.nc break; -#endif -#ifdef MACRO_BUTTON_2_PIN - case CONTROL_PIN_INDEX_MACRO_2: + case 2: WebUI::inputBuffer.push("[ESP220]/2.nc\r"); // run SD card file 2.nc break; -#endif -#ifdef MACRO_BUTTON_3_PIN - case CONTROL_PIN_INDEX_MACRO_3: - break; -#endif default: break; } diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index f69dcaf6..2de0ac50 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -167,40 +167,37 @@ const int DEFAULT_RADIO_MODE = ESP_RADIO_OFF; // GCode programs, maybe selected for interface programs. // NOTE: If changed, manually update help message in report.c. -const uint8_t CMD_RESET = 0x18; // ctrl-x. -const uint8_t CMD_STATUS_REPORT = '?'; -const uint8_t CMD_CYCLE_START = '~'; -const uint8_t CMD_FEED_HOLD = '!'; - // NOTE: All override realtime commands must be in the extended ASCII character set, starting // at character value 128 (0x80) and up to 255 (0xFF). If the normal set of realtime commands, // such as status reports, feed hold, reset, and cycle start, are moved to the extended set // space, serial.c's RX ISR will need to be modified to accommodate the change. -// const uint8_t CMD_RESET = 0x80; -// const uint8_t CMD_STATUS_REPORT = 0x81; -// const uint8_t CMD_CYCLE_START = 0x82; -// const uint8_t CMD_FEED_HOLD = 0x83; -const uint8_t CMD_SAFETY_DOOR = 0x84; -const uint8_t CMD_JOG_CANCEL = 0x85; -const uint8_t CMD_DEBUG_REPORT = 0x86; // Only when DEBUG enabled, sends debug report in '{}' braces. -const uint8_t CMD_FEED_OVR_RESET = 0x90; // Restores feed override value to 100%. -const uint8_t CMD_FEED_OVR_COARSE_PLUS = 0x91; -const uint8_t CMD_FEED_OVR_COARSE_MINUS = 0x92; -const uint8_t CMD_FEED_OVR_FINE_PLUS = 0x93; -const uint8_t CMD_FEED_OVR_FINE_MINUS = 0x94; -const uint8_t CMD_RAPID_OVR_RESET = 0x95; // Restores rapid override value to 100%. -const uint8_t CMD_RAPID_OVR_MEDIUM = 0x96; -const uint8_t CMD_RAPID_OVR_LOW = 0x97; -// const uint8_t CMD_RAPID_OVR_EXTRA_LOW = 0x98; // *NOT SUPPORTED* -const uint8_t CMD_SPINDLE_OVR_RESET = 0x99; // Restores spindle override value to 100%. -const uint8_t CMD_SPINDLE_OVR_COARSE_PLUS = 0x9A; // 154 -const uint8_t CMD_SPINDLE_OVR_COARSE_MINUS = 0x9B; -const uint8_t CMD_SPINDLE_OVR_FINE_PLUS = 0x9C; -const uint8_t CMD_SPINDLE_OVR_FINE_MINUS = 0x9D; -const uint8_t CMD_SPINDLE_OVR_STOP = 0x9E; -const uint8_t CMD_COOLANT_FLOOD_OVR_TOGGLE = 0xA0; -const uint8_t CMD_COOLANT_MIST_OVR_TOGGLE = 0xA1; +enum class Cmd : uint8_t { + Reset = 0x18, // Ctrl-X + StatusReport = '?', + CycleStart = '~', + FeedHold = '!', + SafetyDoor = 0x84, + JogCancel = 0x85, + DebugReport = 0x86, // Only when DEBUG enabled, sends debug report in '{}' braces. + FeedOvrReset = 0x90, // Restores feed override value to 100%. + FeedOvrCoarsePlus = 0x91, + FeedOvrCoarseMinus = 0x92, + FeedOvrFinePlus = 0x93, + FeedOvrFineMinus = 0x94, + RapidOvrReset = 0x95, // Restores rapid override value to 100%. + RapidOvrMedium = 0x96, + RapidOvrLow = 0x97, + RapidOvrExtraLow = 0x98, // *NOT SUPPORTED* + SpindleOvrReset = 0x99, // Restores spindle override value to 100%. + SpindleOvrCoarsePlus = 0x9A, // 154 + SpindleOvrCoarseMinus = 0x9B, + SpindleOvrFinePlus = 0x9C, + SpindleOvrFineMinus = 0x9D, + SpindleOvrStop = 0x9E, + CoolantFloodOvrToggle = 0xA0, + CoolantMistOvrToggle = 0xA1, +}; // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces // the user to perform the homing cycle (or override the locks) before doing anything else. This is @@ -309,22 +306,27 @@ const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds) // Configure rapid, feed, and spindle override settings. These values define the max and min // allowable override values and the coarse and fine increments per command received. Please // note the allowable values in the descriptions following each define. -const int DEFAULT_FEED_OVERRIDE = 100; // 100%. Don't change this value. -const int MAX_FEED_RATE_OVERRIDE = 200; // Percent of programmed feed rate (100-255). Usually 120% or 200% -const int MIN_FEED_RATE_OVERRIDE = 10; // Percent of programmed feed rate (1-100). Usually 50% or 1% -const int FEED_OVERRIDE_COARSE_INCREMENT = 10; // (1-99). Usually 10%. -const int FEED_OVERRIDE_FINE_INCREMENT = 1; // (1-99). Usually 1%. +namespace FeedOverride { + const int Default = 100; // 100%. Don't change this value. + const int Max = 200; // Percent of programmed feed rate (100-255). Usually 120% or 200% + const int Min = 10; // Percent of programmed feed rate (1-100). Usually 50% or 1% + const int CoarseIncrement = 10; // (1-99). Usually 10%. + const int FineIncrement = 1; // (1-99). Usually 1%. +}; +namespace RapidOverride { + const int Default = 100; // 100%. Don't change this value. + const int Medium = 50; // Percent of rapid (1-99). Usually 50%. + const int Low = 25; // Percent of rapid (1-99). Usually 25%. + const int ExtraLow = 5; // Percent of rapid (1-99). Usually 5%. Not Supported +}; -const int DEFAULT_RAPID_OVERRIDE = 100; // 100%. Don't change this value. -const int RAPID_OVERRIDE_MEDIUM = 50; // Percent of rapid (1-99). Usually 50%. -const int RAPID_OVERRIDE_LOW = 25; // Percent of rapid (1-99). Usually 25%. -// const int RAPID_OVERRIDE_EXTRA_LOW = 5; // *NOT SUPPORTED* Percent of rapid (1-99). Usually 5%. - -const int DEFAULT_SPINDLE_SPEED_OVERRIDE = 100; // 100%. Don't change this value. -const int MAX_SPINDLE_SPEED_OVERRIDE = 200; // Percent of programmed spindle speed (100-255). Usually 200%. -const int MIN_SPINDLE_SPEED_OVERRIDE = 10; // Percent of programmed spindle speed (1-100). Usually 10%. -const int SPINDLE_OVERRIDE_COARSE_INCREMENT = 10; // (1-99). Usually 10%. -const int SPINDLE_OVERRIDE_FINE_INCREMENT = 1; // (1-99). Usually 1%. +namespace SpindleSpeedOverride { + const int Default = 100; // 100%. Don't change this value. + const int Max = 200; // Percent of programmed spindle speed (100-255). Usually 200%. + const int Min = 10; // Percent of programmed spindle speed (1-100). Usually 10%. + const int CoarseIncrement = 10; // (1-99). Usually 10%. + const int FineIncrement = 1; // (1-99). Usually 1%. +} // When a M2 or M30 program end command is executed, most GCode states are restored to their defaults. // This compile-time option includes the restoring of the feed, rapid, and spindle speed override values diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 0118bb31..c26f4b53 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1565,8 +1565,8 @@ Error gc_execute_line(char* line, uint8_t client) { case ProgramFlow::Paused: protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on. if (sys.state != State::CheckMode) { - system_set_exec_state_flag(EXEC_FEED_HOLD); // Use feed hold for program pause. - protocol_execute_realtime(); // Execute suspend. + sys_rt_exec_state.bit.feedHold = true; // Use feed hold for program pause. + protocol_execute_realtime(); // Execute suspend. } break; case ProgramFlow::CompletedM2: @@ -1594,9 +1594,9 @@ Error gc_execute_line(char* line, uint8_t client) { #endif // gc_state.modal.override = OVERRIDE_DISABLE; // Not supported. #ifdef RESTORE_OVERRIDES_AFTER_PROGRAM_END - sys.f_override = DEFAULT_FEED_OVERRIDE; - sys.r_override = DEFAULT_RAPID_OVERRIDE; - sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; + sys.f_override = FeedOverride::Default; + sys.r_override = RapidOverride::Default; + sys.spindle_speed_ovr = SpindleSpeedOverride::Default; #endif // Execute coordinate change and spindle/coolant stop. if (sys.state != State::CheckMode) { diff --git a/Grbl_Esp32/src/GCode.h b/Grbl_Esp32/src/GCode.h index f602a8f7..4b070459 100644 --- a/Grbl_Esp32/src/GCode.h +++ b/Grbl_Esp32/src/GCode.h @@ -163,15 +163,6 @@ struct CoolantState { // Modal Group M8: Coolant control // Modal Group M9: Override control -enum class Override : uint8_t { -#ifdef DEACTIVATE_PARKING_UPON_INIT - Disabled = 0, // (Default: Must be zero) - ParkingMotion = 1, // M56 -#else - ParkingMotion = 0, // M56 (Default: Must be zero) - Disabled = 1, // Parking disabled. -#endif -}; // Modal Group M10: User I/O control enum class IoControl : uint8_t { diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index 9743d06b..7a29bc4f 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -80,16 +80,20 @@ static void reset_variables() { State prior_state = sys.state; memset(&sys, 0, sizeof(system_t)); // Clear system struct variable. sys.state = prior_state; - sys.f_override = DEFAULT_FEED_OVERRIDE; // Set to 100% - sys.r_override = DEFAULT_RAPID_OVERRIDE; // Set to 100% - sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; // Set to 100% + sys.f_override = FeedOverride::Default; // Set to 100% + sys.r_override = RapidOverride::Default; // Set to 100% + sys.spindle_speed_ovr = SpindleSpeedOverride::Default; // Set to 100% memset(sys_probe_position, 0, sizeof(sys_probe_position)); // Clear probe position. - sys_probe_state = 0; - sys_rt_exec_state = 0; - cycle_stop = false; - sys_rt_exec_motion_override = 0; - sys_rt_exec_accessory_override = 0; - system_clear_exec_alarm(); + + sys_probe_state = Probe::Off; + sys_rt_exec_state.value = 0; + sys_rt_exec_accessory_override.value = 0; + sys_rt_exec_alarm = ExecAlarm::None; + cycle_stop = false; + sys_rt_f_override = FeedOverride::Default; + sys_rt_r_override = RapidOverride::Default; + sys_rt_s_override = SpindleSpeedOverride::Default; + // Reset Grbl primary systems. serial_reset_read_buffer(CLIENT_ALL); // Clear serial read buffer gc_init(); // Set g-code parser to default state diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 77513749..e898f5eb 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -44,6 +44,7 @@ const char* const GRBL_VERSION_BUILD = "20201004"; #include "Eeprom.h" #include "WebUI/Authentication.h" #include "WebUI/Commands.h" +#include "Probe.h" #include "System.h" #include "GCode.h" @@ -51,7 +52,6 @@ const char* const GRBL_VERSION_BUILD = "20201004"; #include "CoolantControl.h" #include "Limits.h" #include "MotionControl.h" -#include "Probe.h" #include "Protocol.h" #include "Report.h" #include "Serial.h" diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 707d5775..bf8ca591 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -55,12 +55,12 @@ void IRAM_ATTR isr_limit_switches() { # ifdef HARD_LIMIT_FORCE_STATE_CHECK // Check limit pin state. if (limits_get_state()) { - mc_reset(); // Initiate system kill. - system_set_exec_alarm(ExecAlarm::HardLimit); // Indicate hard limit critical event + mc_reset(); // Initiate system kill. + sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event } # else - mc_reset(); // Initiate system kill. - system_set_exec_alarm(ExecAlarm::HardLimit); // Indicate hard limit critical event + mc_reset(); // Initiate system kill. + sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event # endif #endif } @@ -125,9 +125,10 @@ void limits_go_home(uint8_t cycle_mask) { } } // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches. - bool approach = true; - float homing_rate = homing_seek_rate->get(); - uint8_t limit_state, axislock, n_active_axis; + bool approach = true; + float homing_rate = homing_seek_rate->get(); + uint8_t n_active_axis; + AxisMask limit_state, axislock; do { system_convert_array_steps_to_mpos(target, sys_position); // Initialize and declare variables needed for homing routine. @@ -174,11 +175,12 @@ void limits_go_home(uint8_t cycle_mask) { homing_rate *= sqrt(n_active_axis); // [sqrt(number of active axis)] Adjust so individual axes all move at homing rate. sys.homing_axis_lock = axislock; // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle. - pl_data->feed_rate = homing_rate; // Set current homing rate. - plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. - sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // Set to execute homing motion and clear existing flags. - st_prep_buffer(); // Prep and fill segment buffer from newly planned block. - st_wake_up(); // Initiate motion + pl_data->feed_rate = homing_rate; // Set current homing rate. + plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. + sys.step_control = {}; + sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags. + st_prep_buffer(); // Prep and fill segment buffer from newly planned block. + st_wake_up(); // Initiate motion do { if (approach) { // Check limit state. Lock out cycle axes when they change. @@ -202,23 +204,24 @@ void limits_go_home(uint8_t cycle_mask) { } st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. // Exit routines: No time to run protocol_execute_realtime() in this loop. - if ((sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET)) || cycle_stop) { - uint8_t rt_exec = sys_rt_exec_state; + if (sys_rt_exec_state.bit.safetyDoor || sys_rt_exec_state.bit.reset || cycle_stop) { + ExecState rt_exec_state; + rt_exec_state.value = sys_rt_exec_state.value; // Homing failure condition: Reset issued during cycle. - if (rt_exec & EXEC_RESET) { - system_set_exec_alarm(ExecAlarm::HomingFailReset); + if (rt_exec_state.bit.reset) { + sys_rt_exec_alarm = ExecAlarm::HomingFailReset; } // Homing failure condition: Safety door was opened. - if (rt_exec & EXEC_SAFETY_DOOR) { - system_set_exec_alarm(ExecAlarm::HomingFailDoor); + if (rt_exec_state.bit.safetyDoor) { + sys_rt_exec_alarm = ExecAlarm::HomingFailDoor; } // Homing failure condition: Limit switch still engaged after pull-off motion if (!approach && (limits_get_state() & cycle_mask)) { - system_set_exec_alarm(ExecAlarm::HomingFailPulloff); + sys_rt_exec_alarm = ExecAlarm::HomingFailPulloff; } // Homing failure condition: Limit switch not found during approach. if (approach && cycle_stop) { - system_set_exec_alarm(ExecAlarm::HomingFailApproach); + sys_rt_exec_alarm = ExecAlarm::HomingFailApproach; } if (sys_rt_exec_alarm != ExecAlarm::None) { @@ -303,7 +306,7 @@ void limits_go_home(uint8_t cycle_mask) { #endif } } - sys.step_control = STEP_CONTROL_NORMAL_OP; // Return step control to normal operation. + sys.step_control = {}; // Return step control to normal operation. motors_set_homing_mode(cycle_mask, false); // tell motors homing is done } @@ -371,8 +374,8 @@ void limits_disable() { // Returns limit state as a bit-wise uint8 variable. Each bit indicates an axis limit, where // triggered is 1 and not triggered is 0. Invert mask is applied. Axes are defined by their // number in bit position, i.e. Z_AXIS is bit(2), and Y_AXIS is bit(1). -uint8_t limits_get_state() { - uint8_t pinMask = 0; +AxisMask limits_get_state() { + AxisMask pinMask = 0; auto n_axis = number_axis->get(); for (int axis = 0; axis < n_axis; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { @@ -402,7 +405,7 @@ void limits_soft_check(float* target) { // workspace volume so just come to a controlled stop so position is not lost. When complete // enter alarm mode. if (sys.state == State::Cycle) { - system_set_exec_state_flag(EXEC_FEED_HOLD); + sys_rt_exec_state.bit.feedHold = true; do { protocol_execute_realtime(); if (sys.abort) { @@ -410,9 +413,9 @@ void limits_soft_check(float* target) { } } while (sys.state != State::Idle); } - mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - system_set_exec_alarm(ExecAlarm::SoftLimit); // Indicate soft limit critical event - protocol_execute_realtime(); // Execute to enter critical event loop and system abort + mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. + sys_rt_exec_alarm = ExecAlarm::SoftLimit; // Indicate soft limit critical event + protocol_execute_realtime(); // Execute to enter critical event loop and system abort return; } } @@ -423,12 +426,12 @@ void limitCheckTask(void* pvParameters) { int evt; xQueueReceive(limit_sw_queue, &evt, portMAX_DELAY); // block until receive queue vTaskDelay(DEBOUNCE_PERIOD / portTICK_PERIOD_MS); // delay a while - uint8_t switch_state; + AxisMask switch_state; switch_state = limits_get_state(); if (switch_state) { //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Limit Switch State %08d", switch_state); - mc_reset(); // Initiate system kill. - system_set_exec_alarm(ExecAlarm::HardLimit); // Indicate hard limit critical event + mc_reset(); // Initiate system kill. + sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event } } } diff --git a/Grbl_Esp32/src/Limits.h b/Grbl_Esp32/src/Limits.h index c0782ed6..1f27208f 100644 --- a/Grbl_Esp32/src/Limits.h +++ b/Grbl_Esp32/src/Limits.h @@ -36,7 +36,7 @@ void limits_init(); void limits_disable(); // Returns limit state as a bit-wise uint8 variable. -uint8_t limits_get_state(); +AxisMask limits_get_state(); // Perform one portion of the homing cycle based on the input settings. void limits_go_home(uint8_t cycle_mask); diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 33238736..9b454be3 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -283,7 +283,7 @@ void mc_homing_cycle(uint8_t cycle_mask) { #ifdef LIMITS_TWO_SWITCHES_ON_AXES if (limits_get_state()) { mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown. - system_set_exec_alarm(ExecAlarm::HardLimit); + sys_rt_exec_alarm = ExecAlarm::HardLimit; return; } #endif @@ -379,16 +379,16 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par // After syncing, check if probe is already triggered. If so, halt and issue alarm. // NOTE: This probe initialization error applies to all probing cycles. if (probe_get_state() ^ is_probe_away) { // Check probe pin state. - system_set_exec_alarm(ExecAlarm::ProbeFailInitial); + sys_rt_exec_alarm = ExecAlarm::ProbeFailInitial; protocol_execute_realtime(); return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. mc_line(target, pl_data); // Activate the probing state monitor in the stepper module. - sys_probe_state = PROBE_ACTIVE; + sys_probe_state = Probe::Active; // Perform probing cycle. Wait here until probe is triggered or motion completes. - system_set_exec_state_flag(EXEC_CYCLE_START); + sys_rt_exec_state.bit.cycleStart = true; do { protocol_execute_realtime(); if (sys.abort) { @@ -397,17 +397,17 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par } while (sys.state != State::Idle); // Probing cycle complete! // Set state variables and error out, if the probe failed and cycle with error is enabled. - if (sys_probe_state == PROBE_ACTIVE) { + if (sys_probe_state == Probe::Active) { if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); } else { - system_set_exec_alarm(ExecAlarm::ProbeFailContact); + sys_rt_exec_alarm = ExecAlarm::ProbeFailContact; } } else { sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully. } - sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled. - protocol_execute_realtime(); // Check and execute run-time commands + sys_probe_state = Probe::Off; // Ensure probe state monitor is disabled. + protocol_execute_realtime(); // Check and execute run-time commands // Reset the stepper and planner buffers to remove the remainder of the probe motion. st_reset(); // Reset step segment buffer. plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared. @@ -431,9 +431,9 @@ void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) { } uint8_t plan_status = plan_buffer_line(parking_target, pl_data); if (plan_status) { - bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION); - bit_false(sys.step_control, STEP_CONTROL_END_MOTION); // Allow parking motion to execute, if feed hold is active. - st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case + sys.step_control.executeSysMotion = true; + sys.step_control.endMotion = false; // Allow parking motion to execute, if feed hold is active. + st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case st_prep_buffer(); st_wake_up(); do { @@ -441,10 +441,10 @@ void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) { if (sys.abort) { return; } - } while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION); + } while (sys.step_control.executeSysMotion); st_parking_restore_buffer(); // Restore step segment buffer to normal run state. } else { - bit_false(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION); + sys.step_control.executeSysMotion = false; protocol_exec_rt_system(); } } @@ -467,8 +467,8 @@ void mc_override_ctrl_update(uint8_t override_state) { // realtime abort command and hard limits. So, keep to a minimum. void mc_reset() { // Only this function can set the system reset. Helps prevent multiple kill calls. - if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) { - system_set_exec_state_flag(EXEC_RESET); + if (!sys_rt_exec_state.bit.reset) { + sys_rt_exec_state.bit.reset = true; // Kill spindle and coolant. spindle->stop(); coolant_stop(); @@ -489,13 +489,13 @@ void mc_reset() { // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is // violated, by which, all bets are off. if ((sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) || - (sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) { + (sys.step_control.executeHold || sys.step_control.executeSysMotion)) { if (sys.state == State::Homing) { if (sys_rt_exec_alarm == ExecAlarm::None) { - system_set_exec_alarm(ExecAlarm::HomingFailReset); + sys_rt_exec_alarm = ExecAlarm::HomingFailReset; } } else { - system_set_exec_alarm(ExecAlarm::AbortCycle); + sys_rt_exec_alarm = ExecAlarm::AbortCycle; } st_go_idle(); // Force kill steppers. Position has likely been lost. } diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 1ded98fa..591ef5e3 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -123,7 +123,7 @@ void delay_sec(float seconds, uint8_t mode) { } else { // DELAY_MODE_SYS_SUSPEND // Execute rt_system() only to avoid nesting suspend loops. protocol_exec_rt_system(); - if (sys.suspend & SUSPEND_RESTART_RETRACT) { + if (sys.suspend.bit.restartRetract) { return; // Bail, if safety door reopens. } } diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp index 058b936e..661c2d84 100644 --- a/Grbl_Esp32/src/Probe.cpp +++ b/Grbl_Esp32/src/Probe.cpp @@ -58,8 +58,8 @@ bool probe_get_state() { // NOTE: This function must be extremely efficient as to not bog down the stepper ISR. void probe_state_monitor() { if (probe_get_state() ^ is_probe_away) { - sys_probe_state = PROBE_OFF; + sys_probe_state = Probe::Off; memcpy(sys_probe_position, sys_position, sizeof(sys_position)); - bit_true(sys_rt_exec_state, EXEC_MOTION_CANCEL); + sys_rt_exec_state.bit.motionCancel = true; } } diff --git a/Grbl_Esp32/src/Probe.h b/Grbl_Esp32/src/Probe.h index f1612c93..82e4a196 100644 --- a/Grbl_Esp32/src/Probe.h +++ b/Grbl_Esp32/src/Probe.h @@ -24,8 +24,10 @@ */ // Values that define the probing state machine. -const int PROBE_OFF = 0; // Probing disabled or not in use. (Must be zero.) -const int PROBE_ACTIVE = 1; // Actively watching the input pin. +enum class Probe : uint8_t { + Off = 0, // Probing disabled or not in use. (Must be zero.) + Active = 1, // Actively watching the input pin. +}; // Probe pin initialization routine. void probe_init(); diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 67ce2b90..f1a42673 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -267,7 +267,7 @@ Error home_c(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ES return home(bit(C_AXIS)); } Error sleep_grbl(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { - system_set_exec_state_flag(EXEC_SLEEP); + sys_rt_exec_state.bit.sleep = true; return Error::Ok; } Error get_report_build_info(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index abf37c12..982cecca 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -94,7 +94,7 @@ Error execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_l bool can_park() { return #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - sys.override_ctrl == OVERRIDE_PARKING_MOTION && + sys.override_ctrl == Override::ParkingMotion && #endif homing_enable->get() && !laser_mode->get(); } @@ -125,7 +125,7 @@ void protocol_main_loop() { // Check if the safety door is open. sys.state = State::Idle; if (system_check_safety_door_ajar()) { - bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR); + sys_rt_exec_state.bit.safetyDoor = true; protocol_execute_realtime(); // Enter safety door mode. Should return as IDLE state. } // All systems go! @@ -222,8 +222,8 @@ void protocol_buffer_synchronize() { // is finished, single commands), a command that needs to wait for the motions in the buffer to // execute calls a buffer sync, or the planner buffer is full and ready to go. void protocol_auto_cycle_start() { - if (plan_get_current_block() != NULL) { // Check if there are any blocks in the buffer. - system_set_exec_state_flag(EXEC_CYCLE_START); // If so, execute them! + if (plan_get_current_block() != NULL) { // Check if there are any blocks in the buffer. + sys_rt_exec_state.bit.cycleStart = true; // If so, execute them! } } @@ -236,11 +236,11 @@ void protocol_auto_cycle_start() { // handles them, removing the need to define more computationally-expensive volatile variables. This // also provides a controlled way to execute certain tasks without having two or more instances of // the same task, such as the planner recalculating the buffer upon a feedhold or overrides. -// NOTE: The sys_rt_exec_state variable flags are set by any process, step or serial interrupts, pinouts, +// NOTE: The sys_rt_exec_state.bit variable flags are set by any process, step or serial interrupts, pinouts, // limit switches, or the main program. void protocol_execute_realtime() { protocol_exec_rt_system(); - if (sys.suspend) { + if (sys.suspend.value) { protocol_exec_rt_suspend(); } } @@ -259,145 +259,156 @@ void protocol_exec_rt_system() { // Halt everything upon a critical event flag. Currently hard and soft limits flag this. if ((alarm == ExecAlarm::HardLimit) || (alarm == ExecAlarm::SoftLimit)) { report_feedback_message(Message::CriticalEvent); - system_clear_exec_state_flag(EXEC_RESET); // Disable any existing reset + sys_rt_exec_state.bit.reset = false; // Disable any existing reset do { // Block everything, except reset and status reports, until user issues reset or power // cycles. Hard limits typically occur while unattended or not paying attention. Gives // the user and a GUI time to do what is needed before resetting, like killing the // incoming stream. The same could be said about soft limits. While the position is not // lost, continued streaming could cause a serious crash if by chance it gets executed. - } while (bit_isfalse(sys_rt_exec_state, EXEC_RESET)); + } while (!sys_rt_exec_state.bit.reset); } - system_clear_exec_alarm(); // Clear alarm + sys_rt_exec_alarm = ExecAlarm::None; } - uint8_t rt_exec = sys_rt_exec_state; // Copy volatile sys_rt_exec_state. - if (rt_exec || cycle_stop) { + ExecState rt_exec_state; + rt_exec_state.value = sys_rt_exec_state.value; // Copy volatile sys_rt_exec_state. + if (rt_exec_state.value != 0 || cycle_stop) { // Test if any bits are on // Execute system abort. - if (rt_exec & EXEC_RESET) { + if (rt_exec_state.bit.reset) { sys.abort = true; // Only place this is set true. return; // Nothing else to do but exit. } // Execute and serial print status - if (rt_exec & EXEC_STATUS_REPORT) { + if (rt_exec_state.bit.statusReport) { report_realtime_status(CLIENT_ALL); - system_clear_exec_state_flag(EXEC_STATUS_REPORT); + sys_rt_exec_state.bit.statusReport = false; } // NOTE: Once hold is initiated, the system immediately enters a suspend state to block all // main program processes until either reset or resumed. This ensures a hold completes safely. - if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)) { + if (rt_exec_state.bit.motionCancel || rt_exec_state.bit.feedHold || rt_exec_state.bit.safetyDoor || rt_exec_state.bit.sleep) { // State check for allowable states for hold methods. if (!(sys.state == State::Alarm || sys.state == State::CheckMode)) { // If in CYCLE or JOG states, immediately initiate a motion HOLD. if (sys.state == State::Cycle || sys.state == State::Jog) { - if (!(sys.suspend & (SUSPEND_MOTION_CANCEL | SUSPEND_JOG_CANCEL))) { // Block, if already holding. - st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. - sys.step_control = STEP_CONTROL_EXECUTE_HOLD; // Initiate suspend state with active flag. - if (sys.state == State::Jog) { // Jog cancelled upon any hold event, except for sleeping. - if (!(rt_exec & EXEC_SLEEP)) { - sys.suspend |= SUSPEND_JOG_CANCEL; + if (!(sys.suspend.bit.motionCancel || sys.suspend.bit.jogCancel)) { // Block, if already holding. + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.step_control = {}; + sys.step_control.executeHold = true; // Initiate suspend state with active flag. + if (sys.state == State::Jog) { // Jog cancelled upon any hold event, except for sleeping. + if (!rt_exec_state.bit.sleep) { + sys.suspend.bit.jogCancel = true; } } } } // If IDLE, Grbl is not in motion. Simply indicate suspend state and hold is complete. if (sys.state == State::Idle) { - sys.suspend = SUSPEND_HOLD_COMPLETE; + sys.suspend.value = 0; + sys.suspend.bit.holdComplete = true; } // Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing cycle // to halt and cancel the remainder of the motion. - if (rt_exec & EXEC_MOTION_CANCEL) { + if (rt_exec_state.bit.motionCancel) { // MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may been initiated beforehand // to hold the CYCLE. Motion cancel is valid for a single planner block motion only, while jog cancel // will handle and clear multiple planner block motions. if (sys.state != State::Jog) { - sys.suspend |= SUSPEND_MOTION_CANCEL; // NOTE: State is State::Cycle. + sys.suspend.bit.motionCancel = true; // NOTE: State is State::Cycle. } + sys_rt_exec_state.bit.motionCancel = false; } // Execute a feed hold with deceleration, if required. Then, suspend system. - if (rt_exec & EXEC_FEED_HOLD) { + if (rt_exec_state.bit.feedHold) { // Block SAFETY_DOOR, JOG, and SLEEP states from changing to HOLD state. if (!(sys.state == State::SafetyDoor || sys.state == State::Jog || sys.state == State::Sleep)) { sys.state = State::Hold; } + sys_rt_exec_state.bit.feedHold = false; } // Execute a safety door stop with a feed hold and disable spindle/coolant. // NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered // devices (spindle/coolant), and blocks resuming until switch is re-engaged. - if (rt_exec & EXEC_SAFETY_DOOR) { + if (rt_exec_state.bit.safetyDoor) { report_feedback_message(Message::SafetyDoorAjar); // If jogging, block safety door methods until jog cancel is complete. Just flag that it happened. - if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { + if (!(sys.suspend.bit.jogCancel)) { // Check if the safety re-opened during a restore parking motion only. Ignore if // already retracting, parked or in sleep state. if (sys.state == State::SafetyDoor) { - if (sys.suspend & SUSPEND_INITIATE_RESTORE) { // Actively restoring + if (sys.suspend.bit.initiateRestore) { // Actively restoring #ifdef PARKING_ENABLE // Set hold and reset appropriate control flags to restart parking sequence. - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + if (sys.step_control.executeSysMotion) { st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. - sys.step_control = (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION); - sys.suspend &= ~(SUSPEND_HOLD_COMPLETE); + sys.step_control = {}; + sys.step_control.executeHold = true; + sys.step_control.executeSysMotion = true; + sys.suspend.bit.holdComplete = false; } // else NO_MOTION is active. #endif - sys.suspend &= ~(SUSPEND_RETRACT_COMPLETE | SUSPEND_INITIATE_RESTORE | SUSPEND_RESTORE_COMPLETE); - sys.suspend |= SUSPEND_RESTART_RETRACT; + sys.suspend.bit.retractComplete = false; + sys.suspend.bit.initiateRestore = false; + sys.suspend.bit.restoreComplete = false; + sys.suspend.bit.restartRetract = true; } } if (sys.state != State::Sleep) { sys.state = State::SafetyDoor; } + sys_rt_exec_state.bit.safetyDoor = false; } // NOTE: This flag doesn't change when the door closes, unlike sys.state. Ensures any parking motions // are executed if the door switch closes and the state returns to HOLD. - sys.suspend |= SUSPEND_SAFETY_DOOR_AJAR; + sys.suspend.bit.safetyDoorAjar = true; } } - if (rt_exec & EXEC_SLEEP) { + if (rt_exec_state.bit.sleep) { if (sys.state == State::Alarm) { - sys.suspend |= (SUSPEND_RETRACT_COMPLETE | SUSPEND_HOLD_COMPLETE); + sys.suspend.bit.retractComplete = true; + sys.suspend.bit.holdComplete = true; } - sys.state = State::Sleep; + sys.state = State::Sleep; + sys_rt_exec_state.bit.sleep = false; } - system_clear_exec_state_flag((EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)); } // Execute a cycle start by starting the stepper interrupt to begin executing the blocks in queue. - if (rt_exec & EXEC_CYCLE_START) { + if (rt_exec_state.bit.cycleStart) { // Block if called at same time as the hold commands: feed hold, motion cancel, and safety door. // Ensures auto-cycle-start doesn't resume a hold without an explicit user-input. - if (!(rt_exec & (EXEC_FEED_HOLD | EXEC_MOTION_CANCEL | EXEC_SAFETY_DOOR))) { + if (!(rt_exec_state.bit.feedHold || rt_exec_state.bit.motionCancel || rt_exec_state.bit.safetyDoor)) { // Resume door state when parking motion has retracted and door has been closed. - if ((sys.state == State::SafetyDoor) && !(sys.suspend & SUSPEND_SAFETY_DOOR_AJAR)) { - if (sys.suspend & SUSPEND_RESTORE_COMPLETE) { + if (sys.state == State::SafetyDoor && !(sys.suspend.bit.safetyDoorAjar)) { + if (sys.suspend.bit.restoreComplete) { sys.state = State::Idle; // Set to IDLE to immediately resume the cycle. - } else if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { + } else if (sys.suspend.bit.retractComplete) { // Flag to re-energize powered components and restore original position, if disabled by SAFETY_DOOR. // NOTE: For a safety door to resume, the switch must be closed, as indicated by HOLD state, and // the retraction execution is complete, which implies the initial feed hold is not active. To // restore normal operation, the restore procedures must be initiated by the following flag. Once, // they are complete, it will call CYCLE_START automatically to resume and exit the suspend. - sys.suspend |= SUSPEND_INITIATE_RESTORE; + sys.suspend.bit.initiateRestore = true; } } // Cycle start only when IDLE or when a hold is complete and ready to resume. - if ((sys.state == State::Idle) || ((sys.state == State::Hold) && (sys.suspend & SUSPEND_HOLD_COMPLETE))) { - if (sys.state == State::Hold && sys.spindle_stop_ovr) { - sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE_CYCLE; // Set to restore in suspend routine and cycle start after. + if (sys.state == State::Idle || (sys.state == State::Hold && sys.suspend.bit.holdComplete)) { + if (sys.state == State::Hold && sys.spindle_stop_ovr.value) { + sys.spindle_stop_ovr.bit.restoreCycle = true; // Set to restore in suspend routine and cycle start after. } else { // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. - sys.step_control = STEP_CONTROL_NORMAL_OP; // Restore step control to normal operation - if (plan_get_current_block() && bit_isfalse(sys.suspend, SUSPEND_MOTION_CANCEL)) { - sys.suspend = SUSPEND_DISABLE; // Break suspend state. - sys.state = State::Cycle; + sys.step_control = {}; // Restore step control to normal operation + if (plan_get_current_block() && !sys.suspend.bit.motionCancel) { + sys.suspend.value = 0; // Break suspend state. + sys.state = State::Cycle; st_prep_buffer(); // Initialize step segment buffer before beginning cycle. st_wake_up(); - } else { // Otherwise, do nothing. Set and resume IDLE state. - sys.suspend = SUSPEND_DISABLE; // Break suspend state. - sys.state = State::Idle; + } else { // Otherwise, do nothing. Set and resume IDLE state. + sys.suspend.value = 0; // Break suspend state. + sys.state = State::Idle; } } } } - system_clear_exec_state_flag(EXEC_CYCLE_START); + sys_rt_exec_state.bit.cycleStart = false; } if (cycle_stop) { // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by @@ -406,150 +417,95 @@ void protocol_exec_rt_system() { // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. // NOTE: cycle_stop is set by the stepper subsystem when a cycle or feed hold completes. if ((sys.state == State::Hold || sys.state == State::SafetyDoor || sys.state == State::Sleep) && !(sys.soft_limit) && - !(sys.suspend & SUSPEND_JOG_CANCEL)) { + !(sys.suspend.bit.jogCancel)) { // Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user // has issued a resume command or reset. plan_cycle_reinitialize(); - if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { - sys.suspend |= SUSPEND_HOLD_COMPLETE; + if (sys.step_control.executeHold) { + sys.suspend.bit.holdComplete = true; } - bit_false(sys.step_control, (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION)); + sys.step_control.executeHold = false; + sys.step_control.executeSysMotion = false; } else { // Motion complete. Includes CYCLE/JOG/HOMING states and jog cancel/motion cancel/soft limit events. // NOTE: Motion and jog cancel both immediately return to idle after the hold completes. - if (sys.suspend & SUSPEND_JOG_CANCEL) { // For jog cancel, flush buffers and sync positions. - sys.step_control = STEP_CONTROL_NORMAL_OP; + if (sys.suspend.bit.jogCancel) { // For jog cancel, flush buffers and sync positions. + sys.step_control = {}; plan_reset(); st_reset(); gc_sync_position(); plan_sync_position(); } - if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { // Only occurs when safety door opens during jog. - sys.suspend &= ~(SUSPEND_JOG_CANCEL); - sys.suspend |= SUSPEND_HOLD_COMPLETE; - sys.state = State::SafetyDoor; + if (sys.suspend.bit.safetyDoorAjar) { // Only occurs when safety door opens during jog. + sys.suspend.bit.jogCancel = false; + sys.suspend.bit.holdComplete = true; + sys.state = State::SafetyDoor; } else { - sys.suspend = SUSPEND_DISABLE; - sys.state = State::Idle; + sys.suspend.value = 0; + sys.state = State::Idle; } } cycle_stop = false; } } // Execute overrides. - rt_exec = sys_rt_exec_motion_override; // Copy volatile sys_rt_exec_motion_override - if (rt_exec) { - system_clear_exec_motion_overrides(); // Clear all motion override flags. - uint8_t new_f_override = sys.f_override; - if (rt_exec & EXEC_FEED_OVR_RESET) { - new_f_override = DEFAULT_FEED_OVERRIDE; - } - if (rt_exec & EXEC_FEED_OVR_COARSE_PLUS) { - new_f_override += FEED_OVERRIDE_COARSE_INCREMENT; - } - if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) { - new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT; - } - if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) { - new_f_override += FEED_OVERRIDE_FINE_INCREMENT; - } - if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) { - new_f_override -= FEED_OVERRIDE_FINE_INCREMENT; - } - new_f_override = MIN(new_f_override, MAX_FEED_RATE_OVERRIDE); - new_f_override = MAX(new_f_override, MIN_FEED_RATE_OVERRIDE); - uint8_t new_r_override = sys.r_override; - if (rt_exec & EXEC_RAPID_OVR_RESET) { - new_r_override = DEFAULT_RAPID_OVERRIDE; - } - if (rt_exec & EXEC_RAPID_OVR_MEDIUM) { - new_r_override = RAPID_OVERRIDE_MEDIUM; - } - if (rt_exec & EXEC_RAPID_OVR_LOW) { - new_r_override = RAPID_OVERRIDE_LOW; - } - if ((new_f_override != sys.f_override) || (new_r_override != sys.r_override)) { - sys.f_override = new_f_override; - sys.r_override = new_r_override; - sys.report_ovr_counter = 0; // Set to report change immediately - plan_update_velocity_profile_parameters(); - plan_cycle_reinitialize(); + if ((sys_rt_f_override != sys.f_override) || (sys_rt_r_override != sys.r_override)) { + sys.f_override = sys_rt_f_override; + sys.r_override = sys_rt_r_override; + sys.report_ovr_counter = 0; // Set to report change immediately + plan_update_velocity_profile_parameters(); + plan_cycle_reinitialize(); + } + + // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization. + if (sys_rt_s_override != sys.spindle_speed_ovr) { + sys.step_control.updateSpindleRpm = true; + sys.spindle_speed_ovr = sys_rt_s_override; + sys.report_ovr_counter = 0; // Set to report change immediately + // If spinlde is on, tell it the rpm has been overridden + if (gc_state.modal.spindle != SpindleState::Disable) { + spindle->set_rpm(gc_state.spindle_speed); } } - rt_exec = sys_rt_exec_accessory_override; - if (rt_exec) { - system_clear_exec_accessory_overrides(); // Clear all accessory override flags. - // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization. - uint8_t last_s_override = sys.spindle_speed_ovr; - if (rt_exec & EXEC_SPINDLE_OVR_RESET) { - last_s_override = DEFAULT_SPINDLE_SPEED_OVERRIDE; - } - if (rt_exec & EXEC_SPINDLE_OVR_COARSE_PLUS) { - last_s_override += SPINDLE_OVERRIDE_COARSE_INCREMENT; - } - if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) { - last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT; - } - if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) { - last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT; - } - if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) { - last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT; - } - last_s_override = MIN(last_s_override, MAX_SPINDLE_SPEED_OVERRIDE); - last_s_override = MAX(last_s_override, MIN_SPINDLE_SPEED_OVERRIDE); - if (last_s_override != sys.spindle_speed_ovr) { - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); - sys.spindle_speed_ovr = last_s_override; - sys.report_ovr_counter = 0; // Set to report change immediately - // If spinlde is on, tell it the rpm has been overridden - if (gc_state.modal.spindle != SpindleState::Disable) { - spindle->set_rpm(gc_state.spindle_speed); + + if (sys_rt_exec_accessory_override.bit.spindleOvrStop) { + sys_rt_exec_accessory_override.bit.spindleOvrStop = false; + // Spindle stop override allowed only while in HOLD state. + // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed. + if (sys.state == State::Hold) { + if (sys.spindle_stop_ovr.value == 0) { + sys.spindle_stop_ovr.bit.initiate = true; + } else if (sys.spindle_stop_ovr.bit.enabled) { + sys.spindle_stop_ovr.bit.restore = true; } } - if (rt_exec & EXEC_SPINDLE_OVR_STOP) { - // Spindle stop override allowed only while in HOLD state. - // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed. - if (sys.state == State::Hold) { - if (!(sys.spindle_stop_ovr)) { - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_INITIATE; - } else if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_ENABLED) { - sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE; - } - } - } - // NOTE: Since coolant state always performs a planner sync whenever it changes, the current - // run state can be determined by checking the parser state. - if (rt_exec & (EXEC_COOLANT_FLOOD_OVR_TOGGLE | EXEC_COOLANT_MIST_OVR_TOGGLE)) { - if (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold) { - CoolantState coolant_state = gc_state.modal.coolant; + } + + // NOTE: Since coolant state always performs a planner sync whenever it changes, the current + // run state can be determined by checking the parser state. + if (sys_rt_exec_accessory_override.bit.coolantFloodOvrToggle) { + sys_rt_exec_accessory_override.bit.coolantFloodOvrToggle = false; #ifdef COOLANT_FLOOD_PIN - if (rt_exec & EXEC_COOLANT_FLOOD_OVR_TOGGLE) { - if (coolant_state.Flood) { - coolant_state.Flood = 0; - } else { - coolant_state.Flood = 1; - } - } -#endif -#ifdef COOLANT_MIST_PIN - if (rt_exec & EXEC_COOLANT_MIST_OVR_TOGGLE) { - if (coolant_state.Mist) { - coolant_state.Mist = 0; - } else { - coolant_state.Mist = 1; - } - } -#endif - coolant_set_state(coolant_state); // Report counter set in coolant_set_state(). - gc_state.modal.coolant = coolant_state; - } + if (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold) { + gc_state.modal.coolant.Flood = !gc_state.modal.coolant.Flood; + coolant_set_state(gc_state.modal.coolant); // Report counter set in coolant_set_state(). } +#endif } + if (sys_rt_exec_accessory_override.bit.coolantMistOvrToggle) { + sys_rt_exec_accessory_override.bit.coolantMistOvrToggle = false; +#ifdef COOLANT_MIST_PIN + if (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold) { + gc_state.modal.coolant.Mist = !gc_state.modal.coolant.Mist; + coolant_set_state(gc_state.modal.coolant); // Report counter set in coolant_set_state(). + } +#endif + } + #ifdef DEBUG if (sys_rt_exec_debug) { report_realtime_debug(); - sys_rt_exec_debug = 0; + sys_rt_exec_debug = false; } #endif // Reload step segment buffer @@ -603,30 +559,30 @@ static void protocol_exec_rt_suspend() { } #ifdef DISABLE_LASER_DURING_HOLD if (laser_mode->get()) { - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); + sys_rt_exec_accessory_override.bit.spindleOvrStop = true; } #endif - while (sys.suspend) { + while (sys.suspend.value) { if (sys.abort) { return; } // Block until initial hold is complete and the machine has stopped motion. - if (sys.suspend & SUSPEND_HOLD_COMPLETE) { + if (sys.suspend.bit.holdComplete) { // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for // the safety door and sleep states. if (sys.state == State::SafetyDoor || sys.state == State::Sleep) { // Handles retraction motions and de-energizing. - if (bit_isfalse(sys.suspend, SUSPEND_RETRACT_COMPLETE)) { + if (!sys.suspend.bit.retractComplete) { // Ensure any prior spindle stop override is disabled at start of safety door routine. - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; + sys.spindle_stop_ovr.value = 0; // Disable override #ifndef PARKING_ENABLE spindle->set_state(SpindleState::Disable, 0); // De-energize coolant_off(); #else // Get current position and store restore location and spindle retract waypoint. system_convert_array_steps_to_mpos(parking_target, sys_position); - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + if (!sys.suspend.bit.restartRetract) { memcpy(restore_target, parking_target, sizeof(parking_target)); retract_waypoint += restore_target[PARKING_AXIS]; retract_waypoint = MIN(retract_waypoint, PARKING_TARGET); @@ -667,8 +623,8 @@ static void protocol_exec_rt_suspend() { coolant_off(); } #endif - sys.suspend &= ~(SUSPEND_RESTART_RETRACT); - sys.suspend |= SUSPEND_RETRACT_COMPLETE; + sys.suspend.bit.restartRetract = false; + sys.suspend.bit.retractComplete = true; } else { if (sys.state == State::Sleep) { report_feedback_message(Message::SleepMode); @@ -684,11 +640,11 @@ static void protocol_exec_rt_suspend() { // Allows resuming from parking/safety door. Actively checks if safety door is closed and ready to resume. if (sys.state == State::SafetyDoor) { if (!(system_check_safety_door_ajar())) { - sys.suspend &= ~(SUSPEND_SAFETY_DOOR_AJAR); // Reset door ajar flag to denote ready to resume. + sys.suspend.bit.safetyDoorAjar = false; // Reset door ajar flag to denote ready to resume. } } // Handles parking restore and safety door resume. - if (sys.suspend & SUSPEND_INITIATE_RESTORE) { + if (sys.suspend.bit.initiateRestore) { #ifdef PARKING_ENABLE // Execute fast restore motion to the pull-out position. Parking requires homing enabled. // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. @@ -704,10 +660,10 @@ static void protocol_exec_rt_suspend() { // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. if (gc_state.modal.spindle != SpindleState::Disable) { // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + if (!sys.suspend.bit.restartRetract) { if (laser_mode->get()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); + sys.step_control.updateSpindleRpm = true; } else { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND); @@ -716,7 +672,7 @@ static void protocol_exec_rt_suspend() { } if (gc_state.modal.coolant.Flood || gc_state.modal.coolant.Mist) { // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + if (!sys.suspend.bit.restartRetract) { // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. coolant_set_state(restore_coolant); delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); @@ -726,7 +682,7 @@ static void protocol_exec_rt_suspend() { // Execute slow plunge motion from pull-out position to resume position. if (can_park()) { // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + if (!sys.suspend.bit.restartRetract) { // Regardless if the retract parking motion was a valid/safe motion or not, the // restore parking motion should logically be valid, either by returning to the // original position through valid machine space or by not moving at all. @@ -738,46 +694,47 @@ static void protocol_exec_rt_suspend() { } } #endif - if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { - sys.suspend |= SUSPEND_RESTORE_COMPLETE; - system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. + if (!sys.suspend.bit.restartRetract) { + sys.suspend.bit.restoreComplete = true; + sys_rt_exec_state.bit.cycleStart = true; // Set to resume program. } } } } else { // Feed hold manager. Controls spindle stop override states. // NOTE: Hold ensured as completed by condition check at the beginning of suspend routine. - if (sys.spindle_stop_ovr) { + if (sys.spindle_stop_ovr.value) { // Handles beginning of spindle stop - if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_INITIATE) { + if (sys.spindle_stop_ovr.bit.initiate) { if (gc_state.modal.spindle != SpindleState::Disable) { - spindle->set_state(SpindleState::Disable, 0); // De-energize - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_ENABLED; // Set stop override state to enabled, if de-energized. + spindle->set_state(SpindleState::Disable, 0); // De-energize + sys.spindle_stop_ovr.value = 0; + sys.spindle_stop_ovr.bit.enabled = true; // Set stop override state to enabled, if de-energized. } else { - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state + sys.spindle_stop_ovr.value = 0; // Clear stop override state } // Handles restoring of spindle state - } else if (sys.spindle_stop_ovr & (SPINDLE_STOP_OVR_RESTORE | SPINDLE_STOP_OVR_RESTORE_CYCLE)) { + } else if (sys.spindle_stop_ovr.bit.restore || sys.spindle_stop_ovr.bit.restoreCycle) { if (gc_state.modal.spindle != SpindleState::Disable) { report_feedback_message(Message::SpindleRestore); if (laser_mode->get()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); + sys.step_control.updateSpindleRpm = true; } else { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); } } - if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_RESTORE_CYCLE) { - system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. + if (sys.spindle_stop_ovr.bit.restoreCycle) { + sys_rt_exec_state.bit.cycleStart = true; // Set to resume program. } - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state + sys.spindle_stop_ovr.value = 0; // Clear stop override state } } else { // Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state. - // NOTE: STEP_CONTROL_UPDATE_SPINDLE_RPM is automatically reset upon resume in step generator. - if (bit_istrue(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM)) { + // NOTE: sys.step_control.updateSpindleRpm is automatically reset upon resume in step generator. + if (sys.step_control.updateSpindleRpm) { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); - bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); + sys.step_control.updateSpindleRpm = false; } } } diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 077f67d9..8f895b6c 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -487,7 +487,7 @@ void report_gcode_modes(uint8_t client) { } #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (sys.override_ctrl == OVERRIDE_PARKING_MOTION) { + if (sys.override_ctrl == Override::ParkingMotion) { strcat(modes_rpt, " M56"); } #endif @@ -601,13 +601,9 @@ void report_realtime_status(uint8_t client) { strcat(status, "Run"); break; case State::Hold: - if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { + if (!(sys.suspend.bit.jogCancel)) { strcat(status, "Hold:"); - if (sys.suspend & SUSPEND_HOLD_COMPLETE) { - strcat(status, "0"); // Ready to resume - } else { - strcat(status, "1"); // Actively holding - } + strcat(status, sys.suspend.bit.holdComplete ? "0" : "1"); // Ready to resume break; } // Continues to print jog state during jog cancel. case State::Jog: @@ -624,15 +620,11 @@ void report_realtime_status(uint8_t client) { break; case State::SafetyDoor: strcat(status, "Door:"); - if (sys.suspend & SUSPEND_INITIATE_RESTORE) { + if (sys.suspend.bit.initiateRestore) { strcat(status, "3"); // Restoring } else { - if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { - if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { - strcat(status, "1"); // Door ajar - } else { - strcat(status, "0"); - } + if (sys.suspend.bit.retractComplete) { + strcat(status, sys.suspend.bit.safetyDoorAjar ? "1" : "0"); // Door ajar // Door closed and ready to resume } else { strcat(status, "2"); // Retracting @@ -713,10 +705,10 @@ void report_realtime_status(uint8_t client) { strcat(status, temp); #endif #ifdef REPORT_FIELD_PIN_STATE - uint8_t lim_pin_state = limits_get_state(); - uint8_t ctrl_pin_state = system_control_get_state(); - uint8_t prb_pin_state = probe_get_state(); - if (lim_pin_state | ctrl_pin_state | prb_pin_state) { + AxisMask lim_pin_state = limits_get_state(); + ControlPins ctrl_pin_state = system_control_get_state(); + bool prb_pin_state = probe_get_state(); + if (lim_pin_state || ctrl_pin_state.value || prb_pin_state) { strcat(status, "|Pn:"); if (prb_pin_state) { strcat(status, "P"); @@ -742,21 +734,31 @@ void report_realtime_status(uint8_t client) { strcat(status, "C"); } } - if (ctrl_pin_state) { -# ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_SAFETY_DOOR)) { + if (ctrl_pin_state.value) { + if (ctrl_pin_state.bit.safetyDoor) { strcat(status, "D"); } -# endif - if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_RESET)) { + if (ctrl_pin_state.bit.reset) { strcat(status, "R"); } - if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_FEED_HOLD)) { + if (ctrl_pin_state.bit.feedHold) { strcat(status, "H"); } - if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_CYCLE_START)) { + if (ctrl_pin_state.bit.cycleStart) { strcat(status, "S"); } + if (ctrl_pin_state.bit.macro0) { + strcat(status, "M0"); + } + if (ctrl_pin_state.bit.macro1) { + strcat(status, "M1"); + } + if (ctrl_pin_state.bit.macro2) { + strcat(status, "M2"); + } + if (ctrl_pin_state.bit.macro3) { + strcat(status, "M3"); + } } } #endif diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index ae559c4b..f3c75676 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -131,7 +131,7 @@ void serialCheckTask(void* pvParameters) { // Pick off realtime command characters directly from the serial stream. These characters are // not passed into the main buffer, but these set system state flag bits for realtime execution. if (is_realtime_command(data)) { - execute_realtime_command(data, client); + execute_realtime_command(static_cast(data), client); } else { vTaskEnterCritical(&myMutex); client_buffer[client].write(data); @@ -196,91 +196,115 @@ bool any_client_has_data() { // checks to see if a character is a realtime character bool is_realtime_command(uint8_t data) { - return data == CMD_RESET || data == CMD_STATUS_REPORT || data == CMD_CYCLE_START || data == CMD_FEED_HOLD || data > 0x7F; + if (data >= 0x80) { + return true; + } + auto cmd = static_cast(data); + return cmd == Cmd::Reset || cmd == Cmd::StatusReport || cmd == Cmd::CycleStart || cmd == Cmd::FeedHold; } // Act upon a realtime character -void execute_realtime_command(uint8_t command, uint8_t client) { +void execute_realtime_command(Cmd command, uint8_t client) { switch (command) { - case CMD_RESET: + case Cmd::Reset: mc_reset(); // Call motion control reset routine. break; - case CMD_STATUS_REPORT: + case Cmd::StatusReport: report_realtime_status(client); // direct call instead of setting flag break; - case CMD_CYCLE_START: - system_set_exec_state_flag(EXEC_CYCLE_START); // Set as true + case Cmd::CycleStart: + sys_rt_exec_state.bit.cycleStart = true; break; - case CMD_FEED_HOLD: - system_set_exec_state_flag(EXEC_FEED_HOLD); // Set as true + case Cmd::FeedHold: + sys_rt_exec_state.bit.feedHold = true; break; - case CMD_SAFETY_DOOR: - system_set_exec_state_flag(EXEC_SAFETY_DOOR); - break; // Set as true - case CMD_JOG_CANCEL: + case Cmd::SafetyDoor: + sys_rt_exec_state.bit.safetyDoor = true; + break; + case Cmd::JogCancel: if (sys.state == State::Jog) { // Block all other states from invoking motion cancel. - system_set_exec_state_flag(EXEC_MOTION_CANCEL); + sys_rt_exec_state.bit.motionCancel = true; } break; + case Cmd::DebugReport: #ifdef DEBUG - case CMD_DEBUG_REPORT: { - uint8_t sreg = SREG; - cli(); - bit_true(sys_rt_exec_debug, EXEC_DEBUG_REPORT); - SREG = sreg; - } break; + sys_rt_exec_debug = true; #endif - case CMD_FEED_OVR_RESET: - system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break; - case CMD_FEED_OVR_COARSE_PLUS: - system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); + case Cmd::SpindleOvrStop: + sys_rt_exec_accessory_override.bit.spindleOvrStop = 1; break; - case CMD_FEED_OVR_COARSE_MINUS: - system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); + case Cmd::FeedOvrReset: + sys_rt_f_override = FeedOverride::Default; break; - case CMD_FEED_OVR_FINE_PLUS: - system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); + case Cmd::FeedOvrCoarsePlus: + sys_rt_f_override += FeedOverride::CoarseIncrement; + if (sys_rt_f_override > FeedOverride::Max) { + sys_rt_f_override = FeedOverride::Max; + } break; - case CMD_FEED_OVR_FINE_MINUS: - system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); + case Cmd::FeedOvrCoarseMinus: + sys_rt_f_override -= FeedOverride::CoarseIncrement; + if (sys_rt_f_override < FeedOverride::Min) { + sys_rt_f_override = FeedOverride::Min; + } break; - case CMD_RAPID_OVR_RESET: - system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); + case Cmd::FeedOvrFinePlus: + sys_rt_f_override += FeedOverride::FineIncrement; + if (sys_rt_f_override > FeedOverride::Max) { + sys_rt_f_override = FeedOverride::Max; + } break; - case CMD_RAPID_OVR_MEDIUM: - system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); + case Cmd::FeedOvrFineMinus: + sys_rt_f_override -= FeedOverride::FineIncrement; + if (sys_rt_f_override < FeedOverride::Min) { + sys_rt_f_override = FeedOverride::Min; + } break; - case CMD_RAPID_OVR_LOW: - system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); + case Cmd::RapidOvrReset: + sys_rt_r_override = RapidOverride::Default; break; - case CMD_SPINDLE_OVR_RESET: - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); + case Cmd::RapidOvrMedium: + sys_rt_r_override = RapidOverride::Medium; break; - case CMD_SPINDLE_OVR_COARSE_PLUS: - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); + case Cmd::RapidOvrLow: + sys_rt_r_override = RapidOverride::Low; break; - case CMD_SPINDLE_OVR_COARSE_MINUS: - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); + case Cmd::RapidOvrExtraLow: + sys_rt_r_override = RapidOverride::ExtraLow; break; - case CMD_SPINDLE_OVR_FINE_PLUS: - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); + case Cmd::SpindleOvrReset: + sys_rt_s_override = SpindleSpeedOverride::Default; break; - case CMD_SPINDLE_OVR_FINE_MINUS: - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); + case Cmd::SpindleOvrCoarsePlus: + sys_rt_s_override += SpindleSpeedOverride::CoarseIncrement; + if (sys_rt_s_override > SpindleSpeedOverride::Max) { + sys_rt_s_override = SpindleSpeedOverride::Max; + } break; - case CMD_SPINDLE_OVR_STOP: - system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); + case Cmd::SpindleOvrCoarseMinus: + sys_rt_s_override -= SpindleSpeedOverride::CoarseIncrement; + if (sys_rt_s_override < SpindleSpeedOverride::Min) { + sys_rt_s_override = SpindleSpeedOverride::Min; + } break; -#ifdef COOLANT_FLOOD_PIN - case CMD_COOLANT_FLOOD_OVR_TOGGLE: - system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); + case Cmd::SpindleOvrFinePlus: + sys_rt_s_override += SpindleSpeedOverride::FineIncrement; + if (sys_rt_s_override > SpindleSpeedOverride::Max) { + sys_rt_s_override = SpindleSpeedOverride::Max; + } break; -#endif -#ifdef COOLANT_MIST_PIN - case CMD_COOLANT_MIST_OVR_TOGGLE: - system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); + case Cmd::SpindleOvrFineMinus: + sys_rt_s_override -= SpindleSpeedOverride::FineIncrement; + if (sys_rt_s_override < SpindleSpeedOverride::Min) { + sys_rt_s_override = SpindleSpeedOverride::Min; + } + break; + case Cmd::CoolantFloodOvrToggle: + sys_rt_exec_accessory_override.bit.coolantFloodOvrToggle = 1; + break; + case Cmd::CoolantMistOvrToggle: + sys_rt_exec_accessory_override.bit.coolantMistOvrToggle = 1; break; -#endif } } diff --git a/Grbl_Esp32/src/Serial.h b/Grbl_Esp32/src/Serial.h index 8d3f4b93..7cca0d94 100644 --- a/Grbl_Esp32/src/Serial.h +++ b/Grbl_Esp32/src/Serial.h @@ -51,6 +51,6 @@ void serial_reset_read_buffer(uint8_t client); // Returns the number of bytes available in the RX serial buffer. uint8_t serial_get_rx_buffer_available(uint8_t client); -void execute_realtime_command(uint8_t command, uint8_t client); +void execute_realtime_command(Cmd command, uint8_t client); bool any_client_has_data(); bool is_realtime_command(uint8_t data); diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 7df1f4ae..a5e75fcb 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -193,7 +193,7 @@ namespace Spindles { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); if (next_cmd.critical) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); - system_set_exec_alarm(ExecAlarm::SpindleControl); + sys_rt_exec_alarm = ExecAlarm::SpindleControl; } unresponsive = true; } diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 858e15c2..d78a0d88 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -301,7 +301,7 @@ static void stepper_pulse_func() { } } // Check probing state. - if (sys_probe_state == PROBE_ACTIVE) { + if (sys_probe_state == Probe::Active) { probe_state_monitor(); } // Reset step out bits. @@ -796,7 +796,7 @@ static uint8_t st_next_block_index(uint8_t block_index) { */ void st_prep_buffer() { // Block step prep buffer, while in a suspend state and there is no suspend motion to execute. - if (bit_istrue(sys.step_control, STEP_CONTROL_END_MOTION)) { + if (sys.step_control.endMotion) { return; } @@ -804,7 +804,7 @@ void st_prep_buffer() { // Determine if we need to load a new planner block or if the block needs to be recomputed. if (pl_block == NULL) { // Query planner for a queued block - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + if (sys.step_control.executeSysMotion) { pl_block = plan_get_system_motion_block(); } else { pl_block = plan_get_current_block(); @@ -854,7 +854,7 @@ void st_prep_buffer() { prep.step_per_mm = prep.steps_remaining / pl_block->millimeters; prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; prep.dt_remainder = 0.0; // Reset for new segment block - if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || prep.recalculate_flag.decelOverride) { + if ((sys.step_control.executeHold) || prep.recalculate_flag.decelOverride) { // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. prep.current_speed = prep.exit_speed; pl_block->entry_speed_sqr = prep.exit_speed * prep.exit_speed; @@ -879,7 +879,7 @@ void st_prep_buffer() { */ prep.mm_complete = 0.0; // Default velocity profile complete at 0.0mm from end of block. float inv_2_accel = 0.5 / pl_block->acceleration; - if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { // [Forced Deceleration to Zero Velocity] + if (sys.step_control.executeHold) { // [Forced Deceleration to Zero Velocity] // Compute velocity profile parameters for a feed hold in-progress. This profile overrides // the planner block profile, enforcing a deceleration to zero speed. prep.ramp_type = RAMP_DECEL; @@ -898,7 +898,7 @@ void st_prep_buffer() { prep.accelerate_until = pl_block->millimeters; float exit_speed_sqr; float nominal_speed; - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + if (sys.step_control.executeSysMotion) { prep.exit_speed = exit_speed_sqr = 0.0; // Enforce stop at end of system motion. } else { exit_speed_sqr = plan_get_exec_block_exit_speed_sqr(); @@ -956,7 +956,7 @@ void st_prep_buffer() { } } - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); // Force update whenever updating block. + sys.step_control.updateSpindleRpm = true; // Force update whenever updating block. } // Initialize new segment @@ -1075,7 +1075,7 @@ void st_prep_buffer() { /* ----------------------------------------------------------------------------------- Compute spindle speed PWM output for step segment */ - if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_RPM)) { + if (st_prep_block->is_pwm_rate_adjusted || sys.step_control.updateSpindleRpm) { if (pl_block->spindle != SpindleState::Disable) { float rpm = pl_block->spindle_speed; // NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate. @@ -1092,7 +1092,7 @@ void st_prep_buffer() { sys.spindle_speed = 0.0; prep.current_spindle_rpm = 0.0; } - bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM); + sys.step_control.updateSpindleRpm = false; } prep_segment->spindle_rpm = prep.current_spindle_rpm; // Reload segment PWM value @@ -1113,10 +1113,10 @@ void st_prep_buffer() { // Bail if we are at the end of a feed hold and don't have a step to execute. if (prep_segment->n_step == 0) { - if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { + if (sys.step_control.executeHold) { // Less than one step to decelerate to zero speed, but already very close. AMASS // requires full steps to execute. So, just bail. - bit_true(sys.step_control, STEP_CONTROL_END_MOTION); + sys.step_control.endMotion = true; #ifdef PARKING_ENABLE if (!(prep.recalculate_flag.parking)) { prep.recalculate_flag.holdPartialBlock = 1; @@ -1195,7 +1195,7 @@ void st_prep_buffer() { // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete // the segment queue, where realtime protocol will set new state upon receiving the // cycle stop flag from the ISR. Prep_segment is blocked until then. - bit_true(sys.step_control, STEP_CONTROL_END_MOTION); + sys.step_control.endMotion = true; #ifdef PARKING_ENABLE if (!(prep.recalculate_flag.parking)) { prep.recalculate_flag.holdPartialBlock = 1; @@ -1204,8 +1204,8 @@ void st_prep_buffer() { return; // Bail! } else { // End of planner block // The planner block is complete. All steps are set to be executed in the segment buffer. - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { - bit_true(sys.step_control, STEP_CONTROL_END_MOTION); + if (sys.step_control.executeSysMotion) { + sys.step_control.endMotion = true; return; } pl_block = NULL; // Set pointer to indicate check and load next planner block. diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 7c434ec3..ab5c7a62 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -22,18 +22,20 @@ #include "Config.h" // Declare system global variable structure -system_t sys; -int32_t sys_position[MAX_N_AXIS]; // Real-time machine (aka home) position vector in steps. -int32_t sys_probe_position[MAX_N_AXIS]; // Last probe position in machine coordinates and steps. -volatile uint8_t sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. -volatile uint8_t sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. -volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. -volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides. -volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. -volatile bool cycle_stop; // For state transitions, instead of bitflag +system_t sys; +int32_t sys_position[MAX_N_AXIS]; // Real-time machine (aka home) position vector in steps. +int32_t sys_probe_position[MAX_N_AXIS]; // Last probe position in machine coordinates and steps. +volatile Probe sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. +volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. +volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. +volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. +volatile bool cycle_stop; // For state transitions, instead of bitflag #ifdef DEBUG -volatile uint8_t sys_rt_exec_debug; +volatile bool sys_rt_exec_debug; #endif +volatile Percent sys_rt_f_override; // Global realtime executor feedrate override percentage +volatile Percent sys_rt_r_override; // Global realtime executor rapid override percentage +volatile Percent sys_rt_s_override; // Global realtime executor spindle override percentage UserOutput::AnalogOutput* myAnalogOutputs[MaxUserDigitalPin]; UserOutput::DigitalOutput* myDigitalOutputs[MaxUserDigitalPin]; @@ -116,9 +118,9 @@ void controlCheckTask(void* pvParameters) { int evt; xQueueReceive(control_sw_queue, &evt, portMAX_DELAY); // block until receive queue vTaskDelay(CONTROL_SW_DEBOUNCE_PERIOD); // delay a while - uint8_t pin = system_control_get_state(); - if (pin) { - system_exec_control_pin(pin); + ControlPins pins = system_control_get_state(); + if (pins.value) { + system_exec_control_pin(pins); } debouncing = false; } @@ -134,77 +136,20 @@ void IRAM_ATTR isr_control_inputs() { xQueueSendFromISR(control_sw_queue, &evt, NULL); } #else - uint8_t pin = system_control_get_state(); - system_exec_control_pin(pin); + ControlPins pins = system_control_get_state(); + system_exec_control_pin(pins); #endif } // Returns if safety door is ajar(T) or closed(F), based on pin state. uint8_t system_check_safety_door_ajar() { #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - return system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR; + return system_control_get_state().bit.safetyDoor; #else return false; // Input pin not enabled, so just return that it's closed. #endif } -// Special handlers for setting and clearing Grbl's real-time execution flags. -void system_set_exec_state_flag(uint8_t mask) { - // TODO uint8_t sreg = SREG; - // TODO cli(); - sys_rt_exec_state |= (mask); - // TODO SREG = sreg; -} - -void system_clear_exec_state_flag(uint8_t mask) { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_state &= ~(mask); - //SREG = sreg; -} - -void system_set_exec_alarm(ExecAlarm code) { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_alarm = code; - //SREG = sreg; -} - -void system_clear_exec_alarm() { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_alarm = ExecAlarm::None; - //SREG = sreg; -} - -void system_set_exec_motion_override_flag(uint8_t mask) { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_motion_override |= (mask); - //SREG = sreg; -} - -void system_set_exec_accessory_override_flag(uint8_t mask) { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_accessory_override |= (mask); - //SREG = sreg; -} - -void system_clear_exec_motion_overrides() { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_motion_override = 0; - //SREG = sreg; -} - -void system_clear_exec_accessory_overrides() { - //uint8_t sreg = SREG; - //cli(); - sys_rt_exec_accessory_override = 0; - //SREG = sreg; -} - void system_flag_wco_change() { #ifdef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE protocol_buffer_synchronize(); @@ -267,98 +212,88 @@ uint8_t system_check_travel_limits(float* target) { // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is -// defined by the CONTROL_PIN_INDEX in the header file. -uint8_t system_control_get_state() { - uint8_t defined_pin_mask = 0; // a mask of defined pins - uint8_t control_state = 0; +// defined by the ControlPin in System.h. +ControlPins system_control_get_state() { + ControlPins defined_pins; + defined_pins.value = 0; + + ControlPins pin_states; + pin_states.value = 0; #ifdef CONTROL_SAFETY_DOOR_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_SAFETY_DOOR; + defined_pins.bit.safetyDoor = true; if (digitalRead(CONTROL_SAFETY_DOOR_PIN)) { - control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; + pin_states.bit.safetyDoor = true; } #endif #ifdef CONTROL_RESET_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_RESET; + defined_pins.bit.reset = true; if (digitalRead(CONTROL_RESET_PIN)) { - control_state |= CONTROL_PIN_INDEX_RESET; + pin_states.bit.reset = true; } #endif #ifdef CONTROL_FEED_HOLD_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_FEED_HOLD; + defined_pins.bit.feedHold = true; if (digitalRead(CONTROL_FEED_HOLD_PIN)) { - control_state |= CONTROL_PIN_INDEX_FEED_HOLD; + pin_states.bit.feedHold = true; } #endif #ifdef CONTROL_CYCLE_START_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_CYCLE_START; + defined_pins.bit.cycleStart = true; if (digitalRead(CONTROL_CYCLE_START_PIN)) { - control_state |= CONTROL_PIN_INDEX_CYCLE_START; + pin_states.bit.cycleStart = true; } #endif #ifdef MACRO_BUTTON_0_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_0; + defined_pins.bit.macro0 = true; if (digitalRead(MACRO_BUTTON_0_PIN)) { - control_state |= CONTROL_PIN_INDEX_MACRO_0; + pin_states.bit.macro0 = true; } #endif #ifdef MACRO_BUTTON_1_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_1; + defined_pins.bit.macro1 = true; if (digitalRead(MACRO_BUTTON_1_PIN)) { - control_state |= CONTROL_PIN_INDEX_MACRO_1; + pin_states.bit.macro1 = true; } #endif #ifdef MACRO_BUTTON_2_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_2; + defined_pins.bit.macro2 = true; if (digitalRead(MACRO_BUTTON_2_PIN)) { - control_state |= CONTROL_PIN_INDEX_MACRO_2; + pin_states.bit.macro2 = true; } #endif #ifdef MACRO_BUTTON_3_PIN - defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_3; + defined_pins.bit.macro3 = true; if (digitalRead(MACRO_BUTTON_3_PIN)) { - control_state |= CONTROL_PIN_INDEX_MACRO_3; + pin_states.bit.macro3 = true; } #endif #ifdef INVERT_CONTROL_PIN_MASK - control_state ^= (INVERT_CONTROL_PIN_MASK & defined_pin_mask); + pin_states.value ^= (INVERT_CONTROL_PIN_MASK & defined_pins.value); #endif - - return control_state; + return pin_states; } // execute the function of the control pin -void system_exec_control_pin(uint8_t pin) { - if (bit_istrue(pin, CONTROL_PIN_INDEX_RESET)) { +void system_exec_control_pin(ControlPins pins) { + if (pins.bit.reset) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset via control pin"); mc_reset(); - } else if (bit_istrue(pin, CONTROL_PIN_INDEX_CYCLE_START)) { - bit_true(sys_rt_exec_state, EXEC_CYCLE_START); - } else if (bit_istrue(pin, CONTROL_PIN_INDEX_FEED_HOLD)) { - bit_true(sys_rt_exec_state, EXEC_FEED_HOLD); - } else if (bit_istrue(pin, CONTROL_PIN_INDEX_SAFETY_DOOR)) { - bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR); + } else if (pins.bit.cycleStart) { + sys_rt_exec_state.bit.cycleStart = true; + } else if (pins.bit.feedHold) { + sys_rt_exec_state.bit.feedHold = true; + } else if (pins.bit.safetyDoor) { + sys_rt_exec_state.bit.safetyDoor = true; + } else if (pins.bit.macro0) { + user_defined_macro(0); // function must be implemented by user + } else if (pins.bit.macro1) { + user_defined_macro(1); // function must be implemented by user + } else if (pins.bit.macro2) { + user_defined_macro(2); // function must be implemented by user + } else if (pins.bit.macro3) { + user_defined_macro(3); // function must be implemented by user } -#ifdef MACRO_BUTTON_0_PIN - else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_0)) { - user_defined_macro(CONTROL_PIN_INDEX_MACRO_0); // function must be implemented by user - } -#endif -#ifdef MACRO_BUTTON_1_PIN - else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_1)) { - user_defined_macro(CONTROL_PIN_INDEX_MACRO_1); // function must be implemented by user - } -#endif -#ifdef MACRO_BUTTON_2_PIN - else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_2)) { - user_defined_macro(CONTROL_PIN_INDEX_MACRO_2); // function must be implemented by user - } -#endif -#ifdef MACRO_BUTTON_3_PIN - else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_3)) { - user_defined_macro(CONTROL_PIN_INDEX_MACRO_3); // function must be implemented by user - } -#endif } // CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps. @@ -449,3 +384,4 @@ uint8_t sys_calc_pwm_precision(uint32_t freq) { return precision - 1; } +void __attribute__((weak)) user_defined_macro(uint8_t index); diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 6ea38625..31b9b343 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -6,7 +6,7 @@ Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P + CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -37,43 +37,98 @@ enum class State : uint8_t { Sleep, // Sleep state. }; -// Define global system variables -typedef struct { - volatile State state; // Tracks the current system state of Grbl. - uint8_t abort; // System abort flag. Forces exit back to main loop for reset. - uint8_t suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. - uint8_t soft_limit; // Tracks soft limit errors for the state machine. (boolean) - uint8_t step_control; // Governs the step segment generator depending on system state. - uint8_t probe_succeeded; // Tracks if last probing cycle was successful. - uint8_t homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. - uint8_t f_override; // Feed rate override value in percent - uint8_t r_override; // Rapids override value in percent - uint8_t spindle_speed_ovr; // Spindle speed value in percent - uint8_t spindle_stop_ovr; // Tracks spindle stop override states - uint8_t report_ovr_counter; // Tracks when to add override data to status reports. - uint8_t report_wco_counter; // Tracks when to add work coordinate offset data to status reports. -#ifdef ENABLE_PARKING_OVERRIDE_CONTROL - uint8_t override_ctrl; // Tracks override control states. +// Step segment generator state flags. +struct StepControl { + uint8_t endMotion : 1; + uint8_t executeHold : 1; + uint8_t executeSysMotion : 1; + uint8_t updateSpindleRpm : 1; +}; + +// System suspend flags. Used in various ways to manage suspend states and procedures. +struct SuspendBits { + uint8_t holdComplete : 1; // Indicates initial feed hold is complete. + uint8_t restartRetract : 1; // Flag to indicate a retract from a restore parking motion. + uint8_t retractComplete : 1; // (Safety door only) Indicates retraction and de-energizing is complete. + uint8_t initiateRestore : 1; // (Safety door only) Flag to initiate resume procedures from a cycle start. + uint8_t restoreComplete : 1; // (Safety door only) Indicates ready to resume normal operation. + uint8_t safetyDoorAjar : 1; // Tracks safety door state for resuming. + uint8_t motionCancel : 1; // Indicates a canceled resume motion. Currently used by probing routine. + uint8_t jogCancel : 1; // Indicates a jog cancel in process and to reset buffers when complete. +}; +union Suspend { + uint8_t value; + SuspendBits bit; +}; + +typedef uint8_t AxisMask; // Bits indexed by axis number +typedef uint8_t Percent; // Integer percent +typedef uint8_t Counter; // Report interval + +enum class Override : uint8_t { +#ifdef DEACTIVATE_PARKING_UPON_INIT + Disabled = 0, // (Default: Must be zero) + ParkingMotion = 1, // M56 +#else + ParkingMotion = 0, // M56 (Default: Must be zero) + Disabled = 1, // Parking disabled. #endif +}; +// Spindle stop override control states. +struct SpindleStopBits { + uint8_t enabled : 1; + uint8_t initiate : 1; + uint8_t restore : 1; + uint8_t restoreCycle : 1; +}; +union SpindleStop { + uint8_t value; + SpindleStopBits bit; +}; + +// Global system variables +typedef struct { + volatile State state; // Tracks the current system state of Grbl. + bool abort; // System abort flag. Forces exit back to main loop for reset. + Suspend suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. + bool soft_limit; // Tracks soft limit errors for the state machine. (boolean) + StepControl step_control; // Governs the step segment generator depending on system state. + bool probe_succeeded; // Tracks if last probing cycle was successful. + AxisMask homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. + Percent f_override; // Feed rate override value in percent + Percent r_override; // Rapids override value in percent + Percent spindle_speed_ovr; // Spindle speed value in percent + SpindleStop spindle_stop_ovr; // Tracks spindle stop override states + Counter report_ovr_counter; // Tracks when to add override data to status reports. + Counter report_wco_counter; // Tracks when to add work coordinate offset data to status reports. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + Override override_ctrl; // Tracks override control states. +#endif uint32_t spindle_speed; - } system_t; extern system_t sys; -// Define system executor bit map. Used internally by realtime protocol as realtime command flags, +// System executor bit map. Used internally by realtime protocol as realtime command flags, // which notifies the main program to execute the specified realtime command asynchronously. // NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default // flags are always false, so the realtime protocol only needs to check for a non-zero value to // know when there is a realtime command to execute. -#define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 -#define EXEC_CYCLE_START bit(1) // bitmask 00000010 -// #define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 moved to cycle_stop -#define EXEC_FEED_HOLD bit(3) // bitmask 00001000 -#define EXEC_RESET bit(4) // bitmask 00010000 -#define EXEC_SAFETY_DOOR bit(5) // bitmask 00100000 -#define EXEC_MOTION_CANCEL bit(6) // bitmask 01000000 -#define EXEC_SLEEP bit(7) // bitmask 10000000 +struct ExecStateBits { + uint8_t statusReport : 1; + uint8_t cycleStart : 1; + uint8_t cycleStop : 1; // Unused, per cycle_stop variable + uint8_t feedHold : 1; + uint8_t reset : 1; + uint8_t safetyDoor : 1; + uint8_t motionCancel : 1; + uint8_t sleep : 1; +}; + +union ExecState { + uint8_t value; + ExecStateBits bit; +}; // Alarm executor codes. Valid values (1-255). Zero is reserved. enum class ExecAlarm : uint8_t { @@ -92,104 +147,60 @@ enum class ExecAlarm : uint8_t { // Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. // Spindle/coolant and feed/rapids are separated into two controlling flag variables. -#define EXEC_FEED_OVR_RESET bit(0) -#define EXEC_FEED_OVR_COARSE_PLUS bit(1) -#define EXEC_FEED_OVR_COARSE_MINUS bit(2) -#define EXEC_FEED_OVR_FINE_PLUS bit(3) -#define EXEC_FEED_OVR_FINE_MINUS bit(4) -#define EXEC_RAPID_OVR_RESET bit(5) -#define EXEC_RAPID_OVR_MEDIUM bit(6) -#define EXEC_RAPID_OVR_LOW bit(7) -// #define EXEC_RAPID_OVR_EXTRA_LOW bit(*) // *NOT SUPPORTED* -#define EXEC_SPINDLE_OVR_RESET bit(0) -#define EXEC_SPINDLE_OVR_COARSE_PLUS bit(1) -#define EXEC_SPINDLE_OVR_COARSE_MINUS bit(2) -#define EXEC_SPINDLE_OVR_FINE_PLUS bit(3) -#define EXEC_SPINDLE_OVR_FINE_MINUS bit(4) -#define EXEC_SPINDLE_OVR_STOP bit(5) -#define EXEC_COOLANT_FLOOD_OVR_TOGGLE bit(6) -#define EXEC_COOLANT_MIST_OVR_TOGGLE bit(7) +struct ExecAccessoryBits { + uint8_t spindleOvrStop : 1; + uint8_t coolantFloodOvrToggle : 1; + uint8_t coolantMistOvrToggle : 1; +}; -// Define system suspend flags. Used in various ways to manage suspend states and procedures. -#define SUSPEND_DISABLE 0 // Must be zero. -#define SUSPEND_HOLD_COMPLETE bit(0) // Indicates initial feed hold is complete. -#define SUSPEND_RESTART_RETRACT bit(1) // Flag to indicate a retract from a restore parking motion. -#define SUSPEND_RETRACT_COMPLETE bit(2) // (Safety door only) Indicates retraction and de-energizing is complete. -#define SUSPEND_INITIATE_RESTORE bit(3) // (Safety door only) Flag to initiate resume procedures from a cycle start. -#define SUSPEND_RESTORE_COMPLETE bit(4) // (Safety door only) Indicates ready to resume normal operation. -#define SUSPEND_SAFETY_DOOR_AJAR bit(5) // Tracks safety door state for resuming. -#define SUSPEND_MOTION_CANCEL bit(6) // Indicates a canceled resume motion. Currently used by probing routine. -#define SUSPEND_JOG_CANCEL bit(7) // Indicates a jog cancel in process and to reset buffers when complete. +union ExecAccessory { + uint8_t value; + ExecAccessoryBits bit; +}; -// Define step segment generator state flags. -#define STEP_CONTROL_NORMAL_OP 0 // Must be zero. -#define STEP_CONTROL_END_MOTION bit(0) -#define STEP_CONTROL_EXECUTE_HOLD bit(1) -#define STEP_CONTROL_EXECUTE_SYS_MOTION bit(2) -#define STEP_CONTROL_UPDATE_SPINDLE_RPM bit(3) - -// Define control pin index for Grbl internal use. Pin maps may change, but these values don't. -//#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN -#define N_CONTROL_PIN 4 -#define CONTROL_PIN_INDEX_SAFETY_DOOR bit(0) -#define CONTROL_PIN_INDEX_RESET bit(1) -#define CONTROL_PIN_INDEX_FEED_HOLD bit(2) -#define CONTROL_PIN_INDEX_CYCLE_START bit(3) -#define CONTROL_PIN_INDEX_MACRO_0 bit(4) -#define CONTROL_PIN_INDEX_MACRO_1 bit(5) -#define CONTROL_PIN_INDEX_MACRO_2 bit(6) -#define CONTROL_PIN_INDEX_MACRO_3 bit(7) -//#else -//#define N_CONTROL_PIN 3 -//#define CONTROL_PIN_INDEX_RESET bit(0) -//#define CONTROL_PIN_INDEX_FEED_HOLD bit(1) -//#define CONTROL_PIN_INDEX_CYCLE_START bit(2) -//#endif - -// Define spindle stop override control states. -#define SPINDLE_STOP_OVR_DISABLED 0 // Must be zero. -#define SPINDLE_STOP_OVR_ENABLED bit(0) -#define SPINDLE_STOP_OVR_INITIATE bit(1) -#define SPINDLE_STOP_OVR_RESTORE bit(2) -#define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3) +// Control pin states +struct ControlPinBits { + uint8_t safetyDoor : 1; + uint8_t reset : 1; + uint8_t feedHold : 1; + uint8_t cycleStart : 1; + uint8_t macro0 : 1; + uint8_t macro1 : 1; + uint8_t macro2 : 1; + uint8_t macro3 : 1; +}; +union ControlPins { + uint8_t value; + ControlPinBits bit; +}; // NOTE: These position variables may need to be declared as volatiles, if problems arise. extern int32_t sys_position[MAX_N_AXIS]; // Real-time machine (aka home) position vector in steps. extern int32_t sys_probe_position[MAX_N_AXIS]; // Last probe position in machine coordinates and steps. -extern volatile uint8_t sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. -extern volatile uint8_t sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. -extern volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. -extern volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides. -extern volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. -extern volatile bool cycle_stop; - +extern volatile Probe sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. +extern volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. +extern volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. +extern volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. +extern volatile Percent sys_rt_f_override; // Feed override value in percent +extern volatile Percent sys_rt_r_override; // Rapid feed override value in percent +extern volatile Percent sys_rt_s_override; // Spindle override value in percent +extern volatile bool cycle_stop; #ifdef DEBUG -# define EXEC_DEBUG_REPORT bit(0) -extern volatile uint8_t sys_rt_exec_debug; +extern volatile bool sys_rt_exec_debug; #endif void system_ini(); // Renamed from system_init() due to conflict with esp32 files // Returns bitfield of control pin states, organized by CONTROL_PIN_INDEX. (1=triggered, 0=not triggered). -uint8_t system_control_get_state(); +ControlPins system_control_get_state(); // Returns if safety door is ajar(T) or closed(F), based on pin state. uint8_t system_check_safety_door_ajar(); void isr_control_inputs(); -// Special handlers for setting and clearing Grbl's real-time execution flags. -void system_set_exec_state_flag(uint8_t mask); -void system_clear_exec_state_flag(uint8_t mask); -void system_set_exec_alarm(ExecAlarm code); -void system_clear_exec_alarm(); -void system_set_exec_motion_override_flag(uint8_t mask); -void system_set_exec_accessory_override_flag(uint8_t mask); -void system_clear_exec_motion_overrides(); -void system_clear_exec_accessory_overrides(); - // Execute the startup script lines stored in non-volatile storage upon initialization void system_execute_startup(char* line); Error execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_level); @@ -207,27 +218,16 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps); // Checks and reports if target array exceeds machine travel limits. uint8_t system_check_travel_limits(float* target); -// Special handlers for setting and clearing Grbl's real-time execution flags. -void system_set_exec_state_flag(uint8_t mask); -void system_clear_exec_state_flag(uint8_t mask); -void system_set_exec_alarm(ExecAlarm code); -void system_clear_exec_alarm(); -void system_set_exec_motion_override_flag(uint8_t mask); -void system_set_exec_accessory_override_flag(uint8_t mask); -void system_clear_exec_motion_overrides(); -void system_clear_exec_accessory_overrides(); - int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps); int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps); // A task that runs after a control switch interrupt for debouncing. void controlCheckTask(void* pvParameters); -void system_exec_control_pin(uint8_t pin); +void system_exec_control_pin(ControlPins pins); bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized); bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized); -int8_t sys_get_next_RMT_chan_num(); - +int8_t sys_get_next_RMT_chan_num(); int8_t sys_get_next_PWM_chan_num(); uint8_t sys_calc_pwm_precision(uint32_t freq); diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 6fc21ab9..39d5067f 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -430,35 +430,7 @@ namespace WebUI { } # endif - bool Web_Server::is_realtime_cmd(char c) { - switch (c) { - case CMD_STATUS_REPORT: - case CMD_CYCLE_START: - case CMD_RESET: - case CMD_FEED_HOLD: - case CMD_SAFETY_DOOR: - case CMD_JOG_CANCEL: - case CMD_DEBUG_REPORT: - case CMD_FEED_OVR_RESET: - case CMD_FEED_OVR_COARSE_PLUS: - case CMD_FEED_OVR_COARSE_MINUS: - case CMD_FEED_OVR_FINE_PLUS: - case CMD_FEED_OVR_FINE_MINUS: - case CMD_RAPID_OVR_RESET: - case CMD_RAPID_OVR_MEDIUM: - case CMD_RAPID_OVR_LOW: - case CMD_SPINDLE_OVR_COARSE_PLUS: - case CMD_SPINDLE_OVR_COARSE_MINUS: - case CMD_SPINDLE_OVR_FINE_PLUS: - case CMD_SPINDLE_OVR_FINE_MINUS: - case CMD_SPINDLE_OVR_STOP: - case CMD_COOLANT_FLOOD_OVR_TOGGLE: - case CMD_COOLANT_MIST_OVR_TOGGLE: - return true; - default: - return false; - } - } + bool Web_Server::is_realtime_cmd(char c) { return is_realtime_cmd(c); } void Web_Server::_handle_web_command(bool silent) { //to save time if already disconnected From 27061bfca6164ba3da6e560ca7fb5ecefb65b3ff Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 8 Oct 2020 09:13:52 -1000 Subject: [PATCH 24/82] Fixed WebUI crash (#633) While eliminating a redundant definition of is_realtime_command(), I inadvertently introduced a recursion due to the similarity of the names "is_realtime_command()" and "is_realtime_cmd()". The solution is to eliminate the latter entirely. --- Grbl_Esp32/src/WebUI/WebServer.cpp | 4 +--- Grbl_Esp32/src/WebUI/WebServer.h | 1 - 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 39d5067f..f9ef698d 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -430,8 +430,6 @@ namespace WebUI { } # endif - bool Web_Server::is_realtime_cmd(char c) { return is_realtime_cmd(c); } - void Web_Server::_handle_web_command(bool silent) { //to save time if already disconnected //if (_webserver->hasArg ("PAGEID") ) { @@ -501,7 +499,7 @@ namespace WebUI { } if (scmd.length() > 1) { scmd += "\n"; - } else if (!is_realtime_cmd(scmd[0])) { + } else if (!is_realtime_command(scmd[0])) { scmd += "\n"; } if (!Serial2Socket.push(scmd.c_str())) { diff --git a/Grbl_Esp32/src/WebUI/WebServer.h b/Grbl_Esp32/src/WebUI/WebServer.h index 9d8eca30..c4bd8745 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.h +++ b/Grbl_Esp32/src/WebUI/WebServer.h @@ -87,7 +87,6 @@ namespace WebUI { static void handleFileList(); static void handleUpdate(); static void WebUpdateUpload(); - static bool is_realtime_cmd(char c); static void pushError(int code, const char* st, bool web_error = 500, uint16_t timeout = 1000); static void cancelUpload(); #ifdef ENABLE_SD_CARD From d9dba2c9081fbecba79d82fc5c31aaeeba2cedc6 Mon Sep 17 00:00:00 2001 From: odaki Date: Sat, 10 Oct 2020 06:32:32 +0900 Subject: [PATCH 25/82] Fix i2s probing hang (#608) * Fix I2S stepper hung just after the completion of motor moving * Fix recompile issue Fixed a problem with the recompile not being recompiled even if the files under the Custom folder are changed. * More comment for macOS in debug.ini * Fix the timing of calling I2S out's exclusion function and reset sequence The reset sequence did not seem to be correct, so I changed it. According to the ESP-IDF PR, the correct sequence is as follows: 1)TX module 2)DMA 3)FIFO https://github.com/espressif/esp-idf/commit/c7f33524b469e75937f003d4c06336bf4694a043#diff-27688c6b3c29373d2a2b142b8471981c * Changed the message level for I2S swtiching from warning to debug * Add some comments --- Grbl_Esp32/src/GCode.cpp | 14 -------- Grbl_Esp32/src/I2SOut.cpp | 55 ++++++++++++++++++++------------ Grbl_Esp32/src/MotionControl.cpp | 36 +++++++++++++++++++-- Grbl_Esp32/src/Stepper.cpp | 7 ++-- debug.ini | 10 ++++-- platformio.ini | 2 +- 6 files changed, 79 insertions(+), 45 deletions(-) diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index c26f4b53..0134952e 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1502,9 +1502,6 @@ Error gc_execute_line(char* line, uint8_t client) { if (gc_state.modal.motion != Motion::None) { if (axis_command == AxisCommand::MotionMode) { GCUpdatePos gc_update_pos = GCUpdatePos::Target; -#ifdef USE_I2S_STEPS - stepper_id_t save_stepper = current_stepper; -#endif if (gc_state.modal.motion == Motion::Linear) { //mc_line(gc_block.values.xyz, pl_data); mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); @@ -1527,12 +1524,6 @@ Error gc_execute_line(char* line, uint8_t client) { // upon a successful probing cycle, the machine position and the returned value should be the same. #ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES pl_data->motion.noFeedOverride = 1; -#endif -#ifdef USE_I2S_STEPS - save_stepper = current_stepper; // remember the stepper - if (save_stepper == ST_I2S_STREAM) { - stepper_switch(ST_I2S_STATIC); // Change the stepper to reduce the delay for accurate probing. - } #endif gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags); } @@ -1544,11 +1535,6 @@ Error gc_execute_line(char* line, uint8_t client) { } else if (gc_update_pos == GCUpdatePos::System) { gc_sync_position(); // gc_state.position[] = sys_position } // == GCUpdatePos::None -#ifdef USE_I2S_STEPS - if (save_stepper == ST_I2S_STREAM && current_stepper != ST_I2S_STREAM) { - stepper_switch(ST_I2S_STREAM); // Put the stepper back on. - } -#endif } } // [21. Program flow ]: diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index 419caf23..47fcea0d 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -291,9 +291,27 @@ static int IRAM_ATTR i2s_out_start() { // Attach I2S to specified GPIO pin i2s_out_gpio_attach(i2s_out_ws_pin, i2s_out_bck_pin, i2s_out_data_pin); - //start DMA link + + // reest TX/RX module + I2S0.conf.tx_reset = 1; + I2S0.conf.tx_reset = 0; + I2S0.conf.rx_reset = 1; + I2S0.conf.rx_reset = 0; + +# ifdef USE_I2S_OUT_STREAM_IMPL + // reset DMA + I2S0.lc_conf.in_rst = 1; + I2S0.lc_conf.in_rst = 0; + I2S0.lc_conf.out_rst = 1; + I2S0.lc_conf.out_rst = 0; + + I2S0.out_link.addr = (uint32_t)o_dma.desc[0]; +# endif + + // reset FIFO i2s_out_reset_fifo_without_lock(); + // start DMA link # ifdef USE_I2S_OUT_STREAM_IMPL if (i2s_out_pulser_status == PASSTHROUGH) { I2S0.conf_chan.tx_chan_mod = 3; // 3:right+constant 4:left+constant (when tx_msb_right = 1) @@ -304,21 +322,6 @@ static int IRAM_ATTR i2s_out_start() { } # endif -# ifdef USE_I2S_OUT_STREAM_IMPL - //reset DMA - I2S0.lc_conf.in_rst = 1; - I2S0.lc_conf.in_rst = 0; - I2S0.lc_conf.out_rst = 1; - I2S0.lc_conf.out_rst = 0; - - I2S0.out_link.addr = (uint32_t)o_dma.desc[0]; -# endif - - I2S0.conf.tx_reset = 1; - I2S0.conf.tx_reset = 0; - I2S0.conf.rx_reset = 1; - I2S0.conf.rx_reset = 0; - I2S0.conf1.tx_stop_en = 1; // BCK and WCK are suppressed while FIFO is empty # ifdef USE_I2S_OUT_STREAM_IMPL @@ -339,13 +342,14 @@ static int IRAM_ATTR i2s_out_start() { } # ifdef USE_I2S_OUT_STREAM_IMPL - +// Fill out one DMA buffer +// Call with the I2S_OUT_PULSER lock acquired. +// Note that the lock is temporarily released while calling the callback function. static int IRAM_ATTR i2s_fillout_dma_buffer(lldesc_t* dma_desc) { uint32_t* buf = (uint32_t*)dma_desc->buf; o_dma.rw_pos = 0; // It reuses the oldest (just transferred) buffer with the name "current" // and fills the buffer for later DMA. - I2S_OUT_PULSER_ENTER_CRITICAL(); // Lock pulser status if (i2s_out_pulser_status == STEPPING) { // // Fillout the buffer for pulse @@ -408,7 +412,6 @@ static int IRAM_ATTR i2s_fillout_dma_buffer(lldesc_t* dma_desc) { o_dma.rw_pos = 0; // If someone calls i2s_out_push_sample, make sure there is no buffer overflow i2s_out_remain_time_until_next_pulse = 0; } - I2S_OUT_PULSER_EXIT_CRITICAL(); // Unlock pulser status return 0; } @@ -591,10 +594,18 @@ i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status() { int IRAM_ATTR i2s_out_set_passthrough() { I2S_OUT_PULSER_ENTER_CRITICAL(); # ifdef USE_I2S_OUT_STREAM_IMPL + // Triggers a change of mode if it is compiled to use I2S stream. + // The mode is not changed directly by this function. + // Pull the trigger if (i2s_out_pulser_status == STEPPING) { - i2s_out_pulser_status = WAITING; // Start stopping the pulser + i2s_out_pulser_status = WAITING; // Start stopping the pulser (trigger) } + // It is a function that may be called via i2sOutTask(). + // (i2sOutTask() -> stepper_pulse_func() -> st_go_idle() -> Stepper_Timer_Stop() -> this function) + // And only i2sOutTask() can change the state to PASSTHROUGH. + // So, to change the state, you need to return to i2sOutTask() as soon as possible. # else + // If it is compiled to not use I2S streams, change the mode directly. i2s_out_pulser_status = PASSTHROUGH; # endif I2S_OUT_PULSER_EXIT_CRITICAL(); @@ -627,6 +638,7 @@ int IRAM_ATTR i2s_out_set_stepping() { I2S_OUT_PULSER_EXIT_CRITICAL(); return 0; } + // Now, DMA completed. Fallthrough. } // Change I2S state from PASSTHROUGH to STEPPING @@ -772,7 +784,6 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { // // configure I2S data port interface. - i2s_out_reset_fifo(); //reset i2s I2S0.conf.tx_reset = 1; @@ -786,6 +797,8 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { I2S0.lc_conf.out_rst = 1; // Set this bit to reset out DMA FSM. (R/W) I2S0.lc_conf.out_rst = 0; + i2s_out_reset_fifo_without_lock(); + //Enable and configure DMA I2S0.lc_conf.check_owner = 0; I2S0.lc_conf.out_loop_test = 0; diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 9b454be3..a3be9ce9 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -260,6 +260,25 @@ static bool axis_is_squared(uint8_t axis_mask) { return mask_is_single_axis(axis_mask) && mask_has_squared_axis(axis_mask); } +#ifdef USE_I2S_STEPS +# define BACKUP_STEPPER(save_stepper) \ + do { \ + if (save_stepper == ST_I2S_STREAM) { \ + stepper_switch(ST_I2S_STATIC); /* Change the stepper to reduce the delay for accurate probing. */ \ + } \ + } while (0) + +# define RESTORE_STEPPER(save_stepper) \ + do { \ + if (save_stepper == ST_I2S_STREAM && current_stepper != ST_I2S_STREAM) { \ + stepper_switch(ST_I2S_STREAM); /* Put the stepper back on. */ \ + } \ + } while (0) +#else +# define BACKUP_STEPPER(save_stepper) +# define RESTORE_STEPPER(save_stepper) +#endif + // Perform homing cycle to locate and set machine zero. Only '$H' executes this command. // NOTE: There should be no motions in the buffer and Grbl must be in an idle state before // executing the homing cycle. This prevents incorrect buffered plans after homing. @@ -371,6 +390,13 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par if (sys.abort) { return GCUpdatePos::None; // Return if system reset has been issued. } + +#ifdef USE_I2S_STEPS + stepper_id_t save_stepper = current_stepper; /* remember the stepper */ +#endif + // Switch stepper mode to the I2S static (realtime mode) + BACKUP_STEPPER(save_stepper); + // Initialize probing control variables uint8_t is_probe_away = bit_istrue(parser_flags, GCParserProbeIsAway); uint8_t is_no_error = bit_istrue(parser_flags, GCParserProbeIsNoError); @@ -381,7 +407,8 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par if (probe_get_state() ^ is_probe_away) { // Check probe pin state. sys_rt_exec_alarm = ExecAlarm::ProbeFailInitial; protocol_execute_realtime(); - return GCUpdatePos::None; // Nothing else to do but bail. + RESTORE_STEPPER(save_stepper); // Switch the stepper mode to the previous mode + return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. mc_line(target, pl_data); @@ -392,9 +419,14 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par do { protocol_execute_realtime(); if (sys.abort) { - return GCUpdatePos::None; // Check for system abort + RESTORE_STEPPER(save_stepper); // Switch the stepper mode to the previous mode + return GCUpdatePos::None; // Check for system abort } } while (sys.state != State::Idle); + + // Switch the stepper mode to the previous mode + RESTORE_STEPPER(save_stepper); + // Probing cycle complete! // Set state variables and error out, if the probe failed and cycle with error is enabled. if (sys_probe_state == Probe::Active) { diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index d78a0d88..469a65a6 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -457,11 +457,10 @@ void stepper_switch(stepper_id_t new_stepper) { #ifdef USE_I2S_STEPS if (current_stepper == ST_I2S_STREAM) { if (i2s_out_get_pulser_status() != PASSTHROUGH) { - // This switching function should not be called during streaming. - // However, if it is called, it will stop streaming. - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Warning, "Stop the I2S streaming and switch to the passthrough mode."); + // Called during streaming. Stop streaming. + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Debug, "Stop the I2S streaming and switch to the passthrough mode."); i2s_out_set_passthrough(); - i2s_out_delay(); + i2s_out_delay(); // Wait for a change in mode. } } #endif diff --git a/debug.ini b/debug.ini index 0d148f7f..db39152f 100644 --- a/debug.ini +++ b/debug.ini @@ -6,14 +6,18 @@ upload_port = COM3 monitor_port = COM3 ; macOS -;upload_port = /dev/cu.usbserial-* -;monitor_port = /dev/cu.usbserial-* +;upload_port = /dev/cu.SLAB_USBtoUART +;monitor_port = /dev/cu.SLAB_USBtoUART +; macOS ESP-Prog +;upload_port = /dev/cu.usbserial-14200 +;monitor_port = /dev/cu.usbserial-14201 +;upload_protocol = esp-prog ; Linux ;upload_port = /dev/ttyUSB* ;monitor_port = /dev/ttyUSB* build_flags = ${common.build_flags} - -DMACHINE_FILENAME=test_drive.h + -DMACHINE_FILENAME=test_drive.h [env:debug] ; You can switch between debugging tools using debug_tool option diff --git a/platformio.ini b/platformio.ini index c7f14c90..11ad3c32 100644 --- a/platformio.ini +++ b/platformio.ini @@ -49,7 +49,7 @@ board_build.flash_mode = qio build_flags = ${common.build_flags} src_filter = +<*.h> +<*.s> +<*.S> +<*.cpp> +<*.c> +<*.ino> + - -<.git/> - - - - + -<.git/> - - - [env:release] From 01aa02804330770334e82b29f712e56fa9072668 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 15 Oct 2020 09:05:12 -1000 Subject: [PATCH 26/82] Implement stepping through Motors class (#636) * Implement stepping through Motors class WIP for discussion and review - not ready to merge yet * Document Motor methods and variables .. and remove some unused ones and move some that are subclass-specific * Move position_min/max to Limits.cpp ... and coalesced other uses thereof into a unified scheme. * Call motor ->init() explicitly instead of implicitly This makes it possible to inherit constructors without spurious config messages. * Fixed problems with I2S * Changes in class method override syntax per atlaste * Fixed oops * More Motors simplification a) Eliminated can_home() in favor of a return value from set_homing_mode() b) Eliminated axis_name() in favor of reportAxisNameMsg() * Fixes to RcServo and Trinamic - RC Servo was not handling disable ... probably old issue - Display test after config * More tweaks * Define that variable! * Move functions from Motors.cpp to subclasses Created a Servo base class from which RcServo and Dynamixel2 are derived. This gets the servo update task out of Motors. It also eliminates the need for type_id. Now all of the functions that are specific to particular kinds of motors are within their subclasses * Adding Dynamixel to ABC axes. * Removed second #ifndef SPINDLE_TYPE * Fixed potential leak in Report.cpp as reported by @atlaste * Some servo cleanup. Has errors! * min should be max * Removed test rcservo machine definition. * Removed obsolete #defines in machine defs for RcServo cal Co-authored-by: bdring --- Grbl_Esp32/src/Grbl.h | 6 +- Grbl_Esp32/src/I2SOut.cpp | 10 +- Grbl_Esp32/src/I2SOut.h | 12 +- Grbl_Esp32/src/Jog.cpp | 2 +- Grbl_Esp32/src/Limits.cpp | 53 ++- Grbl_Esp32/src/Limits.h | 6 + Grbl_Esp32/src/MachineCommon.h | 4 - Grbl_Esp32/src/Machines/midtbot.h | 3 - Grbl_Esp32/src/Machines/pen_laser.h | 2 - Grbl_Esp32/src/Machines/polar_coaster.h | 2 - Grbl_Esp32/src/Machines/template.h | 2 - Grbl_Esp32/src/Machines/tmc2130_pen.h | 3 - Grbl_Esp32/src/Motors/Dynamixel2.cpp | 99 ++---- Grbl_Esp32/src/Motors/Dynamixel2.h | 25 +- Grbl_Esp32/src/Motors/Motor.cpp | 27 +- Grbl_Esp32/src/Motors/Motor.h | 96 ++++-- Grbl_Esp32/src/Motors/Motors.cpp | 304 +++++------------ Grbl_Esp32/src/Motors/Motors.h | 31 +- Grbl_Esp32/src/Motors/NullMotor.cpp | 28 ++ Grbl_Esp32/src/Motors/NullMotor.h | 6 +- Grbl_Esp32/src/Motors/RcServo.cpp | 77 ++--- Grbl_Esp32/src/Motors/RcServo.h | 36 +- Grbl_Esp32/src/Motors/RcServoSettings.h | 3 - Grbl_Esp32/src/Motors/Servo.cpp | 78 +++++ Grbl_Esp32/src/Motors/Servo.h | 54 +++ Grbl_Esp32/src/Motors/Solenoid.h | 9 +- Grbl_Esp32/src/Motors/StandardStepper.cpp | 82 +++-- Grbl_Esp32/src/Motors/StandardStepper.h | 33 +- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 199 +++++++---- Grbl_Esp32/src/Motors/TrinamicDriver.h | 59 +++- Grbl_Esp32/src/Motors/UnipolarMotor.cpp | 53 +-- Grbl_Esp32/src/Motors/UnipolarMotor.h | 16 +- Grbl_Esp32/src/NutsBolts.h | 5 + Grbl_Esp32/src/Report.cpp | 51 ++- Grbl_Esp32/src/Report.h | 4 + Grbl_Esp32/src/Stepper.cpp | 380 +++------------------- Grbl_Esp32/src/Stepper.h | 11 + Grbl_Esp32/src/System.cpp | 40 +-- Grbl_Esp32/src/System.h | 30 +- 39 files changed, 884 insertions(+), 1057 deletions(-) create mode 100644 Grbl_Esp32/src/Motors/NullMotor.cpp create mode 100644 Grbl_Esp32/src/Motors/Servo.cpp create mode 100644 Grbl_Esp32/src/Motors/Servo.h diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index e898f5eb..a3106010 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 = "20201004"; +const char* const GRBL_VERSION_BUILD = "20201015"; //#include #include @@ -87,9 +87,7 @@ const char* const GRBL_VERSION_BUILD = "20201004"; # endif #endif -#ifdef USE_I2S_OUT -# include "I2SOut.h" -#endif +#include "I2SOut.h" void grbl_init(); void run_once(); diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index 47fcea0d..f6b261b5 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -42,8 +42,6 @@ */ #include "Config.h" -#ifdef USE_I2S_OUT - # include # include # include @@ -566,12 +564,14 @@ uint8_t IRAM_ATTR i2s_out_read(uint8_t pin) { return (!!(port_data & bit(pin))); } -uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t num) { +uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t usec) { + uint32_t num = usec/I2S_OUT_USEC_PER_PULSE; + # ifdef USE_I2S_OUT_STREAM_IMPL if (num > SAMPLE_SAFE_COUNT) { return 0; } - // push at least one sample (even if num is zero) + // push at least one sample, even if num is zero) uint32_t port_data = atomic_load(&i2s_out_port_data); uint32_t n = 0; do { @@ -960,5 +960,3 @@ int IRAM_ATTR i2s_out_init() { }; return i2s_out_init(default_param); } - -#endif diff --git a/Grbl_Esp32/src/I2SOut.h b/Grbl_Esp32/src/I2SOut.h index b5defb10..f672a278 100644 --- a/Grbl_Esp32/src/I2SOut.h +++ b/Grbl_Esp32/src/I2SOut.h @@ -41,8 +41,6 @@ // It should be included at the outset to know the machine configuration. #include "Config.h" -#ifdef USE_I2S_OUT - # include /* Assert */ @@ -126,12 +124,14 @@ void i2s_out_write(uint8_t pin, uint8_t val); /* Set current pin state to the I2S bitstream buffer (This call will generate a future I2S_OUT_USEC_PER_PULSE μs x N bitstream) - num: Number of samples to be generated + usec: The length of time that the pulse should be repeated. + That time will be converted to an integer number of pulses of + length I2S_OUT_USEC_PER_PULSE. The number of samples is limited to (20 / I2S_OUT_USEC_PER_PULSE). - return: number of puhsed samples + return: number of pushed samples 0 .. no space for push */ -uint32_t i2s_out_push_sample(uint32_t num); +uint32_t i2s_out_push_sample(uint32_t usec); /* Set pulser mode to passtrough @@ -184,8 +184,6 @@ i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status(); */ int i2s_out_reset(); -#endif - /* Reference: "ESP32 Technical Reference Manual" by Espressif Systems https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 6dca36e2..5d7b5773 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -33,7 +33,7 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { pl_data->line_number = gc_block->values.n; #endif if (soft_limits->get()) { - if (system_check_travel_limits(gc_block->values.xyz)) { + if (limitsCheckTravel(gc_block->values.xyz)) { return Error::TravelExceeded; } } diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index bf8ca591..bbab9989 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -79,18 +79,12 @@ void limits_go_home(uint8_t cycle_mask) { return; // Block if system reset has been issued. } // Initialize plan data struct for homing motion. Spindle and coolant are disabled. - motors_set_homing_mode(cycle_mask, true); // tell motors homing is about to start - // Remove any motor that cannot be homed from the mask - // Motors with non standard homing can do that during motors_set_homing_mode(...) above - auto n_axis = number_axis->get(); - for (uint8_t idx = 0; idx < n_axis; idx++) { - if (bit_istrue(cycle_mask, bit(idx))) { - if (!motor_can_home(idx)) { - bit_false(cycle_mask, bit(idx)); - } - } - } + // Put motors on axes listed in cycle_mask in homing mode and + // replace cycle_mask with the list of motors that are ready for homing. + // Motors with non standard homing can home during motors_set_homing_mode(...) + cycle_mask = motors_set_homing_mode(cycle_mask, true); // tell motors homing is about to start + // see if any motors are left if (cycle_mask == 0) { return; @@ -111,6 +105,7 @@ void limits_go_home(uint8_t cycle_mask) { float target[MAX_N_AXIS]; float max_travel = 0.0; + auto n_axis = number_axis->get(); for (uint8_t idx = 0; idx < n_axis; idx++) { // Initialize step pin masks step_pin[idx] = bit(idx); @@ -337,9 +332,8 @@ void limits_init() { if (limit_sw_queue == NULL) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%c%s Axis limit switch on pin %s", - report_get_axis_letter(axis), - gang_index ? "2" : " ", + "%s limit switch on pin %s", + reportAxisNameMsg(axis, gang_index), pinName(pin).c_str()); } } @@ -376,7 +370,7 @@ void limits_disable() { // number in bit position, i.e. Z_AXIS is bit(2), and Y_AXIS is bit(1). AxisMask limits_get_state() { AxisMask pinMask = 0; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (int axis = 0; axis < n_axis; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { uint8_t pin = limit_pins[axis][gang_index]; @@ -399,7 +393,7 @@ AxisMask limits_get_state() { // the workspace volume is in all negative space, and the system is in normal operation. // NOTE: Used by jogging to limit travel within soft-limit volume. void limits_soft_check(float* target) { - if (system_check_travel_limits(target)) { + if (limitsCheckTravel(target)) { sys.soft_limit = true; // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within // workspace volume so just come to a controlled stop so position is not lost. When complete @@ -435,3 +429,30 @@ void limitCheckTask(void* pvParameters) { } } } + +float limitsMaxPosition(uint8_t axis) { + float mpos = axis_settings[axis]->home_mpos->get(); + + return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos + axis_settings[axis]->max_travel->get() : mpos; +} + +float limitsMinPosition(uint8_t axis) { + float mpos = axis_settings[axis]->home_mpos->get(); + + return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos : mpos - axis_settings[axis]->max_travel->get(); +} + +// Checks and reports if target array exceeds machine travel limits. +// Return true if exceeding limits +bool limitsCheckTravel(float* target) { + uint8_t idx; + auto n_axis = number_axis->get(); + for (idx = 0; idx < n_axis; idx++) { + float max_mpos, min_mpos; + + if (target[idx] < limitsMinPosition(idx) || target[idx] > limitsMaxPosition(idx)) { + return true; + } + } + return false; +} diff --git a/Grbl_Esp32/src/Limits.h b/Grbl_Esp32/src/Limits.h index 1f27208f..3cc2fccd 100644 --- a/Grbl_Esp32/src/Limits.h +++ b/Grbl_Esp32/src/Limits.h @@ -48,3 +48,9 @@ void isr_limit_switches(); // A task that runs after a limit switch interrupt. void limitCheckTask(void* pvParameters); + +float limitsMaxPosition(uint8_t axis); +float limitsMinPosition(uint8_t axis); + +// Internal factor used by limits_soft_check +bool limitsCheckTravel(float* target); diff --git a/Grbl_Esp32/src/MachineCommon.h b/Grbl_Esp32/src/MachineCommon.h index eea0c7dd..627da376 100644 --- a/Grbl_Esp32/src/MachineCommon.h +++ b/Grbl_Esp32/src/MachineCommon.h @@ -1,9 +1,5 @@ #pragma once -#ifndef SPINDLE_TYPE -# define SPINDLE_TYPE SpindleType::PWM -#endif - // Grbl setting that are common to all machines // It should not be necessary to change anything herein diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index 39e9c721..b9ceee16 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -45,9 +45,6 @@ #define Y_LIMIT_PIN GPIO_NUM_4 #define Z_SERVO_PIN GPIO_NUM_27 -#define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty -#define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty - // Set $Homing/Cycle0=Y and $Homing/Cycle1=X diff --git a/Grbl_Esp32/src/Machines/pen_laser.h b/Grbl_Esp32/src/Machines/pen_laser.h index d41b9f70..880c5094 100644 --- a/Grbl_Esp32/src/Machines/pen_laser.h +++ b/Grbl_Esp32/src/Machines/pen_laser.h @@ -55,8 +55,6 @@ #ifdef USING_SERVO #define Z_SERVO_PIN GPIO_NUM_27 - #define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty - #define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty #endif #define SPINDLE_TYPE SpindleType::NONE diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h index 5e600d76..b0e45e7c 100644 --- a/Grbl_Esp32/src/Machines/polar_coaster.h +++ b/Grbl_Esp32/src/Machines/polar_coaster.h @@ -49,8 +49,6 @@ #define STEPPERS_DISABLE_PIN GPIO_NUM_17 #define Z_SERVO_PIN GPIO_NUM_16 -#define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty -#define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty #define X_LIMIT_PIN GPIO_NUM_4 diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h index 284adde6..644734da 100644 --- a/Grbl_Esp32/src/Machines/template.h +++ b/Grbl_Esp32/src/Machines/template.h @@ -132,8 +132,6 @@ // pins for that axis, but instead include a block like this: // #define SERVO_Z_PIN GPIO_NUM_22 -// #define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty -// #define Z_SERVO_CAL_MIN 1.0 // calibration factor for the maximum PWM duty // === Homing cycles // Set them using $Homing/Cycle0= optionally up to $Homing/Cycle5= diff --git a/Grbl_Esp32/src/Machines/tmc2130_pen.h b/Grbl_Esp32/src/Machines/tmc2130_pen.h index 8c938015..93380577 100644 --- a/Grbl_Esp32/src/Machines/tmc2130_pen.h +++ b/Grbl_Esp32/src/Machines/tmc2130_pen.h @@ -59,9 +59,6 @@ // Define one of these 2 options for spindle or servo #define Z_SERVO_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control. -#define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty -#define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty - // #define X_LIMIT_PIN See version section at beginning of file #define Y_LIMIT_PIN GPIO_NUM_4 diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp index fb0314f4..3c3faedc 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp +++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp @@ -4,7 +4,7 @@ This allows an Dynamixel sero to be used like any other motor. Servos do have limitation in travel and speed, so you do need to respect that. - Protocol 2 + Protocol 2 Part of Grbl_ESP32 @@ -27,33 +27,20 @@ #include "Dynamixel2.h" namespace Motors { - Dynamixel2::Dynamixel2() {} - - Dynamixel2::Dynamixel2(uint8_t axis_index, uint8_t id, uint8_t tx_pin, uint8_t rx_pin, uint8_t rts_pin) { - type_id = DYNAMIXEL2; - this->_axis_index = axis_index % MAX_AXES; - this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged - - _id = id; - _tx_pin = tx_pin; - _rx_pin = rx_pin; - _rts_pin = rts_pin; + bool Motors::Dynamixel2::uart_ready = false; + uint8_t Motors::Dynamixel2::ids[MAX_N_AXIS][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }; + Dynamixel2::Dynamixel2(uint8_t axis_index, uint8_t id, uint8_t tx_pin, uint8_t rx_pin, uint8_t rts_pin) : + Servo(axis_index), _id(id), _tx_pin(tx_pin), _rx_pin(rx_pin), _rts_pin(rts_pin) { if (_tx_pin == UNDEFINED_PIN || _rx_pin == UNDEFINED_PIN || _rts_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dymanixel Error. Missing pin definitions"); - is_active = false; - - return; + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dynamixel Error. Missing pin definitions"); + _has_errors = true; + } else { + _has_errors = false; // The motor can be used } - - set_axis_name(); - init(); } void Dynamixel2::init() { - is_active = true; // as opposed to NullMotors, this is a real motor - _can_home = false; // this axis cannot be conventionally homed - init_uart(_id, _axis_index, _dual_axis_index); // static and only allows one init read_settings(); @@ -61,7 +48,7 @@ namespace Motors { config_message(); // print the config if (!test()) { // ping the motor - is_active = false; + _has_errors = true; return; } @@ -72,18 +59,19 @@ namespace Motors { LED_on(true); vTaskDelay(100); LED_on(false); + + startUpdateTask(); } void Dynamixel2::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis Dynamixel Servo ID:%d Count(%5.0f,%5.0f) Limits(%0.3fmm,%5.3f)", - _axis_name, + "%s Dynamixel Servo ID:%d Count(%5.0f,%5.0f) %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), _id, _dxl_count_min, _dxl_count_max, - _position_min, - _position_max); + reportAxisLimitsMsg(_axis_index)); } bool Dynamixel2::test() { @@ -100,22 +88,23 @@ namespace Motors { if (model_num == 1060) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis Dynamixel Detected ID %d Model XL430-W250 F/W Rev %x", - _axis_name, + "%s Dynamixel Detected ID %d Model XL430-W250 F/W Rev %x", + reportAxisNameMsg(_axis_index, _dual_axis_index), _id, _dxl_rx_message[11]); } else { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis Dynamixel Detected ID %d M/N %d F/W Rev %x", - _axis_name, + "%s Dynamixel Detected ID %d M/N %d F/W Rev %x", + reportAxisNameMsg(_axis_index, _dual_axis_index), _id, model_num, _dxl_rx_message[11]); } } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Axis Dynamixel Servo ID %d Ping failed", _axis_name, _id); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index), +_id); return false; } @@ -123,21 +112,10 @@ namespace Motors { } void Dynamixel2::read_settings() { - float travel = axis_settings[_axis_index]->max_travel->get(); - float mpos = axis_settings[_axis_index]->home_mpos->get(); - - if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) { - _position_min = mpos; - _position_max = mpos + travel; - } else { - _position_min = mpos - travel; - _position_max = mpos; - } - _dxl_count_min = DXL_COUNT_MIN; _dxl_count_max = DXL_COUNT_MAX; - if (bit_istrue(dir_invert_mask->get(), bit(_axis_index))) // normal direction + if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) // normal direction swap(_dxl_count_min, _dxl_count_min); } @@ -150,10 +128,7 @@ namespace Motors { _disabled = disable; - if (_disabled) - dxl_write(DXL_ADDR_TORQUE_EN, param_count, 0); - else - dxl_write(DXL_ADDR_TORQUE_EN, param_count, 1); + dxl_write(DXL_ADDR_TORQUE_EN, param_count, !_disabled); } void Dynamixel2::set_operating_mode(uint8_t mode) { @@ -162,8 +137,9 @@ namespace Motors { } void Dynamixel2::update() { - if (!is_active) + if (_has_errors) { return; + } if (_disabled) { dxl_read_position(); @@ -212,12 +188,16 @@ namespace Motors { // This motor will not do a standard home to a limit switch (maybe future) // If it is in the homing mask it will a quick move to $/Home/Mpos - void Dynamixel2::set_homing_mode(uint8_t homing_mask, bool isHoming) { + bool Dynamixel2::set_homing_mode(bool isHoming) { + if (_has_errors) { + return false; + } sys_position[_axis_index] = axis_settings[_axis_index]->home_mpos->get() * axis_settings[_axis_index]->steps_per_mm->get(); // convert to steps set_disable(false); set_location(); // force the PWM to update now + return false; // Cannot do conventional homing } void Dynamixel2::dxl_goal_position(int32_t position) { @@ -244,8 +224,8 @@ namespace Motors { read_settings(); - int32_t pos_min_steps = lround(_position_min * axis_settings[_axis_index]->steps_per_mm->get()); - int32_t pos_max_steps = lround(_position_max * axis_settings[_axis_index]->steps_per_mm->get()); + int32_t pos_min_steps = lround(limitsMinPosition(_axis_index) * axis_settings[_axis_index]->steps_per_mm->get()); + int32_t pos_max_steps = lround(limitsMaxPosition(_axis_index) * axis_settings[_axis_index]->steps_per_mm->get()); int32_t temp = map(dxl_position, DXL_COUNT_MIN, DXL_COUNT_MAX, pos_min_steps, pos_max_steps); @@ -375,25 +355,14 @@ namespace Motors { //determine the location of the axis float target = system_convert_axis_steps_to_mpos(sys_position, axis); // get the axis machine position in mm - float travel = axis_settings[axis]->max_travel->get(); - float mpos = axis_settings[axis]->home_mpos->get(); - - if (bit_istrue(homing_dir_mask->get(), bit(axis))) { - position_min = mpos; - position_max = mpos + travel; - } else { - position_min = mpos - travel; - position_max = mpos; - } - dxl_count_min = DXL_COUNT_MIN; dxl_count_max = DXL_COUNT_MAX; - if (bit_istrue(dir_invert_mask->get(), bit(axis))) // normal direction + if (bitnum_istrue(dir_invert_mask->get(), axis)) // normal direction swap(dxl_count_min, dxl_count_max); // map the mm range to the servo range - dxl_position = (uint32_t)mapConstrain(target, position_min, position_max, dxl_count_min, dxl_count_max); + dxl_position = (uint32_t)mapConstrain(target, limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max); tx_message[++msg_index] = current_id; // ID of the servo tx_message[++msg_index] = dxl_position & 0xFF; // data diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.h b/Grbl_Esp32/src/Motors/Dynamixel2.h index 99aa5bdb..d4a482c3 100644 --- a/Grbl_Esp32/src/Motors/Dynamixel2.h +++ b/Grbl_Esp32/src/Motors/Dynamixel2.h @@ -8,7 +8,7 @@ Part of Grbl_ESP32 2020 - Bart Dring - + Grbl 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 @@ -69,25 +69,28 @@ const int DXL_CONTROL_MODE_POSITION = 3; #endif #include "Motor.h" +#include "Servo.h" namespace Motors { - class Dynamixel2 : public Motor { + class Dynamixel2 : public Servo { public: - Dynamixel2(); Dynamixel2(uint8_t axis_index, uint8_t address, uint8_t tx_pin, uint8_t rx_pin, uint8_t rts_pin); - virtual void config_message(); - virtual void init(); - virtual void set_disable(bool disable); - virtual void update(); + + // Overrides for inherited methods + void init() override; + void read_settings() override; + bool set_homing_mode(bool isHoming) override; + void set_disable(bool disable) override; + void update() override; static bool uart_ready; static uint8_t ids[MAX_N_AXIS][2]; - void set_homing_mode(uint8_t homing_mask, bool isHoming) override; - void read_settings(); protected: + void config_message() override; + void set_location(); uint8_t _id; @@ -108,7 +111,6 @@ namespace Motors { static uint16_t dxl_update_crc(uint16_t crc_accum, char* data_blk_ptr, uint8_t data_blk_size); static void dxl_bulk_goal_position(); // set all motorsd init_uart(uint8_t id, uint8_t axis_index, uint8_t dual_axis_index); - float _homing_position; float _dxl_count_min; @@ -118,5 +120,8 @@ namespace Motors { uint8_t _rx_pin; uint8_t _rts_pin; uart_port_t _uart_num; + + bool _disabled; + bool _has_errors; }; } diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp index 5c431437..195601f8 100644 --- a/Grbl_Esp32/src/Motors/Motor.cpp +++ b/Grbl_Esp32/src/Motors/Motor.cpp @@ -34,34 +34,11 @@ #include "Motor.h" namespace Motors { - Motor::Motor() { type_id = MOTOR; } + Motor::Motor(uint8_t axis_index) : + _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {} - void Motor::init() { _homing_mask = 0; } - - void Motor::config_message() {} void Motor::debug_message() {} - void Motor::read_settings() { - float max_travel = axis_settings[_axis_index]->max_travel->get(); - float mpos = axis_settings[_axis_index]->home_mpos->get(); - - if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) { - _position_min = mpos; - _position_max = mpos + max_travel; - } else { - _position_min = mpos - max_travel; - _position_max = mpos; - } - } - - void Motor::set_disable(bool disable) {} - void Motor::set_direction_pins(uint8_t onMask) {} - void Motor::step(uint8_t step_mask, uint8_t dir_mask) {} bool Motor::test() { return true; }; // true = OK - void Motor::update() {} - bool Motor::can_home() { return _can_home; }; - void Motor::set_axis_name() { sprintf(_axis_name, "%c%s", report_get_axis_letter(_axis_index), _dual_axis_index ? "2" : " "); } - - void Motor::set_homing_mode(uint8_t homing_mask, bool isHoming) { _homing_mask = homing_mask; } } diff --git a/Grbl_Esp32/src/Motors/Motor.h b/Grbl_Esp32/src/Motors/Motor.h index a32dccab..c9c0300b 100644 --- a/Grbl_Esp32/src/Motors/Motor.h +++ b/Grbl_Esp32/src/Motors/Motor.h @@ -36,38 +36,82 @@ namespace Motors { class Motor { public: - Motor(); + Motor(uint8_t axis_index); - virtual void init(); // not in constructor because this also gets called when $$ settings change - virtual void config_message(); + // init() establishes configured motor parameters. It is called after + // all motor objects have been constructed. + virtual void init() {} + + // debug_message() displays motor-specific information that can be + // used to assist with motor configuration. For many motor types, + // it is a no-op. + // TODO Architecture: Should this be private? It only applies to + // Trinamic drivers so maybe there is a cleaner approach to solving + // the stallguard debugging problem. virtual void debug_message(); - virtual void read_settings(); - virtual void set_homing_mode(uint8_t homing_mask, bool isHoming); - virtual void set_disable(bool disable); - virtual void set_direction_pins(uint8_t onMask); - virtual void step(uint8_t step_mask, uint8_t dir_mask); // only used on Unipolar right now - virtual bool test(); - virtual void set_axis_name(); - virtual void update(); - virtual bool can_home(); - motor_class_id_t type_id; - uint8_t is_active = false; - uint8_t has_errors = false; + // read_settings(), called from init() and motors_read_settings(), + // re-establishes the motor configuration parameters that come + // from $ settings. + // TODO Architecture: Maybe this should be subsumed by init() + virtual void read_settings() {} + + // set_homing_mode() is called from motors_set_homing_mode(), + // which in turn is called at the beginning of a homing cycle + // with isHoming true, and at the end with isHoming false. + // Some motor types require differ setups for homing and + // normal operation. Returns true if the motor can home + virtual bool set_homing_mode(bool isHoming) = 0; + + // set_disable() disables or enables a motor. It is used to + // make a motor transition between idle and non-idle states. + virtual void set_disable(bool disable) {} + + // set_direction() sets the motor movement direction. It is + // invoked for every motion segment. + virtual void set_direction(bool) {} + + // step() initiates a step operation on a motor. It is called + // from motors_step() for ever motor than needs to step now. + // For ordinary step/direction motors, it sets the step pin + // to the active state. + virtual void step() {} + + // unstep() turns off the step pin, if applicable, for a motor. + // It is called from motors_unstep() for all motors, since + // motors_unstep() is used in many contexts where the previous + // states of the step pins are unknown. + virtual void unstep() {} + + // test(), called from init(), checks to see if a motor is + // responsive, returning true on failure. Typical + // implementations also display messages to show the result. + // TODO Architecture: Should this be private? + virtual bool test(); + + // update() is used for some types of "smart" motors that + // can be told to move to a specific position. It is + // called from a periodic task. + virtual void update() {} protected: + // config_message(), called from init(), displays a message describing + // the motor configuration - pins and other motor-specific items + virtual void config_message() {} + + // _axis_index is the axis from XYZABC, while + // _dual_axis_index is 0 for the primary motor on that + // axis and 1 for the ganged motor. + // These variables are used for several purposes: + // * Displaying the axis name in messages + // * When reading settings, determining which setting + // applies to this motor + // * For some motor types, it is necessary to maintain + // tables of all the motors of that type; those + // tables can be indexed by these variables. + // TODO Architecture: It might be useful to cache a + // reference to the axis settings entry. uint8_t _axis_index; // X_AXIS, etc uint8_t _dual_axis_index; // 0 = primary 1=ganged - - bool _showError; - bool _use_mpos = true; - uint8_t _homing_mask; - char _axis_name[10]; // this the name to use when reporting like "X" or "X2" - - float _position_min = 0; - float _position_max = 0; // position in millimeters - - bool _can_home = true; - bool _disabled; }; } diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index a42b920d..8c7d550a 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -44,18 +44,6 @@ #include "TrinamicDriver.h" Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) -static TaskHandle_t readSgTaskHandle = 0; // for realtime stallguard data diaplay -static TaskHandle_t servoUpdateTaskHandle = 0; - -bool Motors::Dynamixel2::uart_ready = false; -uint8_t Motors::Dynamixel2::ids[MAX_N_AXIS][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } }; - -uint8_t rmt_chan_num[MAX_AXES][MAX_GANGED]; -rmt_item32_t rmtItem[2]; -rmt_config_t rmtConfig; - -bool motor_class_steps; // true if at least one motor class is handling steps - void init_motors() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Motors"); @@ -64,9 +52,9 @@ void init_motors() { if (n_axis >= 1) { #ifdef X_TRINAMIC_DRIVER myMotor[X_AXIS][0] = new Motors::TrinamicDriver( - X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE, get_next_trinamic_driver_index()); + X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); #elif defined(X_SERVO_PIN) - myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN, X_SERVO_CAL_MIN, X_SERVO_CAL_MAX); + myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN); #elif defined(X_UNIPOLAR) myMotor[X_AXIS][0] = new Motors::UnipolarMotor(X_AXIS, X_PIN_PHASE_0, X_PIN_PHASE_1, X_PIN_PHASE_2, X_PIN_PHASE_3); #elif defined(X_STEP_PIN) @@ -74,33 +62,31 @@ void init_motors() { #elif defined(X_DYNAMIXEL_ID) myMotor[X_AXIS][0] = new Motors::Dynamixel2(X_AXIS, X_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS); #else - myMotor[X_AXIS][0] = new Motors::Nullmotor(); + myMotor[X_AXIS][0] = new Motors::Nullmotor(X_AXIS); #endif #ifdef X2_TRINAMIC_DRIVER myMotor[X_AXIS][1] = new Motors::TrinamicDriver( - X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE, get_next_trinamic_driver_index()); -#elif defined(X2_SERVO_PIN) - myMotor[X_AXIS][1] = new Motors::RcServo(X2_AXIS, X2_SERVO_PIN, X2_SERVO_CAL_MIN, X2_SERVO_CAL_MAX); + X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); #elif defined(X2_UNIPOLAR) myMotor[X_AXIS][1] = new Motors::UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3); #elif defined(X2_STEP_PIN) myMotor[X_AXIS][1] = new Motors::StandardStepper(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN); #else - myMotor[X_AXIS][1] = new Motors::Nullmotor(); + myMotor[X_AXIS][1] = new Motors::Nullmotor(X2_AXIS); #endif } else { - myMotor[X_AXIS][0] = new Motors::Nullmotor(); - myMotor[X_AXIS][1] = new Motors::Nullmotor(); + myMotor[X_AXIS][0] = new Motors::Nullmotor(X_AXIS); + myMotor[X_AXIS][1] = new Motors::Nullmotor(X2_AXIS); } if (n_axis >= 2) { // this WILL be done better with settings #ifdef Y_TRINAMIC_DRIVER myMotor[Y_AXIS][0] = new Motors::TrinamicDriver( - Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE, get_next_trinamic_driver_index()); + Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); #elif defined(Y_SERVO_PIN) - myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN, Y_SERVO_CAL_MIN, Y_SERVO_CAL_MAX); + myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN); #elif defined(Y_UNIPOLAR) myMotor[Y_AXIS][0] = new Motors::UnipolarMotor(Y_AXIS, Y_PIN_PHASE_0, Y_PIN_PHASE_1, Y_PIN_PHASE_2, Y_PIN_PHASE_3); #elif defined(Y_STEP_PIN) @@ -108,33 +94,31 @@ void init_motors() { #elif defined(Y_DYNAMIXEL_ID) myMotor[Y_AXIS][0] = new Motors::Dynamixel2(Y_AXIS, Y_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS); #else - myMotor[Y_AXIS][0] = new Motors::Nullmotor(); + myMotor[Y_AXIS][0] = new Motors::Nullmotor(Y_AXIS); #endif #ifdef Y2_TRINAMIC_DRIVER myMotor[Y_AXIS][1] = new Motors::TrinamicDriver( - Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE, get_next_trinamic_driver_index()); -#elif defined(Y2_SERVO_PIN) - myMotor[Y_AXIS][1] = new Motors::RcServo(Y2_AXIS, Y2_SERVO_PIN, Y2_SERVO_CAL_MIN, Y2_SERVO_CAL_MAX); + Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); #elif defined(Y2_UNIPOLAR) myMotor[Y_AXIS][1] = new Motors::UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3); #elif defined(Y2_STEP_PIN) myMotor[Y_AXIS][1] = new Motors::StandardStepper(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN); #else - myMotor[Y_AXIS][1] = new Motors::Nullmotor(); + myMotor[Y_AXIS][1] = new Motors::Nullmotor(Y2_AXIS); #endif } else { - myMotor[Y_AXIS][0] = new Motors::Nullmotor(); - myMotor[Y_AXIS][1] = new Motors::Nullmotor(); + myMotor[Y_AXIS][0] = new Motors::Nullmotor(Y_AXIS); + myMotor[Y_AXIS][1] = new Motors::Nullmotor(Y2_AXIS); } if (n_axis >= 3) { // this WILL be done better with settings #ifdef Z_TRINAMIC_DRIVER myMotor[Z_AXIS][0] = new Motors::TrinamicDriver( - Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE, get_next_trinamic_driver_index()); + Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); #elif defined(Z_SERVO_PIN) - myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN, Z_SERVO_CAL_MIN, Z_SERVO_CAL_MAX); + myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN); #elif defined(Z_UNIPOLAR) myMotor[Z_AXIS][0] = new Motors::UnipolarMotor(Z_AXIS, Z_PIN_PHASE_0, Z_PIN_PHASE_1, Z_PIN_PHASE_2, Z_PIN_PHASE_3); #elif defined(Z_STEP_PIN) @@ -142,120 +126,118 @@ void init_motors() { #elif defined(Z_DYNAMIXEL_ID) myMotor[Z_AXIS][0] = new Motors::Dynamixel2(Z_AXIS, Z_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS); #else - myMotor[Z_AXIS][0] = new Motors::Nullmotor(); + myMotor[Z_AXIS][0] = new Motors::Nullmotor(Z_AXIS); #endif #ifdef Z2_TRINAMIC_DRIVER myMotor[Z_AXIS][1] = new Motors::TrinamicDriver( - Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE, get_next_trinamic_driver_index()); -#elif defined(Z2_SERVO_PIN) - myMotor[Z_AXIS][1] = new Motors::RcServo(Z2_AXIS, Z2_SERVO_PIN, Z2_SERVO_CAL_MIN, Z2_SERVO_CAL_MAX); + Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); #elif defined(Z2_UNIPOLAR) myMotor[Z_AXIS][1] = new Motors::UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3); #elif defined(Z2_STEP_PIN) myMotor[Z_AXIS][1] = new Motors::StandardStepper(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN); #else - myMotor[Z_AXIS][1] = new Motors::Nullmotor(); + myMotor[Z_AXIS][1] = new Motors::Nullmotor(Z2_AXIS); #endif } else { - myMotor[Z_AXIS][0] = new Motors::Nullmotor(); - myMotor[Z_AXIS][1] = new Motors::Nullmotor(); + myMotor[Z_AXIS][0] = new Motors::Nullmotor(Z_AXIS); + myMotor[Z_AXIS][1] = new Motors::Nullmotor(Z2_AXIS); } if (n_axis >= 4) { // this WILL be done better with settings #ifdef A_TRINAMIC_DRIVER myMotor[A_AXIS][0] = new Motors::TrinamicDriver( - A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE, get_next_trinamic_driver_index()); + A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); #elif defined(A_SERVO_PIN) - myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN, A_SERVO_CAL_MIN, A_SERVO_CAL_MAX); + myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN); #elif defined(A_UNIPOLAR) myMotor[A_AXIS][0] = new Motors::UnipolarMotor(A_AXIS, A_PIN_PHASE_0, A_PIN_PHASE_1, A_PIN_PHASE_2, A_PIN_PHASE_3); #elif defined(A_STEP_PIN) myMotor[A_AXIS][0] = new Motors::StandardStepper(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN); +#elif defined(A_DYNAMIXEL_ID) + myMotor[A_AXIS][0] = new Motors::Dynamixel2(A_AXIS, A_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS); #else - myMotor[A_AXIS][0] = new Motors::Nullmotor(); + myMotor[A_AXIS][0] = new Motors::Nullmotor(A_AXIS); #endif #ifdef A2_TRINAMIC_DRIVER myMotor[A_AXIS][1] = new Motors::TrinamicDriver( - A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, get_next_trinamic_driver_index()); -#elif defined(A2_SERVO_PIN) - myMotor[A_AXIS][1] = new Motors::RcServo(A2_AXIS, A2_SERVO_PIN, A2_SERVO_CAL_MIN, A2_SERVO_CAL_MAX); + A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); #elif defined(A2_UNIPOLAR) myMotor[A_AXIS][1] = new Motors::UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3); #elif defined(A2_STEP_PIN) myMotor[A_AXIS][1] = new Motors::StandardStepper(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN); #else - myMotor[A_AXIS][1] = new Motors::Nullmotor(); + myMotor[A_AXIS][1] = new Motors::Nullmotor(A2_AXIS); #endif } else { - myMotor[A_AXIS][0] = new Motors::Nullmotor(); - myMotor[A_AXIS][1] = new Motors::Nullmotor(); + myMotor[A_AXIS][0] = new Motors::Nullmotor(A_AXIS); + myMotor[A_AXIS][1] = new Motors::Nullmotor(A2_AXIS); } if (n_axis >= 5) { // this WILL be done better with settings #ifdef B_TRINAMIC_DRIVER myMotor[B_AXIS][0] = new Motors::TrinamicDriver( - B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE, get_next_trinamic_driver_index()); + B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); #elif defined(B_SERVO_PIN) - myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN, B_SERVO_CAL_MIN, B_SERVO_CAL_MAX); + myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN); #elif defined(B_UNIPOLAR) myMotor[B_AXIS][0] = new Motors::UnipolarMotor(B_AXIS, B_PIN_PHASE_0, B_PIN_PHASE_1, B_PIN_PHASE_2, B_PIN_PHASE_3); #elif defined(B_STEP_PIN) myMotor[B_AXIS][0] = new Motors::StandardStepper(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN); +#elif defined(B_DYNAMIXEL_ID) + myMotor[B_AXIS][0] = new Motors::Dynamixel2(B_AXIS, B_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS); #else - myMotor[B_AXIS][0] = new Motors::Nullmotor(); + myMotor[B_AXIS][0] = new Motors::Nullmotor(B_AXIS); #endif #ifdef B2_TRINAMIC_DRIVER myMotor[B_AXIS][1] = new Motors::TrinamicDriver( - B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, get_next_trinamic_driver_index()); -#elif defined(B2_SERVO_PIN) - myMotor[B_AXIS][1] = new Motors::RcServo(B2_AXIS, B2_SERVO_PIN, B2_SERVO_CAL_MIN, B2_SERVO_CAL_MAX); + B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); #elif defined(B2_UNIPOLAR) myMotor[B_AXIS][1] = new Motors::UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3); #elif defined(B2_STEP_PIN) myMotor[B_AXIS][1] = new Motors::StandardStepper(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN); #else - myMotor[B_AXIS][1] = new Motors::Nullmotor(); + myMotor[B_AXIS][1] = new Motors::Nullmotor(B2_AXIS); #endif } else { - myMotor[B_AXIS][0] = new Motors::Nullmotor(); - myMotor[B_AXIS][1] = new Motors::Nullmotor(); + myMotor[B_AXIS][0] = new Motors::Nullmotor(B_AXIS); + myMotor[B_AXIS][1] = new Motors::Nullmotor(B2_AXIS); } if (n_axis >= 6) { // this WILL be done better with settings #ifdef C_TRINAMIC_DRIVER myMotor[C_AXIS][0] = new Motors::TrinamicDriver( - C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE, get_next_trinamic_driver_index()); + C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); #elif defined(C_SERVO_PIN) - myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN, C_SERVO_CAL_MIN, C_SERVO_CAL_MAX); + myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN); #elif defined(C_UNIPOLAR) myMotor[C_AXIS][0] = new Motors::UnipolarMotor(C_AXIS, C_PIN_PHASE_0, C_PIN_PHASE_1, C_PIN_PHASE_2, C_PIN_PHASE_3); #elif defined(C_STEP_PIN) myMotor[C_AXIS][0] = new Motors::StandardStepper(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN); +#elif defined(C_DYNAMIXEL_ID) + myMotor[C_AXIS][0] = new Motors::Dynamixel2(C_AXIS, C_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS); #else - myMotor[C_AXIS][0] = new Motors::Nullmotor(); + myMotor[C_AXIS][0] = new Motors::Nullmotor(C_AXIS); #endif #ifdef C2_TRINAMIC_DRIVER myMotor[C_AXIS][1] = new Motors::TrinamicDriver( - C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, get_next_trinamic_driver_index()); -#elif defined(C2_SERVO_PIN) - myMotor[C_AXIS][1] = new Motors::RcServo(C2_AXIS, C2_SERVO_PIN, C2_SERVO_CAL_MIN, C2_SERVO_CAL_MAX); + C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); #elif defined(C2_UNIPOLAR) myMotor[C_AXIS][1] = new Motors::UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3); #elif defined(C2_STEP_PIN) myMotor[C_AXIS][1] = new Motors::StandardStepper(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN); #else - myMotor[C_AXIS][1] = new Motors::Nullmotor(); + myMotor[C_AXIS][1] = new Motors::Nullmotor(C2_AXIS); #endif } else { - myMotor[C_AXIS][0] = new Motors::Nullmotor(); - myMotor[C_AXIS][1] = new Motors::Nullmotor(); + myMotor[C_AXIS][0] = new Motors::Nullmotor(C_AXIS); + myMotor[C_AXIS][1] = new Motors::Nullmotor(C2_AXIS); } #ifdef USE_STEPSTICK @@ -292,77 +274,9 @@ void init_motors() { // certain motors need features to be turned on. Check them here for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { - if (myMotor[axis][gang_index]->type_id == UNIPOLAR_MOTOR) { - motor_class_steps = true; - } - - // CS Pins of all TMC motors need to be setup before any can be talked to - // ...so init cannot be called via the constructors. This inits them all. - if (myMotor[axis][gang_index]->type_id == TRINAMIC_SPI_MOTOR) { - myMotor[axis][gang_index]->init(); - } + myMotor[axis][gang_index]->init(); } } - - // some motor objects require a step signal - motor_class_steps = motors_have_type_id(UNIPOLAR_MOTOR); - - if (motors_have_type_id(TRINAMIC_SPI_MOTOR)) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TMCStepper Library Ver. 0x%06x", TMCSTEPPER_VERSION); - xTaskCreatePinnedToCore(readSgTask, // task - "readSgTask", // name for task - 4096, // size of task stack - NULL, // parameters - 1, // priority - &readSgTaskHandle, - 0 // core - ); - if (stallguard_debug_mask->get() != 0) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard debug enabled: %d", stallguard_debug_mask->get()); - } - } - - if (motors_have_type_id(RC_SERVO_MOTOR) || motors_have_type_id(DYNAMIXEL2)) { - xTaskCreatePinnedToCore(servoUpdateTask, // task - "servoUpdateTask", // name for task - 4096, // size of task stack - NULL, // parameters - 1, // priority - &servoUpdateTaskHandle, - 0 // core - ); - } -} - -void servoUpdateTask(void* pvParameters) { - TickType_t xLastWakeTime; - const TickType_t xUpdate = SERVO_TIMER_INTERVAL; // in ticks (typically ms) - auto n_axis = number_axis->get(); - - xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. - vTaskDelay(2000); // initial delay - while (true) { // don't ever return from this or the task dies - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { - myMotor[axis][gang_index]->update(); - } - } - - vTaskDelayUntil(&xLastWakeTime, xUpdate); - } -} - -// do any motors match the type_id -bool motors_have_type_id(motor_class_id_t id) { - auto n_axis = number_axis->get(); - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { - if (myMotor[axis][gang_index]->type_id == id) { - return true; - } - } - } - return false; } void motors_set_disable(bool disable) { @@ -370,14 +284,14 @@ void motors_set_disable(bool disable) { //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Motors disable %d", disable); -/* + /* if (previous_state == disable) { return; } - previous_state = disable; + previous_state = disable; */ - // now loop through all the motors to see if they can individually diable + // now loop through all the motors to see if they can individually disable auto n_axis = number_axis->get(); for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) { for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { @@ -390,7 +304,6 @@ void motors_set_disable(bool disable) { disable = !disable; // Apply pin invert. } digitalWrite(STEPPERS_DISABLE_PIN, disable); - } void motors_read_settings() { @@ -405,98 +318,53 @@ void motors_read_settings() { // use this to tell all the motors what the current homing mode is // They can use this to setup things like Stall -void motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { +uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { + uint8_t can_home = 0; auto n_axis = number_axis->get(); - for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - if (bit_istrue(homing_mask, bit(axis)) && (myMotor[axis][gang_index]->is_active)) { - myMotor[axis][gang_index]->set_homing_mode(homing_mask, isHoming); + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { + if (bitnum_istrue(homing_mask, axis)) { + if (myMotor[axis][0]->set_homing_mode(isHoming)) { + bitnum_true(can_home, axis); } + myMotor[axis][1]->set_homing_mode(isHoming); } } + return can_home; } -void motors_set_direction_pins(uint8_t onMask) { - static uint8_t previous_val = 255; // should never be this value - if (previous_val == onMask) { - return; - } - previous_val = onMask; - +void motors_step(uint8_t step_mask, uint8_t dir_mask) { + auto n_axis = number_axis->get(); //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask); - auto n_axis = number_axis->get(); - for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) { - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - myMotor[axis][gang_index]->set_direction_pins(onMask); + // Set the direction pins, but optimize for the common + // situation where the direction bits haven't changed. + static uint8_t previous_dir = 255; // should never be this value + if (dir_mask != previous_dir) { + previous_dir = dir_mask; + + for (int axis = X_AXIS; axis < n_axis; axis++) { + bool thisDir = bitnum_istrue(dir_mask, axis); + myMotor[axis][0]->set_direction(thisDir); + myMotor[axis][1]->set_direction(thisDir); } } -} - -// returns the next spi index. We cannot preassign to axes because ganged (X2 type axes) might -// need to be inserted into the order of axes. -uint8_t get_next_trinamic_driver_index() { -#ifdef TRINAMIC_DAISY_CHAIN - static uint8_t index = 1; // they start at 1 - return index++; -#else - return -1; -#endif -} - -// some motor objects, like unipolar need step signals -void motors_step(uint8_t step_mask, uint8_t dir_mask) { - if (motor_class_steps) { // determined in init_motors if any motors need to handle steps - auto n_axis = number_axis->get(); - for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - myMotor[axis][gang_index]->step(step_mask, dir_mask); + // Turn on step pulses for motors that are supposed to step now + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { + if (bitnum_istrue(step_mask, axis)) { + if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { + myMotor[axis][0]->step(); + } + if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { + myMotor[axis][1]->step(); } } } } - -/* - This will print StallGuard data that is useful for tuning. -*/ -void readSgTask(void* pvParameters) { - TickType_t xLastWakeTime; - const TickType_t xreadSg = 200; // in ticks (typically ms) - auto n_axis = number_axis->get(); - - xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. - while (true) { // don't ever return from this or the task dies - if (motorSettingChanged) { - motors_read_settings(); - motorSettingChanged = false; - } - - if (stallguard_debug_mask->get() != 0) { - if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { - for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - if (stallguard_debug_mask->get() & bit(axis)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); - for (uint8_t gang_index = 0; gang_index < 2; gang_index++) { - myMotor[axis][gang_index]->debug_message(); - } - } - } - } // sys.state - } // if mask - vTaskDelayUntil(&xLastWakeTime, xreadSg); +// Turn all stepper pins off +void motors_unstep() { + auto n_axis = number_axis->get(); + for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { + myMotor[axis][0]->unstep(); + myMotor[axis][1]->unstep(); } } - -bool motor_can_home(uint8_t axis) { - return myMotor[axis][0]->can_home(); -} - -#ifdef USE_I2S_OUT -// -// Override default function and insert a short delay -// -void TMC2130Stepper::switchCSpin(bool state) { - digitalWrite(_pinCS, state); - i2s_out_delay(); -} -#endif diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h index 70536f99..a6451eff 100644 --- a/Grbl_Esp32/src/Motors/Motors.h +++ b/Grbl_Esp32/src/Motors/Motors.h @@ -30,34 +30,17 @@ */ #include "../Grbl.h" -#include // https://github.com/teemuatlut/TMCStepper - -extern uint8_t rmt_chan_num[MAX_AXES][2]; -extern rmt_item32_t rmtItem[2]; -extern rmt_config_t rmtConfig; - -typedef enum { - MOTOR, - NULL_MOTOR, - STANDARD_MOTOR, - TRINAMIC_SPI_MOTOR, - UNIPOLAR_MOTOR, - RC_SERVO_MOTOR, - SOLENOID, - DYNAMIXEL2 -} motor_class_id_t; // These are used for setup and to talk to the motors as a group. void init_motors(); uint8_t get_next_trinamic_driver_index(); -bool motors_have_type_id(motor_class_id_t id); void readSgTask(void* pvParameters); void motors_read_settings(); -void motors_set_homing_mode(uint8_t homing_mask, bool isHoming); -void motors_set_disable(bool disable); -void motors_set_direction_pins(uint8_t onMask); -void motors_step(uint8_t step_mask, uint8_t dir_mask); -void servoUpdateTask(void* pvParameters); -bool motor_can_home(uint8_t index); -extern bool motor_class_steps; // true if at least one motor class is handling steps +// The return value is a bitmask of axes that can home +uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming); +void motors_set_disable(bool disable); +void motors_step(uint8_t step_mask, uint8_t dir_mask); +void motors_unstep(); + +void servoUpdateTask(void* pvParameters); diff --git a/Grbl_Esp32/src/Motors/NullMotor.cpp b/Grbl_Esp32/src/Motors/NullMotor.cpp new file mode 100644 index 00000000..be5b34a2 --- /dev/null +++ b/Grbl_Esp32/src/Motors/NullMotor.cpp @@ -0,0 +1,28 @@ +/* + NullMotor.cpp + + This is a fake motor that does nothing. + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + Grbl 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. + Grbl 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 Grbl. If not, see . +*/ + +#include "NullMotor.h" + +namespace Motors { + Nullmotor::Nullmotor(uint8_t axis_index) : + Motor(axis_index) + {} +} diff --git a/Grbl_Esp32/src/Motors/NullMotor.h b/Grbl_Esp32/src/Motors/NullMotor.h index e360392d..6e859855 100644 --- a/Grbl_Esp32/src/Motors/NullMotor.h +++ b/Grbl_Esp32/src/Motors/NullMotor.h @@ -3,5 +3,9 @@ #include "Motor.h" namespace Motors { - class Nullmotor : public Motor {}; + class Nullmotor : public Motor { + public: + Nullmotor(uint8_t axis_index); + bool set_homing_mode(bool isHoming) { return false; } + }; } diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index eabcf27b..a5716701 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -1,7 +1,7 @@ /* RcServo.cpp - This allows an RcServo to be used like any other motor. Serrvos + This allows an RcServo to be used like any other motor. Servos do have limitation in travel and speed, so you do need to respect that. Part of Grbl_ESP32 @@ -10,9 +10,9 @@ The servo's travel will be mapped against the axis with $X/MaxTravel - The rotation can be inverted with by $Stepper/DirInvert + The rotation can be inverted with by $Stepper/DirInvert - Homing simply sets the axis Mpos to the endpoint as determined by $Homing/DirInver + Homing simply sets the axis Mpos to the endpoint as determined by $Homing/DirInvert Calibration is part of the setting (TBD) fixed at 1.00 now @@ -31,45 +31,40 @@ #include "RcServo.h" namespace Motors { - RcServo::RcServo() {} - - RcServo::RcServo(uint8_t axis_index, uint8_t pwm_pin, float cal_min, float cal_max) { - type_id = RC_SERVO_MOTOR; - this->_axis_index = axis_index % MAX_AXES; - this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged - this->_pwm_pin = pwm_pin; - _cal_min = cal_min; - _cal_max = cal_max; - init(); - } + RcServo::RcServo(uint8_t axis_index, uint8_t pwm_pin) : Servo(axis_index), _pwm_pin(pwm_pin) {} void RcServo::init() { + char* setting_cal_min = (char*)malloc(20); + sprintf(setting_cal_min, "%c/RcServo/Cal/Min", report_get_axis_letter(_axis_index)); // + rc_servo_cal_min = new FloatSetting(EXTENDED, WG, NULL, setting_cal_min, 1.0, 0.5, 2.0); + + char* setting_cal_max = (char*)malloc(20); + sprintf(setting_cal_max, "%c/RcServo/Cal/Max", report_get_axis_letter(_axis_index)); // + rc_servo_cal_max = new FloatSetting(EXTENDED, WG, NULL, setting_cal_max, 1.0, 0.5, 2.0); + read_settings(); _channel_num = sys_get_next_PWM_chan_num(); ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); ledcAttachPin(_pwm_pin, _channel_num); _current_pwm_duty = 0; - is_active = true; // as opposed to NullMotors, this is a real motor - _can_home = false; // this axis cannot be confensionally homed - set_axis_name(); config_message(); + startUpdateTask(); } void RcServo::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis RC Servo Pin:%d Pulse Len(%.0f,%.0f) Limits(%.3f,%.3f)", - _axis_name, + "%s RC Servo Pin:%d Pulse Len(%.0f,%.0f) %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), _pwm_pin, _pwm_pulse_min, _pwm_pulse_max, - _position_min, - _position_max); + reportAxisLimitsMsg(_axis_index)); } void RcServo::_write_pwm(uint32_t duty) { - // to prevent excessive calls to ledcWrite, make sure duty hass changed + // to prevent excessive calls to ledcWrite, make sure duty has changed if (duty == _current_pwm_duty) { return; } @@ -80,7 +75,10 @@ namespace Motors { // sets the PWM to zero. This allows most servos to be manually moved void RcServo::set_disable(bool disable) { - return; + if (_disabled == disable) { + return; + } + _disabled = disable; if (_disabled) { _write_pwm(0); @@ -88,15 +86,15 @@ namespace Motors { } // Homing justs sets the new system position and the servo will move there - void RcServo::set_homing_mode(uint8_t homing_mask, bool isHoming) { + bool RcServo::set_homing_mode(bool isHoming) { float home_pos = 0.0; sys_position[_axis_index] = axis_settings[_axis_index]->home_mpos->get() * axis_settings[_axis_index]->steps_per_mm->get(); // convert to steps - set_location(); // force the PWM to update now - + set_location(); // force the PWM to update now vTaskDelay(750); // give time to move + return false; // Cannot be homed in the conventional way } void RcServo::update() { set_location(); } @@ -105,42 +103,33 @@ namespace Motors { uint32_t servo_pulse_len; float servo_pos, mpos, offset; - read_settings(); + if (_disabled) + return; if (sys.state == State::Alarm) { set_disable(true); return; } + read_settings(); + mpos = system_convert_axis_steps_to_mpos(sys_position, _axis_index); // get the axis machine position in mm // TBD working in MPos offset = 0; // gc_state.coord_system[axis_index] + gc_state.coord_offset[axis_index]; // get the current axis work offset servo_pos = mpos - offset; // determine the current work position // determine the pulse length - servo_pulse_len = (uint32_t)mapConstrain(servo_pos, _position_min, _position_max, _pwm_pulse_min, _pwm_pulse_max); + servo_pulse_len = (uint32_t)mapConstrain( + servo_pos, limitsMinPosition(_axis_index), limitsMaxPosition(_axis_index), _pwm_pulse_min, _pwm_pulse_max); _write_pwm(servo_pulse_len); } void RcServo::read_settings() { - float travel = axis_settings[_axis_index]->max_travel->get(); - float mpos = axis_settings[_axis_index]->home_mpos->get(); - //float max_mpos, min_mpos; + _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get(); + _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get(); - if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) { - _position_min = mpos; - _position_max = mpos + travel; - } else { - _position_min = mpos - travel; - _position_max = mpos; - } - - _pwm_pulse_min = SERVO_MIN_PULSE * _cal_min; - _pwm_pulse_max = SERVO_MAX_PULSE * _cal_max; - - if (bit_istrue(dir_invert_mask->get(), bit(_axis_index))) // normal direction + if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) // normal direction swap(_pwm_pulse_min, _pwm_pulse_max); } - } diff --git a/Grbl_Esp32/src/Motors/RcServo.h b/Grbl_Esp32/src/Motors/RcServo.h index 818b6167..77540891 100644 --- a/Grbl_Esp32/src/Motors/RcServo.h +++ b/Grbl_Esp32/src/Motors/RcServo.h @@ -6,7 +6,7 @@ Part of Grbl_ESP32 2020 - Bart Dring - + Grbl 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 @@ -21,35 +21,41 @@ #include "Motor.h" +#include "Servo.h" #include "RcServoSettings.h" namespace Motors { - class RcServo : public Motor { + class RcServo : public Servo { public: - RcServo(); - RcServo(uint8_t axis_index, uint8_t pwm_pin, float cal_min, float cal_max); - virtual void config_message(); - virtual void init(); - void _write_pwm(uint32_t duty); - virtual void set_disable(bool disable); - virtual void update(); - void read_settings(); - void set_homing_mode(uint8_t homing_mask, bool isHoming) override; + RcServo(uint8_t axis_index, uint8_t pwm_pin); + + // Overrides for inherited methods + void init() override; + void read_settings() override; + bool set_homing_mode(bool isHoming) override; + void set_disable(bool disable) override; + void update() override; + + void _write_pwm(uint32_t duty); + protected: + void config_message() override; + void set_location(); uint8_t _pwm_pin; uint8_t _channel_num; uint32_t _current_pwm_duty; - float _homing_position; float _pwm_pulse_min; float _pwm_pulse_max; - float _cal_min = 1.0; - float _cal_max = 1.0; - }; + bool _disabled; + + FloatSetting* rc_servo_cal_min; + FloatSetting* rc_servo_cal_max; + }; } diff --git a/Grbl_Esp32/src/Motors/RcServoSettings.h b/Grbl_Esp32/src/Motors/RcServoSettings.h index 3502f746..a11c20bd 100644 --- a/Grbl_Esp32/src/Motors/RcServoSettings.h +++ b/Grbl_Esp32/src/Motors/RcServoSettings.h @@ -20,9 +20,6 @@ const uint16_t SERVO_MAX_PULSE = (uint16_t)(SERVO_MAX_PULSE_SEC / SERVO_TIME_PER const uint16_t SERVO_PULSE_RANGE = (SERVO_MAX_PULSE - SERVO_MIN_PULSE); -const double SERVO_CAL_MIN = 20.0; // Percent: the minimum allowable calibration value -const double SERVO_CAL_MAX = 180.0; // Percent: the maximum allowable calibration value - const double SERVO_TIMER_INT_FREQ = 50.0; // Hz This is the task frequency const int SERVO_FULL_MOVE_TIME = 750; // rtos ticks diff --git a/Grbl_Esp32/src/Motors/Servo.cpp b/Grbl_Esp32/src/Motors/Servo.cpp new file mode 100644 index 00000000..1e0fa2b7 --- /dev/null +++ b/Grbl_Esp32/src/Motors/Servo.cpp @@ -0,0 +1,78 @@ +/* + Servo.cpp + + This is a base class for servo-type motors - ones that autonomously + move to a specified position, instead of being moved incrementally + by stepping. Specific kinds of servo motors inherit from it. + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + The servo's travel will be mapped against the axis with $X/MaxTravel + + The rotation can be inverted with by $Stepper/DirInvert + + Homing simply sets the axis Mpos to the endpoint as determined by $Homing/DirInvert + + Calibration is part of the setting (TBD) fixed at 1.00 now + + Grbl_ESP32 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. + Grbl 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 Grbl. If not, see . +*/ + +#include "Servo.h" + +namespace Motors { + Servo* Servo::List = NULL; + + Servo::Servo(uint8_t axis_index) : Motor(axis_index) { + link = List; + List = this; + } + + void Servo::startUpdateTask() { + if (this == List) { + xTaskCreatePinnedToCore(updateTask, // task + "servoUpdateTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + NULL, // handle + 0 // core + ); + } + } + + void Servo::updateTask(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xUpdate = SERVO_TIMER_INTERVAL; // in ticks (typically ms) + auto n_axis = number_axis->get(); + + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. + vTaskDelay(2000); // initial delay + while (true) { // don't ever return from this or the task dies + + // static UBaseType_t uxHighWaterMark = 0; + // if (uxHighWaterMark != uxTaskGetStackHighWaterMark(NULL)) { + // uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL); + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Servo Task Min Stack Space: %d", uxHighWaterMark); + // } + + for (Servo* p = List; p; p = p->link) { + p->update(); + } + + vTaskDelayUntil(&xLastWakeTime, xUpdate); + } + } + +} diff --git a/Grbl_Esp32/src/Motors/Servo.h b/Grbl_Esp32/src/Motors/Servo.h new file mode 100644 index 00000000..09b6e35c --- /dev/null +++ b/Grbl_Esp32/src/Motors/Servo.h @@ -0,0 +1,54 @@ +#pragma once + +/* + Servo.h + + This is a base class for servo-type motors - ones that autonomously + move to a specified position, instead of being moved incrementally + by stepping. Specific kinds of servo motors inherit from it. + + Part of Grbl_ESP32 + + 2020 - Bart Dring + + Grbl 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. + Grbl 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 Grbl. If not, see . +*/ + +#include "Motor.h" + +namespace Motors { + class Servo : public Motor { + public: + Servo(uint8_t axis_index); +#if 0 + // Overrides for inherited methods + void init() override; + void read_settings() override; + bool set_homing_mode(bool isHoming) override; + void set_disable(bool disable) override; +#endif + virtual void update() = 0; // This must be implemented by derived classes + + protected: + // Start the servo update task. Each derived subclass instance calls this + // during init(), which happens after all objects have been constructed. + // startUpdateTask() ignores all such calls except for the last one, where + // it starts the task. + void startUpdateTask(); + + private: + // Linked list of servo instances, used by the servo task + static Servo* List; + Servo* link; + static void updateTask(void*); + }; +} diff --git a/Grbl_Esp32/src/Motors/Solenoid.h b/Grbl_Esp32/src/Motors/Solenoid.h index fb973083..60b62077 100644 --- a/Grbl_Esp32/src/Motors/Solenoid.h +++ b/Grbl_Esp32/src/Motors/Solenoid.h @@ -7,12 +7,13 @@ namespace Motors { public: Solenoid(); Solenoid(uint8_t axis_index, gpio_num_t pwm_pin, float transition_poiont); - void config_message(); void set_location(); - void update(); - void init(); - void set_disable(bool disable); + void update() override; + void init() override; + void set_disable(bool disable) override; float _transition_poiont; + protected: + void config_message() override; }; } diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index 82b62c1c..d2a2e51a 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -24,31 +24,34 @@ #include "StandardStepper.h" namespace Motors { - StandardStepper::StandardStepper() {} + rmt_item32_t StandardStepper::rmtItem[2]; + rmt_config_t StandardStepper::rmtConfig; - StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) { - type_id = STANDARD_MOTOR; - this->_axis_index = axis_index % MAX_AXES; - this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged - this->step_pin = step_pin; - this->dir_pin = dir_pin; - this->disable_pin = disable_pin; - init(); + // Get an RMT channel number + // returns RMT_CHANNEL_MAX for error + rmt_channel_t StandardStepper::get_next_RMT_chan_num() { + static uint8_t next_RMT_chan_num = uint8_t(RMT_CHANNEL_0); // channels 0-7 are valid + if (next_RMT_chan_num < RMT_CHANNEL_MAX) { + next_RMT_chan_num++; + } else { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Error, "Error: out of RMT channels"); + } + return rmt_channel_t(next_RMT_chan_num); + } + + StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) : + Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) { } void StandardStepper::init() { - _homing_mask = 0; - is_active = true; // as opposed to NullMotors, this is a real motor - set_axis_name(); init_step_dir_pins(); - read_settings(); config_message(); } void StandardStepper::init_step_dir_pins() { - // TODO Step pin, but RMT complicates things - _invert_step_pin = bit_istrue(step_invert_mask->get(), bit(_axis_index)); - pinMode(dir_pin, OUTPUT); + _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index); + _invert_dir_pin = bitnum_istrue(dir_invert_mask->get(), _axis_index); + pinMode(_dir_pin, OUTPUT); #ifdef USE_RMT_STEPS rmtConfig.rmt_mode = RMT_MODE_TX; @@ -71,36 +74,53 @@ namespace Motors { rmtItem[1].duration0 = 0; rmtItem[1].duration1 = 0; - rmt_chan_num[_axis_index][_dual_axis_index] = sys_get_next_RMT_chan_num(); - rmt_set_source_clk((rmt_channel_t)rmt_chan_num[_axis_index][_dual_axis_index], RMT_BASECLK_APB); - rmtConfig.channel = (rmt_channel_t)rmt_chan_num[_axis_index][_dual_axis_index]; + _rmt_chan_num = get_next_RMT_chan_num(); + if (_rmt_chan_num == RMT_CHANNEL_MAX) { + return; + } + rmt_set_source_clk(_rmt_chan_num, RMT_BASECLK_APB); + rmtConfig.channel = _rmt_chan_num; rmtConfig.tx_config.idle_level = _invert_step_pin ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; - rmtConfig.gpio_num = gpio_num_t(step_pin); // c is a wacky lang + rmtConfig.gpio_num = gpio_num_t(_step_pin); rmtItem[0].level0 = rmtConfig.tx_config.idle_level; rmtItem[0].level1 = !rmtConfig.tx_config.idle_level; rmt_config(&rmtConfig); rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0); #else - pinMode(step_pin, OUTPUT); + pinMode(_step_pin, OUTPUT); #endif // USE_RMT_STEPS - pinMode(disable_pin, OUTPUT); + pinMode(_disable_pin, OUTPUT); } void StandardStepper::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis Standard Stepper Step:%s Dir:%s Disable:%s Limits(%0.3f,%0.3f)", - _axis_name, - pinName(step_pin).c_str(), - pinName(dir_pin).c_str(), - pinName(disable_pin).c_str(), - _position_min, - _position_max); + "%s Standard Stepper Step:%s Dir:%s Disable:%s %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + pinName(_step_pin).c_str(), + pinName(_dir_pin).c_str(), + pinName(_disable_pin).c_str(), + reportAxisLimitsMsg(_axis_index)); } - void StandardStepper::set_direction_pins(uint8_t onMask) { digitalWrite(dir_pin, (onMask & bit(_axis_index))); } + void StandardStepper::step() { +#ifdef USE_RMT_STEPS + RMT.conf_ch[_rmt_chan_num].conf1.mem_rd_rst = 1; + RMT.conf_ch[_rmt_chan_num].conf1.tx_start = 1; +#else + digitalWrite(_step_pin, !_invert_step_pin); +#endif // USE_RMT_STEPS + } - void StandardStepper::set_disable(bool disable) { digitalWrite(disable_pin, disable); } + void StandardStepper::unstep() { +#ifndef USE_RMT_STEPS + digitalWrite(_step_pin, _invert_step_pin); +#endif // USE_RMT_STEPS + } + + void StandardStepper::set_direction(bool dir) { digitalWrite(_dir_pin, dir ^ _invert_dir_pin); } + + void StandardStepper::set_disable(bool disable) { digitalWrite(_disable_pin, disable); } } diff --git a/Grbl_Esp32/src/Motors/StandardStepper.h b/Grbl_Esp32/src/Motors/StandardStepper.h index 8a4d7e8c..efc72f66 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.h +++ b/Grbl_Esp32/src/Motors/StandardStepper.h @@ -5,19 +5,34 @@ namespace Motors { class StandardStepper : public Motor { public: - StandardStepper(); StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin); - virtual void config_message(); - virtual void init(); - virtual void set_direction_pins(uint8_t onMask); - void init_step_dir_pins(); - virtual void set_disable(bool disable); - uint8_t step_pin; + // Overrides for inherited methods + void init() override; + // No special action, but return true to say homing is possible + bool set_homing_mode(bool isHoming) override { return true; } + void set_disable(bool) override; + void set_direction(bool) override; + void step() override; + void unstep() override; + + void init_step_dir_pins(); protected: + void config_message() override; + +#ifdef USE_RMT_STEPS + rmt_channel_t _rmt_chan_num; +#endif bool _invert_step_pin; - uint8_t dir_pin; - uint8_t disable_pin; + bool _invert_dir_pin; + uint8_t _step_pin; + uint8_t _dir_pin; + uint8_t _disable_pin; + + private: + static rmt_channel_t get_next_RMT_chan_num(); + static rmt_item32_t rmtItem[2]; + static rmt_config_t rmtConfig; }; } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index f785862c..c100b1de 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -21,7 +21,26 @@ #include +#ifdef USE_I2S_OUT + +// Override default function and insert a short delay +void TMC2130Stepper::switchCSpin(bool state) { + digitalWrite(_pinCS, state); + i2s_out_delay(); +} +#endif + namespace Motors { + uint8_t TrinamicDriver::get_next_index() { +#ifdef TRINAMIC_DAISY_CHAIN + static uint8_t index = 1; // they start at 1 + return index++; +#else + return -1; +#endif + } + TrinamicDriver* TrinamicDriver::List = NULL; + TrinamicDriver::TrinamicDriver(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, @@ -29,62 +48,76 @@ namespace Motors { uint8_t cs_pin, uint16_t driver_part_number, float r_sense, - int8_t spi_index) { - type_id = TRINAMIC_SPI_MOTOR; - this->_axis_index = axis_index % MAX_AXES; - this->_dual_axis_index = axis_index < 6 ? 0 : 1; // 0 = primary 1 = ganged - _driver_part_number = driver_part_number; - _r_sense = r_sense; - this->step_pin = step_pin; - this->dir_pin = dir_pin; - this->disable_pin = disable_pin; - this->cs_pin = cs_pin; - this->spi_index = spi_index; - - _homing_mode = TRINAMIC_HOMING_MODE; - _homing_mask = 0; // no axes homing - - set_axis_name(); - + int8_t spi_index) : + StandardStepper(axis_index, step_pin, dir_pin, disable_pin), + _homing_mode(TRINAMIC_HOMING_MODE), _cs_pin(cs_pin), _driver_part_number(driver_part_number), _r_sense(r_sense), + _spi_index(spi_index) { + _has_errors = false; if (_driver_part_number == 2130) { - tmcstepper = new TMC2130Stepper(cs_pin, _r_sense, spi_index); + tmcstepper = new TMC2130Stepper(_cs_pin, _r_sense, _spi_index); } else if (_driver_part_number == 5160) { - tmcstepper = new TMC5160Stepper(cs_pin, _r_sense, spi_index); + tmcstepper = new TMC5160Stepper(_cs_pin, _r_sense, _spi_index); } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Axis Unsupported Trinamic part number TMC%d", _axis_name, _driver_part_number); - has_errors = true; // as opposed to NullMotors, this is a real motor + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Unsupported Trinamic part number TMC%d", + reportAxisNameMsg(_axis_index, _dual_axis_index), + _driver_part_number); + _has_errors = true; // This motor cannot be used return; } + _has_errors = false; init_step_dir_pins(); // from StandardStepper - digitalWrite(cs_pin, HIGH); - pinMode(cs_pin, OUTPUT); + digitalWrite(_cs_pin, HIGH); + pinMode(_cs_pin, OUTPUT); // use slower speed if I2S - if (cs_pin >= I2S_OUT_PIN_BASE) { + if (_cs_pin >= I2S_OUT_PIN_BASE) { tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ); } - config_message(); + link = List; + List = this; // init() must be called later, after all TMC drivers have CS pins setup. } void TrinamicDriver::init() { - if (has_errors) { + if (_has_errors) { return; } + // Display the stepper library version message once, before the first + // TMC config message. Link is NULL for the first TMC instance. + if (!link) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TMCStepper Library Ver. 0x%06x", TMCSTEPPER_VERSION); + } + + config_message(); + SPI.begin(); // this will get called for each motor, but does not seem to hurt anything tmcstepper->begin(); - test(); // Try communicating with motor. Prints an error if there is a problem. + + _has_errors = !test(); // Try communicating with motor. Prints an error if there is a problem. + read_settings(); // pull info from settings set_mode(false); - _homing_mask = 0; - is_active = true; + // After initializing all of the TMC drivers, create a task to + // display StallGuard data. List == this for the final instance. + if (List == this) { + xTaskCreatePinnedToCore(readSgTask, // task + "readSgTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + NULL, + 0 // core + ); + } } /* @@ -93,28 +126,33 @@ namespace Motors { void TrinamicDriver::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d Limits(%0.3f,%0.3f)", - _axis_name, + "%s Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), _driver_part_number, - pinName(step_pin).c_str(), - pinName(dir_pin).c_str(), - pinName(cs_pin).c_str(), - pinName(disable_pin).c_str(), - spi_index, - _position_min, - _position_max); + pinName(_step_pin).c_str(), + pinName(_dir_pin).c_str(), + pinName(_cs_pin).c_str(), + pinName(_disable_pin).c_str(), + _spi_index, + reportAxisLimitsMsg(_axis_index)); } bool TrinamicDriver::test() { - if (has_errors) { + if (_has_errors) { return false; } switch (tmcstepper->test_connection()) { case 1: - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test failed. Check connection", _axis_name); + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Trinamic driver test failed. Check connection", + reportAxisNameMsg(_axis_index, _dual_axis_index)); return false; case 2: - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test failed. Check motor power", _axis_name); + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Trinamic driver test failed. Check motor power", + reportAxisNameMsg(_axis_index, _dual_axis_index)); return false; default: // driver responded, so check for other errors from the DRV_STATUS register @@ -123,12 +161,12 @@ namespace Motors { status.sr = tmcstepper->DRV_STATUS(); bool err = false; - // look for open loan or short 2 ground on a and b + // look for open or short to ground on a and b if (status.s2ga || status.s2gb) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Motor Short Coil a:%s b:%s", - _axis_name, + reportAxisNameMsg(_axis_index, _dual_axis_index), status.s2ga ? "Y" : "N", status.s2gb ? "Y" : "N"); err = true; @@ -138,7 +176,7 @@ namespace Motors { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Driver Temp Warning:%s Fault:%s", - _axis_name, + reportAxisNameMsg(_axis_index, _dual_axis_index), status.otpw ? "Y" : "N", status.ot ? "Y" : "N"); err = true; @@ -148,7 +186,8 @@ namespace Motors { return false; } - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test passed", _axis_name); + grbl_msg_sendf( + CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test passed", reportAxisNameMsg(_axis_index, _dual_axis_index)); return true; } } @@ -159,9 +198,11 @@ namespace Motors { uint16_t run (mA) float hold (as a percentage of run) */ + void TrinamicDriver::read_settings() { - if (has_errors) + if (_has_errors) { return; + } uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0); float hold_i_percent; @@ -173,15 +214,15 @@ namespace Motors { if (hold_i_percent > 1.0) hold_i_percent = 1.0; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Current run %d hold %f", _axis_name, run_i_ma, hold_i_percent); + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Current run %d hold %f", reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, hold_i_percent); tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); tmcstepper->rms_current(run_i_ma, hold_i_percent); } - void TrinamicDriver::set_homing_mode(uint8_t homing_mask, bool isHoming) { - _homing_mask = homing_mask; + bool TrinamicDriver::set_homing_mode(bool isHoming) { set_mode(isHoming); + return true; } /* @@ -190,19 +231,16 @@ namespace Motors { Coolstep mode, so it will need to switch to Coolstep when homing */ void TrinamicDriver::set_mode(bool isHoming) { - if (has_errors) { + if (_has_errors) { return; } - if (isHoming) { - _mode = TRINAMIC_HOMING_MODE; - } else { - _mode = TRINAMIC_RUN_MODE; - } - if (_lastMode == _mode) { + TrinamicMode newMode = isHoming ? TRINAMIC_HOMING_MODE : TRINAMIC_RUN_MODE; + + if (newMode == _mode) { return; } - _lastMode = _mode; + _mode = newMode; switch (_mode) { case TrinamicMode ::StealthChop: @@ -211,7 +249,7 @@ namespace Motors { tmcstepper->pwm_autoscale(true); tmcstepper->diag1_stall(false); break; - case TrinamicMode :: CoolStep: + case TrinamicMode ::CoolStep: //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); tmcstepper->en_pwm_mode(false); tmcstepper->pwm_autoscale(false); @@ -237,7 +275,7 @@ namespace Motors { This is the stallguard tuning info. It is call debug, so it could be generic across all classes. */ void TrinamicDriver::debug_message() { - if (has_errors) { + if (_has_errors) { return; } uint32_t tstep = tmcstepper->TSTEP(); @@ -250,7 +288,7 @@ namespace Motors { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Stallguard %d SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", - _axis_name, + reportAxisNameMsg(_axis_index, _dual_axis_index), tmcstepper->stallguard(), tmcstepper->sg_result(), feedrate, @@ -272,15 +310,17 @@ namespace Motors { // this can use the enable feature over SPI. The dedicated pin must be in the enable mode, // but that can be hardwired that way. void TrinamicDriver::set_disable(bool disable) { - if (has_errors) + if (_has_errors) { return; + } - if (_disabled == disable) + if (_disabled == disable) { return; + } _disabled = disable; - digitalWrite(disable_pin, _disabled); + digitalWrite(_disable_pin, _disabled); #ifdef USE_TRINAMIC_ENABLE if (_disabled) { @@ -296,4 +336,37 @@ namespace Motors { // the pin based enable could be added here. // This would be for individual motors, not the single pin for all motors. } + + // Prints StallGuard data that is useful for tuning. + void TrinamicDriver::readSgTask(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xreadSg = 200; // in ticks (typically ms) + auto n_axis = number_axis->get(); + + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. + while (true) { // don't ever return from this or the task dies + if (motorSettingChanged) { + motors_read_settings(); + motorSettingChanged = false; + } + if (stallguard_debug_mask->get() != 0) { + if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { + for (TrinamicDriver* p = List; p; p = p->link) { + if (bitnum_istrue(stallguard_debug_mask->get(), p->_axis_index)) { + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); + p->debug_message(); + } + } + } // sys.state + } // if mask + + // static UBaseType_t uxHighWaterMark = 0; + // if (uxHighWaterMark != uxTaskGetStackHighWaterMark(NULL)) { + // uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL); + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG Task Stack Space: %d", uxHighWaterMark); + // } + + vTaskDelayUntil(&xLastWakeTime, xreadSg); + } + } } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h index d7e7b681..004aa0dd 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h @@ -63,14 +63,25 @@ const double TRINAMIC_FCLK = 12700000.0; // Internal clock Approx (Hz) used to namespace Motors { enum class TrinamicMode : uint8_t { - None = 0, // not for machine defs! - StealthChop = 1, - CoolStep = 2, - StallGuard = 3, + None = 0, // not for machine defs! + StealthChop = 1, + CoolStep = 2, + StallGuard = 3, }; class TrinamicDriver : public StandardStepper { public: + TrinamicDriver(uint8_t axis_index, + uint8_t step_pin, + uint8_t dir_pin, + uint8_t disable_pin, + uint8_t cs_pin, + uint16_t driver_part_number, + float r_sense) : + TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, + cs_pin, driver_part_number, r_sense, get_next_index()) + {} + TrinamicDriver(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, @@ -80,29 +91,41 @@ namespace Motors { float r_sense, int8_t spi_index); - void config_message(); - void init(); - void set_mode(bool isHoming); - void read_settings(); - void trinamic_test_response(); - void trinamic_stepper_enable(bool enable); + // Overrides for inherited methods + void init() override; + void read_settings() override; + bool set_homing_mode(bool ishoming) override; + void set_disable(bool disable) override; + void debug_message(); - void set_homing_mode(uint8_t homing_mask, bool ishoming); - void set_disable(bool disable); - bool test(); private: uint32_t calc_tstep(float speed, float percent); TMC2130Stepper* tmcstepper; // all other driver types are subclasses of this one TrinamicMode _homing_mode; - uint8_t cs_pin = UNDEFINED_PIN; // The chip select pin (can be the same for daisy chain) - uint16_t _driver_part_number; // example: use 2130 for TMC2130 + uint8_t _cs_pin = UNDEFINED_PIN; // The chip select pin (can be the same for daisy chain) + uint16_t _driver_part_number; // example: use 2130 for TMC2130 float _r_sense; - int8_t spi_index; + int8_t _spi_index; + bool _has_errors; + bool _disabled; + + TrinamicMode _mode = TrinamicMode::None; + bool test(); + void set_mode(bool isHoming); + void trinamic_test_response(); + void trinamic_stepper_enable(bool enable); + + uint8_t get_next_index(); + + // Linked list of Trinamic driver instances, used by the + // StallGuard reporting task. + static TrinamicDriver* List; + TrinamicDriver* link; + static void readSgTask(void*); protected: - TrinamicMode _mode; - TrinamicMode _lastMode = TrinamicMode::None; + void config_message() override; }; } diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp index 68e45c3c..d8c844a4 100644 --- a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp +++ b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp @@ -1,23 +1,12 @@ #include "UnipolarMotor.h" namespace Motors { - UnipolarMotor::UnipolarMotor() {} + UnipolarMotor::UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3) : + Motor(axis_index), _pin_phase0(pin_phase0), _pin_phase1(pin_phase1), _pin_phase2(pin_phase2), + _pin_phase3(pin_phase3), - UnipolarMotor::UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3) { - type_id = UNIPOLAR_MOTOR; - this->_axis_index = axis_index % MAX_AXES; - this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1; // 0 = primary 1 = ganged - _pin_phase0 = pin_phase0; - _pin_phase1 = pin_phase1; - _pin_phase2 = pin_phase2; - _pin_phase3 = pin_phase3; - - _half_step = true; // TODO read from settings ... microstep > 1 = half step - - set_axis_name(); - init(); - config_message(); - } + _half_step(true) // TODO read from settings ... microstep > 1 = half step + {} void UnipolarMotor::init() { pinMode(_pin_phase0, OUTPUT); @@ -25,19 +14,19 @@ namespace Motors { pinMode(_pin_phase2, OUTPUT); pinMode(_pin_phase3, OUTPUT); _current_phase = 0; + config_message(); } void UnipolarMotor::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Axis Unipolar Stepper Ph0:%s Ph1:%s Ph2:%s Ph3:%s Limits(%0.3f,%0.3f)", - _axis_name, + "%s Unipolar Stepper Ph0:%s Ph1:%s Ph2:%s Ph3:%s %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), pinName(_pin_phase0).c_str(), pinName(_pin_phase1).c_str(), pinName(_pin_phase2).c_str(), pinName(_pin_phase3).c_str(), - _position_min, - _position_max); + reportAxisLimitsMsg(_axis_index)); } void UnipolarMotor::set_disable(bool disable) { @@ -50,31 +39,21 @@ namespace Motors { _enabled = !disable; } - void UnipolarMotor::step(uint8_t step_mask, uint8_t dir_mask) { + void UnipolarMotor::set_direction(bool dir) { _dir = dir; } + + void UnipolarMotor::step() { uint8_t _phase[8] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // temporary phase values...all start as off uint8_t phase_max; - if (!(step_mask & bit(_axis_index))) - return; // a step is not required on this interrupt - if (!_enabled) return; // don't do anything, phase is not changed or lost - if (_half_step) - phase_max = 7; - else - phase_max = 3; + phase_max = _half_step ? 7 : 3; - if (dir_mask & bit(_axis_index)) { // count up - if (_current_phase == phase_max) - _current_phase = 0; - else - _current_phase++; + if (_dir) { // count up + _current_phase = _current_phase == phase_max ? 0 : _current_phase + 1; } else { // count down - if (_current_phase == 0) - _current_phase = phase_max; - else - _current_phase--; + _current_phase = _current_phase == 0 ? phase_max : _current_phase - 1; } /* 8 Step : A – AB – B – BC – C – CD – D – DA diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.h b/Grbl_Esp32/src/Motors/UnipolarMotor.h index d9594815..ba554646 100644 --- a/Grbl_Esp32/src/Motors/UnipolarMotor.h +++ b/Grbl_Esp32/src/Motors/UnipolarMotor.h @@ -5,12 +5,14 @@ namespace Motors { class UnipolarMotor : public Motor { public: - UnipolarMotor(); UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3); - void init(); - void config_message(); - void set_disable(bool disable); - void step(uint8_t step_mask, uint8_t dir_mask); // only used on Unipolar right now + + // Overrides for inherited methods + void init() override; + bool set_homing_mode(bool isHoming) override { return true; } + void set_disable(bool disable) override; + void set_direction(bool) override; + void step() override; private: uint8_t _pin_phase0; @@ -20,5 +22,9 @@ namespace Motors { uint8_t _current_phase; bool _half_step; bool _enabled; + bool _dir; + + protected: + void config_message() override; }; } diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index 5661c178..5d6b0fb7 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -49,6 +49,9 @@ const int GANGED_MOTOR = 1; #define A2_AXIS (A_AXIS + MAX_AXES) #define B2_AXIS (B_AXIS + MAX_AXES) #define C2_AXIS (C_AXIS + MAX_AXES) +static inline int toMotor2(int axis) { + return axis + MAX_AXES; +} // CoreXY motor assignments. DO NOT ALTER. // NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. @@ -81,6 +84,8 @@ const int DELAY_MODE_SYS_SUSPEND = 1; #define bit_false(x, mask) (x) &= ~(mask) #define bit_istrue(x, mask) ((x & mask) != 0) #define bit_isfalse(x, mask) ((x & mask) == 0) +#define bitnum_true(x, num) (x) |= bit(num) +#define bitnum_istrue(x, num) ((x & bit(num)) != 0) // Read a floating point value from a string. Line points to the input buffer, char_counter // is the indexer pointing to the current character of the line, while float_ptr is diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 8f895b6c..532ab295 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -102,7 +102,7 @@ void grbl_sendf(uint8_t client, const char* format, ...) { len = vsnprintf(temp, len + 1, format, arg); grbl_send(client, temp); va_end(arg); - if (len > 64) { + if (temp != loc_buf) { delete[] temp; } } @@ -131,7 +131,7 @@ void grbl_msg_sendf(uint8_t client, MsgLevel level, const char* format, ...) { len = vsnprintf(temp, len + 1, format, arg); grbl_sendf(client, "[MSG:%s]\r\n", temp); va_end(arg); - if (len > 100) { + if (temp != loc_buf) { delete[] temp; } } @@ -161,29 +161,29 @@ void grbl_notifyf(const char* title, const char* format, ...) { len = vsnprintf(temp, len + 1, format, arg); grbl_notify(title, temp); va_end(arg); - if (len > 64) { + if (temp != loc_buf) { delete[] temp; } } static const int coordStringLen = 20; -static const int axesStringLen = coordStringLen * MAX_N_AXIS; +static const int axesStringLen = coordStringLen * MAX_N_AXIS; // formats axis values into a string and returns that string in rpt // NOTE: rpt should have at least size: axesStringLen static void report_util_axis_values(float* axis_value, char* rpt) { - uint8_t idx; - char axisVal[coordStringLen]; - float unit_conv = 1.0; // unit conversion multiplier..default is mm - const char* format = "%4.3f"; // Default - report mm to 3 decimal places - rpt[0] = '\0'; + uint8_t idx; + char axisVal[coordStringLen]; + float unit_conv = 1.0; // unit conversion multiplier..default is mm + const char* format = "%4.3f"; // Default - report mm to 3 decimal places + rpt[0] = '\0'; if (report_inches->get()) { unit_conv = 1.0 / MM_PER_INCH; - format = "%4.4f"; // Report inches to 4 decimal places + format = "%4.4f"; // Report inches to 4 decimal places } auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { - snprintf(axisVal, coordStringLen-1, format, axis_value[idx] * unit_conv); + snprintf(axisVal, coordStringLen - 1, format, axis_value[idx] * unit_conv); strcat(rpt, axisVal); if (idx < (number_axis->get() - 1)) { strcat(rpt, ","); @@ -193,14 +193,14 @@ static void report_util_axis_values(float* axis_value, char* rpt) { // This version returns the axis values as a String static String report_util_axis_values(const float* axis_value) { - String rpt = ""; + String rpt = ""; uint8_t idx; char axisVal[coordStringLen]; float unit_conv = 1.0; // unit conversion multiplier..default is mm - int decimals = 3; // Default - report mm to 3 decimal places + int decimals = 3; // Default - report mm to 3 decimal places if (report_inches->get()) { unit_conv = 1.0 / MM_PER_INCH; - decimals = 4; // Report inches to 4 decimal places + decimals = 4; // Report inches to 4 decimal places } auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { @@ -321,7 +321,7 @@ void report_probe_parameters(uint8_t client) { // Prints Grbl NGC parameters (coordinate offsets, probing) void report_ngc_parameters(uint8_t client) { - String ngc_rpt = ""; + String ngc_rpt = ""; // Print persistent offsets G54 - G59, G28, and G30 for (auto coord_select = CoordIndex::Begin; coord_select < CoordIndex::End; ++coord_select) { @@ -339,7 +339,8 @@ void report_ngc_parameters(uint8_t client) { if (report_inches->get()) { tlo *= INCH_PER_MM; } - ngc_rpt += String(tlo, 3);; + ngc_rpt += String(tlo, 3); + ; ngc_rpt += "]\r\n"; grbl_send(client, ngc_rpt.c_str()); report_probe_parameters(client); @@ -919,3 +920,21 @@ char report_get_axis_letter(uint8_t axis) { return '?'; } } + +char* reportAxisLimitsMsg(uint8_t axis) { + static char msg[40]; + sprintf(msg, "Limits(%0.3f,%0.3f)", limitsMinPosition(axis), limitsMaxPosition(axis)); + return msg; +} + +char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis) { + static char name[10]; + sprintf(name, "%c%c Axis", report_get_axis_letter(axis), dual_axis ? '2' : ' '); + return name; +} + +char* reportAxisNameMsg(uint8_t axis) { + static char name[10]; + sprintf(name, "%c Axis", report_get_axis_letter(axis)); + return name; +} diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index efe9ed3c..6b49ca93 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -123,3 +123,7 @@ void report_hex_msg(char* buf, const char* prefix, int len); void report_hex_msg(uint8_t* buf, const char* prefix, int len); char report_get_axis_letter(uint8_t axis); + +char* reportAxisLimitsMsg(uint8_t axis); +char* reportAxisNameMsg(uint8_t axis); +char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis); diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 469a65a6..8fa21906 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -60,7 +60,7 @@ static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; typedef struct { // Used by the bresenham line algorithm - uint32_t counter_x, counter_y, counter_z, counter_a, counter_b, counter_c; // Counter variables for the bresenham line tracer + uint32_t counter[MAX_N_AXIS]; // Counter variables for the bresenham line tracer #ifdef STEP_PULSE_DELAY uint8_t step_bits; // Stores out_bits output to complete the step pulse delay @@ -139,15 +139,6 @@ const char* stepper_names[] = { "I2S Steps, Static", }; -#ifndef DEFAULT_STEPPER -# if defined(USE_I2S_STEPS) -# define DEFAULT_STEPPER ST_I2S_STREAM -# elif defined(USE_RMT_STEPS) -# define DEFAULT_STEPPER ST_RMT -# else -# define DEFAULT_STEPPER ST_TIMED -# endif -#endif stepper_id_t current_stepper = DEFAULT_STEPPER; /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs @@ -205,9 +196,6 @@ stepper_id_t current_stepper = DEFAULT_STEPPER; */ -#ifdef USE_RMT_STEPS -inline IRAM_ATTR static void stepperRMT_Outputs(); -#endif static void stepper_pulse_func(); @@ -239,17 +227,15 @@ void IRAM_ATTR onStepperDriverTimer( static void stepper_pulse_func() { auto n_axis = number_axis->get(); - motors_set_direction_pins(st.dir_outbits); -#ifdef USE_RMT_STEPS - stepperRMT_Outputs(); -#else - set_stepper_pins_on(st.step_outbits); - uint64_t step_pulse_start_time = esp_timer_get_time(); -#endif - - // some motor objects, like unipolar, handle steps themselves motors_step(st.step_outbits, st.dir_outbits); + // If we are using GPIO stepping as opposed to RMT, record the + // time that we turned on the step pins so we can turn them off + // at the end of this routine without incurring another interrupt. + // This is unnecessary with RMT and I2S stepping since both of + // those methods time the turn off automatically. + uint64_t step_pulse_start_time = esp_timer_get_time(); + // If there is no step segment, attempt to pop one from the stepper buffer if (st.exec_segment == NULL) { // Anything in the buffer? If so, load and initialize next step segment. @@ -265,24 +251,17 @@ static void stepper_pulse_func() { st.exec_block_index = st.exec_segment->st_block_index; st.exec_block = &st_block_buffer[st.exec_block_index]; // Initialize Bresenham line and distance counters - st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1); + // XXX the original code only inits X, Y, Z here, instead of n_axis. Is that correct? + for (int axis = 0; axis < 3; axis++) { + st.counter[axis] = (st.exec_block->step_event_count >> 1); + } // TODO ABC } - st.dir_outbits = st.exec_block->direction_bits ^ dir_invert_mask->get(); + st.dir_outbits = st.exec_block->direction_bits; #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. - st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; - st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; - st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; - - if (n_axis > A_AXIS) { - st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.exec_segment->amass_level; - if (n_axis > B_AXIS) { - st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.exec_segment->amass_level; - if (n_axis > C_AXIS) { - st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level; - } - } + for (int axis = 0; axis < n_axis; axis++) { + st.steps[axis] = st.exec_block->steps[axis] >> st.exec_segment->amass_level; } #endif // Set real-time spindle output as segment is loaded, just prior to the first step. @@ -306,97 +285,17 @@ static void stepper_pulse_func() { } // Reset step out bits. st.step_outbits = 0; - // Execute step displacement profile by Bresenham line algorithm -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_x += st.steps[X_AXIS]; -#else - st.counter_x += st.exec_block->steps[X_AXIS]; -#endif - if (st.counter_x > st.exec_block->step_event_count) { - st.step_outbits |= bit(X_AXIS); - st.counter_x -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & bit(X_AXIS)) { - sys_position[X_AXIS]--; - } else { - sys_position[X_AXIS]++; - } - } -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_y += st.steps[Y_AXIS]; -#else - st.counter_y += st.exec_block->steps[Y_AXIS]; -#endif - if (st.counter_y > st.exec_block->step_event_count) { - st.step_outbits |= bit(Y_AXIS); - st.counter_y -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & bit(Y_AXIS)) { - sys_position[Y_AXIS]--; - } else { - sys_position[Y_AXIS]++; - } - } -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_z += st.steps[Z_AXIS]; -#else - st.counter_z += st.exec_block->steps[Z_AXIS]; -#endif - if (st.counter_z > st.exec_block->step_event_count) { - st.step_outbits |= bit(Z_AXIS); - st.counter_z -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & bit(Z_AXIS)) { - sys_position[Z_AXIS]--; - } else { - sys_position[Z_AXIS]++; - } - } - if (n_axis > A_AXIS) { -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_a += st.steps[A_AXIS]; -#else - st.counter_a += st.exec_block->steps[A_AXIS]; -#endif - if (st.counter_a > st.exec_block->step_event_count) { - st.step_outbits |= bit(A_AXIS); - st.counter_a -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & bit(A_AXIS)) { - sys_position[A_AXIS]--; + for (int axis = 0; axis < n_axis; axis++) { + // Execute step displacement profile by Bresenham line algorithm + st.counter[axis] += st.steps[axis]; + if (st.counter[axis] > st.exec_block->step_event_count) { + st.step_outbits |= bit(axis); + st.counter[axis] -= st.exec_block->step_event_count; + if (st.exec_block->direction_bits & bit(axis)) { + sys_position[axis]--; } else { - sys_position[A_AXIS]++; - } - } - - if (n_axis > B_AXIS) { -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_b += st.steps[B_AXIS]; -#else - st.counter_b += st.exec_block->steps[B_AXIS]; -#endif - if (st.counter_b > st.exec_block->step_event_count) { - st.step_outbits |= bit(B_AXIS); - st.counter_b -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & bit(B_AXIS)) { - sys_position[B_AXIS]--; - } else { - sys_position[B_AXIS]++; - } - } - - if (n_axis > C_AXIS) { -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_c += st.steps[C_AXIS]; -#else - st.counter_c += st.exec_block->steps[C_AXIS]; -#endif - if (st.counter_c > st.exec_block->step_event_count) { - st.step_outbits |= bit(C_AXIS); - st.counter_c -= st.exec_block->step_event_count; - if (st.exec_block->direction_bits & bit(C_AXIS)) { - sys_position[C_AXIS]--; - } else { - sys_position[C_AXIS]++; - } - } + sys_position[axis]++; } } } @@ -414,26 +313,23 @@ static void stepper_pulse_func() { } } -#ifdef USE_I2S_STEPS - if (current_stepper == ST_I2S_STREAM) { - // - // Generate pulse (at least one pulse) - // The pulse resolution is limited by I2S_OUT_USEC_PER_PULSE - // - i2s_out_push_sample(pulse_microseconds->get() / I2S_OUT_USEC_PER_PULSE); - set_stepper_pins_on(0); // turn all off - return; + switch (current_stepper) { + case ST_I2S_STREAM: + // Generate the number of pulses needed to span pulse_microseconds + i2s_out_push_sample(pulse_microseconds->get()); + motors_unstep(); + break; + case ST_I2S_STATIC: + case ST_TIMED: + // wait for step pulse time to complete...some time expired during code above + while (esp_timer_get_time() - step_pulse_start_time < pulse_microseconds->get()) { + NOP(); // spin here until time to turn off step + } + motors_unstep(); + break; + case ST_RMT: + break; } -#endif -#ifdef USE_RMT_STEPS - return; -#else - // wait for step pulse time to complete...some of it should have expired during code above - while (esp_timer_get_time() - step_pulse_start_time < pulse_microseconds->get()) { - NOP(); // spin here until time to turn off step - } - set_stepper_pins_on(0); // turn all off -#endif } void stepper_init() { @@ -506,202 +402,10 @@ void st_reset() { segment_next_head = 1; busy = false; st.step_outbits = 0; - st.dir_outbits = dir_invert_mask->get(); // Initialize direction bits to default. + st.dir_outbits = 0; // Initialize direction bits to default. // TODO do we need to turn step pins off? } -void set_stepper_pins_on(uint8_t onMask) { - onMask ^= step_invert_mask->get(); // invert pins as required by invert mask -#ifdef X_STEP_PIN -# ifndef X2_STEP_PIN // if not a ganged axis - digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS))); -# else // is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS))); - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - digitalWrite(X2_STEP_PIN, (onMask & bit(X_AXIS))); - } -# endif -#endif -#ifdef Y_STEP_PIN -# ifndef Y2_STEP_PIN // if not a ganged axis - digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS))); -# else // is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS))); - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - digitalWrite(Y2_STEP_PIN, (onMask & bit(Y_AXIS))); - } -# endif -#endif - -#ifdef Z_STEP_PIN -# ifndef Z2_STEP_PIN // if not a ganged axis - digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS))); -# else // is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS))); - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - digitalWrite(Z2_STEP_PIN, (onMask & bit(Z_AXIS))); - } -# endif -#endif - -#ifdef A_STEP_PIN -# ifndef A2_STEP_PIN // if not a ganged axis - digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS))); -# else // is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS))); - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - digitalWrite(A2_STEP_PIN, (onMask & bit(A_AXIS))); - } -# endif -#endif - -#ifdef B_STEP_PIN -# ifndef B2_STEP_PIN // if not a ganged axis - digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS))); -# else // is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS))); - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - digitalWrite(B2_STEP_PIN, (onMask & bit(B_AXIS))); - } -# endif -#endif - -#ifdef C_STEP_PIN -# ifndef C2_STEP_PIN // if not a ganged axis - digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS))); -# else // is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS))); - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - digitalWrite(C2_STEP_PIN, (onMask & bit(C_AXIS))); - } -# endif -#endif -} -//#endif - -#ifdef USE_RMT_STEPS -inline IRAM_ATTR static void stepperRMT_Outputs() { -# ifdef X_STEP_PIN - if (st.step_outbits & bit(X_AXIS)) { -# ifndef X2_STEP_PIN // if not a ganged axis - RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; -# else // it is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - RMT.conf_ch[rmt_chan_num[X_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[X_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; - } -# endif - } -# endif -# ifdef Y_STEP_PIN - if (st.step_outbits & bit(Y_AXIS)) { -# ifndef Y2_STEP_PIN // if not a ganged axis - RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; -# else // it is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - RMT.conf_ch[rmt_chan_num[Y_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[Y_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; - } -# endif - } -# endif - -# ifdef Z_STEP_PIN - if (st.step_outbits & bit(Z_AXIS)) { -# ifndef Z2_STEP_PIN // if not a ganged axis - RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; -# else // it is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - RMT.conf_ch[rmt_chan_num[Z_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[Z_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; - } -# endif - } -# endif - -# ifdef A_STEP_PIN - if (st.step_outbits & bit(A_AXIS)) { -# ifndef A2_STEP_PIN // if not a ganged axis - RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; -# else // it is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - RMT.conf_ch[rmt_chan_num[A_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[A_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; - } -# endif - } -# endif - -# ifdef B_STEP_PIN - if (st.step_outbits & bit(B_AXIS)) { -# ifndef Z2_STEP_PIN // if not a ganged axis - RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; -# else // it is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - RMT.conf_ch[rmt_chan_num[B_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[B_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; - } -# endif - } -# endif - -# ifdef C_STEP_PIN - if (st.step_outbits & bit(C_AXIS)) { -# ifndef Z2_STEP_PIN // if not a ganged axis - RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; -# else // it is a ganged axis - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) { - RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.tx_start = 1; - } - if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) { - RMT.conf_ch[rmt_chan_num[C_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1; - RMT.conf_ch[rmt_chan_num[C_AXIS][GANGED_MOTOR]].conf1.tx_start = 1; - } -# endif - } -# endif -} -#endif - // Stepper shutdown void st_go_idle() { // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active. @@ -725,7 +429,7 @@ void st_go_idle() { motors_set_disable(false); } - set_stepper_pins_on(0); + motors_unstep(); st.step_outbits = 0; } diff --git a/Grbl_Esp32/src/Stepper.h b/Grbl_Esp32/src/Stepper.h index b7f36265..bf5e5267 100644 --- a/Grbl_Esp32/src/Stepper.h +++ b/Grbl_Esp32/src/Stepper.h @@ -81,6 +81,17 @@ enum stepper_id_t { ST_I2S_STREAM, ST_I2S_STATIC, }; + +#ifndef DEFAULT_STEPPER +# if defined(USE_I2S_STEPS) +# define DEFAULT_STEPPER ST_I2S_STREAM +# elif defined(USE_RMT_STEPS) +# define DEFAULT_STEPPER ST_RMT +# else +# define DEFAULT_STEPPER ST_TIMED +# endif +#endif + extern const char* stepper_names[]; extern stepper_id_t current_stepper; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index ab5c7a62..7686a819 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -26,8 +26,8 @@ system_t sys; int32_t sys_position[MAX_N_AXIS]; // Real-time machine (aka home) position vector in steps. int32_t sys_probe_position[MAX_N_AXIS]; // Last probe position in machine coordinates and steps. volatile Probe sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. -volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. -volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. +volatile ExecState sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. +volatile ExecAlarm sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. volatile ExecAccessory sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. volatile bool cycle_stop; // For state transitions, instead of bitflag #ifdef DEBUG @@ -186,30 +186,6 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps) { return; } -// Checks and reports if target array exceeds machine travel limits. -// Return true if exceeding limits -uint8_t system_check_travel_limits(float* target) { - uint8_t idx; - auto n_axis = number_axis->get(); - for (idx = 0; idx < n_axis; idx++) { - float travel = axis_settings[idx]->max_travel->get(); - float mpos = axis_settings[idx]->home_mpos->get(); - float max_mpos, min_mpos; - - if (bit_istrue(homing_dir_mask->get(), bit(idx))) { - min_mpos = mpos; - max_mpos = mpos + travel; - } else { - min_mpos = mpos - travel; - max_mpos = mpos; - } - - if (target[idx] < min_mpos || target[idx] > max_mpos) - return true; - } - return false; -} - // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is // defined by the ControlPin in System.h. @@ -337,18 +313,6 @@ bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized) { return cmd_ok; } -// Call this function to get an RMT channel number -// returns -1 for error -int8_t sys_get_next_RMT_chan_num() { - static uint8_t next_RMT_chan_num = 0; // channels 0-7 are valid - if (next_RMT_chan_num < 8) { // 7 is the max PWM channel number - return next_RMT_chan_num++; - } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Error, "Error: out of RMT channels"); - return -1; - } -} - /* This returns an unused pwm channel. The 8 channels share 4 timers, so pairs 0,1 & 2,3 , etc diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 31b9b343..83e376e0 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -89,19 +89,19 @@ union SpindleStop { // Global system variables typedef struct { - volatile State state; // Tracks the current system state of Grbl. - bool abort; // System abort flag. Forces exit back to main loop for reset. - Suspend suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. - bool soft_limit; // Tracks soft limit errors for the state machine. (boolean) - StepControl step_control; // Governs the step segment generator depending on system state. - bool probe_succeeded; // Tracks if last probing cycle was successful. - AxisMask homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. - Percent f_override; // Feed rate override value in percent - Percent r_override; // Rapids override value in percent - Percent spindle_speed_ovr; // Spindle speed value in percent - SpindleStop spindle_stop_ovr; // Tracks spindle stop override states - Counter report_ovr_counter; // Tracks when to add override data to status reports. - Counter report_wco_counter; // Tracks when to add work coordinate offset data to status reports. + volatile State state; // Tracks the current system state of Grbl. + bool abort; // System abort flag. Forces exit back to main loop for reset. + Suspend suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. + bool soft_limit; // Tracks soft limit errors for the state machine. (boolean) + StepControl step_control; // Governs the step segment generator depending on system state. + bool probe_succeeded; // Tracks if last probing cycle was successful. + AxisMask homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. + Percent f_override; // Feed rate override value in percent + Percent r_override; // Rapids override value in percent + Percent spindle_speed_ovr; // Spindle speed value in percent + SpindleStop spindle_stop_ovr; // Tracks spindle stop override states + Counter report_ovr_counter; // Tracks when to add override data to status reports. + Counter report_wco_counter; // Tracks when to add work coordinate offset data to status reports. #ifdef ENABLE_PARKING_OVERRIDE_CONTROL Override override_ctrl; // Tracks override control states. #endif @@ -215,9 +215,6 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx); // Updates a machine 'position' array based on the 'step' array sent. void system_convert_array_steps_to_mpos(float* position, int32_t* steps); -// Checks and reports if target array exceeds machine travel limits. -uint8_t system_check_travel_limits(float* target); - int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps); int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps); @@ -228,6 +225,5 @@ void system_exec_control_pin(ControlPins pins); bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized); bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized); -int8_t sys_get_next_RMT_chan_num(); int8_t sys_get_next_PWM_chan_num(); uint8_t sys_calc_pwm_precision(uint32_t freq); From ab36c5a86d8250cc0e1c87b5ed8b1866cd26e387 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 16 Oct 2020 12:50:31 -1000 Subject: [PATCH 27/82] Cleaned up AMASS code (#635) * Cleaned up AMASS code More #defines gone 74 lines shorter Tested by comparing the result of original AMASS computation code to the new code with values surrounding all of the cutoff frequencies. * I2SOut tick calculation * Sorted out units for stepper pulse periods I tried to make it clear what the units are at different places in the code, and to use argument datatypes that clearly show the value range at different points, instead of relying on implicit type promotion. Hopefully this will make it easier to understand when, where, and why unit conversions occur. * Update Stepper.h * Deleted AMASS Config.h option ... as it is no longer optional --- Grbl_Esp32/src/Config.h | 7 --- Grbl_Esp32/src/I2SOut.cpp | 9 ++- Grbl_Esp32/src/I2SOut.h | 5 +- Grbl_Esp32/src/MachineCommon.h | 3 +- Grbl_Esp32/src/NutsBolts.h | 1 - Grbl_Esp32/src/Stepper.cpp | 102 +++++++++++---------------------- Grbl_Esp32/src/Stepper.h | 18 +++--- 7 files changed, 50 insertions(+), 95 deletions(-) diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 2de0ac50..658cade3 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -369,13 +369,6 @@ const int REPORT_WCO_REFRESH_IDLE_COUNT = 10; // (2-255) Must be less than or e // certain the step segment buffer is increased/decreased to account for these changes. const int ACCELERATION_TICKS_PER_SECOND = 100; -// Adaptive Multi-Axis Step Smoothing (AMASS) is an advanced feature that does what its name implies, -// smoothing the stepping of multi-axis motions. This feature smooths motion particularly at low step -// frequencies below 10kHz, where the aliasing between axes of multi-axis motions can cause audible -// noise and shake your machine. At even lower step frequencies, AMASS adapts and provides even better -// step smoothing. See Stepper.c for more details on the AMASS system works. -#define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Default enabled. Comment to disable. - // Sets the maximum step rate allowed to be written as a Grbl setting. This option enables an error // check in the settings module to prevent settings values that will exceed this limitation. The maximum // step rate is strictly limited by the CPU speed and will change if something other than an AVR running diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index f6b261b5..471731d0 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -116,8 +116,8 @@ static portMUX_TYPE i2s_out_spinlock = portMUX_INITIALIZER_UNLOCKED; static int i2s_out_initialized = 0; # ifdef USE_I2S_OUT_STREAM_IMPL -static volatile uint64_t i2s_out_pulse_period; -static uint64_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) +static volatile uint32_t i2s_out_pulse_period; +static uint32_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) static volatile i2s_out_pulse_func_t i2s_out_pulse_func; # endif @@ -657,10 +657,9 @@ int IRAM_ATTR i2s_out_set_stepping() { return 0; } -int IRAM_ATTR i2s_out_set_pulse_period(uint64_t period) { +int IRAM_ATTR i2s_out_set_pulse_period(uint32_t period) { # ifdef USE_I2S_OUT_STREAM_IMPL - // Use 64-bit values to avoid overflowing during the calculation. - i2s_out_pulse_period = period * 1000000 / F_STEPPER_TIMER; + i2s_out_pulse_period = period; # endif return 0; } diff --git a/Grbl_Esp32/src/I2SOut.h b/Grbl_Esp32/src/I2SOut.h index f672a278..5ae31c16 100644 --- a/Grbl_Esp32/src/I2SOut.h +++ b/Grbl_Esp32/src/I2SOut.h @@ -156,10 +156,9 @@ int i2s_out_set_stepping(); void i2s_out_delay(); /* - Set the pulse callback period in ISR ticks. - (same value of the timer period for the ISR) + Set the pulse callback period in microseconds */ -int i2s_out_set_pulse_period(uint64_t period); +int i2s_out_set_pulse_period(uint32_t usec); /* Register a callback function to generate pulse data diff --git a/Grbl_Esp32/src/MachineCommon.h b/Grbl_Esp32/src/MachineCommon.h index 627da376..95cea998 100644 --- a/Grbl_Esp32/src/MachineCommon.h +++ b/Grbl_Esp32/src/MachineCommon.h @@ -15,8 +15,7 @@ #endif // ESP32 CPU Settings -const int32_t F_TIMERS = 80000000; // a reference to the speed of ESP32 timers -#define F_STEPPER_TIMER 20000000 // frequency of step pulse timer +const uint32_t fTimers = 80000000; // a reference to the speed of ESP32 timers // =============== Don't change or comment these out ====================== // They are for legacy purposes and will not affect your I/O diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index 5d6b0fb7..185be0a7 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -61,7 +61,6 @@ static inline int toMotor2(int axis) { // Conversions const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); -#define TICKS_PER_MICROSECOND (F_STEPPER_TIMER / 1000000) // Different from AVR version const int DELAY_MODE_DWELL = 0; const int DELAY_MODE_SYS_SUSPEND = 1; diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 8fa21906..82c4a921 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -45,14 +45,10 @@ static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1]; // the planner, where the remaining planner block steps still can. typedef struct { uint16_t n_step; // Number of step events to be executed for this segment - uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. + uint16_t isrPeriod; // Time to next ISR tick, in units of timer ticks uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - uint8_t amass_level; // Indicates AMASS level for the ISR to execute this segment -#else - uint8_t prescaler; // Without AMASS, a prescaler is required to adjust for slow timing. -#endif - uint16_t spindle_rpm; // TODO get rid of this. + uint8_t amass_level; // AMASS level for the ISR to execute this segment + uint16_t spindle_rpm; // TODO get rid of this. } segment_t; static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; @@ -70,9 +66,7 @@ typedef struct { uint8_t step_pulse_time; // Step pulse reset time after step rise uint8_t step_outbits; // The next stepping-bits to be output uint8_t dir_outbits; -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING uint32_t steps[MAX_N_AXIS]; -#endif uint16_t step_count; // Steps remaining in line segment motion uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. @@ -94,7 +88,7 @@ static volatile uint8_t busy; static plan_block_t* pl_block; // Pointer to the planner block being prepped static st_block_t* st_prep_block; // Pointer to the stepper block data being prepped -// esp32 work around for diable in main loop +// esp32 work around for disable in main loop uint64_t stepper_idle_counter; // used to count down until time to disable stepper drivers bool stepper_idle; @@ -243,7 +237,7 @@ static void stepper_pulse_func() { // Initialize new step segment and load number of steps to execute st.exec_segment = &segment_buffer[segment_buffer_tail]; // Initialize step segment timing per step and load number of steps to execute. - Stepper_Timer_WritePeriod(st.exec_segment->cycles_per_tick); + Stepper_Timer_WritePeriod(st.exec_segment->isrPeriod); st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. // If the new segment starts a new planner block, initialize stepper variables and counters. // NOTE: When the segment data index changes, this indicates a new planner block. @@ -258,12 +252,10 @@ static void stepper_pulse_func() { // TODO ABC } st.dir_outbits = st.exec_block->direction_bits; -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. + // Adjust Bresenham axis increment counters according to AMASS level. for (int axis = 0; axis < n_axis; axis++) { st.steps[axis] = st.exec_block->steps[axis] >> st.exec_segment->amass_level; } -#endif // Set real-time spindle output as segment is loaded, just prior to the first step. spindle->set_rpm(st.exec_segment->spindle_rpm); } else { @@ -374,7 +366,7 @@ void st_wake_up() { // Step pulse delay handling is not require with ESP32...the RMT function does it. #else // Normal operation // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. - st.step_pulse_time = -(((pulse_microseconds->get() - 2) * TICKS_PER_MICROSECOND) >> 3); + st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3); #endif // Enable Stepper Driver Interrupt Stepper_Timer_Start(); @@ -538,20 +530,15 @@ void st_prep_buffer() { st_prep_block->direction_bits = pl_block->direction_bits; uint8_t idx; auto n_axis = number_axis->get(); -#ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - for (idx = 0; idx < n_axis; idx++) { - st_prep_block->steps[idx] = pl_block->steps[idx]; - } - st_prep_block->step_event_count = pl_block->step_event_count; -#else - // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS - // level, such that we never divide beyond the original data anywhere in the algorithm. + + // Bit-shift multiply all Bresenham data by the max AMASS level so that + // we never divide beyond the original data anywhere in the algorithm. // If the original data is divided, we can lose a step from integer roundoff. for (idx = 0; idx < n_axis; idx++) { - st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL; + st_prep_block->steps[idx] = pl_block->steps[idx] << maxAmassLevel; } - st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; -#endif + st_prep_block->step_event_count = pl_block->step_event_count << maxAmassLevel; + // Initialize segment buffer data for generating the segments. prep.steps_remaining = (float)pl_block->step_event_count; prep.step_per_mm = prep.steps_remaining / pl_block->millimeters; @@ -839,49 +826,28 @@ void st_prep_buffer() { // outputs the exact acceleration and velocity profiles as computed by the planner. dt += prep.dt_remainder; // Apply previous segment partial step execute time + // dt is in minutes so inv_rate is in minutes float inv_rate = dt / (last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse // Compute CPU cycles per step for the prepped segment. - uint32_t cycles = ceil((TICKS_PER_MICROSECOND * 1000000 * 60) * inv_rate); // (cycles/step) + // fStepperTimer is in units of timerTicks/sec, so the dimensional analysis is + // timerTicks/sec * 60 sec/minute * minutes = timerTicks + uint32_t timerTicks = ceil((fStepperTimer * 60) * inv_rate); // (timerTicks/step) + int level; -#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING // Compute step timing and multi-axis smoothing level. - // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required. - if (cycles < AMASS_LEVEL1) { - prep_segment->amass_level = 0; - } else { - if (cycles < AMASS_LEVEL2) { - prep_segment->amass_level = 1; - } else if (cycles < AMASS_LEVEL3) { - prep_segment->amass_level = 2; - } else { - prep_segment->amass_level = 3; + for (level = 0; level < maxAmassLevel; level++) { + if (timerTicks < amassThreshold) { + break; } - cycles >>= prep_segment->amass_level; - prep_segment->n_step <<= prep_segment->amass_level; + timerTicks >>= 1; } - if (cycles < (1UL << 16)) { - prep_segment->cycles_per_tick = cycles; // < 65536 (4.1ms @ 16MHz) - } else { - prep_segment->cycles_per_tick = 0xffff; // Just set the slowest speed possible. - } -#else - // Compute step timing and timer prescalar for normal step generation. - if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) - prep_segment->prescaler = 1; // prescaler: 0 - prep_segment->cycles_per_tick = cycles; - } else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) - prep_segment->prescaler = 2; // prescaler: 8 - prep_segment->cycles_per_tick = cycles >> 3; - } else { - prep_segment->prescaler = 3; // prescaler: 64 - if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) - prep_segment->cycles_per_tick = cycles >> 6; - } else { // Just set the slowest speed possible. (Around 4 step/sec.) - prep_segment->cycles_per_tick = 0xffff; - } - } -#endif + prep_segment->amass_level = level; + prep_segment->n_step <<= level; + // isrPeriod is stored as 16 bits, so limit timerTicks to the + // largest value that will fit in a uint16_t. + prep_segment->isrPeriod = timerTicks > 0xffff ? 0xffff : timerTicks; + // Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it. segment_buffer_head = segment_next_head; if (++segment_next_head == SEGMENT_BUFFER_SIZE) { @@ -935,21 +901,23 @@ float st_get_realtime_rate() { } } -void IRAM_ATTR Stepper_Timer_WritePeriod(uint64_t alarm_val) { +// The argument is in units of ticks of the timer that generates ISRs +void IRAM_ATTR Stepper_Timer_WritePeriod(uint16_t timerTicks) { if (current_stepper == ST_I2S_STREAM) { #ifdef USE_I2S_STEPS - // 1 tick = F_TIMERS / F_STEPPER_TIMER + // 1 tick = fTimers / fStepperTimer // Pulse ISR is called for each tick of alarm_val. - i2s_out_set_pulse_period(alarm_val); + // The argument to i2s_out_set_pulse_period is in units of microseconds + i2s_out_set_pulse_period(((uint32_t)timerTicks) / ticksPerMicrosecond); #endif } else { - timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, alarm_val); + timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, (uint64_t)timerTicks); } } void IRAM_ATTR Stepper_Timer_Init() { timer_config_t config; - config.divider = F_TIMERS / F_STEPPER_TIMER; + config.divider = fTimers / fStepperTimer; config.counter_dir = TIMER_COUNT_UP; config.counter_en = TIMER_PAUSE; config.alarm_en = TIMER_ALARM_EN; diff --git a/Grbl_Esp32/src/Stepper.h b/Grbl_Esp32/src/Stepper.h index bf5e5267..5bd04482 100644 --- a/Grbl_Esp32/src/Stepper.h +++ b/Grbl_Esp32/src/Stepper.h @@ -46,25 +46,23 @@ struct PrepFlag { uint8_t decelOverride : 1; }; +// fStepperTimer should be an integer divisor of the bus speed, i.e. of fTimers +const uint32_t fStepperTimer = 20000000; // frequency of step pulse timer +const int ticksPerMicrosecond = fStepperTimer / 1000000; + // Define Adaptive Multi-Axis Step-Smoothing(AMASS) levels and cutoff frequencies. The highest level // frequency bin starts at 0Hz and ends at its cutoff frequency. The next lower level frequency bin // starts at the next higher cutoff frequency, and so on. The cutoff frequencies for each level must // be considered carefully against how much it over-drives the stepper ISR, the accuracy of the 16-bit // timer, and the CPU overhead. Level 0 (no AMASS, normal operation) frequency bin starts at the // Level 1 cutoff frequency and up to as fast as the CPU allows (over 30kHz in limited testing). +// For efficient computation, each cutoff frequency is twice the previous one. // NOTE: AMASS cutoff frequency multiplied by ISR overdrive factor must not exceed maximum step frequency. // NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead // and timer accuracy. Do not alter these settings unless you know what you are doing. -///#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING -#define MAX_AMASS_LEVEL 3 -// AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. Starts at LEVEL1 cutoff frequency. -// Note ESP32 use F_STEPPER_TIMER rather than the AVR F_CPU -const int AMASS_LEVEL1 = (F_STEPPER_TIMER / 8000); // Over-drives ISR (x2). Defined as F_CPU/(Cutoff frequency in Hz) -const int AMASS_LEVEL2 = (F_STEPPER_TIMER / 4000); // Over-drives ISR (x4) -const int AMASS_LEVEL3 = (F_STEPPER_TIMER / 2000); // Over-drives ISR (x8) -static_assert(MAX_AMASS_LEVEL >= 1, "AMASS must have 1 or more levels to operate correctly."); -//#endif +const uint32_t amassThreshold = fStepperTimer / 8000; +const int maxAmassLevel = 3; // Each level increase doubles the threshold const timer_group_t STEP_TIMER_GROUP = TIMER_GROUP_0; const timer_idx_t STEP_TIMER_INDEX = TIMER_0; @@ -132,7 +130,7 @@ bool get_stepper_disable(); // returns the state of the pin void set_stepper_pins_on(uint8_t onMask); void set_direction_pins_on(uint8_t onMask); -void Stepper_Timer_WritePeriod(uint64_t alarm_val); +void Stepper_Timer_WritePeriod(uint16_t timerTicks); void Stepper_Timer_Init(); void Stepper_Timer_Start(); void Stepper_Timer_Stop(); From b72cd4c727a71c90fd0040fa7392c6616f8d8a81 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 16 Oct 2020 14:13:21 -1000 Subject: [PATCH 28/82] Use less memory (#644) a) closeFile() now does SD.end() to release memory after running a file from SD. b) Several task stacks are smaller c) All tasks now check their free space if DEBUG_REPORT_STACK_FREE is defined. platformio.ini has a commented-out line that can be uncommented to turn that on. d) Similarly, platformio.ini can turn on DEBUG_REPORT_HEAP_SIZE e) Fixed a small leak that occurred when listing local files. With these changes, the heap size tends to hover around 53K, dropping to about 37K when running a file from SD. --- Grbl_Esp32/src/I2SOut.cpp | 251 ++++++++++++----------- Grbl_Esp32/src/Limits.cpp | 13 +- Grbl_Esp32/src/Motors/Servo.cpp | 10 +- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 9 +- Grbl_Esp32/src/Report.cpp | 10 + Grbl_Esp32/src/Report.h | 2 + Grbl_Esp32/src/SDCard.cpp | 3 +- Grbl_Esp32/src/Serial.cpp | 36 +++- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 3 + Grbl_Esp32/src/System.cpp | 3 + Grbl_Esp32/src/System.h | 2 - Grbl_Esp32/src/WebUI/WebSettings.cpp | 52 +++-- platformio.ini | 2 + 13 files changed, 221 insertions(+), 175 deletions(-) diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index 471731d0..d41fed02 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -42,19 +42,27 @@ */ #include "Config.h" -# include -# include -# include -# include -# include +// This block of #includes is necessary for Report.h +#include "Error.h" +#include "WebUI/Authentication.h" +#include "WebUI/ESPResponse.h" +#include "Probe.h" +#include "System.h" +#include "Report.h" -# include +#include +#include +#include +#include +#include -# include "Pins.h" -# include "I2SOut.h" +#include + +#include "Pins.h" +#include "I2SOut.h" // Always enable I2S streaming logic -# define USE_I2S_OUT_STREAM_IMPL +#define USE_I2S_OUT_STREAM_IMPL // // Configrations for DMA connected I2S @@ -76,7 +84,7 @@ const int I2S_SAMPLE_SIZE = 4; /* 4 bytes, const int DMA_SAMPLE_COUNT = I2S_OUT_DMABUF_LEN / I2S_SAMPLE_SIZE; /* number of samples per buffer */ const int SAMPLE_SAFE_COUNT = (20 / I2S_OUT_USEC_PER_PULSE); /* prevent buffer overrun (GRBL's $0 should be less than or equal 20) */ -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL typedef struct { uint32_t** buffers; uint32_t* current; @@ -87,39 +95,39 @@ typedef struct { static i2s_out_dma_t o_dma; static intr_handle_t i2s_out_isr_handle; -# endif +#endif // output value static atomic_uint_least32_t i2s_out_port_data = ATOMIC_VAR_INIT(0); // inner lock static portMUX_TYPE i2s_out_spinlock = portMUX_INITIALIZER_UNLOCKED; -# define I2S_OUT_ENTER_CRITICAL() \ - do { \ - if (xPortInIsrContext()) { \ - portENTER_CRITICAL_ISR(&i2s_out_spinlock); \ - } else { \ - portENTER_CRITICAL(&i2s_out_spinlock); \ - } \ - } while (0) -# define I2S_OUT_EXIT_CRITICAL() \ - do { \ - if (xPortInIsrContext()) { \ - portEXIT_CRITICAL_ISR(&i2s_out_spinlock); \ - } else { \ - portEXIT_CRITICAL(&i2s_out_spinlock); \ - } \ - } while (0) -# define I2S_OUT_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_spinlock) -# define I2S_OUT_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_spinlock) +#define I2S_OUT_ENTER_CRITICAL() \ + do { \ + if (xPortInIsrContext()) { \ + portENTER_CRITICAL_ISR(&i2s_out_spinlock); \ + } else { \ + portENTER_CRITICAL(&i2s_out_spinlock); \ + } \ + } while (0) +#define I2S_OUT_EXIT_CRITICAL() \ + do { \ + if (xPortInIsrContext()) { \ + portEXIT_CRITICAL_ISR(&i2s_out_spinlock); \ + } else { \ + portEXIT_CRITICAL(&i2s_out_spinlock); \ + } \ + } while (0) +#define I2S_OUT_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_spinlock) +#define I2S_OUT_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_spinlock) static int i2s_out_initialized = 0; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL static volatile uint32_t i2s_out_pulse_period; static uint32_t i2s_out_remain_time_until_next_pulse; // Time remaining until the next pulse (μsec) static volatile i2s_out_pulse_func_t i2s_out_pulse_func; -# endif +#endif static uint8_t i2s_out_ws_pin = 255; static uint8_t i2s_out_bck_pin = 255; @@ -129,24 +137,24 @@ static volatile i2s_out_pulser_status_t i2s_out_pulser_status = PASSTHROUGH; // outer lock static portMUX_TYPE i2s_out_pulser_spinlock = portMUX_INITIALIZER_UNLOCKED; -# define I2S_OUT_PULSER_ENTER_CRITICAL() \ - do { \ - if (xPortInIsrContext()) { \ - portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock); \ - } else { \ - portENTER_CRITICAL(&i2s_out_pulser_spinlock); \ - } \ - } while (0) -# define I2S_OUT_PULSER_EXIT_CRITICAL() \ - do { \ - if (xPortInIsrContext()) { \ - portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock); \ - } else { \ - portEXIT_CRITICAL(&i2s_out_pulser_spinlock); \ - } \ - } while (0) -# define I2S_OUT_PULSER_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock) -# define I2S_OUT_PULSER_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock) +#define I2S_OUT_PULSER_ENTER_CRITICAL() \ + do { \ + if (xPortInIsrContext()) { \ + portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock); \ + } else { \ + portENTER_CRITICAL(&i2s_out_pulser_spinlock); \ + } \ + } while (0) +#define I2S_OUT_PULSER_EXIT_CRITICAL() \ + do { \ + if (xPortInIsrContext()) { \ + portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock); \ + } else { \ + portEXIT_CRITICAL(&i2s_out_pulser_spinlock); \ + } \ + } while (0) +#define I2S_OUT_PULSER_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock) +#define I2S_OUT_PULSER_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock) // // Internal functions @@ -161,13 +169,13 @@ static inline void gpio_matrix_out_check(uint8_t gpio, uint32_t signal_idx, bool } static inline void i2s_out_single_data() { -# if I2S_OUT_NUM_BITS == 16 +#if I2S_OUT_NUM_BITS == 16 uint32_t port_data = atomic_load(&i2s_out_port_data); port_data <<= 16; // Shift needed. This specification is not spelled out in the manual. I2S0.conf_single_data = port_data; // Apply port data in real-time (static I2S) -# else +#else I2S0.conf_single_data = atomic_load(&i2s_out_port_data); // Apply port data in real-time (static I2S) -# endif +#endif } static inline void i2s_out_reset_fifo_without_lock() { @@ -183,7 +191,7 @@ static void IRAM_ATTR i2s_out_reset_fifo() { I2S_OUT_EXIT_CRITICAL(); } -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL static int IRAM_ATTR i2s_clear_dma_buffer(lldesc_t* dma_desc, uint32_t port_data) { uint32_t* buf = (uint32_t*)dma_desc->buf; for (int i = 0; i < DMA_SAMPLE_COUNT; i++) { @@ -210,7 +218,7 @@ static int IRAM_ATTR i2s_clear_o_dma_buffers(uint32_t port_data) { } return 0; } -# endif +#endif static int IRAM_ATTR i2s_out_gpio_attach(uint8_t ws, uint8_t bck, uint8_t data) { // Route the i2s pins to the appropriate GPIO @@ -243,13 +251,13 @@ static int IRAM_ATTR i2s_out_gpio_shiftout(uint32_t port_data) { static int IRAM_ATTR i2s_out_stop() { I2S_OUT_ENTER_CRITICAL(); -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // Stop FIFO DMA I2S0.out_link.stop = 1; // Disconnect DMA from FIFO I2S0.fifo_conf.dscr_en = 0; //Unset this bit to disable I2S DMA mode. (R/W) -# endif +#endif // stop TX module I2S0.conf.tx_start = 0; @@ -269,10 +277,10 @@ static int IRAM_ATTR i2s_out_stop() { uint32_t port_data = atomic_load(&i2s_out_port_data); // current expanded port value i2s_out_gpio_shiftout(port_data); -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL //clear pending interrupt I2S0.int_clr.val = I2S0.int_st.val; -# endif +#endif I2S_OUT_EXIT_CRITICAL(); return 0; } @@ -296,7 +304,7 @@ static int IRAM_ATTR i2s_out_start() { I2S0.conf.rx_reset = 1; I2S0.conf.rx_reset = 0; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // reset DMA I2S0.lc_conf.in_rst = 1; I2S0.lc_conf.in_rst = 0; @@ -304,13 +312,13 @@ static int IRAM_ATTR i2s_out_start() { I2S0.lc_conf.out_rst = 0; I2S0.out_link.addr = (uint32_t)o_dma.desc[0]; -# endif +#endif // reset FIFO i2s_out_reset_fifo_without_lock(); // start DMA link -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL if (i2s_out_pulser_status == PASSTHROUGH) { I2S0.conf_chan.tx_chan_mod = 3; // 3:right+constant 4:left+constant (when tx_msb_right = 1) I2S0.conf_single_data = port_data; @@ -318,17 +326,17 @@ static int IRAM_ATTR i2s_out_start() { I2S0.conf_chan.tx_chan_mod = 4; // 3:right+constant 4:left+constant (when tx_msb_right = 1) I2S0.conf_single_data = 0; } -# endif +#endif I2S0.conf1.tx_stop_en = 1; // BCK and WCK are suppressed while FIFO is empty -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // Connect DMA to FIFO I2S0.fifo_conf.dscr_en = 1; // Set this bit to enable I2S DMA mode. (R/W) I2S0.int_clr.val = 0xFFFFFFFF; I2S0.out_link.start = 1; -# endif +#endif I2S0.conf.tx_start = 1; // Wait for the first FIFO data to prevent the unintentional generation of 0 data ets_delay_us(20); @@ -339,7 +347,7 @@ static int IRAM_ATTR i2s_out_start() { return 0; } -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // Fill out one DMA buffer // Call with the I2S_OUT_PULSER lock acquired. // Note that the lock is temporarily released while calling the callback function. @@ -516,15 +524,18 @@ static void IRAM_ATTR i2sOutTask(void* parameter) { o_dma.rw_pos = 0; // If someone calls i2s_out_push_sample, make sure there is no buffer overflow } I2S_OUT_PULSER_EXIT_CRITICAL(); // Unlock pulser status + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); } } -# endif +#endif // // External funtions // void IRAM_ATTR i2s_out_delay() { -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL I2S_OUT_PULSER_ENTER_CRITICAL(); if (i2s_out_pulser_status == PASSTHROUGH) { // Depending on the timing, it may not be reflected immediately, @@ -536,9 +547,9 @@ void IRAM_ATTR i2s_out_delay() { delay(I2S_OUT_DELAY_MS); } I2S_OUT_PULSER_EXIT_CRITICAL(); -# else +#else ets_delay_us(I2S_OUT_USEC_PER_PULSE * 2); -# endif +#endif } void IRAM_ATTR i2s_out_write(uint8_t pin, uint8_t val) { @@ -548,15 +559,15 @@ void IRAM_ATTR i2s_out_write(uint8_t pin, uint8_t val) { } else { atomic_fetch_and(&i2s_out_port_data, ~bit); } -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // It needs a lock for access, but I've given up because I need speed. // This is not a problem as long as there is no overlap between the status change and digitalWrite(). if (i2s_out_pulser_status == PASSTHROUGH) { i2s_out_single_data(); } -# else +#else i2s_out_single_data(); -# endif +#endif } uint8_t IRAM_ATTR i2s_out_read(uint8_t pin) { @@ -565,9 +576,9 @@ uint8_t IRAM_ATTR i2s_out_read(uint8_t pin) { } uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t usec) { - uint32_t num = usec/I2S_OUT_USEC_PER_PULSE; + uint32_t num = usec / I2S_OUT_USEC_PER_PULSE; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL if (num > SAMPLE_SAFE_COUNT) { return 0; } @@ -579,9 +590,9 @@ uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t usec) { n++; } while (n < num); return n; -# else +#else return 0; -# endif +#endif } i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status() { @@ -593,7 +604,7 @@ i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status() { int IRAM_ATTR i2s_out_set_passthrough() { I2S_OUT_PULSER_ENTER_CRITICAL(); -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // Triggers a change of mode if it is compiled to use I2S stream. // The mode is not changed directly by this function. // Pull the trigger @@ -604,17 +615,17 @@ int IRAM_ATTR i2s_out_set_passthrough() { // (i2sOutTask() -> stepper_pulse_func() -> st_go_idle() -> Stepper_Timer_Stop() -> this function) // And only i2sOutTask() can change the state to PASSTHROUGH. // So, to change the state, you need to return to i2sOutTask() as soon as possible. -# else +#else // If it is compiled to not use I2S streams, change the mode directly. i2s_out_pulser_status = PASSTHROUGH; -# endif +#endif I2S_OUT_PULSER_EXIT_CRITICAL(); return 0; } int IRAM_ATTR i2s_out_set_stepping() { I2S_OUT_PULSER_ENTER_CRITICAL(); -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL if (i2s_out_pulser_status == STEPPING) { // Re-entered (fail safe) I2S_OUT_PULSER_EXIT_CRITICAL(); @@ -650,31 +661,31 @@ int IRAM_ATTR i2s_out_set_stepping() { // because the process in i2s_out_start() is different depending on the status. i2s_out_pulser_status = STEPPING; i2s_out_start(); -# else +#else i2s_out_pulser_status = STEPPING; -# endif +#endif I2S_OUT_PULSER_EXIT_CRITICAL(); return 0; } int IRAM_ATTR i2s_out_set_pulse_period(uint32_t period) { -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL i2s_out_pulse_period = period; -# endif +#endif return 0; } int IRAM_ATTR i2s_out_set_pulse_callback(i2s_out_pulse_func_t func) { -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL i2s_out_pulse_func = func; -# endif +#endif return 0; } int IRAM_ATTR i2s_out_reset() { I2S_OUT_PULSER_ENTER_CRITICAL(); i2s_out_stop(); -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL if (i2s_out_pulser_status == STEPPING) { uint32_t port_data = atomic_load(&i2s_out_port_data); i2s_clear_o_dma_buffers(port_data); @@ -682,7 +693,7 @@ int IRAM_ATTR i2s_out_reset() { i2s_clear_o_dma_buffers(0); i2s_out_pulser_status = PASSTHROUGH; } -# endif +#endif // You need to set the status before calling i2s_out_start() // because the process in i2s_out_start() is different depending on the status. i2s_out_start(); @@ -733,7 +744,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { * M = 2 */ -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // Allocate the array of pointers to the buffers o_dma.buffers = (uint32_t**)malloc(sizeof(uint32_t*) * I2S_OUT_DMABUF_COUNT); if (o_dma.buffers == nullptr) { @@ -770,7 +781,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { // Set the first DMA descriptor I2S0.out_link.addr = (uint32_t)o_dma.desc[0]; -# endif +#endif // stop i2s I2S0.out_link.stop = 1; @@ -806,9 +817,9 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { I2S0.lc_conf.outdscr_burst_en = 0; I2S0.lc_conf.out_no_restart_clr = 0; I2S0.lc_conf.indscr_burst_en = 0; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL I2S0.lc_conf.out_eof_mode = 1; // I2S_OUT_EOF_INT generated when DMA has popped all data from the FIFO; -# endif +#endif I2S0.conf2.lcd_en = 0; I2S0.conf2.camera_en = 0; I2S0.pdm_conf.pcm2pdm_conv_en = 0; @@ -816,7 +827,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { I2S0.fifo_conf.dscr_en = 0; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL if (i2s_out_pulser_status == STEPPING) { // Stream output mode I2S0.conf_chan.tx_chan_mod = 4; // 3:right+constant 4:left+constant (when tx_msb_right = 1) @@ -826,31 +837,31 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { I2S0.conf_chan.tx_chan_mod = 3; // 3:right+constant 4:left+constant (when tx_msb_right = 1) I2S0.conf_single_data = init_param.init_val; } -# else +#else // For the static output mode I2S0.conf_chan.tx_chan_mod = 3; // 3:right+constant 4:left+constant (when tx_msb_right = 1) I2S0.conf_single_data = init_param.init_val; // initial constant value -# endif -# if I2S_OUT_NUM_BITS == 16 +#endif +#if I2S_OUT_NUM_BITS == 16 I2S0.fifo_conf.tx_fifo_mod = 0; // 0: 16-bit dual channel data, 3: 32-bit single channel data I2S0.fifo_conf.rx_fifo_mod = 0; // 0: 16-bit dual channel data, 3: 32-bit single channel data I2S0.sample_rate_conf.tx_bits_mod = 16; // default is 16-bits I2S0.sample_rate_conf.rx_bits_mod = 16; // default is 16-bits -# else +#else I2S0.fifo_conf.tx_fifo_mod = 3; // 0: 16-bit dual channel data, 3: 32-bit single channel data I2S0.fifo_conf.rx_fifo_mod = 3; // 0: 16-bit dual channel data, 3: 32-bit single channel data // Data width is 32-bit. Forgetting this setting will result in a 16-bit transfer. I2S0.sample_rate_conf.tx_bits_mod = 32; I2S0.sample_rate_conf.rx_bits_mod = 32; -# endif +#endif I2S0.conf.tx_mono = 0; // Set this bit to enable transmitter’s mono mode in PCM standard mode. I2S0.conf_chan.rx_chan_mod = 1; // 1: right+right I2S0.conf.rx_mono = 0; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL I2S0.fifo_conf.dscr_en = 1; //connect DMA to fifo -# endif +#endif I2S0.conf.tx_start = 0; I2S0.conf.rx_start = 0; @@ -858,9 +869,9 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { I2S0.conf.tx_right_first = 0; // Setting this bit allows the right-channel data to be sent first. I2S0.conf.tx_slave_mod = 0; // Master -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL I2S0.fifo_conf.tx_fifo_mod_force_en = 1; //The bit should always be set to 1. -# endif +#endif I2S0.pdm_conf.rx_pdm_en = 0; // Set this bit to enable receiver’s PDM mode. I2S0.pdm_conf.tx_pdm_en = 0; // Set this bit to enable transmitter’s PDM mode. @@ -877,13 +888,13 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { // set clock (fi2s) 160MHz / 5 I2S0.clkm_conf.clka_en = 0; // Use 160 MHz PLL_D2_CLK as reference // N + b/a = 0 -# if I2S_OUT_NUM_BITS == 16 +#if I2S_OUT_NUM_BITS == 16 // N = 10 I2S0.clkm_conf.clkm_div_num = 10; // minimum value of 2, reset value of 4, max 256 (I²S clock divider’s integral value) -# else +#else // N = 5 I2S0.clkm_conf.clkm_div_num = 5; // minimum value of 2, reset value of 4, max 256 (I²S clock divider’s integral value) -# endif +#endif // b/a = 0 I2S0.clkm_conf.clkm_div_b = 0; // 0 at reset I2S0.clkm_conf.clkm_div_a = 0; // 0 at reset, what about divide by 0? (not an issue) @@ -893,7 +904,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { I2S0.sample_rate_conf.tx_bck_div_num = 2; // minimum value of 2 defaults to 6 I2S0.sample_rate_conf.rx_bck_div_num = 2; -# ifdef USE_I2S_OUT_STREAM_IMPL +#ifdef USE_I2S_OUT_STREAM_IMPL // Enable TX interrupts (DMA Interrupts) I2S0.int_ena.out_eof = 1; // Triggered when rxlink has finished sending a packet. I2S0.int_ena.out_dscr_err = 0; // Triggered when invalid rxlink descriptors are encountered. @@ -907,7 +918,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { // Create the task that will feed the buffer xTaskCreatePinnedToCore(i2sOutTask, "I2SOutTask", - 1024 * 10, + 4096, NULL, 1, nullptr, @@ -917,7 +928,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { // Allocate and Enable the I2S interrupt esp_intr_alloc(ETS_I2S0_INTR_SOURCE, 0, i2s_out_intr_handler, nullptr, &i2s_out_isr_handle); esp_intr_enable(i2s_out_isr_handle); -# endif +#endif // Remember GPIO pin numbers i2s_out_ws_pin = init_param.ws_pin; @@ -931,18 +942,18 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) { return 0; } -# ifndef I2S_OUT_WS -# define I2S_OUT_WS GPIO_NUM_17 -# endif -# ifndef I2S_OUT_BCK -# define I2S_OUT_BCK GPIO_NUM_22 -# endif -# ifndef I2S_OUT_DATA -# define I2S_OUT_DATA GPIO_NUM_21 -# endif -# ifndef I2S_OUT_INIT_VAL -# define I2S_OUT_INIT_VAL 0 -# endif +#ifndef I2S_OUT_WS +# define I2S_OUT_WS GPIO_NUM_17 +#endif +#ifndef I2S_OUT_BCK +# define I2S_OUT_BCK GPIO_NUM_22 +#endif +#ifndef I2S_OUT_DATA +# define I2S_OUT_DATA GPIO_NUM_21 +#endif +#ifndef I2S_OUT_INIT_VAL +# define I2S_OUT_INIT_VAL 0 +#endif /* Initialize I2S out by default parameters. diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index bbab9989..2aed18b7 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -330,11 +330,8 @@ void limits_init() { } if (limit_sw_queue == NULL) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "%s limit switch on pin %s", - reportAxisNameMsg(axis, gang_index), - pinName(pin).c_str()); + grbl_msg_sendf( + CLIENT_SERIAL, MsgLevel::Info, "%s limit switch on pin %s", reportAxisNameMsg(axis, gang_index), pinName(pin).c_str()); } } } @@ -427,17 +424,19 @@ void limitCheckTask(void* pvParameters) { mc_reset(); // Initiate system kill. sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event } + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); } } float limitsMaxPosition(uint8_t axis) { - float mpos = axis_settings[axis]->home_mpos->get(); + float mpos = axis_settings[axis]->home_mpos->get(); return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos + axis_settings[axis]->max_travel->get() : mpos; } float limitsMinPosition(uint8_t axis) { - float mpos = axis_settings[axis]->home_mpos->get(); + float mpos = axis_settings[axis]->home_mpos->get(); return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos : mpos - axis_settings[axis]->max_travel->get(); } diff --git a/Grbl_Esp32/src/Motors/Servo.cpp b/Grbl_Esp32/src/Motors/Servo.cpp index 1e0fa2b7..65e213a1 100644 --- a/Grbl_Esp32/src/Motors/Servo.cpp +++ b/Grbl_Esp32/src/Motors/Servo.cpp @@ -60,18 +60,14 @@ namespace Motors { xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. vTaskDelay(2000); // initial delay while (true) { // don't ever return from this or the task dies - - // static UBaseType_t uxHighWaterMark = 0; - // if (uxHighWaterMark != uxTaskGetStackHighWaterMark(NULL)) { - // uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL); - // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Servo Task Min Stack Space: %d", uxHighWaterMark); - // } - for (Servo* p = List; p; p = p->link) { p->update(); } vTaskDelayUntil(&xLastWakeTime, xUpdate); + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); } } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index c100b1de..d2c6e595 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -360,13 +360,10 @@ namespace Motors { } // sys.state } // if mask - // static UBaseType_t uxHighWaterMark = 0; - // if (uxHighWaterMark != uxTaskGetStackHighWaterMark(NULL)) { - // uxHighWaterMark = uxTaskGetStackHighWaterMark(NULL); - // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG Task Stack Space: %d", uxHighWaterMark); - // } - vTaskDelayUntil(&xLastWakeTime, xreadSg); + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); } } } diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 532ab295..27ba9ea8 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -938,3 +938,13 @@ char* reportAxisNameMsg(uint8_t axis) { sprintf(name, "%c Axis", report_get_axis_letter(axis)); return name; } + +void reportTaskStackSize(UBaseType_t& saved) { +#ifdef DEBUG_REPORT_STACK_FREE + UBaseType_t newHighWater = uxTaskGetStackHighWaterMark(NULL); + if (newHighWater != saved) { + saved = newHighWater; + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Min Stack Space: %d", pcTaskGetTaskName(NULL), saved); + } +#endif +} diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index 6b49ca93..5b128b5d 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -127,3 +127,5 @@ char report_get_axis_letter(uint8_t axis); char* reportAxisLimitsMsg(uint8_t axis); char* reportAxisNameMsg(uint8_t axis); char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis); + +void reportTaskStackSize(UBaseType_t& saved); diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index f675c870..c2d8d3c1 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -81,6 +81,7 @@ boolean closeFile() { SD_ready_next = false; sd_current_line_number = 0; myFile.close(); + SD.end(); return true; } @@ -147,7 +148,7 @@ uint8_t get_sd_state(bool refresh) { sd_state = SDCARD_NOT_PRESENT; //using default value for speed ? should be parameter //refresh content if card was removed - if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ)) { + if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ, "/sd", 2)) { if (SD.cardSize() > 0) { sd_state = SDCARD_IDLE; } diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index f3c75676..caa78bb6 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -68,7 +68,27 @@ uint8_t serial_get_rx_buffer_available(uint8_t client) { return client_buffer[client].availableforwrite(); } +void heapCheckTask(void* pvParameters) { + static uint32_t heapSize = 0; + while (true) { + uint32_t newHeapSize = xPortGetFreeHeapSize(); + if (newHeapSize != heapSize) { + heapSize = newHeapSize; + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "heap %d", heapSize); + } + vTaskDelay(3000 / portTICK_RATE_MS); // Yield to other tasks + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); + } +} + void serial_init() { +#ifdef DEBUG_REPORT_HEAP_SIZE + // For a 2000-word stack, uxTaskGetStackHighWaterMark reports 288 words available + xTaskCreatePinnedToCore(heapCheckTask, "heapTask", 2000, NULL, 1, NULL, 1); +#endif + Serial.begin(BAUD_RATE); Serial.setRxBufferSize(256); // reset all buffers @@ -76,9 +96,11 @@ void serial_init() { grbl_send(CLIENT_SERIAL, "\r\n"); // create some white space after ESP32 boot info serialCheckTaskHandle = 0; // create a task to check for incoming data + // For a 4096-word stack, uxTaskGetStackHighWaterMark reports 244 words available + // after WebUI attaches. xTaskCreatePinnedToCore(serialCheckTask, // task "serialCheckTask", // name for task - 8192, // size of task stack + 4096, // size of task stack NULL, // parameters 1, // priority &serialCheckTaskHandle, @@ -89,9 +111,10 @@ void serial_init() { // this task runs and checks for data on all interfaces // REaltime stuff is acted upon, then characters are added to the appropriate buffer void serialCheckTask(void* pvParameters) { - uint8_t data = 0; - uint8_t client = CLIENT_ALL; // who sent the data - while (true) { // run continuously + uint8_t data = 0; + uint8_t client = CLIENT_ALL; // who sent the data + static UBaseType_t uxHighWaterMark = 0; + while (true) { // run continuously while (any_client_has_data()) { if (Serial.available()) { client = CLIENT_SERIAL; @@ -149,7 +172,10 @@ void serialCheckTask(void* pvParameters) { WebUI::Serial2Socket.handle_flush(); #endif vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks - } // while(true) + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); + } // while(true) } void serial_reset_read_buffer(uint8_t client) { diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index a5e75fcb..3a1305a6 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -185,6 +185,9 @@ namespace Spindles { // Wait a bit before we retry. Set the delay to poll-rate. Not sure // if we should use a different value... vTaskDelay(VFD_RS485_POLL_RATE); + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); } } diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 7686a819..d8d18d00 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -123,6 +123,9 @@ void controlCheckTask(void* pvParameters) { system_exec_control_pin(pins); } debouncing = false; + + static UBaseType_t uxHighWaterMark = 0; + reportTaskStackSize(uxHighWaterMark); } } #endif diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 83e376e0..e1ea051a 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -20,8 +20,6 @@ along with Grbl. If not, see . */ -#include "Grbl.h" - // System states. The state variable primarily tracks the individual functions // of Grbl to manage each without overlapping. It is also used as a messaging flag for // critical events. diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 6bd60972..09539a77 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -600,9 +600,9 @@ namespace WebUI { #ifdef ENABLE_WIFI static Error listAPs(char* parameter, AuthenticationLevel auth_level) { // ESP410 - JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI); - j->begin(); - j->begin_array("AP_LIST"); + JSONencoder j(espresponse->client() != CLIENT_WEBUI); + j.begin(); + j.begin_array("AP_LIST"); // An initial async scanNetworks was issued at startup, so there // is a good chance that scan information is already available. int n = WiFi.scanComplete(); @@ -614,12 +614,12 @@ namespace WebUI { break; default: for (int i = 0; i < n; ++i) { - j->begin_object(); - j->member("SSID", WiFi.SSID(i)); - j->member("SIGNAL", wifi_config.getSignal(WiFi.RSSI(i))); - j->member("IS_PROTECTED", WiFi.encryptionType(i) != WIFI_AUTH_OPEN); + j.begin_object(); + j.member("SSID", WiFi.SSID(i)); + j.member("SIGNAL", wifi_config.getSignal(WiFi.RSSI(i))); + j.member("IS_PROTECTED", WiFi.encryptionType(i) != WIFI_AUTH_OPEN); // j->member("IS_PROTECTED", WiFi.encryptionType(i) == WIFI_AUTH_OPEN ? "0" : "1"); - j->end_object(); + j.end_object(); } WiFi.scanDelete(); // Restart the scan in async mode so new data will be available @@ -630,9 +630,8 @@ namespace WebUI { } break; } - j->end_array(); - webPrint(j->end()); - delete j; + j.end_array(); + webPrint(j.end()); if (espresponse->client() != CLIENT_WEBUI) { espresponse->println(""); } @@ -657,17 +656,16 @@ namespace WebUI { } static Error listSettings(char* parameter, AuthenticationLevel auth_level) { // ESP400 - JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI); - j->begin(); - j->begin_array("EEPROM"); + JSONencoder j(espresponse->client() != CLIENT_WEBUI); + j.begin(); + j.begin_array("EEPROM"); for (Setting* js = Setting::List; js; js = js->next()) { if (js->getType() == WEBSET) { - js->addWebui(j); + js->addWebui(&j); } } - j->end_array(); - webPrint(j->end()); - delete j; + j.end_array(); + webPrint(j.end()); return Error::Ok; } @@ -833,15 +831,15 @@ namespace WebUI { } static Error listLocalFilesJSON(char* parameter, AuthenticationLevel auth_level) { // No ESP command - JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI); - j->begin(); - j->begin_array("files"); - listDirJSON(SPIFFS, "/", 4, j); - j->end_array(); - j->member("total", SPIFFS.totalBytes()); - j->member("used", SPIFFS.usedBytes()); - j->member("occupation", String(100 * SPIFFS.usedBytes() / SPIFFS.totalBytes())); - webPrint(j->end()); + JSONencoder j(espresponse->client() != CLIENT_WEBUI); + j.begin(); + j.begin_array("files"); + listDirJSON(SPIFFS, "/", 4, &j); + j.end_array(); + j.member("total", SPIFFS.totalBytes()); + j.member("used", SPIFFS.usedBytes()); + j.member("occupation", String(100 * SPIFFS.usedBytes() / SPIFFS.totalBytes())); + webPrint(j.end()); if (espresponse->client() != CLIENT_WEBUI) { webPrintln(""); } diff --git a/platformio.ini b/platformio.ini index 11ad3c32..cdd2e8a0 100644 --- a/platformio.ini +++ b/platformio.ini @@ -28,6 +28,8 @@ build_flags = -DCORE_DEBUG_LEVEL=0 -Wno-unused-variable -Wno-unused-function + ;-DDEBUG_REPORT_HEAP_SIZE + ;-DDEBUG_REPORT_STACK_FREE [env] lib_deps = From 8c01797c3b283a1a7561cf7d9634827f8ed4ea89 Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 22 Oct 2020 19:19:56 -0500 Subject: [PATCH 29/82] Add coolant pin messages to startup (#647) * Add coolant pin messages to startup Help with user support. * Removing incorrect STEPPER_RESET definition * Fix laser mode startup message * cleanup - coolant_init() will behave as before - update build date - return default machine to test_drive --- Grbl_Esp32/src/CoolantControl.cpp | 13 +++++++++++++ Grbl_Esp32/src/Grbl.h | 2 +- .../src/Machines/6_pack_Lowrider_stepstick_v1.h | 2 -- Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h | 2 -- Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h | 3 --- Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h | 2 -- Grbl_Esp32/src/Spindles/Laser.cpp | 4 ++-- 7 files changed, 16 insertions(+), 12 deletions(-) diff --git a/Grbl_Esp32/src/CoolantControl.cpp b/Grbl_Esp32/src/CoolantControl.cpp index ca14a8f1..6e823c77 100644 --- a/Grbl_Esp32/src/CoolantControl.cpp +++ b/Grbl_Esp32/src/CoolantControl.cpp @@ -24,12 +24,25 @@ #include "Grbl.h" void coolant_init() { + static bool init_message = true; // used to show messages only once. + #ifdef COOLANT_FLOOD_PIN pinMode(COOLANT_FLOOD_PIN, OUTPUT); #endif #ifdef COOLANT_MIST_PIN pinMode(COOLANT_MIST_PIN, OUTPUT); #endif + + if (init_message) { +#ifdef COOLANT_FLOOD_PIN + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Flood coolant on pin %s", pinName(COOLANT_FLOOD_PIN).c_str()); +#endif +#ifdef COOLANT_MIST_PIN + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Mist coolant on pin %s", pinName(COOLANT_MIST_PIN).c_str()); +#endif + init_message = false; + } + coolant_stop(); } diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index a3106010..b2e9c2d8 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 = "20201015"; +const char* const GRBL_VERSION_BUILD = "20201022"; //#include #include diff --git a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h index 0107cd94..9e935607 100644 --- a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h @@ -45,8 +45,6 @@ #define X2_STEPPER_MS3 I2SO(14) // A_CS #define Y2_STEPPER_MS3 I2SO(19) // B_CS -#define STEPPER_RESET GPIO_NUM_19 - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) diff --git a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h index d1f36e54..034dade2 100644 --- a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h @@ -45,8 +45,6 @@ #define X2_STEPPER_MS3 I2SO(14) // A_CS #define Y2_STEPPER_MS3 I2SO(19) // B_CS -#define STEPPER_RESET GPIO_NUM_19 - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index ea00a45e..28ab79db 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -45,8 +45,6 @@ #define Y_STEPPER_MS3 I2SO(6) // Y_CS #define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define STEPPER_RESET GPIO_NUM_19 - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) @@ -122,7 +120,6 @@ Socket #5 // #define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_27 // //#define INVERT_CONTROL_PIN_MASK B0000 - // ================= Setting Defaults ========================== #define DEFAULT_X_STEPS_PER_MM 800 #define DEFAULT_Y_STEPS_PER_MM 800 diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h index d08faf2b..caf673be 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h @@ -47,8 +47,6 @@ #define B_STEPPER_MS3 I2SO(19) // B_CS #define C_STEPPER_MS3 I2SO(22) // C_CS -#define STEPPER_RESET GPIO_NUM_19 - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp index 4500600d..b9fa0c72 100644 --- a/Grbl_Esp32/src/Spindles/Laser.cpp +++ b/Grbl_Esp32/src/Spindles/Laser.cpp @@ -31,11 +31,11 @@ namespace Spindles { void Laser::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "Laser spindle on Pin:%s, Freq:%.2fHz, Res:%dbits Laser mode:$32=%d", + "Laser spindle on Pin:%s, Freq:%dHz, Res:%dbits Laser mode:%s", pinName(_output_pin).c_str(), _pwm_freq, _pwm_precision, - isRateAdjusted()); // the current mode + laser_mode->getStringValue()); // the current mode use_delays = false; // this will override the value set in Spindle::PWM::init() } From 13f6b37031048df73a904d36361b120a0f600d01 Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 28 Oct 2020 09:21:06 -0500 Subject: [PATCH 30/82] Move CoreXY out of main Grbl (#653) * Created branch * WIP * Update parallel_delta.cpp * Wip * WIP * Wip * Still working on kinematics - Added an interface into the jogging section * WIP * WIP * wip * WIP * WIP * Wip * WIP * WIP * Wip * Update machine defs * Created branch * WIP * Update parallel_delta.cpp * Wip * WIP * Wip * Still working on kinematics - Added an interface into the jogging section * WIP * WIP * wip * WIP * WIP * Wip * WIP * WIP * Wip * Update machine defs * Machine def change. Moved switches to module 1 * WIP * Cleanup before P.R. - Fixed ranges for delta geometry - Added post homing delay option for servos - renamed and removed old machine defs. * Fixing initialization problem when not in USE_KINEMATICS mode * Fixing Git Mess * Publishing Branch - Not ready yet. Issues with Z axis - Need to add midTbot option * WIP - Seems to be fully functional now. - Need to add midTbot option. * Update CoreXY.cpp * I think it is ready for PR - fixed $RST=# - added midTbot geometry factor * Fine tune midtbot definition * Removed more unneeded corexy code. * Fixed doubled #define in machine def file. * Update after review comments --- Grbl_Esp32/Custom/CoreXY.cpp | 307 ++++++++++++++++++ Grbl_Esp32/Custom/atari_1020.cpp | 2 +- Grbl_Esp32/Custom/custom_code_template.cpp | 8 +- .../Custom/esp32_printer_controller.cpp | 4 +- Grbl_Esp32/Custom/parallel_delta.cpp | 213 +++++++++--- Grbl_Esp32/src/Config.h | 9 +- Grbl_Esp32/src/Defaults.h | 24 ++ Grbl_Esp32/src/GCode.cpp | 4 +- Grbl_Esp32/src/Grbl.h | 13 +- Grbl_Esp32/src/Jog.cpp | 7 +- Grbl_Esp32/src/Limits.cpp | 55 +--- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 1 - Grbl_Esp32/src/Machines/midtbot.h | 34 +- .../src/Machines/{tapster3.h => tapster_3.h} | 51 +-- .../src/Machines/tapster_pro_6P_trinamic.h | 197 +++++++++++ .../src/Machines/tapster_pro_stepstick.h | 155 +++++++++ Grbl_Esp32/src/Machines/template.h | 2 +- Grbl_Esp32/src/Machines/test_drive.h | 2 + Grbl_Esp32/src/MotionControl.cpp | 14 +- Grbl_Esp32/src/Motors/RcServo.cpp | 2 - Grbl_Esp32/src/NutsBolts.cpp | 6 + Grbl_Esp32/src/NutsBolts.h | 7 +- Grbl_Esp32/src/Planner.cpp | 47 +-- Grbl_Esp32/src/ProcessSettings.cpp | 10 +- Grbl_Esp32/src/Report.cpp | 5 +- Grbl_Esp32/src/Settings.cpp | 4 +- Grbl_Esp32/src/Settings.h | 25 +- Grbl_Esp32/src/SettingsDefinitions.cpp | 24 +- Grbl_Esp32/src/System.cpp | 21 +- Grbl_Esp32/src/System.h | 3 - platformio.ini | 1 + 31 files changed, 991 insertions(+), 266 deletions(-) create mode 100644 Grbl_Esp32/Custom/CoreXY.cpp rename Grbl_Esp32/src/Machines/{tapster3.h => tapster_3.h} (66%) create mode 100644 Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h create mode 100644 Grbl_Esp32/src/Machines/tapster_pro_stepstick.h diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp new file mode 100644 index 00000000..cefab842 --- /dev/null +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -0,0 +1,307 @@ +/* + CoreXY.cpp - + + Copyright (c) 2020 Barton Dring @buildlog + + https://corexy.com/theory.html + + Limitations + - Must home via $H. $HX type homes not allowed + - Must home one axis per cycle + - limited to 3 axis systems...easy fix in increase (just donate) + + ============================================================================ + + Grbl 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. + Grbl 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 Grbl. If not, see . + FYI: http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ + Better: http://hypertriangle.com/~alex/delta-robot-tutorial/ +*/ +#include "../src/Settings.h" + +// Homing axis search distance multiplier. Computed by this value times the cycle travel. +#ifndef HOMING_AXIS_SEARCH_SCALAR +# define HOMING_AXIS_SEARCH_SCALAR 1.1 // Must be > 1 to ensure limit switch will be engaged. +#endif +#ifndef HOMING_AXIS_LOCATE_SCALAR +# define HOMING_AXIS_LOCATE_SCALAR 2.0 // Must be > 1 to ensure limit switch is cleared. +#endif + +// The midTbot has a quirk where the x motor has to move twice as far as it would +// on a normal T-Bot or CoreXY +#ifndef MIDTBOT +const float geometry_factor = 1.0; +#else +const float geometry_factor = 2.0; +#endif + +static float last_motors[MAX_N_AXIS] = { 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs +static float last_cartesian[MAX_N_AXIS] = {}; + +// prototypes for helper functions +float three_axis_dist(float* point1, float* point2); + +void machine_init() { + // print a startup message to show the kinematics are enable + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Kinematics Init"); +} + +// This will always return true to prevent the normal Grbl homing cycle +bool user_defined_homing(uint8_t cycle_mask) { + uint8_t n_cycle; // each home is a multi cycle operation approach, pulloff, approach..... + float target[MAX_N_AXIS] = { 0.0 }; // The target for each move in the cycle + float max_travel; + uint8_t axis; + + // check for single axis homing + if (cycle_mask != 0) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Single axis homing not allowed. Use $H only"); + return true; + } + + // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY + bool setting_error = false; + for (int cycle = 0; cycle < 3; cycle++) { + if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "CoreXY Multi axis homing cycles not allowed. $Homing/Cycle%d=%s", + cycle, + homing_cycle[cycle]->getStringValue()); + setting_error = true; + } + } + if (setting_error) + return true; + + // setup the motion parameters + plan_line_data_t plan_data; + plan_line_data_t* pl_data = &plan_data; + memset(pl_data, 0, sizeof(plan_line_data_t)); + pl_data->motion = {}; + pl_data->motion.systemMotion = 1; + pl_data->motion.noFeedOverride = 1; + + for (int cycle = 0; cycle < 3; cycle++) { + AxisMask mask = homing_cycle[cycle]->get(); + mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask + for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) { + if (bit(axis) == mask) { + // setup for the homing of this axis + bool approach = true; + float homing_rate = homing_seek_rate->get(); + max_travel = HOMING_AXIS_SEARCH_SCALAR * axis_settings[axis]->max_travel->get(); + sys.homing_axis_lock = 0xFF; // we don't need to lock any motors in CoreXY + n_cycle = (2 * NHomingLocateCycle + 1); // approach + ((pulloff + approach) * Cycles) + + do { + bool switch_touched = false; + + // zero all X&Y posiitons before each cycle + for (int idx = X_AXIS; idx <= Y_AXIS; idx++) { + sys_position[idx] = 0.0; + target[idx] = 0.0; + } + + if (bit_istrue(homing_dir_mask->get(), bit(axis))) { + approach ? target[axis] = -max_travel : target[axis] = max_travel; + } else { + approach ? target[axis] = max_travel : target[axis] = -max_travel; + } + + target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + + // convert back to motor steps + inverse_kinematics(target); + + pl_data->feed_rate = homing_rate; // feed or seek rates + plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. + sys.step_control = {}; + sys.step_control.executeSysMotion = true; // Set to execute homing motion and clear existing flags. + st_prep_buffer(); // Prep and fill segment buffer from newly planned block. + st_wake_up(); // Initiate motion + + do { + if (approach) { + switch_touched = bitnum_istrue(limits_get_state(), axis); + } + st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. + // Exit routines: No time to run protocol_execute_realtime() in this loop. + if (sys_rt_exec_state.bit.safetyDoor || sys_rt_exec_state.bit.reset || cycle_stop) { + ExecState rt_exec_state; + rt_exec_state.value = sys_rt_exec_state.value; + // Homing failure condition: Reset issued during cycle. + if (rt_exec_state.bit.reset) { + sys_rt_exec_alarm = ExecAlarm::HomingFailReset; + } + // Homing failure condition: Safety door was opened. + if (rt_exec_state.bit.safetyDoor) { + sys_rt_exec_alarm = ExecAlarm::HomingFailDoor; + } + // Homing failure condition: Limit switch still engaged after pull-off motion + if (!approach && (limits_get_state() & cycle_mask)) { + sys_rt_exec_alarm = ExecAlarm::HomingFailPulloff; + } + // Homing failure condition: Limit switch not found during approach. + if (approach && cycle_stop) { + sys_rt_exec_alarm = ExecAlarm::HomingFailApproach; + } + + if (sys_rt_exec_alarm != ExecAlarm::None) { + motors_set_homing_mode(cycle_mask, false); // tell motors homing is done...failed + mc_reset(); // Stop motors, if they are running. + protocol_execute_realtime(); + return true; + } else { + // Pull-off motion complete. Disable CYCLE_STOP from executing. + cycle_stop = false; + break; + } + } + } while (!switch_touched); + +#ifdef USE_I2S_STEPS + if (current_stepper == ST_I2S_STREAM) { + if (!approach) { + delay_ms(I2S_OUT_DELAY_MS); + } + } +#endif + st_reset(); // Immediately force kill steppers and reset step segment buffer. + delay_ms(homing_debounce->get()); // Delay to allow transient dynamics to dissipate. + + approach = !approach; + // After first cycle, homing enters locating phase. Shorten search to pull-off distance. + if (approach) { + max_travel = homing_pulloff->get() * HOMING_AXIS_LOCATE_SCALAR; + homing_rate = homing_feed_rate->get(); + } else { + max_travel = homing_pulloff->get(); + homing_rate = homing_seek_rate->get(); + } + } while (n_cycle-- > 0); + } + } + } + + // after sussefully setting X & Y axes, we set the current positions + + // set the cartesian axis position + for (axis = X_AXIS; axis <= Y_AXIS; axis++) { + if (bitnum_istrue(homing_dir_mask->get(), axis)) { + target[axis] = limitsMinPosition(axis) + homing_pulloff->get(); + } else { + target[axis] = limitsMaxPosition(axis) - homing_pulloff->get(); + } + } + + last_cartesian[X_AXIS] = target[X_AXIS]; + last_cartesian[Y_AXIS] = target[Y_AXIS]; + last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + + // convert to motors + inverse_kinematics(target); + // convert to steps + for (axis = X_AXIS; axis <= Y_AXIS; axis++) { + sys_position[axis] = target[axis] * axis_settings[axis]->steps_per_mm->get(); + } + + sys.step_control = {}; // Return step control to normal operation. + + gc_sync_position(); + plan_sync_position(); + kinematics_post_homing(); + limits_init(); + + return true; +} + +// This function is used by Grbl convert Cartesian to motors +// this does not do any motion control +void inverse_kinematics(float* position) { + float motors[3]; + + motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; + motors[Z_AXIS] = position[Z_AXIS]; + + position[0] = motors[0]; + position[1] = motors[1]; + position[2] = motors[2]; +} + +// Inverse Kinematics calculates motor positions from real world cartesian positions +// position is the current position +// Breaking into segments is not needed with CoreXY, because it is a linear system. +void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos +{ + float dx, dy, dz; // distances in each cartesian axis + float motors[MAX_N_AXIS]; + + float feed_rate = pl_data->feed_rate; // save original feed rate + + // calculate cartesian move distance for each axis + dx = target[X_AXIS] - position[X_AXIS]; + dy = target[Y_AXIS] - position[Y_AXIS]; + dz = target[Z_AXIS] - position[Z_AXIS]; + float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); + + motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS]; + motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS]; + motors[Z_AXIS] = target[Z_AXIS]; + + float motor_distance = three_axis_dist(motors, last_motors); + + if (!pl_data->motion.rapidMotion) { + pl_data->feed_rate *= (motor_distance / dist); + } + + memcpy(last_motors, motors, sizeof(motors)); + + mc_line(motors, pl_data); +} + +// motors -> cartesian +void forward_kinematics(float* position) { + float calc_fwd[MAX_N_AXIS]; + + // https://corexy.com/theory.html + calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); + calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]); + + position[X_AXIS] = calc_fwd[X_AXIS]; + position[Y_AXIS] = calc_fwd[Y_AXIS]; +} + +bool kinematics_pre_homing(uint8_t cycle_mask) { + return false; +} + +void kinematics_post_homing() { + gc_state.position[X_AXIS] = last_cartesian[X_AXIS]; + gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS]; + gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS]; +} + +// this is used used by Grbl soft limits to see if the range of the machine is exceeded. +uint8_t kinematic_limits_check(float* target) { + return true; +} + +void user_m30() {} + +// ================ Local Helper functions ================= + +// Determine the unit distance between (2) 3D points +float three_axis_dist(float* point1, float* point2) { + return sqrt(((point1[0] - point2[0]) * (point1[0] - point2[0])) + ((point1[1] - point2[1]) * (point1[1] - point2[1])) + + ((point1[2] - point2[2]) * (point1[2] - point2[2]))); +} diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 278e052a..5661ae6d 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -90,7 +90,7 @@ void solenoidSyncTask(void* pvParameters) { // continue with regular homing after setup // return true if this completes homing -bool user_defined_homing() { +bool user_defined_homing(uint8_t cycle_mask) { // create and start a task to do the special homing homing_phase = HOMING_PHASE_FULL_APPROACH; atari_homing = true; diff --git a/Grbl_Esp32/Custom/custom_code_template.cpp b/Grbl_Esp32/Custom/custom_code_template.cpp index 7a1e962d..bd468f6d 100644 --- a/Grbl_Esp32/Custom/custom_code_template.cpp +++ b/Grbl_Esp32/Custom/custom_code_template.cpp @@ -56,13 +56,13 @@ void machine_init() {} #ifdef USE_CUSTOM_HOMING /* - user_defined_homing() is called at the begining of the normal Grbl_ESP32 homing - sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 + user_defined_homing(uint8_t cycle_mask) is called at the begining of the normal Grbl_ESP32 homing + sequence. If user_defined_homing(uint8_t cycle_mask) returns false, the rest of normal Grbl_ESP32 homing is skipped if it returns false, other normal homing continues. For example, if you need to manually prep the machine for homing, you could implement - user_defined_homing() to wait for some button to be pressed, then return true. + user_defined_homing(uint8_t cycle_mask) to wait for some button to be pressed, then return true. */ -bool user_defined_homing() { +bool user_defined_homing(uint8_t cycle_mask) { // True = done with homing, false = continue with normal Grbl_ESP32 homing return true; } diff --git a/Grbl_Esp32/Custom/esp32_printer_controller.cpp b/Grbl_Esp32/Custom/esp32_printer_controller.cpp index cc356075..6c2a8cd1 100644 --- a/Grbl_Esp32/Custom/esp32_printer_controller.cpp +++ b/Grbl_Esp32/Custom/esp32_printer_controller.cpp @@ -82,13 +82,13 @@ void machine_init() { #ifdef USE_CUSTOM_HOMING /* - user_defined_homing() is called at the begining of the normal Grbl_ESP32 homing + (user_defined_homing) is called at the begining of the normal Grbl_ESP32 homing sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 homing is skipped if it returns false, other normal homing continues. For example, if you need to manually prep the machine for homing, you could implement user_defined_homing() to wait for some button to be pressed, then return true. */ -bool user_defined_homing() { +bool user_defined_homing(uint8_t cycle_mask) { // True = done with homing, false = continue with normal Grbl_ESP32 homing return true; } diff --git a/Grbl_Esp32/Custom/parallel_delta.cpp b/Grbl_Esp32/Custom/parallel_delta.cpp index b49f77e1..a82de59b 100644 --- a/Grbl_Esp32/Custom/parallel_delta.cpp +++ b/Grbl_Esp32/Custom/parallel_delta.cpp @@ -6,6 +6,9 @@ Jason Huggins, Tapster Robotics Kinematics for a parallel delta robot. + + Note: You must do a clean before compiling whenever this file is altered! + ==================== How it Works ==================================== @@ -24,16 +27,17 @@ The arm 0 values (angle) are the arms at horizontal. Positive angles are below horizontal. The machine's Z zero point in the kinematics is parallel to the arm axes. - Delta_z_offset is the offset to the end effector joints at arm angle zero. - The is calculated at startup and used in the forward kinematics + The offset of the Z distance from the arm axes to the end effector joints + at arm angle zero will be printed at startup on the serial port. - Feedrate in gcode is in the cartesian uints. This must be converted to the + Feedrate in gcode is in the cartesian units. This must be converted to the angles. This is done by calculating the segment move distance and the angle move distance and applying that ration to the feedrate. TODO Cleanup Update so extra axes get delt with ... passed through properly - + Have MPos use kinematics too + ============================================================================ Grbl is free software: you can redistribute it and/or modify @@ -50,13 +54,23 @@ Better: http://hypertriangle.com/~alex/delta-robot-tutorial/ */ +#include "../src/Settings.h" + enum class KinematicError : uint8_t { NONE = 0, OUT_OF_RANGE = 1, ANGLE_TOO_NEGATIVE = 2, + ANGLE_TOO_POSITIVE = 3, }; -// trigonometric constants to speed calculations +// Create custom run time $ settings +FloatSetting* kinematic_segment_len; +FloatSetting* delta_crank_len; +FloatSetting* delta_link_len; +FloatSetting* delta_crank_side_len; +FloatSetting* delta_effector_side_len; + +// trigonometric constants to speed up calculations const float sqrt3 = 1.732050807; const float dtr = M_PI / (float)180.0; // degrees to radians const float sin120 = sqrt3 / 2.0; @@ -66,60 +80,98 @@ const float sin30 = 0.5; const float tan30 = 1.0 / sqrt3; // the geometry of the delta -const float rf = RADIUS_FIXED; // radius of the fixed side (length of motor cranks) -const float re = RADIUS_EFF; // radius of end effector side (length of linkages) -const float f = LENGTH_FIXED_SIDE; // sized of fixed side triangel -const float e = LENGTH_EFF_SIDE; // size of end effector side triangle +float rf; // radius of the fixed side (length of motor cranks) +float re; // radius of end effector side (length of linkages) +float f; // sized of fixed side triangel +float e; // size of end effector side triangle -static float last_angle[N_AXIS] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs -float delta_z_offset; // Z offset of the effector from the arm centers +static float last_angle[3] = { 0.0, 0.0, 0.0 }; // A place to save the previous motor angles for distance/feed rate calcs +static float last_cartesian[N_AXIS] = { + 0.0, 0.0, 0.0 +}; // A place to save the previous motor angles for distance/feed rate calcs // Z offset of the effector from the arm centers // prototypes for helper functions int calc_forward_kinematics(float* angles, float* cartesian); KinematicError delta_calcInverse(float* cartesian, float* angles); KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta); float three_axis_dist(float* point1, float* point2); +void read_settings(); void machine_init() { - // Calculate the Z offset at the motor zero angles ... - // Z offset is the z distance from the motor axes to the end effector axes at zero angle float angles[N_AXIS] = { 0.0, 0.0, 0.0 }; float cartesian[N_AXIS] = { 0.0, 0.0, 0.0 }; + // Custom $ settings + kinematic_segment_len = new FloatSetting(EXTENDED, WG, NULL, "Kinematics/SegmentLength", KINEMATIC_SEGMENT_LENGTH, 0.2, 1000.0); + delta_crank_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankLength", RADIUS_FIXED, 50.0, 500.0); + delta_link_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/LinkLength", RADIUS_EFF, 50.0, 500.0); + delta_crank_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/CrankSideLength", LENGTH_FIXED_SIDE, 20.0, 500.0); + delta_effector_side_len = new FloatSetting(EXTENDED, WG, NULL, "Delta/EffectorSideLength", LENGTH_EFF_SIDE, 20.0, 500.0); + + read_settings(); + + // Calculate the Z offset at the arm zero angles ... + // Z offset is the z distance from the motor axes to the end effector axes at zero angle calc_forward_kinematics(angles, cartesian); // Sets the cartesian values + // print a startup message to show the kinematics are enabled. Print the offset for reference + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); - // print a startup message to show the kinematics are enables - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delata Kinematics Init: %s Z Offset:%4.3f", MACHINE_NAME, cartesian[Z_AXIS]); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Delta Angle Range %3.3f, %3.3f", MAX_NEGATIVE_ANGLE, MAX_POSITIVE_ANGLE); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "DXL_COUNT_MIN %4.0f CENTER %d MAX %4.0f PER_RAD %d", + // DXL_COUNT_MIN, + // DXL_CENTER, + // DXL_COUNT_MAX, + // DXL_COUNT_PER_RADIAN); } - -bool user_defined_homing() { // true = do not continue with normal Grbl homing +bool user_defined_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +#ifdef USE_CUSTOM_HOMING return true; +#else + return false; +#endif } +// This function is used by Grbl +void inverse_kinematics(float* position) { + float motor_angles[3]; + + read_settings(); + delta_calcInverse(position, motor_angles); + position[0] = motor_angles[0]; + position[1] = motor_angles[1]; + position[2] = motor_angles[2]; +} + +// This function is used by Grbl void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) //The target and position are provided in MPos { float dx, dy, dz; // distances in each cartesian axis - float motor_angles[N_AXIS]; + float motor_angles[3]; - float seg_target[N_AXIS]; // The target of the current segment - float feed_rate = pl_data->feed_rate; // save original feed rate - bool start_position_erorr = false; - bool show_error = true; // shows error once + float seg_target[3]; // The target of the current segment + float feed_rate = pl_data->feed_rate; // save original feed rate + bool show_error = true; // shows error once - // see if start is in work area...if not skip segments and try to go to target - KinematicError status = delta_calcInverse(position, motor_angles); + KinematicError status; + read_settings(); + + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start %3.3f %3.3f %3.3f", position[0], position[1], position[2]); + // grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target %3.3f %3.3f %3.3f", target[0], target[1], target[2]); + + status = delta_calcInverse(position, motor_angles); if (status == KinematicError::OUT_OF_RANGE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error"); - start_position_erorr = true; + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start position error %3.3f %3.3f %3.3f", position[0], position[1], position[2]); + //start_position_error = true; } // Check the destination to see if it is in work area status = delta_calcInverse(target, motor_angles); - if (status == KinematicError::OUT_OF_RANGE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable"); - return; + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Target unreachable error %3.3f %3.3f %3.3f", target[0], target[1], target[2]); } position[X_AXIS] += gc_state.coord_offset[X_AXIS]; @@ -127,14 +179,13 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio position[Z_AXIS] += gc_state.coord_offset[Z_AXIS]; // calculate cartesian move distance for each axis - dx = target[X_AXIS] - position[X_AXIS]; dy = target[Y_AXIS] - position[Y_AXIS]; dz = target[Z_AXIS] - position[Z_AXIS]; float dist = sqrt((dx * dx) + (dy * dy) + (dz * dz)); // determine the number of segments we need ... round up so there is at least 1 (except when dist is 0) - uint32_t segment_count = ceil(dist / SEGMENT_LENGTH); + uint32_t segment_count = ceil(dist / kinematic_segment_len->get()); float segment_dist = dist / ((float)segment_count); // distance of each segment...will be used for feedrate conversion @@ -163,19 +214,46 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio } else { if (show_error) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f]\r\n\r\n", - status, - motor_angles[0], - motor_angles[1], - motor_angles[2]); + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "Error:%d, Angs X:%4.3f Y:%4.3f Z:%4.3f", + // status, + // motor_angles[0], + // motor_angles[1], + // motor_angles[2]); show_error = false; } } } } +// this is used used by Grbl soft limits to see if the range of the machine is exceeded. +uint8_t kinematic_limits_check(float* target) { + float motor_angles[3]; + + read_settings(); + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin Soft Check %3.3f, %3.3f, %3.3f", target[0], target[1], target[2]); + + KinematicError status = delta_calcInverse(target, motor_angles); + + switch (status) { + case KinematicError::OUT_OF_RANGE: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target out of range"); + break; + case KinematicError::ANGLE_TOO_NEGATIVE: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max negative"); + break; + case KinematicError::ANGLE_TOO_POSITIVE: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Kin target max positive"); + break; + case KinematicError::NONE: + break; + } + + return (status == KinematicError::NONE); +} + // inverse kinematics: cartesian -> angles // returned status: 0=OK, -1=non-existing position KinematicError delta_calcInverse(float* cartesian, float* angles) { @@ -271,6 +349,10 @@ KinematicError delta_calcAngleYZ(float x0, float y0, float z0, float& theta) { return KinematicError::ANGLE_TOO_NEGATIVE; } + if (theta > MAX_POSITIVE_ANGLE) { + return KinematicError::ANGLE_TOO_POSITIVE; + } + return KinematicError::NONE; } @@ -284,12 +366,17 @@ void forward_kinematics(float* position) { float calc_fwd[N_AXIS]; int status; + read_settings(); + // convert the system position in steps to radians float position_radians[N_AXIS]; int32_t position_steps[N_AXIS]; // Copy current state of the system position variable memcpy(position_steps, sys_position, sizeof(sys_position)); system_convert_array_steps_to_mpos(position_radians, position_steps); + // grbl_msg_sendf( + // CLIENT_SERIAL, MsgLevel::Info, "Fwd Kin Angs %1.3f, %1.3f, %1.3f ", position_radians[0], position_radians[1], position_radians[2]); + // detmine the position of the end effector joint center. status = calc_forward_kinematics(position_radians, calc_fwd); @@ -303,10 +390,56 @@ void forward_kinematics(float* position) { } } -bool kinematics_pre_homing(uint8_t cycle_mask) { +bool kinematics_pre_homing(uint8_t cycle_mask) { // true = do not continue with normal Grbl homing +#ifdef USE_CUSTOM_HOMING return true; +#else + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "kinematics_pre_homing"); + return false; +#endif } -void kinematics_post_homing() {} +void kinematics_post_homing() { +#ifdef USE_CUSTOM_HOMING + +#else + last_angle[X_AXIS] = sys_position[X_AXIS] / axis_settings[X_AXIS]->steps_per_mm->get(); + last_angle[Y_AXIS] = sys_position[Y_AXIS] / axis_settings[Y_AXIS]->steps_per_mm->get(); + last_angle[Z_AXIS] = sys_position[Z_AXIS] / axis_settings[Z_AXIS]->steps_per_mm->get(); + + read_settings(); + + calc_forward_kinematics(last_angle, last_cartesian); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "kinematics_post_homing Angles: %3.3f, %3.3f, %3.3f", + // last_angle[X_AXIS], + // last_angle[Y_AXIS], + // last_angle[Z_AXIS]); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "kinematics_post_homing Cartesian: %3.3f, %3.3f, %3.3f", + // last_cartesian[X_AXIS], + // last_cartesian[Y_AXIS], + // last_cartesian[Z_AXIS]); + + gc_state.position[X_AXIS] = last_cartesian[X_AXIS]; + gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS]; + gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS]; + +#endif +#ifdef USE_POST_HOMING_DELAY + delay(1000); // give time for servo type homing +#endif +} void user_m30() {} + +void read_settings() { + rf = delta_crank_len->get(); // radius of the fixed side (length of motor cranks) + re = delta_link_len->get(); // radius of end effector side (length of linkages) + f = delta_crank_side_len->get(); // sized of fixed side triangel + e = delta_effector_side_len->get(); // size of end effector side triangle +} diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 658cade3..50b014f8 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -263,13 +263,6 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128) const double SAFETY_DOOR_SPINDLE_DELAY = 4.0; // Float (seconds) const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds) -// Enable CoreXY kinematics. Use ONLY with CoreXY machines. -// NOTE: This configuration option alters the motion of the X and Y axes to principle of operation -// defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as -// described, if not, motions may move in strange directions. Grbl requires the CoreXY A and B motors -// have the same steps per mm internally. -// #define COREXY // Default disabled. Uncomment to enable. - // Inverts select limit pin states based on the following mask. This effects all limit pin functions, // such as hard limits and homing. However, this is different from overall invert limits setting. // This build option will invert only the limit pins defined here, and then the invert limits setting @@ -539,7 +532,7 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds // these commands may be undesirable. Simply comment the desired macro to disable it. #define ENABLE_RESTORE_WIPE_ALL // '$RST=*' Default enabled. Comment to disable. #define ENABLE_RESTORE_DEFAULT_SETTINGS // '$RST=$' Default enabled. Comment to disable. -#define ENABLE_RESTORE_PARAMETERS // '$RST=#' Default enabled. Comment to disable. +#define ENABLE_RESTORE_CLEAR_PARAMETERS // '$RST=#' Default enabled. Comment to disable. // Additional settings have been added to the original set that you see with the $$ command // Some senders may not be able to parse anything different from the original set diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index f75812a0..42d7d9b9 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -340,6 +340,30 @@ # define DEFAULT_C_HOMING_MPOS 0.0 #endif +#ifndef DEFAULT_HOMING_CYCLE_0 +# define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS) +#endif + +#ifndef DEFAULT_HOMING_CYCLE_1 +# define DEFAULT_HOMING_CYCLE_1 (bit(X_AXIS) | bit(Y_AXIS)) +#endif + +#ifndef DEFAULT_HOMING_CYCLE_2 +# define DEFAULT_HOMING_CYCLE_2 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_3 +# define DEFAULT_HOMING_CYCLE_3 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_4 +# define DEFAULT_HOMING_CYCLE_4 0 +#endif + +#ifndef DEFAULT_HOMING_CYCLE_5 +# define DEFAULT_HOMING_CYCLE_5 0 +#endif + // ========== Motor current (SPI Drivers ) ============= #ifndef DEFAULT_X_CURRENT # define DEFAULT_X_CURRENT 0.25 // $140 current in amps (extended set) diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 0134952e..5a24d342 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1473,9 +1473,9 @@ Error gc_execute_line(char* line, uint8_t client) { // and absolute and incremental modes. pl_data->motion.rapidMotion = 1; // Set rapid motion flag. if (axis_command != AxisCommand::None) { - mc_line(gc_block.values.xyz, pl_data); // kinematics kinematics not used for homing righ now + mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position); } - mc_line(coord_data, pl_data); + mc_line_kins(coord_data, pl_data, gc_state.position); memcpy(gc_state.position, coord_data, sizeof(gc_state.position)); break; case NonModal::SetHome0: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index b2e9c2d8..9dfc5291 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 = "20201022"; +const char* const GRBL_VERSION_BUILD = "20201027"; //#include #include @@ -96,14 +96,17 @@ void run_once(); void machine_init(); // Called if USE_CUSTOM_HOMING is defined -bool user_defined_homing(); +bool user_defined_homing(uint8_t cycle_mask); // Called if USE_KINEMATICS is defined -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); -bool kinematics_pre_homing(uint8_t cycle_mask); -void kinematics_post_homing(); + +void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position); +bool kinematics_pre_homing(uint8_t cycle_mask); +void kinematics_post_homing(); +uint8_t kinematic_limits_check(float* target); // Called if USE_FWD_KINEMATICS is defined +void inverse_kinematics(float* position); // used to return a converted value void forward_kinematics(float* position); // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp index 5d7b5773..1c0c3d00 100644 --- a/Grbl_Esp32/src/Jog.cpp +++ b/Grbl_Esp32/src/Jog.cpp @@ -37,8 +37,13 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) { return Error::TravelExceeded; } } - // Valid jog command. Plan, set state, and execute. +// Valid jog command. Plan, set state, and execute. +#ifndef USE_KINEMATICS mc_line(gc_block->values.xyz, pl_data); +#else // else use kinematics + inverse_kinematics(gc_block->values.xyz, pl_data, gc_state.position); +#endif + if (sys.state == State::Idle) { if (plan_get_current_block() != NULL) { // Check if there is a block to execute. sys.state = State::Jog; diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 2aed18b7..41198dcf 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -109,11 +109,6 @@ void limits_go_home(uint8_t cycle_mask) { for (uint8_t idx = 0; idx < n_axis; idx++) { // Initialize step pin masks step_pin[idx] = bit(idx); -#ifdef COREXY - if ((idx == A_MOTOR) || (idx == B_MOTOR)) { - step_pin[idx] = (bit(X_AXIS) | bit(Y_AXIS)); - } -#endif if (bit_istrue(cycle_mask, bit(idx))) { // Set target based on max_travel setting. Ensure homing switches engaged with search scalar. max_travel = MAX(max_travel, (HOMING_AXIS_SEARCH_SCALAR)*axis_settings[idx]->max_travel->get()); @@ -133,20 +128,7 @@ void limits_go_home(uint8_t cycle_mask) { // Set target location for active axes and setup computation for homing rate. if (bit_istrue(cycle_mask, bit(idx))) { n_active_axis++; -#ifdef COREXY - if (idx == X_AXIS) { - int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position); - sys_position[A_MOTOR] = axis_position; - sys_position[B_MOTOR] = -axis_position; - } else if (idx == Y_AXIS) { - int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position); - sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position; - } else { - sys_position[Z_AXIS] = 0; - } -#else sys_position[idx] = 0; -#endif // Set target direction based on cycle mask and homing cycle approach state. // NOTE: This happens to compile smaller than any other implementation tried. auto mask = homing_dir_mask->get(); @@ -183,15 +165,7 @@ void limits_go_home(uint8_t cycle_mask) { for (uint8_t idx = 0; idx < n_axis; idx++) { if (axislock & step_pin[idx]) { if (limit_state & bit(idx)) { -#ifdef COREXY - if (idx == Z_AXIS) { - axislock &= ~(step_pin[Z_AXIS]); - } else { - axislock &= ~(step_pin[A_MOTOR] | step_pin[B_MOTOR]); - } -#else axislock &= ~(step_pin[idx]); -#endif } } } @@ -272,33 +246,6 @@ void limits_go_home(uint8_t cycle_mask) { } else { sys_position[idx] = (mpos - pulloff) * steps; } - -#ifdef COREXY - if (idx == X_AXIS) { - int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position); - sys_position[A_MOTOR] = set_axis_position + off_axis_position; - sys_position[B_MOTOR] = set_axis_position - off_axis_position; - } else if (idx == Y_AXIS) { - int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position); - sys_position[A_MOTOR] = off_axis_position + set_axis_position; - sys_position[B_MOTOR] = off_axis_position - set_axis_position; - } else { - sys_position[idx] = set_axis_position; - } -#else - //sys_position[idx] = set_axis_position; - /* - float max_mpos, min_mpos; - - if (bit_istrue(homing_dir_mask->get(), bit(idx))) { - min_mpos = mpos; - max_mpos = mpos + travel; - } else { - min_mpos = mpos - travel; - max_mpos = mpos; - } - */ -#endif } } sys.step_control = {}; // Return step control to normal operation. @@ -386,7 +333,7 @@ AxisMask limits_get_state() { return pinMask; } -// Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed, +// Performs a soft limit check. Called from mcline() only. Assumes the machine has been homed, // the workspace volume is in all negative space, and the system is in normal operation. // NOTE: Used by jogging to limit travel within soft-limit volume. void limits_soft_check(float* target) { diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index 28ab79db..a258d32f 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -124,4 +124,3 @@ Socket #5 #define DEFAULT_X_STEPS_PER_MM 800 #define DEFAULT_Y_STEPS_PER_MM 800 #define DEFAULT_Z_STEPS_PER_MM 800 - diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h index b9ceee16..41e35c2a 100644 --- a/Grbl_Esp32/src/Machines/midtbot.h +++ b/Grbl_Esp32/src/Machines/midtbot.h @@ -25,7 +25,15 @@ along with Grbl_ESP32. If not, see . */ -#define MACHINE_NAME "MIDTBOT" +#define MACHINE_NAME "midTbot" + +#define CUSTOM_CODE_FILENAME "../Custom/CoreXY.cpp" + +#define MIDTBOT // applies the geometry correction to the kinematics +#define USE_KINEMATICS // there are kinematic equations for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine +#define USE_CUSTOM_HOMING #define SPINDLE_TYPE SpindleType::NONE @@ -35,10 +43,6 @@ #define X_DIRECTION_PIN GPIO_NUM_26 #define Y_DIRECTION_PIN GPIO_NUM_25 -#ifndef COREXY // maybe set in config.h - #define COREXY -#endif - #define STEPPERS_DISABLE_PIN GPIO_NUM_13 #define X_LIMIT_PIN GPIO_NUM_2 @@ -51,8 +55,11 @@ #define SPINDLE_TYPE SpindleType::NONE // defaults -#define DEFAULT_HOMING_CYCLE_0 bit(Y_AXIS) -#define DEFAULT_HOMING_CYCLE_1 bit(X_AXIS) +#define DEFAULT_HOMING_CYCLE_0 bit(Z_AXIS) +#define DEFAULT_HOMING_CYCLE_1 bit(Y_AXIS) +#define DEFAULT_HOMING_CYCLE_2 bit(X_AXIS) + +#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit (Z_AXIS)) // these home negative #define DEFAULT_STEP_PULSE_MICROSECONDS 3 #define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // stay on @@ -73,13 +80,12 @@ #define DEFAULT_HARD_LIMIT_ENABLE 0 // false #define DEFAULT_HOMING_ENABLE 1 -#define DEFAULT_HOMING_DIR_MASK 1 -#define DEFAULT_HOMING_FEED_RATE 200.0 // mm/min -#define DEFAULT_HOMING_SEEK_RATE 1000.0 // mm/min +#define DEFAULT_HOMING_FEED_RATE 500.0 // mm/min +#define DEFAULT_HOMING_SEEK_RATE 2000.0 // mm/min #define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) #define DEFAULT_HOMING_PULLOFF 3.0 // mm -#define DEFAULT_X_STEPS_PER_MM 200.0 +#define DEFAULT_X_STEPS_PER_MM 100.0 #define DEFAULT_Y_STEPS_PER_MM 100.0 #define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode @@ -93,4 +99,8 @@ #define DEFAULT_X_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. #define DEFAULT_Y_MAX_TRAVEL 100.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode +#define DEFAULT_Z_MAX_TRAVEL 5.0 // This is percent in servo mode + +#define DEFAULT_X_HOMING_MPOS DEFAULT_Z_MAX_TRAVEL // stays up after homing + + diff --git a/Grbl_Esp32/src/Machines/tapster3.h b/Grbl_Esp32/src/Machines/tapster_3.h similarity index 66% rename from Grbl_Esp32/src/Machines/tapster3.h rename to Grbl_Esp32/src/Machines/tapster_3.h index c29d018d..b4de2b1c 100644 --- a/Grbl_Esp32/src/Machines/tapster3.h +++ b/Grbl_Esp32/src/Machines/tapster_3.h @@ -26,17 +26,24 @@ // ================= Firmware Customization =================== #define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine // ================== Delta Geometry =========================== -#define RADIUS_FIXED 70.0; // radius of the fixed side (length of motor cranks) -#define RADIUS_EFF 133.5; // radius of end effector side (length of linkages) -#define LENGTH_FIXED_SIDE 179.437f; // sized of fixed side triangel -#define LENGTH_EFF_SIDE 86.6025f; // size of end effector side triangle -#define SEGMENT_LENGTH 0.5 // segment length in mm -#define MAX_NEGATIVE_ANGLE -45 // in negative radians how far can the arms go up before damaging machine (max pi/2) +#define RADIUS_FIXED 70.0 // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 133.5 // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 179.437f // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm +#define MAX_NEGATIVE_ANGLE -(M_PI / 3.0) // 60 degrees up +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // 90 degrees down +#define ARM_INTERNAL_ANGLE 0.05 // radians 2.866° // due to mounting angle +#define USE_POST_HOMING_DELAY // Servos need time to reach destination before idle kicks in + +#define DEFAULT_X_MAX_TRAVEL MAX_POSITIVE_ANGLE - MAX_NEGATIVE_ANGLE +#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // +#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // // =================== Machine Hardware Definition ============= @@ -48,9 +55,17 @@ #define Y_DYNAMIXEL_ID 2 // protocol ID #define Z_DYNAMIXEL_ID 3 // protocol ID -// limit servo to 180 degree motion -#define DXL_COUNT_MIN 1024 -#define DXL_COUNT_MAX 3072 +// limit servo to motion range +#define DXL_COUNTS 4096 +#define DXL_COUNT_PER_RADIAN 652 // (DXL_COUNTS / 2.0 * M_PI) + +#define DXL_CENTER 2015 // (DXL_COUNTS / 2) - (ARM_INTERNAL_ANGLE * DXL_COUNT_PER_RADIAN) + +#undef DXL_COUNT_MIN +#define DXL_COUNT_MIN (DXL_CENTER + (MAX_NEGATIVE_ANGLE * DXL_COUNT_PER_RADIAN)) + +#undef DXL_COUNT_MAX +#define DXL_COUNT_MAX (DXL_CENTER + (MAX_POSITIVE_ANGLE * DXL_COUNT_PER_RADIAN)) #define SERVO_TIMER_INTERVAL 50 @@ -84,6 +99,11 @@ #define DEFAULT_HOMING_ENABLE 0 #define DEFAULT_HOMING_DIR_MASK 0 +#define DEFAULT_HOMING_CYCLE_0 (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_X_HOMING_MPOS MAX_NEGATIVE_ANGLE +#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS +#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS + // the following must have the same values for each axis #define DEFAULT_X_STEPS_PER_MM 800 // value is actually arbitrary, but keep it smallish @@ -98,14 +118,5 @@ #define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION // 10*60*60 mm/min^2 = 10 mm/sec^2 #define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION -#define DEFAULT_X_MAX_TRAVEL 3.14159265 // 180° in radians -#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // -#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL // - -#define ARM_INTERNAL_ANGLE 2.866 // due to mounting angle - -#define DEFAULT_X_HOMING_MPOS (DEFAULT_X_MAX_TRAVEL / 2.0) -#define DEFAULT_Y_HOMING_MPOS DEFAULT_X_HOMING_MPOS -#define DEFAULT_Z_HOMING_MPOS DEFAULT_X_HOMING_MPOS #define SPINDLE_TYPE SpindleType::NONE diff --git a/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h new file mode 100644 index 00000000..6f88a816 --- /dev/null +++ b/Grbl_Esp32/src/Machines/tapster_pro_6P_trinamic.h @@ -0,0 +1,197 @@ +#pragma once +// clang-format off + +/* + tapster_pro_stepstick.h + + 2020 - Bart Dring, Jason Huggins (Tapster Robotics) + + Grbl_ESP32 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. + Grbl 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 Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "Tapster Pro Delta 6P Trinamic" +#define CUSTOM_CODE_FILENAME "Custom/parallel_delta.cpp" +/* +// enable these special machine functions to be called from the main program +#define USE_KINEMATICS // there are kinematic equations for this machine +#define FWD_KINEMATICS_REPORTING // report in cartesian +#define USE_RMT_STEPS // Use the RMT periferal to generate step pulses +#define USE_TRINAMIC // some Trinamic motors are used on this machine +#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers +#define USE_MACHINE_INIT // There is some custom initialization for this machine + +#define SEGMENT_LENGTH 0.5 // segment length in mm +#define KIN_ANGLE_CALC_OK 0 +#define KIN_ANGLE_ERROR -1 + +#define MAX_NEGATIVE_ANGLE -36 // in degrees how far can the arms go up? + +#define HOMING_CURRENT_REDUCTION 1.0 + +*/ + +#define N_AXIS 3 + +#define USE_KINEMATICS // there are kinematic equations for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine + +// ================== Delta Geometry =========================== + +#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm +#define MAX_NEGATIVE_ANGLE -0.75f // +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // + + +// ================== Config ====================== + +// Set $Homing/Cycle0=XYZ + + + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +#define USE_I2S_STEPS +//#define DEFAULT_STEPPER ST_I2S_STATIC +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE + +// #define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + +// ================== CPU MAP ====================== + +// Motor Socket #1 +#define X_TRINAMIC_DRIVER 2130 +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) +#define X_CS_PIN I2SO(3) +#define X_RSENSE TMC2130_RSENSE_DEFAULT + +// Motor Socket #2 +#define Y_TRINAMIC_DRIVER X_TRINAMIC_DRIVER +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) +#define Y_CS_PIN I2SO(6) +#define Y_RSENSE X_RSENSE + +// Motor Socket #3 +#define Z_TRINAMIC_DRIVER X_TRINAMIC_DRIVER +#define Z_DISABLE_PIN I2SO(8) +#define Z_DIRECTION_PIN I2SO(9) +#define Z_STEP_PIN I2SO(10) +#define Z_CS_PIN I2SO(11) +#define Z_RSENSE X_RSENSE + +// 6 Pack Pin Mapping +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Socket-Pin-Number-Mapping + +// // 4x Switch input module on CNC I/O module Socket #1 +// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +#define X_LIMIT_PIN GPIO_NUM_33 +#define Y_LIMIT_PIN GPIO_NUM_32 +#define Z_LIMIT_PIN GPIO_NUM_35 + +// 4x Switch input module on CNC I/O module Socket #2 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module +// #define X_LIMIT_PIN GPIO_NUM_2 +// #define Y_LIMIT_PIN GPIO_NUM_25 +// #define Z_LIMIT_PIN GPIO_NUM_39 +// #define PROBE_PIN GPIO_NUM_36 + +//Example Quad MOSFET module on socket #3 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module +#define USER_DIGITAL_PIN_0 GPIO_NUM_26 +#define USER_DIGITAL_PIN_1 GPIO_NUM_4 +#define USER_DIGITAL_PIN_2 GPIO_NUM_16 +#define USER_DIGITAL_PIN_3 GPIO_NUM_27 + +// Example Servo module in socket #4 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/RC-Servo-BESC-CNC-I-O-Module +// https://github.com/bdring/Grbl_Esp32/wiki/M62,-M63,-M64,-M65-&-M67-User-I-O-Commands +#define USER_ANALOG_PIN_0 GPIO_NUM_14 +#define USER_ANALOG_PIN_1 GPIO_NUM_13 +#define USER_ANALOG_PIN_2 GPIO_NUM_15 +#define USER_ANALOG_PIN_3 GPIO_NUM_12 +#define USER_ANALOG_PIN_0_FREQ 50 // for use with RC servos +#define USER_ANALOG_PIN_1_FREQ 50 +#define USER_ANALOG_PIN_2_FREQ 50 +#define USER_ANALOG_PIN_3_FREQ 50 + +// ================= defaults =========================== + +#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // keep them on, the trinamics will reduce power at idle + +#define DEFAULT_X_MICROSTEPS 8 +#define DEFAULT_Y_MICROSTEPS DEFAULT_X_MICROSTEPS +#define DEFAULT_Z_MICROSTEPS DEFAULT_X_MICROSTEPS + +// some math to figure out microsteps per unit // units could bedegrees or radians +#define UNITS_PER_REV (2.0 * M_PI) // 360.0 degrees or 6.2831853 radians +#define STEPS_PER_REV 400.0 +#define REDUCTION_RATIO (60.0 / 16.0) // the pulleys on arm and motor +#define MICROSTEPS_PER_REV (STEPS_PER_REV * (float)DEFAULT_X_MICROSTEPS * REDUCTION_RATIO) + +#define DEFAULT_X_STEPS_PER_MM (MICROSTEPS_PER_REV / UNITS_PER_REV) +#define DEFAULT_Y_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM +#define DEFAULT_Z_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM + +#define DEFAULT_X_MAX_RATE 100.0 // mm/min +#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE +#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE + +#define DEFAULT_X_ACCELERATION 20.0 +#define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION +#define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION + +#define DEFAULT_X_CURRENT 1.0 +#define DEFAULT_X_HOLD_CURRENT 0.5 +#define DEFAULT_Y_CURRENT 1.0 +#define DEFAULT_Y_HOLD_CURRENT 0.5 +#define DEFAULT_Z_CURRENT 1.0 +#define DEFAULT_Z_HOLD_CURRENT 0.5 + +// homing +#define DEFAULT_HOMING_FEED_RATE 25 +#define DEFAULT_HOMING_SEEK_RATE 100 +#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) // all axes home negative +#define DEFAULT_HOMING_ENABLE 1 +#define DEFAULT_INVERT_LIMIT_PINS 0 +#define DEFAULT_HOMING_CYCLE_0 (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_HOMING_CYCLE_1 0 // override this one in defaults.h + +// The machine homes up and above center. MPos is the axis angle in radians +// at the homing posiiton + +#define DEFAULT_X_HOMING_MPOS -0.75 // neagtive because above horizontal +#define DEFAULT_Y_HOMING_MPOS -0.75 +#define DEFAULT_Z_HOMING_MPOS -0.75 + +// the total travel is straight down from horizontal (pi/2) + the up travel +#define DEFAULT_X_MAX_TRAVEL ((M_PI / 2.0) - DEFAULT_X_HOMING_MPOS) +#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL +#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL + +#define DEFAULT_HOMING_PULLOFF -DEFAULT_X_HOMING_MPOS + +#define DEFAULT_INVERT_PROBE_PIN 1 + +#define SPINDLE_TYPE SpindleType::NONE \ No newline at end of file diff --git a/Grbl_Esp32/src/Machines/tapster_pro_stepstick.h b/Grbl_Esp32/src/Machines/tapster_pro_stepstick.h new file mode 100644 index 00000000..366157c5 --- /dev/null +++ b/Grbl_Esp32/src/Machines/tapster_pro_stepstick.h @@ -0,0 +1,155 @@ +#pragma once +// clang-format off + +/* + tapster_pro_stepstick.h + + 2020 - Bart Dring, Jason Huggins (Tapster Robotics) + + Grbl_ESP32 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. + Grbl 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 Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "Tapster Pro Delta (StepStick)" + +#define CUSTOM_CODE_FILENAME "Custom/parallel_delta.cpp" +/* +// enable these special machine functions to be called from the main program +#define USE_KINEMATICS // there are kinematic equations for this machine +#define FWD_KINEMATICS_REPORTING // report in cartesian +#define USE_RMT_STEPS // Use the RMT periferal to generate step pulses +#define USE_TRINAMIC // some Trinamic motors are used on this machine +#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers +#define USE_MACHINE_INIT // There is some custom initialization for this machine + +#define SEGMENT_LENGTH 0.5 // segment length in mm +#define KIN_ANGLE_CALC_OK 0 +#define KIN_ANGLE_ERROR -1 + +#define MAX_NEGATIVE_ANGLE -36 // in degrees how far can the arms go up? + +#define HOMING_CURRENT_REDUCTION 1.0 + +*/ + +#define N_AXIS 3 + +#define USE_KINEMATICS // there are kinematic equations for this machine +#define USE_FWD_KINEMATICS // report in cartesian +#define USE_MACHINE_INIT // There is some custom initialization for this machine + +// ================== Delta Geometry =========================== + +#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) +#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages) +#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel +#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle +#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm +#define MAX_NEGATIVE_ANGLE -0.75f // +#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // + + +// ================== Config ====================== + +// Set $Homing/Cycle0=XYZ + +// === Special Features + +// I2S (steppers & other output-only pins) +#define USE_I2S_OUT +#define USE_I2S_STEPS +//#define DEFAULT_STEPPER ST_I2S_STATIC +// === Default settings +#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE + +#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define I2S_OUT_BCK GPIO_NUM_22 +#define I2S_OUT_WS GPIO_NUM_17 +#define I2S_OUT_DATA GPIO_NUM_21 + +// ================== CPU MAP ====================== + +#define X_STEPPER_MS3 I2SO(3) // X_CS +#define Y_STEPPER_MS3 I2SO(6) // Y_CS +#define Z_STEPPER_MS3 I2SO(11) // Z_CS + +#define STEPPER_RESET GPIO_NUM_19 + +// Motor Socket #1 +#define X_DISABLE_PIN I2SO(0) +#define X_DIRECTION_PIN I2SO(1) +#define X_STEP_PIN I2SO(2) + +// Motor Socket #2 +#define Y_DIRECTION_PIN I2SO(4) +#define Y_STEP_PIN I2SO(5) +#define Y_DISABLE_PIN I2SO(7) + +// Motor Socket #3 +#define Z_DISABLE_PIN I2SO(8) +#define Z_DIRECTION_PIN I2SO(9) +#define Z_STEP_PIN I2SO(10) + +// CNC I/O Modules + +#define X_LIMIT_PIN GPIO_NUM_33 +#define Y_LIMIT_PIN GPIO_NUM_32 +#define Z_LIMIT_PIN GPIO_NUM_35 + +// ================= defaults =========================== + +#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // keep them on, the trinamics will reduce power at idle + +#define DEFAULT_X_MICROSTEPS 32 +#define DEFAULT_Y_MICROSTEPS DEFAULT_X_MICROSTEPS +#define DEFAULT_Z_MICROSTEPS DEFAULT_X_MICROSTEPS + +// some math to figure out microsteps per unit // units could bedegrees or radians +#define UNITS_PER_REV (2.0 * M_PI) // 360.0 degrees or 6.2831853 radians +#define STEPS_PER_REV 400.0 +#define REDUCTION_RATIO (60.0 / 16.0) // the pulleys on arm and motor +#define MICROSTEPS_PER_REV (STEPS_PER_REV * (float)DEFAULT_X_MICROSTEPS * REDUCTION_RATIO) + +#define DEFAULT_X_STEPS_PER_MM (MICROSTEPS_PER_REV / UNITS_PER_REV) +#define DEFAULT_Y_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM +#define DEFAULT_Z_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM + +#define DEFAULT_X_MAX_RATE 100.0 // mm/min +#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE +#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE + +#define DEFAULT_X_ACCELERATION 20.0 +#define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION +#define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION + +// homing +#define DEFAULT_HOMING_FEED_RATE 25 +#define DEFAULT_HOMING_SEEK_RATE 100 +#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) // all axes home negative +#define DEFAULT_HOMING_ENABLE 1 +#define DEFAULT_INVERT_LIMIT_PINS 0 + +// The machine homes up and above center. MPos is the axis angle in radians +// at the homing posiiton + +#define DEFAULT_X_HOMING_MPOS -0.75 // neagtive because above horizontal +#define DEFAULT_Y_HOMING_MPOS -0.75 +#define DEFAULT_Z_HOMING_MPOS -0.75 + +// the total travel is straight down from horizontal (pi/2) + the up travel +#define DEFAULT_X_MAX_TRAVEL ((M_PI / 2.0) - DEFAULT_X_HOMING_MPOS) +#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL +#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL + +#define DEFAULT_HOMING_PULLOFF -DEFAULT_X_HOMING_MPOS + +#define SPINDLE_TYPE SpindleType::NONE \ No newline at end of file diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h index 644734da..7672d31f 100644 --- a/Grbl_Esp32/src/Machines/template.h +++ b/Grbl_Esp32/src/Machines/template.h @@ -182,7 +182,7 @@ // functions. custom_code_template.cpp describes the functions // that you can implement. The ifdef guards are described below: // -// USE_CUSTOM_HOMING enables the user_defined_homing() function +// USE_CUSTOM_HOMING enables the user_defined_homing(uint8_t cycle_mask) function // that can implement an arbitrary homing sequence. // #define USE_CUSTOM_HOMING diff --git a/Grbl_Esp32/src/Machines/test_drive.h b/Grbl_Esp32/src/Machines/test_drive.h index d92e6d50..998f40f7 100644 --- a/Grbl_Esp32/src/Machines/test_drive.h +++ b/Grbl_Esp32/src/Machines/test_drive.h @@ -37,6 +37,8 @@ #define MACHINE_NAME "Test Drive - Demo Only No I/O!" + +#define N_AXIS 3 // This cannot use homing because there are no switches #ifdef DEFAULT_HOMING_CYCLE_0 #undef DEFAULT_HOMING_CYCLE_0 diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index a3be9ce9..5d725ac9 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -115,8 +115,10 @@ void mc_arc(float* target, float r_axis1 = -offset[axis_1]; float rt_axis0 = target[axis_0] - center_axis0; float rt_axis1 = target[axis_1] - center_axis1; + + float previous_position[MAX_N_AXIS] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; #ifdef USE_KINEMATICS - float previous_position[MAX_N_AXIS]; + uint16_t n; auto n_axis = number_axis->get(); for (n = 0; n < n_axis; n++) { @@ -218,11 +220,7 @@ void mc_arc(float* target, } } // Ensure last segment arrives at target location. -#ifdef USE_KINEMATICS mc_line_kins(target, pl_data, previous_position); -#else - mc_line(target, pl_data); -#endif } // Execute dwell in seconds. @@ -285,7 +283,7 @@ static bool axis_is_squared(uint8_t axis_mask) { void mc_homing_cycle(uint8_t cycle_mask) { bool no_cycles_defined = true; #ifdef USE_CUSTOM_HOMING - if (user_defined_homing()) { + if (user_defined_homing(cycle_mask)) { return; } #endif @@ -352,7 +350,6 @@ void mc_homing_cycle(uint8_t cycle_mask) { } } } - if (no_cycles_defined) { report_status_message(Error::HomingNoCycles, CLIENT_ALL); } @@ -411,7 +408,8 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par return GCUpdatePos::None; // Nothing else to do but bail. } // Setup and queue probing motion. Auto cycle-start should not start the cycle. - mc_line(target, pl_data); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Found"); + mc_line_kins(target, pl_data, gc_state.position); // Activate the probing state monitor in the stepper module. sys_probe_state = Probe::Active; // Perform probing cycle. Wait here until probe is triggered or motion completes. diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index a5716701..4d9a6dd7 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -87,8 +87,6 @@ namespace Motors { // Homing justs sets the new system position and the servo will move there bool RcServo::set_homing_mode(bool isHoming) { - float home_pos = 0.0; - sys_position[_axis_index] = axis_settings[_axis_index]->home_mpos->get() * axis_settings[_axis_index]->steps_per_mm->get(); // convert to steps diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 591ef5e3..036be861 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -231,3 +231,9 @@ char* trim(char* str) { end[1] = '\0'; return str; } + +// Returns the number of set number of set bits +// Would return 3 for 01100010 +int numberOfSetBits(uint32_t i) { + return __builtin_popcount(i); +} diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index 185be0a7..d133638f 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -53,11 +53,6 @@ static inline int toMotor2(int axis) { return axis + MAX_AXES; } -// CoreXY motor assignments. DO NOT ALTER. -// NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations. -#define A_MOTOR X_AXIS // Must be X_AXIS -#define B_MOTOR Y_AXIS // Must be Y_AXIS - // Conversions const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); @@ -111,6 +106,8 @@ float constrain_float(float in, float min, float max); bool char_is_numeric(char value); char* trim(char* value); +int numberOfSetBits(uint32_t i); + template void swap(T& a, T& b) { T c(a); diff --git a/Grbl_Esp32/src/Planner.cpp b/Grbl_Esp32/src/Planner.cpp index 47920960..9c06c77b 100644 --- a/Grbl_Esp32/src/Planner.cpp +++ b/Grbl_Esp32/src/Planner.cpp @@ -38,7 +38,7 @@ typedef struct { // from g-code position for movements requiring multiple line motions, // i.e. arcs, canned cycles, and backlash compensation. float previous_unit_vec[MAX_N_AXIS]; // Unit vector of previous path line segment - float previous_nominal_speed; // Nominal speed of previous path line segment + float previous_nominal_speed; // Nominal speed of previous path line segment } planner_t; static planner_t pl; @@ -313,49 +313,20 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) { uint8_t idx; // Copy position data based on type of motion being planned. if (block->motion.systemMotion) { -#ifdef COREXY - position_steps[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); - position_steps[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); - position_steps[Z_AXIS] = sys_position[Z_AXIS]; -#else memcpy(position_steps, sys_position, sizeof(sys_position)); -#endif } else { memcpy(position_steps, pl.position, sizeof(pl.position)); } -#ifdef COREXY - target_steps[A_MOTOR] = lround(target[A_MOTOR] * axis_settings[A_MOTOR]->steps_per_mm->get()); - target_steps[B_MOTOR] = lround(target[B_MOTOR] * axis_settings[B_MOTOR]->steps_per_mm->get()); - block->steps[A_MOTOR] = labs((target_steps[X_AXIS] - position_steps[X_AXIS]) + (target_steps[Y_AXIS] - position_steps[Y_AXIS])); - block->steps[B_MOTOR] = labs((target_steps[X_AXIS] - position_steps[X_AXIS]) - (target_steps[Y_AXIS] - position_steps[Y_AXIS])); -#endif auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { // Calculate target position in absolute steps, number of steps for each axis, and determine max step events. // Also, compute individual axes distance for move and prep unit vector calculations. // NOTE: Computes true distance from converted step values. -#ifdef COREXY - if (!(idx == A_MOTOR) && !(idx == B_MOTOR)) { - target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get()); - block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); - } - block->step_event_count = MAX(block->step_event_count, block->steps[idx]); - if (idx == A_MOTOR) { - delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] + target_steps[Y_AXIS] - position_steps[Y_AXIS]) / - axis_settings[idx]->steps_per_mm->get(); - } else if (idx == B_MOTOR) { - delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] - target_steps[Y_AXIS] + position_steps[Y_AXIS]) / - axis_settings[idx]->steps_per_mm->get(); - } else { - delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get(); - } -#else target_steps[idx] = lround(target[idx] * axis_settings[idx]->steps_per_mm->get()); block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); block->step_event_count = MAX(block->step_event_count, block->steps[idx]); delta_mm = (target_steps[idx] - position_steps[idx]) / axis_settings[idx]->steps_per_mm->get(); -#endif - unit_vec[idx] = delta_mm; // Store unit vector numerator + unit_vec[idx] = delta_mm; // Store unit vector numerator // Set direction bits. Bit enabled always means direction is negative. if (delta_mm < 0.0) { block->direction_bits |= bit(idx); @@ -456,19 +427,9 @@ void plan_sync_position() { // TODO: For motor configurations not in the same coordinate frame as the machine position, // this function needs to be updated to accomodate the difference. uint8_t idx; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (idx = 0; idx < n_axis; idx++) { -#ifdef COREXY - if (idx == X_AXIS) { - pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); - } else if (idx == Y_AXIS) { - pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); - } else { - pl.position[idx] = sys_position[idx]; - } -#else - pl.position[idx] = sys_position[idx]; -#endif + pl.position[idx] = sys_position[idx]; } } diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index f1a42673..57744e65 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -285,15 +285,15 @@ Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_le std::map restoreCommands = { #ifdef ENABLE_RESTORE_DEFAULT_SETTINGS - { "$", SettingsRestore::Defaults }, { "settings", SettingsRestore::Defaults }, + { "$", SettingsRestore::Defaults }, { "settings", SettingsRestore::Defaults }, #endif #ifdef ENABLE_RESTORE_CLEAR_PARAMETERS - { "#", SettingsRestore::Parameters }, { "gcode", SettingsRestore::Parameters }, + { "#", SettingsRestore::Parameters }, { "gcode", SettingsRestore::Parameters }, #endif #ifdef ENABLE_RESTORE_WIPE_ALL - { "*", SettingsRestore::All }, { "all", SettingsRestore::All }, + { "*", SettingsRestore::All }, { "all", SettingsRestore::All }, #endif - { "@", SettingsRestore::Wifi }, { "wifi", SettingsRestore::Wifi }, + { "@", SettingsRestore::Wifi }, { "wifi", SettingsRestore::Wifi }, }; Error restore_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { if (!value) { @@ -381,7 +381,7 @@ void make_grbl_commands() { new GrblCommand("+", "ExtendedSettings/List", report_extended_settings, notCycleOrHold); new GrblCommand("L", "GrblNames/List", list_grbl_names, notCycleOrHold); new GrblCommand("S", "Settings/List", list_settings, notCycleOrHold); - new GrblCommand("SC","Settings/ListChanged", list_changed_settings, notCycleOrHold); + new GrblCommand("SC", "Settings/ListChanged", list_changed_settings, notCycleOrHold); new GrblCommand("CMD", "Commands/List", list_commands, notCycleOrHold); new GrblCommand("E", "ErrorCodes/List", listErrorCodes, anyState); new GrblCommand("G", "GCode/Modes", report_gcode, anyState); diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 27ba9ea8..a7dda585 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -519,9 +519,6 @@ void report_build_info(const char* line, uint8_t client) { #ifdef COOLANT_MIST_PIN grbl_send(client, "M"); // TODO Need to deal with M8...it could be disabled #endif -#ifdef COREXY - grbl_send(client, "C"); -#endif #ifdef PARKING_ENABLE grbl_send(client, "P"); #endif @@ -941,7 +938,7 @@ char* reportAxisNameMsg(uint8_t axis) { void reportTaskStackSize(UBaseType_t& saved) { #ifdef DEBUG_REPORT_STACK_FREE - UBaseType_t newHighWater = uxTaskGetStackHighWaterMark(NULL); + UBaseType_t newHighWater = uxTaskGetStackHighWaterMark(NULL); if (newHighWater != saved) { saved = newHighWater; grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Min Stack Space: %d", pcTaskGetTaskName(NULL), saved); diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 5e356d99..5450b444 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -233,7 +233,7 @@ const char* AxisMaskSetting::getCompatibleValue() { } static char* maskToString(uint32_t mask, char* strval) { - char* s = strval; + char* s = strval; for (int i = 0; i < MAX_N_AXIS; i++) { if (mask & bit(i)) { *s++ = "XYZABC"[i]; @@ -566,7 +566,7 @@ void FlagSetting::setDefault() { } Error FlagSetting::setStringValue(char* s) { - s = trim(s); + s = trim(s); Error err = check(s); if (err != Error::Ok) { return err; diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index c513b8b4..63dce4f5 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -10,12 +10,12 @@ void settings_init(); // Define settings restore bitflags. enum SettingsRestore { - Defaults = bit(0), - Parameters = bit(1), + Defaults = bit(0), + Parameters = bit(1), StartupLines = bit(2), // BuildInfo = bit(3), // Obsolete Wifi = bit(4), - All = 0xff, + All = 0xff, }; // Restore subsets of settings to default values @@ -101,9 +101,9 @@ protected: public: static nvs_handle _handle; - static void init(); - static Setting* List; - Setting* next() { return link; } + static void init(); + static Setting* List; + Setting* next() { return link; } Error check(char* s); @@ -227,22 +227,25 @@ public: class Coordinates { private: - float _currentValue[MAX_N_AXIS]; + float _currentValue[MAX_N_AXIS]; const char* _name; + public: Coordinates(const char* name) : _name(name) {} const char* getName() { return _name; } - bool load(); - void setDefault() { - float zeros[MAX_N_AXIS] = { 0.0, }; + bool load(); + void setDefault() { + float zeros[MAX_N_AXIS] = { + 0.0, + }; set(zeros); }; // Copy the value to an array void get(float* value) { memcpy(value, _currentValue, sizeof(_currentValue)); } // Return a pointer to the array const float* get() { return _currentValue; } - void set(float *value); + void set(float* value); }; extern Coordinates* coords[CoordIndex::End]; diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index ef5f0f5a..53a561e8 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -35,19 +35,19 @@ IntSetting* status_mask; FloatSetting* junction_deviation; FloatSetting* arc_tolerance; -FloatSetting* homing_feed_rate; -FloatSetting* homing_seek_rate; -FloatSetting* homing_debounce; -FloatSetting* homing_pulloff; +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; -FloatSetting* spindle_delay_spinup; -FloatSetting* spindle_delay_spindown; -FlagSetting* spindle_enbl_off_with_zero_speed; -FlagSetting* spindle_enable_invert; -FlagSetting* spindle_output_invert; +FloatSetting* spindle_pwm_freq; +FloatSetting* rpm_max; +FloatSetting* rpm_min; +FloatSetting* spindle_delay_spinup; +FloatSetting* spindle_delay_spindown; +FlagSetting* spindle_enbl_off_with_zero_speed; +FlagSetting* spindle_enable_invert; +FlagSetting* spindle_output_invert; FloatSetting* spindle_pwm_off_value; FloatSetting* spindle_pwm_min_value; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index d8d18d00..498915f1 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -166,17 +166,7 @@ void system_flag_wco_change() { float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx) { float pos; float steps_per_mm = axis_settings[idx]->steps_per_mm->get(); -#ifdef COREXY - if (idx == X_AXIS) { - pos = (float)system_convert_corexy_to_x_axis_steps(steps) / steps_per_mm; - } else if (idx == Y_AXIS) { - pos = (float)system_convert_corexy_to_y_axis_steps(steps) / steps_per_mm; - } else { - pos = steps[idx] / steps_per_mm; - } -#else - pos = steps[idx] / steps_per_mm; -#endif + pos = steps[idx] / steps_per_mm; return pos; } @@ -275,15 +265,6 @@ void system_exec_control_pin(ControlPins pins) { } } -// CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps. -int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps) { - return (steps[A_MOTOR] + steps[B_MOTOR]) / 2; -} - -int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps) { - return (steps[A_MOTOR] - steps[B_MOTOR]) / 2; -} - // io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx // It uses a mask so all can be turned of in ms_reset bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized) { diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index e1ea051a..d416511a 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -213,9 +213,6 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx); // Updates a machine 'position' array based on the 'step' array sent. void system_convert_array_steps_to_mpos(float* position, int32_t* steps); -int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps); -int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps); - // A task that runs after a control switch interrupt for debouncing. void controlCheckTask(void* pvParameters); void system_exec_control_pin(ControlPins pins); diff --git a/platformio.ini b/platformio.ini index cdd2e8a0..99e632b0 100644 --- a/platformio.ini +++ b/platformio.ini @@ -31,6 +31,7 @@ build_flags = ;-DDEBUG_REPORT_HEAP_SIZE ;-DDEBUG_REPORT_STACK_FREE + [env] lib_deps = TMCStepper@>=0.7.0,<1.0.0 From 0e19f1e583439c454014f42937f70e6309c3c7a5 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 29 Oct 2020 09:25:33 -1000 Subject: [PATCH 31/82] Added $A AKA Alarms/List command (#654) * Added $A AKA Alarms/List command Similar to $E AKA Errors/List $E used to be AKA ErrorCodes/List Also added $Errors/Verbose setting to display full error text instead of the error number. It defaults to true because it works with every sender I have tried so far - cncjs, UGS, and Chrome GCode Sender. If you have problems with some sender you can set it to false. * Added static_assert per atlaste's comment * Added a default and fixed Authentication issue Co-authored-by: bdring --- Grbl_Esp32/src/Defaults.h | 4 ++ Grbl_Esp32/src/Error.cpp | 2 +- Grbl_Esp32/src/Error.h | 2 +- Grbl_Esp32/src/Exec.cpp | 15 +++++++ Grbl_Esp32/src/Exec.h | 57 ++++++++++++++++++++++++++ Grbl_Esp32/src/ProcessSettings.cpp | 43 ++++++++++++++++--- Grbl_Esp32/src/Report.cpp | 9 +++- Grbl_Esp32/src/SettingsDefinitions.cpp | 4 ++ Grbl_Esp32/src/SettingsDefinitions.h | 2 + Grbl_Esp32/src/System.h | 53 ++---------------------- Grbl_Esp32/src/WebUI/WebSettings.cpp | 10 ++--- 11 files changed, 137 insertions(+), 64 deletions(-) create mode 100644 Grbl_Esp32/src/Exec.cpp create mode 100644 Grbl_Esp32/src/Exec.h diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 42d7d9b9..6ca3728c 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -70,6 +70,10 @@ # define DEFAULT_STATUS_REPORT_MASK 1 // $10 #endif +#ifndef DEFAULT_VERBOSE_ERRORS +# define DEFAULT_VERBOSE_ERRORS 0 +#endif + #ifndef DEFAULT_JUNCTION_DEVIATION # define DEFAULT_JUNCTION_DEVIATION 0.01 // $11 mm #endif diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index e7fa5de6..149b0665 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -21,7 +21,7 @@ #include "Error.h" -std::map ErrorCodes = { +std::map ErrorNames = { { Error::Ok, "No error" }, { Error::ExpectedCommandLetter, "Expected GCodecommand letter" }, { Error::BadNumberFormat, "Bad GCode number format" }, diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index cfc2b379..09a54030 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -85,4 +85,4 @@ enum class Error : uint8_t { Eol = 111, }; -extern std::map ErrorCodes; +extern std::map ErrorNames; diff --git a/Grbl_Esp32/src/Exec.cpp b/Grbl_Esp32/src/Exec.cpp new file mode 100644 index 00000000..3cba113d --- /dev/null +++ b/Grbl_Esp32/src/Exec.cpp @@ -0,0 +1,15 @@ +#include "Exec.h" + +std::map AlarmNames = { + { ExecAlarm::None, "None"}, + { ExecAlarm::HardLimit, "Hard Limit"}, + { ExecAlarm::SoftLimit, "Soft Limit"}, + { ExecAlarm::AbortCycle, "Abort Cycle"}, + { ExecAlarm::ProbeFailInitial, "Probe Fail Initial"}, + { ExecAlarm::ProbeFailContact, "Probe Fail Contact"}, + { ExecAlarm::HomingFailReset, "Homing Fail Reset"}, + { ExecAlarm::HomingFailDoor, "Homing Fail Door"}, + { ExecAlarm::HomingFailPulloff, "Homing Fail Pulloff"}, + { ExecAlarm::HomingFailApproach, "Homing Fail Approach"}, + { ExecAlarm::SpindleControl, "Spindle Control"}, +}; diff --git a/Grbl_Esp32/src/Exec.h b/Grbl_Esp32/src/Exec.h new file mode 100644 index 00000000..23d7110d --- /dev/null +++ b/Grbl_Esp32/src/Exec.h @@ -0,0 +1,57 @@ +#pragma once + +#include + +// System executor bit map. Used internally by realtime protocol as realtime command flags, +// which notifies the main program to execute the specified realtime command asynchronously. +// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default +// flags are always false, so the realtime protocol only needs to check for a non-zero value to +// know when there is a realtime command to execute. +struct ExecStateBits { + uint8_t statusReport : 1; + uint8_t cycleStart : 1; + uint8_t cycleStop : 1; // Unused, per cycle_stop variable + uint8_t feedHold : 1; + uint8_t reset : 1; + uint8_t safetyDoor : 1; + uint8_t motionCancel : 1; + uint8_t sleep : 1; +}; + +union ExecState { + uint8_t value; + ExecStateBits bit; +}; + +static_assert(sizeof(ExecStateBits) == sizeof(uint8_t), "ExecStateBits is not an uint8"); + +// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. +// Spindle/coolant and feed/rapids are separated into two controlling flag variables. + +struct ExecAccessoryBits { + uint8_t spindleOvrStop : 1; + uint8_t coolantFloodOvrToggle : 1; + uint8_t coolantMistOvrToggle : 1; +}; + +union ExecAccessory { + uint8_t value; + ExecAccessoryBits bit; +}; + +// Alarm codes. +enum class ExecAlarm : uint8_t { + None = 0, + HardLimit = 1, + SoftLimit = 2, + AbortCycle = 3, + ProbeFailInitial = 4, + ProbeFailContact = 5, + HomingFailReset = 6, + HomingFailDoor = 7, + HomingFailPulloff = 8, + HomingFailApproach = 9, + SpindleControl = 10, +}; + +extern std::map AlarmNames; diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 57744e65..7e13ab83 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -325,12 +325,42 @@ Error doJog(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESP return gc_execute_line(jogLine, out->client()); } -const char* errorString(Error errorNumber) { - auto it = ErrorCodes.find(errorNumber); - return it == ErrorCodes.end() ? NULL : it->second; +const char* alarmString(ExecAlarm alarmNumber) { + auto it = AlarmNames.find(alarmNumber); + return it == AlarmNames.end() ? NULL : it->second; } -Error listErrorCodes(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { +Error listAlarms(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + if (value) { + char* endptr = NULL; + uint8_t alarmNumber = strtol(value, &endptr, 10); + if (*endptr) { + grbl_sendf(out->client(), "Malformed alarm number: %s\r\n", value); + return Error::InvalidValue; + } + const char* alarmName = alarmString(static_cast(alarmNumber)); + if (alarmName) { + grbl_sendf(out->client(), "%d: %s\r\n", alarmNumber, alarmName); + return Error::Ok; + } else { + grbl_sendf(out->client(), "Unknown alarm number: %d\r\n", alarmNumber); + return Error::InvalidValue; + } + } + + for (auto it = AlarmNames.begin(); it != AlarmNames.end(); it++) { + grbl_sendf(out->client(), "%d: %s\r\n", it->first, it->second); + } + return Error::Ok; +} + + +const char* errorString(Error errorNumber) { + auto it = ErrorNames.find(errorNumber); + return it == ErrorNames.end() ? NULL : it->second; +} + +Error listErrors(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { if (value) { char* endptr = NULL; uint8_t errorNumber = strtol(value, &endptr, 10); @@ -348,7 +378,7 @@ Error listErrorCodes(const char* value, WebUI::AuthenticationLevel auth_level, W } } - for (auto it = ErrorCodes.begin(); it != ErrorCodes.end(); it++) { + for (auto it = ErrorNames.begin(); it != ErrorNames.end(); it++) { grbl_sendf(out->client(), "%d: %s\r\n", it->first, it->second); } return Error::Ok; @@ -383,7 +413,8 @@ void make_grbl_commands() { new GrblCommand("S", "Settings/List", list_settings, notCycleOrHold); new GrblCommand("SC", "Settings/ListChanged", list_changed_settings, notCycleOrHold); new GrblCommand("CMD", "Commands/List", list_commands, notCycleOrHold); - new GrblCommand("E", "ErrorCodes/List", listErrorCodes, anyState); + new GrblCommand("A", "Alarms/List", listAlarms, anyState); + new GrblCommand("E", "Errors/List", listErrors, anyState); new GrblCommand("G", "GCode/Modes", report_gcode, anyState); new GrblCommand("C", "GCode/Check", toggle_check_mode, anyState); new GrblCommand("X", "Alarm/Disable", disable_alarm_lock, anyState); diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index a7dda585..20cff520 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -247,7 +247,14 @@ void report_status_message(Error status_code, uint8_t client) { return; } #endif - grbl_sendf(client, "error:%d\r\n", static_cast(status_code)); + // With verbose errors, the message text is displayed instead of the number. + // Grbl 0.9 used to display the text, while Grbl 1.1 switched to the number. + // Many senders support both formats. + if (verbose_errors->get()) { + grbl_sendf(client, "error: %s\r\n", errorString(status_code)); + } else { + grbl_sendf(client, "error:%d\r\n", static_cast(status_code)); + } } } diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 53a561e8..5e2c2e2c 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -2,6 +2,8 @@ bool motorSettingChanged = false; +FlagSetting* verbose_errors; + FakeSetting* number_axis; StringSetting* startup_line_0; @@ -237,6 +239,8 @@ void make_settings() { make_coordinate(CoordIndex::G28, "G28"); make_coordinate(CoordIndex::G30, "G30"); + verbose_errors = new FlagSetting(EXTENDED, WG, NULL, "Errors/Verbose", DEFAULT_VERBOSE_ERRORS); + // number_axis = new IntSetting(EXTENDED, WG, NULL, "NumberAxis", N_AXIS, 0, 6, NULL, true); number_axis = new FakeSetting(N_AXIS); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 0fd68b33..b01038bf 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -2,6 +2,8 @@ extern bool motorSettingChanged; +extern FlagSetting* verbose_errors; + extern FakeSetting* number_axis; extern AxisSettings* x_axis_settings; diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index d416511a..6803f7e2 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -20,6 +20,9 @@ along with Grbl. If not, see . */ +// Execution states and alarm +#include "Exec.h" + // System states. The state variable primarily tracks the individual functions // of Grbl to manage each without overlapping. It is also used as a messaging flag for // critical events. @@ -107,56 +110,6 @@ typedef struct { } system_t; extern system_t sys; -// System executor bit map. Used internally by realtime protocol as realtime command flags, -// which notifies the main program to execute the specified realtime command asynchronously. -// NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default -// flags are always false, so the realtime protocol only needs to check for a non-zero value to -// know when there is a realtime command to execute. -struct ExecStateBits { - uint8_t statusReport : 1; - uint8_t cycleStart : 1; - uint8_t cycleStop : 1; // Unused, per cycle_stop variable - uint8_t feedHold : 1; - uint8_t reset : 1; - uint8_t safetyDoor : 1; - uint8_t motionCancel : 1; - uint8_t sleep : 1; -}; - -union ExecState { - uint8_t value; - ExecStateBits bit; -}; - -// Alarm executor codes. Valid values (1-255). Zero is reserved. -enum class ExecAlarm : uint8_t { - None = 0, - HardLimit = 1, - SoftLimit = 2, - AbortCycle = 3, - ProbeFailInitial = 4, - ProbeFailContact = 5, - HomingFailReset = 6, - HomingFailDoor = 7, - HomingFailPulloff = 8, - HomingFailApproach = 9, - SpindleControl = 10, -}; - -// Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. -// Spindle/coolant and feed/rapids are separated into two controlling flag variables. - -struct ExecAccessoryBits { - uint8_t spindleOvrStop : 1; - uint8_t coolantFloodOvrToggle : 1; - uint8_t coolantMistOvrToggle : 1; -}; - -union ExecAccessory { - uint8_t value; - ExecAccessoryBits bit; -}; - // Control pin states struct ControlPinBits { uint8_t safetyDoor : 1; diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 09539a77..b9a34b8c 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -306,9 +306,9 @@ namespace WebUI { return Error::SdFailedOpenFile; } //until no line in file - Error err; - Error accumErr = Error::Ok; - uint8_t client = (espresponse) ? espresponse->client() : CLIENT_ALL; + Error err; + Error accumErr = Error::Ok; + uint8_t client = (espresponse) ? espresponse->client() : CLIENT_ALL; while (currentfile.available()) { String currentline = currentfile.readStringUntil('\n'); if (currentline.length() > 0) { @@ -398,7 +398,7 @@ namespace WebUI { user_password->setDefault(); return Error::Ok; } - if (user_password->setStringValue(parameter)) { + if (user_password->setStringValue(parameter) != Error::Ok) { webPrintln("Invalid Password"); return Error::InvalidValue; } @@ -730,7 +730,7 @@ namespace WebUI { webPrintln(""); return Error::Ok; } - SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; + SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL; SD_auth_level = auth_level; // execute the first line now; Protocol.cpp handles later ones when SD_ready_next report_status_message(execute_line(fileLine, SD_client, SD_auth_level), SD_client); From 54d78582eaf8a8790ddb580dc8b783b67652ffa9 Mon Sep 17 00:00:00 2001 From: Pete Wildsmith Date: Sun, 1 Nov 2020 17:16:26 +0100 Subject: [PATCH 32/82] TMC2130 plotter machine servo config update (#657) * TMC2130 plotter machine servo config update based on Slack conversation https://buildlog.slack.com/archives/CBZKZ8LHL/p1604243530253000 * Update Grbl.h --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Machines/tmc2130_pen.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 9dfc5291..10275f4b 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 = "20201027"; +const char* const GRBL_VERSION_BUILD = "20201101"; //#include #include diff --git a/Grbl_Esp32/src/Machines/tmc2130_pen.h b/Grbl_Esp32/src/Machines/tmc2130_pen.h index 93380577..24a0a88e 100644 --- a/Grbl_Esp32/src/Machines/tmc2130_pen.h +++ b/Grbl_Esp32/src/Machines/tmc2130_pen.h @@ -59,10 +59,10 @@ // Define one of these 2 options for spindle or servo #define Z_SERVO_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control. +#define DEFAULT_Z_MAX_TRAVEL 5.0 // Range of travel is 5mm +#define DEFAULT_Z_HOMING_MPOS 5.0 // MPos will be set to 5mm after homing +#define Z_SERVO_CAL_MIN 1.0 // calibration factor for the minimum PWM duty +#define Z_SERVO_CAL_MAX 1.0 // calibration factor for the maximum PWM duty // #define X_LIMIT_PIN See version section at beginning of file #define Y_LIMIT_PIN GPIO_NUM_4 - -// defaults -#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is used as the servo calibration -#define DEFAULT_Z_MAX_TRAVEL 300.0 // This is used as the servo calibration From 44cd6f89542d6ba07f8c186777211b996cba68a2 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 2 Nov 2020 15:34:29 -0600 Subject: [PATCH 33/82] Trinamic reporting (#656) * Enhanced reporting of errors * Change "motor" to "driver" for clarity. * Added better way to show changed Setting values from Mitch * Update build date --- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 100 +++++++++++++++++++---- Grbl_Esp32/src/Motors/TrinamicDriver.h | 21 +++-- Grbl_Esp32/src/ProcessSettings.cpp | 10 ++- 3 files changed, 101 insertions(+), 30 deletions(-) diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index d2c6e595..be0428ab 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -126,7 +126,7 @@ namespace Motors { void TrinamicDriver::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d %s", + "%s Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d R:%0.3f %s", reportAxisNameMsg(_axis_index, _dual_axis_index), _driver_part_number, pinName(_step_pin).c_str(), @@ -134,6 +134,7 @@ namespace Motors { pinName(_cs_pin).c_str(), pinName(_disable_pin).c_str(), _spi_index, + _r_sense, reportAxisLimitsMsg(_axis_index)); } @@ -161,24 +162,17 @@ namespace Motors { status.sr = tmcstepper->DRV_STATUS(); bool err = false; - // look for open or short to ground on a and b - if (status.s2ga || status.s2gb) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "%s Motor Short Coil a:%s b:%s", - reportAxisNameMsg(_axis_index, _dual_axis_index), - status.s2ga ? "Y" : "N", - status.s2gb ? "Y" : "N"); + + // look for errors + if (report_short_to_ground(status)) { err = true; } - // check for over temp or pre-warning - if (status.ot || status.otpw) { - grbl_msg_sendf(CLIENT_SERIAL, - MsgLevel::Info, - "%s Driver Temp Warning:%s Fault:%s", - reportAxisNameMsg(_axis_index, _dual_axis_index), - status.otpw ? "Y" : "N", - status.ot ? "Y" : "N"); + + if (report_over_temp(status)) { + err = true; + } + + if (report_short_to_ps(status)) { err = true; } @@ -214,7 +208,6 @@ namespace Motors { if (hold_i_percent > 1.0) hold_i_percent = 1.0; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Current run %d hold %f", reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, hold_i_percent); tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); tmcstepper->rms_current(run_i_ma, hold_i_percent); @@ -293,6 +286,22 @@ namespace Motors { tmcstepper->sg_result(), feedrate, axis_settings[_axis_index]->stallguard->get()); + + TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. + status.sr = tmcstepper->DRV_STATUS(); + + // these only report if there is a fault condition + report_open_load(status); + report_short_to_ground(status); + report_over_temp(status); + report_short_to_ps(status); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "%s Status Register %08x GSTAT %02x", + // reportAxisNameMsg(_axis_index, _dual_axis_index), + // status.sr, + // tmcstepper->GSTAT()); } // calculate a tstep from a rate @@ -366,4 +375,59 @@ namespace Motors { reportTaskStackSize(uxHighWaterMark); } } + + // =========== Reporting functions ======================== + + bool TrinamicDriver::report_open_load(TMC2130_n ::DRV_STATUS_t status) { + if (status.ola || status.olb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Open Load a:%s b:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.ola ? "Y" : "N", + status.olb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicDriver::report_short_to_ground(TMC2130_n ::DRV_STATUS_t status) { + if (status.s2ga || status.s2gb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Short Coil a:%s b:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.s2ga ? "Y" : "N", + status.s2gb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicDriver::report_over_temp(TMC2130_n ::DRV_STATUS_t status) { + if (status.ot || status.otpw) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Temp Warning:%s Fault:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.otpw ? "Y" : "N", + status.ot ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicDriver::report_short_to_ps(TMC2130_n ::DRV_STATUS_t status) { + // check for short to power supply + if ((status.sr & bit(12)) || (status.sr & bit(13))) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver Short vsa:%s vsb:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + (status.sr & bit(12)) ? "Y" : "N", + (status.sr & bit(13)) ? "Y" : "N"); + return true; + } + return false; // no error + } } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h index 004aa0dd..facedf0d 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h @@ -78,9 +78,7 @@ namespace Motors { uint8_t cs_pin, uint16_t driver_part_number, float r_sense) : - TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, - cs_pin, driver_part_number, r_sense, get_next_index()) - {} + TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin, cs_pin, driver_part_number, r_sense, get_next_index()) {} TrinamicDriver(uint8_t axis_index, uint8_t step_pin, @@ -112,18 +110,23 @@ namespace Motors { bool _disabled; TrinamicMode _mode = TrinamicMode::None; - bool test(); - void set_mode(bool isHoming); - void trinamic_test_response(); - void trinamic_stepper_enable(bool enable); + bool test(); + void set_mode(bool isHoming); + void trinamic_test_response(); + void trinamic_stepper_enable(bool enable); + + bool report_open_load(TMC2130_n ::DRV_STATUS_t status); + bool report_short_to_ground(TMC2130_n ::DRV_STATUS_t status); + bool report_over_temp(TMC2130_n ::DRV_STATUS_t status); + bool report_short_to_ps(TMC2130_n ::DRV_STATUS_t status); uint8_t get_next_index(); // Linked list of Trinamic driver instances, used by the // StallGuard reporting task. static TrinamicDriver* List; - TrinamicDriver* link; - static void readSgTask(void*); + TrinamicDriver* link; + static void readSgTask(void*); protected: void config_message() override; diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 7e13ab83..1a64d945 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -156,9 +156,13 @@ Error list_settings(const char* value, WebUI::AuthenticationLevel auth_level, We } Error list_changed_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { for (Setting* s = Setting::List; s; s = s->next()) { - const char* value = s->getStringValue(); - if (!auth_failed(s, value, auth_level) && strcmp(value, s->getDefaultString())) { - show_setting(s->getName(), value, NULL, out); + const char* value = s->getStringValue(); + const char* defval = s->getDefaultString(); + if (!auth_failed(s, value, auth_level) && strcmp(value, defval)) { + String message = "(Default="; + message += defval; + message += ")"; + show_setting(s->getName(), value, message.c_str(), out); } } grbl_sendf(out->client(), "(Passwords not shown)\r\n"); From 205e0bd5cfad0759d115cef07aaa8a204a3dbb42 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 2 Nov 2020 15:35:14 -0600 Subject: [PATCH 34/82] Machine Definition Cleanup (#658) - Removed machine definitions to speed up testing. - Moved 6 pack CS/MS3 pins with other axis pins to help them stay in sync with the aixs letters --- Grbl_Esp32/src/Machines/3axis_v3.h | 57 ------- Grbl_Esp32/src/Machines/3axis_xyx.h | 59 ------- .../Machines/6_pack_Lowrider_stepstick_v1.h | 16 +- .../src/Machines/6_pack_MPCNC_stepstick_v1.h | 11 +- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 6 +- Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h | 13 +- Grbl_Esp32/src/Machines/espduino.h | 61 ------- .../src/Machines/spi_daisy_4axis_xyyz.h | 95 ----------- .../src/Machines/tapster_pro_stepstick.h | 155 ------------------ 9 files changed, 21 insertions(+), 452 deletions(-) delete mode 100644 Grbl_Esp32/src/Machines/3axis_v3.h delete mode 100644 Grbl_Esp32/src/Machines/3axis_xyx.h delete mode 100644 Grbl_Esp32/src/Machines/espduino.h delete mode 100644 Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h delete mode 100644 Grbl_Esp32/src/Machines/tapster_pro_stepstick.h diff --git a/Grbl_Esp32/src/Machines/3axis_v3.h b/Grbl_Esp32/src/Machines/3axis_v3.h deleted file mode 100644 index 7ed13584..00000000 --- a/Grbl_Esp32/src/Machines/3axis_v3.h +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once -// clang-format off - -/* - 3axis_v3.h - Part of Grbl_ESP32 - - Pin assignments for the ESP32 Development Controller, v3.5. - https://github.com/bdring/Grbl_ESP32_Development_Controller - https://www.tindie.com/products/33366583/grbl_esp32-cnc-development-board-v35/ - - 2018 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 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. - - Grbl 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 Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESP32_V3.5" - -#define X_STEP_PIN GPIO_NUM_12 -#define X_DIRECTION_PIN GPIO_NUM_26 -#define Y_STEP_PIN GPIO_NUM_14 -#define Y_DIRECTION_PIN GPIO_NUM_25 -#define Z_STEP_PIN GPIO_NUM_27 -#define Z_DIRECTION_PIN GPIO_NUM_33 - -#define X_LIMIT_PIN GPIO_NUM_2 // labeled X Limit -#define Y_LIMIT_PIN GPIO_NUM_4 // labeled Y Limit -#define Z_LIMIT_PIN GPIO_NUM_15 // labeled Z Limit - -// OK to comment out to use pin for other features -#define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_17 // labeled SpinPWM - -#define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl - -#define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist -#define COOLANT_FLOOD_PIN GPIO_NUM_16 // labeled Flood -#define PROBE_PIN GPIO_NUM_32 // labeled Probe - -#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_35 // labeled Door, needs external pullup -#define CONTROL_RESET_PIN GPIO_NUM_34 // labeled Reset, needs external pullup -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_36 // labeled Hold, needs external pullup -#define CONTROL_CYCLE_START_PIN GPIO_NUM_39 // labeled Start, needs external pullup diff --git a/Grbl_Esp32/src/Machines/3axis_xyx.h b/Grbl_Esp32/src/Machines/3axis_xyx.h deleted file mode 100644 index f7b9bda9..00000000 --- a/Grbl_Esp32/src/Machines/3axis_xyx.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once -// clang-format off - -/* - 3axis_xyx.h - Part of Grbl_ESP32 - - Pin assignments for the ESP32 Development Controller - used to drive a dual motor gantry where the drivers - labeled X, Y and Z drive the machine axes X, Y and X. - https://github.com/bdring/Grbl_ESP32_Development_Controller - https://www.tindie.com/products/33366583/grbl_esp32-cnc-development-board-v35/ - - 2020 - Mitch Bradley - - Grbl_ESP32 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. - - Grbl 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 Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESP32_V4_XYX" -#define X_STEP_PIN GPIO_NUM_26 /* labeled Y */ -#define X_DIRECTION_PIN GPIO_NUM_15 /* labeled Y */ -#define Y_STEP_PIN GPIO_NUM_12 /* labeled X */ -#define Y_DIRECTION_PIN GPIO_NUM_14 /* labeled X */ -#define Y2_STEP_PIN GPIO_NUM_27 /* labeled Z */ -#define Y2_DIRECTION_PIN GPIO_NUM_33 /* labeled Z */ - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_2 -#define SPINDLE_ENABLE_PIN GPIO_NUM_22 - -#define X_LIMIT_PIN GPIO_NUM_17 -#define Y_LIMIT_PIN GPIO_NUM_4 -// #define Z_LIMIT_PIN GPIO_NUM_16 - -#define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -#define COOLANT_MIST_PIN GPIO_NUM_21 -#define COOLANT_FLOOD_PIN GPIO_NUM_25 - - - -// see versions for X and Z -#define PROBE_PIN GPIO_NUM_32 - -#define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_35 // needs external pullup -#define CONTROL_RESET_PIN GPIO_NUM_34 // needs external pullup -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_36 // needs external pullup -#define CONTROL_CYCLE_START_PIN GPIO_NUM_39 // needs external pullup diff --git a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h index 9e935607..8131f6d8 100644 --- a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h @@ -39,37 +39,35 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // Labeled X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define X2_STEPPER_MS3 I2SO(14) // A_CS -#define Y2_STEPPER_MS3 I2SO(19) // B_CS - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Y2_DISABLE_PIN I2SO(8) #define Y2_DIRECTION_PIN I2SO(9) #define Y2_STEP_PIN I2SO(10) +#define Y2_STEPPER_MS3 I2SO(11) // Motor Socket #4 #define Z_DIRECTION_PIN I2SO(12) #define Z_STEP_PIN I2SO(13) +#define Z_STEPPER_MS3 I2SO(14) #define Z_DISABLE_PIN I2SO(15) // Motor Socket #5 #define Z2_DISABLE_PIN I2SO(16) #define Z2_DIRECTION_PIN I2SO(17) #define Z2_STEP_PIN I2SO(18) - +#define Z2_STEPPER_MS3 I2SO(19) /* Socket I/O reference @@ -112,8 +110,8 @@ Socket #5 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module #define X_LIMIT_PIN GPIO_NUM_33 #define Y_LIMIT_PIN GPIO_NUM_32 -#define Y2_LIMIT_PIN GPIO_NUM_35 -#define Z_LIMIT_PIN GPIO_NUM_34 +#define Y2_LIMIT_PIN GPIO_NUM_35 +#define Z_LIMIT_PIN GPIO_NUM_34 // 4x Input Module in Socket #2 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module diff --git a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h index 034dade2..5d3ca518 100644 --- a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h @@ -39,36 +39,35 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // Labeled X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define X2_STEPPER_MS3 I2SO(14) // A_CS -#define Y2_STEPPER_MS3 I2SO(19) // B_CS - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) +#define Z_STEPPER_MS3 I2SO(11) // Motor Socket #4 #define X2_DIRECTION_PIN I2SO(12) #define X2_STEP_PIN I2SO(13) +#define X2_STEPPER_MS3 I2SO(14) #define X2_DISABLE_PIN I2SO(15) // Motor Socket #5 #define Y2_DISABLE_PIN I2SO(16) #define Y2_DIRECTION_PIN I2SO(17) #define Y2_STEP_PIN I2SO(18) +#define Y2_STEPPER_MS3 I2SO(19) /* diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index a258d32f..b86c0c26 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -41,24 +41,24 @@ #define I2S_OUT_WS GPIO_NUM_17 #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) +#define Z_STEPPER_MS3 I2SO(11) /* Socket I/O reference diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h index caf673be..084136b0 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h @@ -40,41 +40,40 @@ #define I2S_OUT_DATA GPIO_NUM_21 -#define X_STEPPER_MS3 I2SO(3) // X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS -#define A_STEPPER_MS3 I2SO(14) // A_CS -#define B_STEPPER_MS3 I2SO(19) // B_CS -#define C_STEPPER_MS3 I2SO(22) // C_CS - // Motor Socket #1 #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) +#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) +#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) +#define Z_STEPPER_MS3 I2SO(11) // Motor Socket #4 #define A_DIRECTION_PIN I2SO(12) #define A_STEP_PIN I2SO(13) +#define A_STEPPER_MS3 I2SO(14) #define A_DISABLE_PIN I2SO(15) // Motor Socket #5 #define B_DISABLE_PIN I2SO(16) #define B_DIRECTION_PIN I2SO(17) #define B_STEP_PIN I2SO(18) +#define B_STEPPER_MS3 I2SO(19) // Motor Socket #5 #define C_DIRECTION_PIN I2SO(20) #define C_STEP_PIN I2SO(21) +#define C_STEPPER_MS3 I2SO(22) #define C_DISABLE_PIN I2SO(23) diff --git a/Grbl_Esp32/src/Machines/espduino.h b/Grbl_Esp32/src/Machines/espduino.h deleted file mode 100644 index 7bc6b8a4..00000000 --- a/Grbl_Esp32/src/Machines/espduino.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once -// clang-format off - -/* - espduino.h - Part of Grbl_ESP32 - - Pin assignments for ESPDUINO-32 Boards and Protoneer V3 boards - Note: Probe pin is mapped, but will require a 10k external pullup to 3.3V to work. - - Rebooting...See this issue https://github.com/bdring/Grbl_Esp32/issues/314 - !!!! Experimental Untested !!!!! - - 2019 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 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. - - Grbl 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 Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESPDUINO_32" - -#define X_STEP_PIN GPIO_NUM_26 -#define X_DIRECTION_PIN GPIO_NUM_16 - -#define Y_STEP_PIN GPIO_NUM_25 -#define Y_DIRECTION_PIN GPIO_NUM_27 - -#define Z_STEP_PIN GPIO_NUM_17 -#define Z_DIRECTION_PIN GPIO_NUM_14 - -// OK to comment out to use pin for other features -#define STEPPERS_DISABLE_PIN GPIO_NUM_12 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_19 - -#define SPINDLE_DIR_PIN GPIO_NUM_18 - -#define COOLANT_FLOOD_PIN GPIO_NUM_34 -#define COOLANT_MIST_PIN GPIO_NUM_36 - -#define X_LIMIT_PIN GPIO_NUM_13 -#define Y_LIMIT_PIN GPIO_NUM_5 -#define Z_LIMIT_PIN GPIO_NUM_19 - -#define PROBE_PIN GPIO_NUM_39 - -#define CONTROL_RESET_PIN GPIO_NUM_2 -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 -#define CONTROL_CYCLE_START_PIN GPIO_NUM_35 // ESP32 needs external pullup diff --git a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h b/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h deleted file mode 100644 index 1bd15efd..00000000 --- a/Grbl_Esp32/src/Machines/spi_daisy_4axis_xyyz.h +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once -// clang-format off - -/* - spi_daisy_4axis_xyyz.h - Part of Grbl_ESP32 - - Pin assignments for a 3-axis with Y ganged using Triaminic drivers - in daisy-chained SPI mode. - https://github.com/bdring/4_Axis_SPI_CNC - - 2019 - Bart Dring - 2020 - Mitch Bradley - - Grbl_ESP32 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. - - Grbl 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 Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "SPI_DAISY_4X_xyyz" - -#ifdef N_AXIS - #undef N_AXIS -#endif -#define N_AXIS 3 // can be 3 or 4. (if 3 install bypass jumper next to the A driver) - -#define TRINAMIC_DAISY_CHAIN - -#define TRINAMIC_RUN_MODE TrinamicMode :: CoolStep -#define TRINAMIC_HOMING_MODE TrinamicMode :: CoolStep - -// Use SPI enable instead of the enable pin -// The hardware enable pin is tied to ground -#define USE_TRINAMIC_ENABLE - -#define DEFAULT_HOMING_SQUARED_AXES bit(Y_AXIS) - -// Y motor connects to the 1st driver -#define X_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define X_RSENSE TMC2130_RSENSE_DEFAULT -#define X_STEP_PIN GPIO_NUM_12 -#define X_DIRECTION_PIN GPIO_NUM_14 -#define X_CS_PIN GPIO_NUM_17 // Daisy Chain, all share same CS pin - -// Y motor connects to the 2nd driver -#define Y_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Y_RSENSE TMC2130_RSENSE_DEFAULT -#define Y_STEP_PIN GPIO_NUM_27 -#define Y_DIRECTION_PIN GPIO_NUM_26 -#define Y_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -// Y2 motor connects to the 2nd driver -#define Y2_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Y2_RSENSE TMC2130_RSENSE_DEFAULT -#define Y2_STEP_PIN GPIO_NUM_15 // Z on schem -#define Y2_DIRECTION_PIN GPIO_NUM_2 // Z on schem -#define Y2_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -// Z Axis motor connects to the 4th driver -#define Z_TRINAMIC_DRIVER 2130 // Which Driver Type? -#define Z_RSENSE TMC2130_RSENSE_DEFAULT -#define Z_STEP_PIN GPIO_NUM_33 // A on schem -#define Z_DIRECTION_PIN GPIO_NUM_32 // A on schem -#define Z_CS_PIN X_CS_PIN // Daisy Chain, all share same CS pin - -// Mist is a 3.3V output -// Turn on with M7 and off with M9 -#define COOLANT_MIST_PIN GPIO_NUM_21 - -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_25 -#define SPINDLE_ENABLE_PIN GPIO_NUM_4 - -// Relay operation -// Install Jumper near relay -// For spindle Use max RPM of 1 -// For PWM remove jumper and set MAX RPM to something higher ($30 setting) -// Interlock jumper along top edge needs to be installed for both versions -#define DEFAULT_SPINDLE_RPM_MAX 1 // Should be 1 for relay operation - -#define PROBE_PIN GPIO_NUM_22 - -#define X_LIMIT_PIN GPIO_NUM_36 -#define Y_LIMIT_PIN GPIO_NUM_39 -#define Z_LIMIT_PIN GPIO_NUM_34 - diff --git a/Grbl_Esp32/src/Machines/tapster_pro_stepstick.h b/Grbl_Esp32/src/Machines/tapster_pro_stepstick.h deleted file mode 100644 index 366157c5..00000000 --- a/Grbl_Esp32/src/Machines/tapster_pro_stepstick.h +++ /dev/null @@ -1,155 +0,0 @@ -#pragma once -// clang-format off - -/* - tapster_pro_stepstick.h - - 2020 - Bart Dring, Jason Huggins (Tapster Robotics) - - Grbl_ESP32 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. - Grbl 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 Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "Tapster Pro Delta (StepStick)" - -#define CUSTOM_CODE_FILENAME "Custom/parallel_delta.cpp" -/* -// enable these special machine functions to be called from the main program -#define USE_KINEMATICS // there are kinematic equations for this machine -#define FWD_KINEMATICS_REPORTING // report in cartesian -#define USE_RMT_STEPS // Use the RMT periferal to generate step pulses -#define USE_TRINAMIC // some Trinamic motors are used on this machine -#define USE_MACHINE_TRINAMIC_INIT // there is a machine specific setup for the drivers -#define USE_MACHINE_INIT // There is some custom initialization for this machine - -#define SEGMENT_LENGTH 0.5 // segment length in mm -#define KIN_ANGLE_CALC_OK 0 -#define KIN_ANGLE_ERROR -1 - -#define MAX_NEGATIVE_ANGLE -36 // in degrees how far can the arms go up? - -#define HOMING_CURRENT_REDUCTION 1.0 - -*/ - -#define N_AXIS 3 - -#define USE_KINEMATICS // there are kinematic equations for this machine -#define USE_FWD_KINEMATICS // report in cartesian -#define USE_MACHINE_INIT // There is some custom initialization for this machine - -// ================== Delta Geometry =========================== - -#define RADIUS_FIXED 100.0f // radius of the fixed side (length of motor cranks) -#define RADIUS_EFF 220.0f // radius of end effector side (length of linkages) -#define LENGTH_FIXED_SIDE 294.449f // sized of fixed side triangel -#define LENGTH_EFF_SIDE 86.6025f // size of end effector side triangle -#define KINEMATIC_SEGMENT_LENGTH 1.0f // segment length in mm -#define MAX_NEGATIVE_ANGLE -0.75f // -#define MAX_POSITIVE_ANGLE (M_PI / 2.0) // - - -// ================== Config ====================== - -// Set $Homing/Cycle0=XYZ - -// === Special Features - -// I2S (steppers & other output-only pins) -#define USE_I2S_OUT -#define USE_I2S_STEPS -//#define DEFAULT_STEPPER ST_I2S_STATIC -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE - -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set - -#define I2S_OUT_BCK GPIO_NUM_22 -#define I2S_OUT_WS GPIO_NUM_17 -#define I2S_OUT_DATA GPIO_NUM_21 - -// ================== CPU MAP ====================== - -#define X_STEPPER_MS3 I2SO(3) // X_CS -#define Y_STEPPER_MS3 I2SO(6) // Y_CS -#define Z_STEPPER_MS3 I2SO(11) // Z_CS - -#define STEPPER_RESET GPIO_NUM_19 - -// Motor Socket #1 -#define X_DISABLE_PIN I2SO(0) -#define X_DIRECTION_PIN I2SO(1) -#define X_STEP_PIN I2SO(2) - -// Motor Socket #2 -#define Y_DIRECTION_PIN I2SO(4) -#define Y_STEP_PIN I2SO(5) -#define Y_DISABLE_PIN I2SO(7) - -// Motor Socket #3 -#define Z_DISABLE_PIN I2SO(8) -#define Z_DIRECTION_PIN I2SO(9) -#define Z_STEP_PIN I2SO(10) - -// CNC I/O Modules - -#define X_LIMIT_PIN GPIO_NUM_33 -#define Y_LIMIT_PIN GPIO_NUM_32 -#define Z_LIMIT_PIN GPIO_NUM_35 - -// ================= defaults =========================== - -#define DEFAULT_STEPPER_IDLE_LOCK_TIME 255 // keep them on, the trinamics will reduce power at idle - -#define DEFAULT_X_MICROSTEPS 32 -#define DEFAULT_Y_MICROSTEPS DEFAULT_X_MICROSTEPS -#define DEFAULT_Z_MICROSTEPS DEFAULT_X_MICROSTEPS - -// some math to figure out microsteps per unit // units could bedegrees or radians -#define UNITS_PER_REV (2.0 * M_PI) // 360.0 degrees or 6.2831853 radians -#define STEPS_PER_REV 400.0 -#define REDUCTION_RATIO (60.0 / 16.0) // the pulleys on arm and motor -#define MICROSTEPS_PER_REV (STEPS_PER_REV * (float)DEFAULT_X_MICROSTEPS * REDUCTION_RATIO) - -#define DEFAULT_X_STEPS_PER_MM (MICROSTEPS_PER_REV / UNITS_PER_REV) -#define DEFAULT_Y_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM -#define DEFAULT_Z_STEPS_PER_MM DEFAULT_X_STEPS_PER_MM - -#define DEFAULT_X_MAX_RATE 100.0 // mm/min -#define DEFAULT_Y_MAX_RATE DEFAULT_X_MAX_RATE -#define DEFAULT_Z_MAX_RATE DEFAULT_X_MAX_RATE - -#define DEFAULT_X_ACCELERATION 20.0 -#define DEFAULT_Y_ACCELERATION DEFAULT_X_ACCELERATION -#define DEFAULT_Z_ACCELERATION DEFAULT_X_ACCELERATION - -// homing -#define DEFAULT_HOMING_FEED_RATE 25 -#define DEFAULT_HOMING_SEEK_RATE 100 -#define DEFAULT_HOMING_DIR_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) // all axes home negative -#define DEFAULT_HOMING_ENABLE 1 -#define DEFAULT_INVERT_LIMIT_PINS 0 - -// The machine homes up and above center. MPos is the axis angle in radians -// at the homing posiiton - -#define DEFAULT_X_HOMING_MPOS -0.75 // neagtive because above horizontal -#define DEFAULT_Y_HOMING_MPOS -0.75 -#define DEFAULT_Z_HOMING_MPOS -0.75 - -// the total travel is straight down from horizontal (pi/2) + the up travel -#define DEFAULT_X_MAX_TRAVEL ((M_PI / 2.0) - DEFAULT_X_HOMING_MPOS) -#define DEFAULT_Y_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL -#define DEFAULT_Z_MAX_TRAVEL DEFAULT_X_MAX_TRAVEL - -#define DEFAULT_HOMING_PULLOFF -DEFAULT_X_HOMING_MPOS - -#define SPINDLE_TYPE SpindleType::NONE \ No newline at end of file From 42c37bbfe8ee677f7f584ba0a835bcbbdc9783a1 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 22 Nov 2020 12:02:57 -0600 Subject: [PATCH 35/82] Spindle delay and Telnet Fix (#676) * Removed early saving of old state Was causing later tests to be wrong * Update Grbl.h * Update TelnetServer.cpp Remove filtering of '\r' character. --- .../Custom/esp32_printer_controller.cpp | 183 ------------------ Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 2 - Grbl_Esp32/src/WebUI/TelnetServer.cpp | 10 +- 4 files changed, 6 insertions(+), 191 deletions(-) delete mode 100644 Grbl_Esp32/Custom/esp32_printer_controller.cpp diff --git a/Grbl_Esp32/Custom/esp32_printer_controller.cpp b/Grbl_Esp32/Custom/esp32_printer_controller.cpp deleted file mode 100644 index 6c2a8cd1..00000000 --- a/Grbl_Esp32/Custom/esp32_printer_controller.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - esp32_printer_controller.cpp (copy and use your machine name) - Part of Grbl_ESP32 - - copyright (c) 2020 - Bart Dring. This file was intended for use on the ESP32 - - ...add your date and name here. - - CPU. Do not use this with Grbl for atMega328P - - Grbl_ESP32 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. - - Grbl_ESP32 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 Grbl. If not, see . - - ======================================================================= - -This is a template for user-defined C++ code functions. Grbl can be -configured to call some optional functions, enabled by #define statements -in the machine definition .h file. Implement the functions thus enabled -herein. The possible functions are listed and described below. - -To use this file, copy it to a name of your own choosing, and also copy -Machines/template.h to a similar name. - -Example: -Machines/my_machine.h -Custom/my_machine.cpp - -Edit machine.h to include your Machines/my_machine.h file - -Edit Machines/my_machine.h according to the instructions therein. - -Fill in the function definitions below for the functions that you have -enabled with USE_ defines in Machines/my_machine.h - -=============================================================================== - -*/ - -#ifdef USE_MACHINE_INIT -/* -machine_init() is called when Grbl_ESP32 first starts. You can use it to do any -special things your machine needs at startup. -*/ -# define STEPPERS_DISABLE_PIN_X 138 -# define STEPPERS_DISABLE_PIN_Y 134 -# define STEPPERS_DISABLE_PIN_Z 131 -# define STEPPERS_DISABLE_PIN_A 139 - -# define FAN1_PIN 13 -# define FAN2_PIN 142 -# define FAN3_PIN 143 - -# define BED_PIN 4 -# define NOZZLE_PIN 2 - -void machine_init() { - // Enable steppers - digitalWrite(STEPPERS_DISABLE_PIN_X, LOW); // enable - digitalWrite(STEPPERS_DISABLE_PIN_Y, LOW); // enable - digitalWrite(STEPPERS_DISABLE_PIN_Z, LOW); // enable - digitalWrite(STEPPERS_DISABLE_PIN_A, LOW); // enable - - // digitalWrite(FAN1_PIN, LOW); // comment out for JTAG debugging - - digitalWrite(FAN2_PIN, LOW); // disable - digitalWrite(FAN3_PIN, LOW); // disable - - digitalWrite(BED_PIN, LOW); // disable - digitalWrite(NOZZLE_PIN, LOW); // disable -} -#endif - -#ifdef USE_CUSTOM_HOMING -/* - (user_defined_homing) is called at the begining of the normal Grbl_ESP32 homing - sequence. If user_defined_homing() returns false, the rest of normal Grbl_ESP32 - homing is skipped if it returns false, other normal homing continues. For - example, if you need to manually prep the machine for homing, you could implement - user_defined_homing() to wait for some button to be pressed, then return true. -*/ -bool user_defined_homing(uint8_t cycle_mask) { - // True = done with homing, false = continue with normal Grbl_ESP32 homing - return true; -} -#endif - -#ifdef USE_KINEMATICS -/* - Inverse Kinematics converts X,Y,Z cartesian coordinate to the steps - on your "joint" motors. It requires the following three functions: -*/ - -/* - inverse_kinematics() looks at incoming move commands and modifies - them before Grbl_ESP32 puts them in the motion planner. - - Grbl_ESP32 processes arcs by converting them into tiny little line segments. - Kinematics in Grbl_ESP32 works the same way. Search for this function across - Grbl_ESP32 for examples. You are basically converting cartesian X,Y,Z... targets to - - target = an N_AXIS array of target positions (where the move is supposed to go) - pl_data = planner data (see the definition of this type to see what it is) - position = an N_AXIS array of where the machine is starting from for this move -*/ -void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* position) { - // this simply moves to the target. Replace with your kinematics. - mc_line(target, pl_data); -} - -/* - kinematics_pre_homing() is called before normal homing - You can use it to do special homing or just to set stuff up - - cycle_mask is a bit mask of the axes being homed this time. -*/ -bool kinematics_pre_homing(uint8_t cycle_mask)) -{ - return false; // finish normal homing cycle -} - -/* - kinematics_post_homing() is called at the end of normal homing -*/ -void kinematics_post_homing() {} -#endif - -#ifdef USE_FWD_KINEMATICS -/* - The status command uses forward_kinematics() to convert - your motor positions to cartesian X,Y,Z... coordinates. - - Convert the N_AXIS array of motor positions to cartesian in your code. -*/ -void forward_kinematics(float* position) { - // position[X_AXIS] = - // position[Y_AXIS] = -} -#endif - -#ifdef USE_TOOL_CHANGE -/* - user_tool_change() is called when tool change gcode is received, - to perform appropriate actions for your machine. -*/ -void user_tool_change(uint8_t new_tool) {} -#endif - -#if defined(MACRO_BUTTON_0_PIN) || defined(MACRO_BUTTON_1_PIN) || defined(MACRO_BUTTON_2_PIN) -/* - options. user_defined_macro() is called with the button number to - perform whatever actions you choose. -*/ -void user_defined_macro(uint8_t index) {} -#endif - -#ifdef USE_M30 -/* - user_m30() is called when an M30 gcode signals the end of a gcode file. -*/ -void user_m30() {} -#endif - -#ifdef USE_MACHINE_TRINAMIC_INIT -/* - machine_triaminic_setup() replaces the normal setup of trinamic - drivers with your own code. For example, you could setup StallGuard -*/ -void machine_trinamic_setup() {} -#endif - -// If you add any additional functions specific to your machine that -// require calls from common code, guard their calls in the common code with -// #ifdef USE_WHATEVER and add function prototypes (also guarded) to grbl.h diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 10275f4b..2d0a3ddb 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 = "20201101"; +const char* const GRBL_VERSION_BUILD = "20201121"; //#include #include diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 53c87720..3b47c72d 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -155,8 +155,6 @@ namespace Spindles { return; // Block during abort. } - _current_state = state; - if (_current_state == SpindleState::Disable) { // Halt or set spindle direction and rpm. sys.spindle_speed = 0; stop(); diff --git a/Grbl_Esp32/src/WebUI/TelnetServer.cpp b/Grbl_Esp32/src/WebUI/TelnetServer.cpp index 0829bddb..b5cfaa1e 100644 --- a/Grbl_Esp32/src/WebUI/TelnetServer.cpp +++ b/Grbl_Esp32/src/WebUI/TelnetServer.cpp @@ -208,11 +208,11 @@ namespace WebUI { if (current > (TELNETRXBUFFERSIZE - 1)) { current = 0; } - if (char(data[i]) != '\r') { - _RXbuffer[current] = data[i]; - current++; - data_processed++; - } + + _RXbuffer[current] = data[i]; + current++; + data_processed++; + COMMANDS::wait(0); //vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks } From 6d2c609ad37fcd9ee43f401a130fc0c0619f6397 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 22 Nov 2020 16:50:01 -0600 Subject: [PATCH 36/82] ABC Bresenham counter init fix --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Stepper.cpp | 26 ++++++++++++-------------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 2d0a3ddb..d5e9349f 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 = "20201121"; +const char* const GRBL_VERSION_BUILD = "20201122"; //#include #include diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 82c4a921..ae4a56b2 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -44,11 +44,11 @@ static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1]; // planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by // the planner, where the remaining planner block steps still can. typedef struct { - uint16_t n_step; // Number of step events to be executed for this segment - uint16_t isrPeriod; // Time to next ISR tick, in units of timer ticks - uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. - uint8_t amass_level; // AMASS level for the ISR to execute this segment - uint16_t spindle_rpm; // TODO get rid of this. + uint16_t n_step; // Number of step events to be executed for this segment + uint16_t isrPeriod; // Time to next ISR tick, in units of timer ticks + uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. + uint8_t amass_level; // AMASS level for the ISR to execute this segment + uint16_t spindle_rpm; // TODO get rid of this. } segment_t; static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; @@ -62,10 +62,10 @@ typedef struct { uint8_t step_bits; // Stores out_bits output to complete the step pulse delay #endif - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t step_outbits; // The next stepping-bits to be output - uint8_t dir_outbits; + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t step_outbits; // The next stepping-bits to be output + uint8_t dir_outbits; uint32_t steps[MAX_N_AXIS]; uint16_t step_count; // Steps remaining in line segment motion @@ -245,11 +245,9 @@ static void stepper_pulse_func() { st.exec_block_index = st.exec_segment->st_block_index; st.exec_block = &st_block_buffer[st.exec_block_index]; // Initialize Bresenham line and distance counters - // XXX the original code only inits X, Y, Z here, instead of n_axis. Is that correct? - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < n_axis; axis++) { st.counter[axis] = (st.exec_block->step_event_count >> 1); } - // TODO ABC } st.dir_outbits = st.exec_block->direction_bits; // Adjust Bresenham axis increment counters according to AMASS level. @@ -825,7 +823,7 @@ void st_prep_buffer() { // typically very small and do not adversely effect performance, but ensures that Grbl // outputs the exact acceleration and velocity profiles as computed by the planner. - dt += prep.dt_remainder; // Apply previous segment partial step execute time + dt += prep.dt_remainder; // Apply previous segment partial step execute time // dt is in minutes so inv_rate is in minutes float inv_rate = dt / (last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse @@ -833,7 +831,7 @@ void st_prep_buffer() { // fStepperTimer is in units of timerTicks/sec, so the dimensional analysis is // timerTicks/sec * 60 sec/minute * minutes = timerTicks uint32_t timerTicks = ceil((fStepperTimer * 60) * inv_rate); // (timerTicks/step) - int level; + int level; // Compute step timing and multi-axis smoothing level. for (level = 0; level < maxAmassLevel; level++) { From 60c5a6c9e1a2eabd43d8a03505939139b8d0056d Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 23 Nov 2020 15:14:47 -0600 Subject: [PATCH 37/82] Rst responses (#679) * Added verification of changes from $RST command When sending $RST=$ you only get these responses. [MSG:WiFi reset done] [MSG:BT reset done] Added the other things that change. [MSG:WiFi reset done] [MSG:BT reset done] [MSG:Settings reset done] [MSG:Postion offsets reset done] * Update ProcessSettings.cpp * Update Grbl.h * Update ProcessSettings.cpp --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/ProcessSettings.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index d5e9349f..ed7e9bf9 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 = "20201122"; +const char* const GRBL_VERSION_BUILD = "20201123"; //#include #include diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 1a64d945..f8539203 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -54,12 +54,14 @@ void settings_restore(uint8_t restore_flag) { } } } + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Settings reset done"); } if (restore_flag & SettingsRestore::Parameters) { for (auto idx = CoordIndex::Begin; idx < CoordIndex::End; ++idx) { coords[idx]->setDefault(); } } + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Position offsets reset done"); } // Get settings values from non volatile storage into memory @@ -358,7 +360,6 @@ Error listAlarms(const char* value, WebUI::AuthenticationLevel auth_level, WebUI return Error::Ok; } - const char* errorString(Error errorNumber) { auto it = ErrorNames.find(errorNumber); return it == ErrorNames.end() ? NULL : it->second; From 8ac74d40171524eb50fd6dda9ef382f290f19b3e Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 24 Nov 2020 09:45:18 -0600 Subject: [PATCH 38/82] Fix Spindle State broken in earlier PR --- Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 3b47c72d..3e71ee7b 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -155,7 +155,7 @@ namespace Spindles { return; // Block during abort. } - if (_current_state == SpindleState::Disable) { // Halt or set spindle direction and rpm. + if (state == SpindleState::Disable) { // Halt or set spindle direction and rpm. sys.spindle_speed = 0; stop(); if (use_delays && (_current_state != state)) { @@ -164,9 +164,9 @@ namespace Spindles { //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Done"); } } else { - set_dir_pin(_current_state == SpindleState::Cw); + set_dir_pin(state == SpindleState::Cw); set_rpm(rpm); - set_enable_pin(_current_state != SpindleState::Disable); // must be done after setting rpm for enable features to work + set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work if (use_delays && (_current_state != state)) { //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Start %d", rpm); mc_dwell(spindle_delay_spinup->get()); From 920cc20693fc95ad927891b9fea34f598f5e4a5c Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 24 Nov 2020 09:46:05 -0600 Subject: [PATCH 39/82] Update Grbl.h --- Grbl_Esp32/src/Grbl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index ed7e9bf9..647e0c06 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 = "20201123"; +const char* const GRBL_VERSION_BUILD = "20201124"; //#include #include From abfb33e2d33da544483b2731b7ecb99c6a4136a5 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 29 Nov 2020 08:22:30 -0600 Subject: [PATCH 40/82] Spindle and laser (#683) * WIP * Updates * Updates - Added Laser/FullPower - Move some stuff from PWM to Laser * WIP * Used the stop function before resetiing pins. * Updates from discussion - Reset_pins is now deinit() - VFD task is now deleted when ... deinit() - Added a Motor/Disable command * Added Mitch's gambit * Cleanup - Finished VFD - Fixed Settings (Thanks Brian!) - changed task cores. * Update VFDSpindle.cpp * Update Laser.cpp * Fixing reset - gpio_reset_pin sets a pullup, which could turn on a device * Changed Spindle messages to CLIENT_ALL * Update Grbl.h * Updates after review * P.R. Cleanup * Most spindle settings cause a new init() --- Grbl_Esp32/Custom/atari_1020.cpp | 6 +- Grbl_Esp32/src/Defaults.h | 4 + Grbl_Esp32/src/Grbl.h | 2 +- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 23 +++-- .../src/Machines/mpcnc_laser_module_v1p2.h | 4 +- Grbl_Esp32/src/Motors/Motors.cpp | 69 +++++++------- Grbl_Esp32/src/Motors/Motors.h | 2 +- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 3 +- Grbl_Esp32/src/ProcessSettings.cpp | 34 +++++++ Grbl_Esp32/src/Serial.cpp | 3 +- Grbl_Esp32/src/Settings.cpp | 29 ++++-- Grbl_Esp32/src/Settings.h | 13 ++- Grbl_Esp32/src/SettingsDefinitions.cpp | 90 ++++++++++--------- Grbl_Esp32/src/SettingsDefinitions.h | 1 + Grbl_Esp32/src/Spindles/10vSpindle.cpp | 26 +++++- Grbl_Esp32/src/Spindles/10vSpindle.h | 1 + Grbl_Esp32/src/Spindles/BESCSpindle.cpp | 4 +- Grbl_Esp32/src/Spindles/DacSpindle.cpp | 6 +- Grbl_Esp32/src/Spindles/H2ASpindle.cpp | 2 +- Grbl_Esp32/src/Spindles/Laser.cpp | 61 ++++++++++++- Grbl_Esp32/src/Spindles/Laser.h | 2 + Grbl_Esp32/src/Spindles/NullSpindle.cpp | 2 +- Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 36 +++++--- Grbl_Esp32/src/Spindles/PWMSpindle.h | 5 +- Grbl_Esp32/src/Spindles/RelaySpindle.cpp | 2 +- Grbl_Esp32/src/Spindles/Spindle.cpp | 2 + Grbl_Esp32/src/Spindles/Spindle.h | 1 + Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 79 +++++++++------- Grbl_Esp32/src/Spindles/VFDSpindle.h | 7 +- Grbl_Esp32/src/WebUI/BTConfig.cpp | 3 + Grbl_Esp32/src/WebUI/Commands.cpp | 3 + Grbl_Esp32/src/WebUI/WebSettings.cpp | 10 +-- Grbl_Esp32/src/WebUI/WifiConfig.cpp | 9 ++ 33 files changed, 371 insertions(+), 173 deletions(-) diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp index 5661ae6d..27a55021 100644 --- a/Grbl_Esp32/Custom/atari_1020.cpp +++ b/Grbl_Esp32/Custom/atari_1020.cpp @@ -57,7 +57,8 @@ void machine_init() { NULL, // parameters 1, // priority &solenoidSyncTaskHandle, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); // setup a task that will do the custom homing sequence xTaskCreatePinnedToCore(atari_home_task, // task @@ -66,7 +67,8 @@ void machine_init() { NULL, // parameters 1, // priority &atariHomingTaskHandle, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 6ca3728c..3421c670 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -159,6 +159,10 @@ # define DEFAULT_LASER_MODE 0 // false #endif +#ifndef DEFAULT_LASER_FULL_POWER +# define DEFAULT_LASER_FULL_POWER 1000 +#endif + #ifndef DEFAULT_SPINDLE_RPM_MAX // $30 # define DEFAULT_SPINDLE_RPM_MAX 1000.0 // rpm #endif diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 647e0c06..e5b624da 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 = "20201124"; +const char* const GRBL_VERSION_BUILD = "20201128"; //#include #include diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index b86c0c26..e3a8e1c6 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -112,13 +112,22 @@ Socket #5 // #define Y_LIMIT_PIN GPIO_NUM_25 // #define Z_LIMIT_PIN GPIO_NUM_39 -// // 4x Input Module in Socket #3 -// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -// #define CONTROL_CYCLE_START_PIN GPIO_NUM_26 -// #define CONTROL_FEED_HOLD_PIN GPIO_NUM_4 -// #define CONTROL_RESET_PIN GPIO_NUM_16 -// #define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_27 -// //#define INVERT_CONTROL_PIN_MASK B0000 +// 5V output CNC module in socket #4 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module +#define SPINDLE_TYPE SpindleType::PWM +#define SPINDLE_OUTPUT_PIN GPIO_NUM_14 +#define SPINDLE_ENABLE_PIN GPIO_NUM_13 // optional +#define LASER_OUTPUT_PIN GPIO_NUM_15 // optional +#define LASER_ENABLE_PIN GPIO_NUM_12 + + + + +// // RS485 Modbus In socket #3 +// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/RS485-Modbus-Module +// #define VFD_RS485_TXD_PIN GPIO_NUM_26 +// #define VFD_RS485_RTS_PIN GPIO_NUM_4 +// #define VFD_RS485_RXD_PIN GPIO_NUM_16 // ================= Setting Defaults ========================== #define DEFAULT_X_STEPS_PER_MM 800 diff --git a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h index f7b02d29..43f2864c 100644 --- a/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h +++ b/Grbl_Esp32/src/Machines/mpcnc_laser_module_v1p2.h @@ -53,8 +53,8 @@ // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_16 +#define SPINDLE_TYPE SpindleType::LASER +#define LASER_OUTPUT_PIN GPIO_NUM_16 #define COOLANT_MIST_PIN GPIO_NUM_2 diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 8c7d550a..1a08c0c8 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -43,16 +43,16 @@ #include "Dynamixel2.h" #include "TrinamicDriver.h" -Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) -void init_motors() { +Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) +void init_motors() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Motors"); auto n_axis = number_axis->get(); if (n_axis >= 1) { #ifdef X_TRINAMIC_DRIVER - myMotor[X_AXIS][0] = new Motors::TrinamicDriver( - X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); + myMotor[X_AXIS][0] = + new Motors::TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); #elif defined(X_SERVO_PIN) myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN); #elif defined(X_UNIPOLAR) @@ -66,8 +66,8 @@ void init_motors() { #endif #ifdef X2_TRINAMIC_DRIVER - myMotor[X_AXIS][1] = new Motors::TrinamicDriver( - X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); + myMotor[X_AXIS][1] = + new Motors::TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); #elif defined(X2_UNIPOLAR) myMotor[X_AXIS][1] = new Motors::UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3); #elif defined(X2_STEP_PIN) @@ -83,8 +83,8 @@ void init_motors() { if (n_axis >= 2) { // this WILL be done better with settings #ifdef Y_TRINAMIC_DRIVER - myMotor[Y_AXIS][0] = new Motors::TrinamicDriver( - Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); + myMotor[Y_AXIS][0] = + new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); #elif defined(Y_SERVO_PIN) myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN); #elif defined(Y_UNIPOLAR) @@ -98,8 +98,8 @@ void init_motors() { #endif #ifdef Y2_TRINAMIC_DRIVER - myMotor[Y_AXIS][1] = new Motors::TrinamicDriver( - Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); + myMotor[Y_AXIS][1] = + new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); #elif defined(Y2_UNIPOLAR) myMotor[Y_AXIS][1] = new Motors::UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3); #elif defined(Y2_STEP_PIN) @@ -115,8 +115,8 @@ void init_motors() { if (n_axis >= 3) { // this WILL be done better with settings #ifdef Z_TRINAMIC_DRIVER - myMotor[Z_AXIS][0] = new Motors::TrinamicDriver( - Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); + myMotor[Z_AXIS][0] = + new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); #elif defined(Z_SERVO_PIN) myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN); #elif defined(Z_UNIPOLAR) @@ -130,8 +130,8 @@ void init_motors() { #endif #ifdef Z2_TRINAMIC_DRIVER - myMotor[Z_AXIS][1] = new Motors::TrinamicDriver( - Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); + myMotor[Z_AXIS][1] = + new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); #elif defined(Z2_UNIPOLAR) myMotor[Z_AXIS][1] = new Motors::UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3); #elif defined(Z2_STEP_PIN) @@ -147,8 +147,8 @@ void init_motors() { if (n_axis >= 4) { // this WILL be done better with settings #ifdef A_TRINAMIC_DRIVER - myMotor[A_AXIS][0] = new Motors::TrinamicDriver( - A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); + myMotor[A_AXIS][0] = + new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); #elif defined(A_SERVO_PIN) myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN); #elif defined(A_UNIPOLAR) @@ -162,8 +162,8 @@ void init_motors() { #endif #ifdef A2_TRINAMIC_DRIVER - myMotor[A_AXIS][1] = new Motors::TrinamicDriver( - A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); + myMotor[A_AXIS][1] = + new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); #elif defined(A2_UNIPOLAR) myMotor[A_AXIS][1] = new Motors::UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3); #elif defined(A2_STEP_PIN) @@ -179,8 +179,8 @@ void init_motors() { if (n_axis >= 5) { // this WILL be done better with settings #ifdef B_TRINAMIC_DRIVER - myMotor[B_AXIS][0] = new Motors::TrinamicDriver( - B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); + myMotor[B_AXIS][0] = + new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); #elif defined(B_SERVO_PIN) myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN); #elif defined(B_UNIPOLAR) @@ -194,8 +194,8 @@ void init_motors() { #endif #ifdef B2_TRINAMIC_DRIVER - myMotor[B_AXIS][1] = new Motors::TrinamicDriver( - B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); + myMotor[B_AXIS][1] = + new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); #elif defined(B2_UNIPOLAR) myMotor[B_AXIS][1] = new Motors::UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3); #elif defined(B2_STEP_PIN) @@ -211,8 +211,8 @@ void init_motors() { if (n_axis >= 6) { // this WILL be done better with settings #ifdef C_TRINAMIC_DRIVER - myMotor[C_AXIS][0] = new Motors::TrinamicDriver( - C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); + myMotor[C_AXIS][0] = + new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); #elif defined(C_SERVO_PIN) myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN); #elif defined(C_UNIPOLAR) @@ -226,8 +226,8 @@ void init_motors() { #endif #ifdef C2_TRINAMIC_DRIVER - myMotor[C_AXIS][1] = new Motors::TrinamicDriver( - C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); + myMotor[C_AXIS][1] = + new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); #elif defined(C2_UNIPOLAR) myMotor[C_AXIS][1] = new Motors::UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3); #elif defined(C2_STEP_PIN) @@ -279,23 +279,16 @@ void init_motors() { } } -void motors_set_disable(bool disable) { +void motors_set_disable(bool disable, uint8_t mask) { static bool previous_state = true; - //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Motors disable %d", disable); - - /* - if (previous_state == disable) { - return; - } - previous_state = disable; -*/ - // now loop through all the motors to see if they can individually disable auto n_axis = number_axis->get(); for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) { for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { - myMotor[axis][gang_index]->set_disable(disable); + if (bitnum_istrue(mask, axis)) { + myMotor[axis][gang_index]->set_disable(disable); + } } } @@ -320,7 +313,7 @@ void motors_read_settings() { // They can use this to setup things like Stall uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { uint8_t can_home = 0; - auto n_axis = number_axis->get(); + auto n_axis = number_axis->get(); for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { if (bitnum_istrue(homing_mask, axis)) { if (myMotor[axis][0]->set_homing_mode(isHoming)) { diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h index a6451eff..14550f68 100644 --- a/Grbl_Esp32/src/Motors/Motors.h +++ b/Grbl_Esp32/src/Motors/Motors.h @@ -39,7 +39,7 @@ void motors_read_settings(); // The return value is a bitmask of axes that can home uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming); -void motors_set_disable(bool disable); +void motors_set_disable(bool disable, uint8_t mask = B11111111); // default is all axes void motors_step(uint8_t step_mask, uint8_t dir_mask); void motors_unstep(); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index af0f39e7..9c808c4e 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -115,7 +115,8 @@ namespace Motors { NULL, // parameters 1, // priority NULL, - 0 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } } diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index f8539203..2fe07b5a 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -389,6 +389,38 @@ Error listErrors(const char* value, WebUI::AuthenticationLevel auth_level, WebUI return Error::Ok; } +Error motor_disable(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + char* s; + if (value == NULL) { + value = "\0"; + } + + s = strdup(value); + s = trim(s); + + int32_t convertedValue; + char* endptr; + if (*s == '\0') { + convertedValue = 255; // all axes + } else { + convertedValue = strtol(s, &endptr, 10); + if (endptr == s || *endptr != '\0') { + // Try to convert as an axis list + convertedValue = 0; + auto axisNames = String("XYZABC"); + while (*s) { + int index = axisNames.indexOf(toupper(*s++)); + if (index < 0) { + return Error::BadNumberFormat; + } + convertedValue |= bit(index); + } + } + } + motors_set_disable(true, convertedValue); + return Error::Ok; +} + static bool anyState() { return false; } @@ -427,6 +459,8 @@ void make_grbl_commands() { new GrblCommand("V", "Settings/Stats", Setting::report_nvs_stats, idleOrAlarm); new GrblCommand("#", "GCode/Offsets", report_ngc, idleOrAlarm); new GrblCommand("H", "Home", home_all, idleOrAlarm); + new GrblCommand("MD", "Motor/Disable", motor_disable, idleOrAlarm); + #ifdef HOMING_SINGLE_AXIS_COMMANDS new GrblCommand("HX", "Home/X", home_x, idleOrAlarm); new GrblCommand("HY", "Home/Y", home_y, idleOrAlarm); diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index caa78bb6..8a1403cf 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -104,7 +104,8 @@ void serial_init() { NULL, // parameters 1, // priority &serialCheckTaskHandle, - 1 // core + CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core + // core ); } diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 5450b444..ce7af34a 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -126,6 +126,7 @@ Error IntSetting::setStringValue(char* s) { _storedValue = convertedValue; } } + check(NULL); return Error::Ok; } @@ -223,6 +224,7 @@ Error AxisMaskSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -323,6 +325,7 @@ Error FloatSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -412,6 +415,7 @@ Error StringSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -442,11 +446,15 @@ void StringSetting::addWebui(WebUI::JSONencoder* j) { typedef std::map enum_opt_t; -EnumSetting::EnumSetting( - const char* description, type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts) - // No checker function because enumerations have an exact set of value - : - Setting(description, type, permissions, grblName, name, NULL), +EnumSetting::EnumSetting(const char* description, + type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + int8_t defVal, + enum_opt_t* opts, + bool (*checker)(char*) = NULL) : + Setting(description, type, permissions, grblName, name, checker), _defaultValue(defVal), _options(opts) {} void EnumSetting::load() { @@ -471,7 +479,11 @@ void EnumSetting::setDefault() { // This is necessary for WebUI, which uses the number // for setting. Error EnumSetting::setStringValue(char* s) { - s = trim(s); + s = trim(s); + Error err = check(s); + if (err != Error::Ok) { + return err; + } enum_opt_t::iterator it = _options->find(s); if (it == _options->end()) { // If we don't find the value in keys, look for it in the numeric values @@ -497,7 +509,7 @@ Error EnumSetting::setStringValue(char* s) { } _currentValue = it->second; if (_storedValue != _currentValue) { - if (_storedValue == _defaultValue) { + if (_currentValue == _defaultValue) { nvs_erase_key(_handle, _keyName); } else { if (nvs_set_i8(_handle, _keyName, _currentValue)) { @@ -506,6 +518,7 @@ Error EnumSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } @@ -585,6 +598,7 @@ Error FlagSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } const char* FlagSetting::getDefaultString() { @@ -665,6 +679,7 @@ Error IPaddrSetting::setStringValue(char* s) { _storedValue = _currentValue; } } + check(NULL); return Error::Ok; } diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 63dce4f5..8e02a125 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -343,10 +343,17 @@ public: const char* grblName, const char* name, int8_t defVal, - enum_opt_t* opts); + enum_opt_t* opts, + bool (*checker)(char*)); - EnumSetting(type_t type, permissions_t permissions, const char* grblName, const char* name, int8_t defVal, enum_opt_t* opts) : - EnumSetting(NULL, type, permissions, grblName, name, defVal, opts) {} + EnumSetting(type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + int8_t defVal, + enum_opt_t* opts, + bool (*checker)(char*) = NULL) : + EnumSetting(NULL, type, permissions, grblName, name, defVal, opts, checker) {} void load(); void setDefault(); diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 5e2c2e2c..0a11cdee 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -32,6 +32,7 @@ FlagSetting* homing_enable; // TODO Settings - also need to clear, but not set, soft_limits FlagSetting* laser_mode; // TODO Settings - also need to call my_spindle->init; +IntSetting* laser_full_power; IntSetting* status_mask; FloatSetting* junction_deviation; @@ -166,34 +167,32 @@ static const char* makename(const char* axisName, const char* tail) { } static bool checkStartupLine(char* value) { + if (!value) { // No POST functionality + return true; + } if (sys.state != State::Idle) { return false; } return gc_execute_line(value, CLIENT_SERIAL) == Error::Ok; } -static bool checkStallguard(char* value) { - motorSettingChanged = true; +static bool postTMC(char* value) { + if (!value) { // No POST functionality + motorSettingChanged = true; + } return true; } -static bool checkMicrosteps(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkRunCurrent(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkHoldcurrent(char* value) { - motorSettingChanged = true; - return true; -} - -static bool checkStallguardDebugMask(char* val) { - motorSettingChanged = true; +static bool checkSpindleChange(char* val) { + if (!val) { + spindle->deinit(); + Spindles::Spindle::select(); + return true; + } + if (gc_state.modal.spindle != SpindleState::Disable) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle must be off to make this change"); + return false; + } return true; } @@ -209,8 +208,8 @@ static const char* makeGrblName(int axisNum, int base) { void make_coordinate(CoordIndex index, const char* name) { float coord_data[MAX_N_AXIS] = { 0.0 }; - auto coord = new Coordinates(name); - coords[index] = coord; + auto coord = new Coordinates(name); + coords[index] = coord; if (!coord->load()) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Propagating %s data to NVS format", coord->getName()); // If coord->load() returns false it means that no entry @@ -259,30 +258,30 @@ void make_settings() { b_axis_settings = axis_settings[B_AXIS]; c_axis_settings = axis_settings[C_AXIS]; for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { - def = &axis_defaults[axis]; - auto setting = new IntSetting( - EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, checkStallguard); + def = &axis_defaults[axis]; + auto setting = + new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, postTMC); setting->setAxis(axis); axis_settings[axis]->stallguard = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { - def = &axis_defaults[axis]; - auto setting = new IntSetting( - EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, checkMicrosteps); + def = &axis_defaults[axis]; + auto setting = + new IntSetting(EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postTMC); setting->setAxis(axis); axis_settings[axis]->microsteps = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, checkHoldcurrent); // Amps + EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, postTMC); // Amps setting->setAxis(axis); axis_settings[axis]->hold_current = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, checkRunCurrent); // Amps + EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, postTMC); // Amps setting->setAxis(axis); axis_settings[axis]->run_current = setting; } @@ -322,31 +321,38 @@ void make_settings() { } // Spindle Settings - spindle_pwm_max_value = new FloatSetting(EXTENDED, WG, "36", "Spindle/PWM/Max", DEFAULT_SPINDLE_MAX_VALUE, 0.0, 100.0); - spindle_pwm_min_value = new FloatSetting(EXTENDED, WG, "35", "Spindle/PWM/Min", DEFAULT_SPINDLE_MIN_VALUE, 0.0, 100.0); - spindle_pwm_off_value = - new FloatSetting(EXTENDED, WG, "34", "Spindle/PWM/Off", DEFAULT_SPINDLE_OFF_VALUE, 0.0, 100.0); // these are percentages + spindle_type = + new EnumSetting(NULL, EXTENDED, WG, NULL, "Spindle/Type", static_cast(SPINDLE_TYPE), &spindleTypes, checkSpindleChange); + + spindle_pwm_max_value = + new FloatSetting(EXTENDED, WG, "36", "Spindle/PWM/Max", DEFAULT_SPINDLE_MAX_VALUE, 0.0, 100.0, checkSpindleChange); + spindle_pwm_min_value = + new FloatSetting(EXTENDED, WG, "35", "Spindle/PWM/Min", DEFAULT_SPINDLE_MIN_VALUE, 0.0, 100.0, checkSpindleChange); + spindle_pwm_off_value = new FloatSetting( + EXTENDED, WG, "34", "Spindle/PWM/Off", DEFAULT_SPINDLE_OFF_VALUE, 0.0, 100.0, checkSpindleChange); // these are percentages // IntSetting spindle_pwm_bit_precision(EXTENDED, WG, "Spindle/PWM/Precision", DEFAULT_SPINDLE_BIT_PRECISION, 1, 16); - spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000); - spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN); + spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000, checkSpindleChange); + spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN, checkSpindleChange); spindle_delay_spinup = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); spindle_delay_spindown = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); spindle_enbl_off_with_zero_speed = - new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED); + new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange); - spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN); + spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN, checkSpindleChange); // GRBL Non-numbered settings startup_line_0 = new StringSetting(GRBL, WG, "N0", "GCode/Line0", "", checkStartupLine); startup_line_1 = new StringSetting(GRBL, WG, "N1", "GCode/Line1", "", checkStartupLine); // GRBL Numbered Settings - laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE); + laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE); + laser_full_power = new IntSetting(EXTENDED, WG, NULL, "Laser/FullPower", DEFAULT_LASER_FULL_POWER, 0, 10000, checkSpindleChange); + // TODO Settings - also need to call my_spindle->init(); - rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000); - rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000); + rpm_min = new FloatSetting(GRBL, WG, "31", "GCode/MinS", DEFAULT_SPINDLE_RPM_MIN, 0, 100000, checkSpindleChange); + rpm_max = new FloatSetting(GRBL, WG, "30", "GCode/MaxS", DEFAULT_SPINDLE_RPM_MAX, 0, 100000, checkSpindleChange); homing_pulloff = new FloatSetting(GRBL, WG, "27", "Homing/Pulloff", DEFAULT_HOMING_PULLOFF, 0, 1000); homing_debounce = new FloatSetting(GRBL, WG, "26", "Homing/Debounce", DEFAULT_HOMING_DEBOUNCE_DELAY, 0, 10000); @@ -377,8 +383,8 @@ void make_settings() { step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK); stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); 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); + + stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postTMC); homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index b01038bf..41599b99 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -35,6 +35,7 @@ extern FlagSetting* soft_limits; extern FlagSetting* hard_limits; extern FlagSetting* homing_enable; extern FlagSetting* laser_mode; +extern IntSetting* laser_full_power; extern IntSetting* status_mask; extern FloatSetting* junction_deviation; diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.cpp b/Grbl_Esp32/src/Spindles/10vSpindle.cpp index c2f050da..dd2284b5 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/10vSpindle.cpp @@ -44,7 +44,7 @@ namespace Spindles { #endif if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } @@ -66,7 +66,7 @@ namespace Spindles { // prints the startup message of the spindle config void _10v::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "0-10V spindle Out:%s Enbl:%s, Dir:%s, Fwd:%s, Rev:%s, Freq:%dHz Res:%dbits", pinName(_output_pin).c_str(), @@ -144,7 +144,6 @@ namespace Spindles { } void _10v::set_enable_pin(bool enable) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle::_10v::set_enable_pin"); if (_off_with_zero_speed && sys.spindle_speed == 0) { enable = false; } @@ -164,9 +163,28 @@ namespace Spindles { } void _10v::set_dir_pin(bool Clockwise) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle::_10v::set_dir_pin"); digitalWrite(_direction_pin, Clockwise); digitalWrite(_forward_pin, Clockwise); digitalWrite(_reverse_pin, !Clockwise); } + + void _10v::deinit() { +#ifdef SPINDLE_OUTPUT_PIN + pinMode(SPINDLE_OUTPUT_PIN, INPUT); +#endif +#ifdef SPINDLE_ENABLE_PIN + pinMode(SPINDLE_ENABLE_PIN, INPUT); +#endif + +#ifdef SPINDLE_DIR_PIN + pinMode(SPINDLE_DIR_PIN, INPUT); +#endif +#ifdef SPINDLE_FORWARD_PIN + pinMode(SPINDLE_FORWARD_PIN, INPUT); +#endif + +#ifdef SPINDLE_REVERSE_PIN + pinMode(SPINDLE_FORWARD_PIN, INPUT); +#endif + } } diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.h b/Grbl_Esp32/src/Spindles/10vSpindle.h index a0205320..d45a9cb5 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.h +++ b/Grbl_Esp32/src/Spindles/10vSpindle.h @@ -45,6 +45,7 @@ namespace Spindles { SpindleState get_state() override; void stop() override; + void deinit() override; virtual ~_10v() {} diff --git a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp index 55630bcb..fceb8bc5 100644 --- a/Grbl_Esp32/src/Spindles/BESCSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/BESCSpindle.cpp @@ -55,7 +55,7 @@ namespace Spindles { get_pins_and_settings(); // these gets the standard PWM settings, but many need to be changed for BESC if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: BESC output pin not defined"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: BESC output pin not defined"); return; // We cannot continue without the output pin } @@ -82,7 +82,7 @@ namespace Spindles { // prints the startup message of the spindle config void BESC::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "BESC spindle on Pin:%s Min:%0.2fms Max:%0.2fms Freq:%dHz Res:%dbits", pinName(_output_pin).c_str(), diff --git a/Grbl_Esp32/src/Spindles/DacSpindle.cpp b/Grbl_Esp32/src/Spindles/DacSpindle.cpp index b679c857..26910ced 100644 --- a/Grbl_Esp32/src/Spindles/DacSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/DacSpindle.cpp @@ -40,7 +40,7 @@ namespace Spindles { if (_output_pin != GPIO_NUM_25 && _output_pin != GPIO_NUM_26) { // DAC can only be used on these pins _gpio_ok = false; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "DAC spindle pin invalid GPIO_NUM_%d (pin 25 or 26 only)", _output_pin); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "DAC spindle pin invalid GPIO_NUM_%d (pin 25 or 26 only)", _output_pin); return; } @@ -54,7 +54,7 @@ namespace Spindles { } void Dac::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "DAC spindle Output:%s, Enbl:%s, Dir:%s, Res:8bits", pinName(_output_pin).c_str(), @@ -85,7 +85,7 @@ namespace Spindles { rpm = _min_rpm; sys.spindle_speed = rpm; pwm_value = 0; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RPM less than min RPM:%5.2f %d", rpm, pwm_value); } } else { // Compute intermediate PWM value with linear spindle speed model. diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp index b18249e5..8629965e 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp @@ -98,7 +98,7 @@ namespace Spindles { uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); vfd->_max_rpm = rpm; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); return true; }; diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp index b9fa0c72..befdc0d9 100644 --- a/Grbl_Esp32/src/Spindles/Laser.cpp +++ b/Grbl_Esp32/src/Spindles/Laser.cpp @@ -29,14 +29,71 @@ namespace Spindles { } void Laser::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, - "Laser spindle on Pin:%s, Freq:%dHz, Res:%dbits Laser mode:%s", + "Laser spindle on Pin:%s, Enbl:%s, Freq:%dHz, Res:%dbits Laser mode:%s", pinName(_output_pin).c_str(), + pinName(_enable_pin).c_str(), _pwm_freq, _pwm_precision, laser_mode->getStringValue()); // the current mode use_delays = false; // this will override the value set in Spindle::PWM::init() } + + // Get the GPIO from the machine definition + void Laser::get_pins_and_settings() { + // setup all the pins + +#ifdef LASER_OUTPUT_PIN + _output_pin = LASER_OUTPUT_PIN; +#else + _output_pin = UNDEFINED_PIN; +#endif + + _invert_pwm = spindle_output_invert->get(); + +#ifdef LASER_ENABLE_PIN + _enable_pin = LASER_ENABLE_PIN; +#else + _enable_pin = UNDEFINED_PIN; +#endif + + if (_output_pin == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: LASER_OUTPUT_PIN not defined"); + return; // We cannot continue without the output pin + } + + _off_with_zero_speed = spindle_enbl_off_with_zero_speed->get(); + + _direction_pin = UNDEFINED_PIN; + is_reversable = false; + + _pwm_freq = spindle_pwm_freq->get(); + _pwm_precision = calc_pwm_precision(_pwm_freq); // detewrmine the best precision + _pwm_period = (1 << _pwm_precision); + + // pre-caculate some PWM count values + _pwm_off_value = 0; + _pwm_min_value = 0; + _pwm_max_value = _pwm_period; + + _min_rpm = 0; + _max_rpm = laser_full_power->get(); + + _piecewide_linear = false; + + _pwm_chan_num = 0; // Channel 0 is reserved for spindle use + } + + void Laser::deinit() { + stop(); +#ifdef LASER_OUTPUT_PIN + pinMode(LASER_OUTPUT_PIN, INPUT); +#endif + +#ifdef LASER_ENABLE_PIN + pinMode(LASER_ENABLE_PIN, INPUT); +#endif + } } diff --git a/Grbl_Esp32/src/Spindles/Laser.h b/Grbl_Esp32/src/Spindles/Laser.h index 220770b0..87673f62 100644 --- a/Grbl_Esp32/src/Spindles/Laser.h +++ b/Grbl_Esp32/src/Spindles/Laser.h @@ -36,6 +36,8 @@ namespace Spindles { bool isRateAdjusted() override; void config_message() override; + void get_pins_and_settings() override; + void deinit() override; virtual ~Laser() {} }; diff --git a/Grbl_Esp32/src/Spindles/NullSpindle.cpp b/Grbl_Esp32/src/Spindles/NullSpindle.cpp index c1266019..ddcdb7a2 100644 --- a/Grbl_Esp32/src/Spindles/NullSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/NullSpindle.cpp @@ -40,5 +40,5 @@ namespace Spindles { } SpindleState Null::get_state() { return _current_state; } void Null::stop() {} - void Null::config_message() { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "No spindle"); } + void Null::config_message() { grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "No spindle"); } } diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 3e71ee7b..1ade8aa8 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -34,12 +34,11 @@ namespace Spindles { get_pins_and_settings(); if (_output_pin == UNDEFINED_PIN) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin not defined"); return; // We cannot continue without the output pin } if (_output_pin >= I2S_OUT_PIN_BASE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle output pin %s cannot do PWM", pinName(_output_pin).c_str()); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle output pin %s cannot do PWM", pinName(_output_pin).c_str()); return; } @@ -80,6 +79,11 @@ namespace Spindles { _direction_pin = UNDEFINED_PIN; #endif + if (_output_pin == UNDEFINED_PIN) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: SPINDLE_OUTPUT_PIN not defined"); + return; // We cannot continue without the output pin + } + is_reversable = (_direction_pin != UNDEFINED_PIN); _pwm_freq = spindle_pwm_freq->get(); @@ -87,7 +91,7 @@ namespace Spindles { _pwm_period = (1 << _pwm_precision); if (spindle_pwm_min_value->get() > spindle_pwm_min_value->get()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Spindle min pwm is greater than max. Check $35 and $36"); } // pre-caculate some PWM count values @@ -117,8 +121,6 @@ namespace Spindles { return rpm; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "set_rpm(%d)", rpm); - // apply override rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) @@ -134,7 +136,7 @@ namespace Spindles { if (_piecewide_linear) { //pwm_value = piecewise_linear_fit(rpm); TODO pwm_value = 0; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Warning: Linear fit not implemented yet."); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Warning: Linear fit not implemented yet."); } else { if (rpm == 0) { @@ -159,18 +161,14 @@ namespace Spindles { sys.spindle_speed = 0; stop(); if (use_delays && (_current_state != state)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Start "); mc_dwell(spindle_delay_spindown->get()); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinDown Done"); } } else { set_dir_pin(state == SpindleState::Cw); set_rpm(rpm); set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work if (use_delays && (_current_state != state)) { - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Start %d", rpm); mc_dwell(spindle_delay_spinup->get()); - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SpinUp Done"); } } @@ -197,7 +195,7 @@ namespace Spindles { // prints the startup message of the spindle config void PWM::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "PWM spindle Output:%s, Enbl:%s, Dir:%s, Freq:%dHz, Res:%dbits", pinName(_output_pin).c_str(), @@ -223,8 +221,6 @@ namespace Spindles { duty = (1 << _pwm_precision) - duty; } - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "set_output(%d)", duty); - ledcWrite(_pwm_chan_num, duty); } @@ -262,4 +258,18 @@ namespace Spindles { return precision - 1; } + + void PWM::deinit() { + stop(); +#ifdef SPINDLE_OUTPUT_PIN + pinMode(SPINDLE_OUTPUT_PIN, INPUT); +#endif +#ifdef SPINDLE_ENABLE_PIN + pinMode(SPINDLE_ENABLE_PIN, INPUT); +#endif + +#ifdef SPINDLE_DIR_PIN + pinMode(SPINDLE_DIR_PIN, INPUT); +#endif + } } diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.h b/Grbl_Esp32/src/Spindles/PWMSpindle.h index cb306a1d..07ffdf25 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.h +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.h @@ -65,8 +65,9 @@ namespace Spindles { virtual void set_dir_pin(bool Clockwise); virtual void set_output(uint32_t duty); virtual void set_enable_pin(bool enable_pin); + virtual void deinit(); - void get_pins_and_settings(); - uint8_t calc_pwm_precision(uint32_t freq); + virtual void get_pins_and_settings(); + uint8_t calc_pwm_precision(uint32_t freq); }; } diff --git a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp index eadeb918..41762d4c 100644 --- a/Grbl_Esp32/src/Spindles/RelaySpindle.cpp +++ b/Grbl_Esp32/src/Spindles/RelaySpindle.cpp @@ -46,7 +46,7 @@ namespace Spindles { // prints the startup message of the spindle config void Relay ::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Relay spindle Output:%s, Enbl:%s, Dir:%s", pinName(_output_pin).c_str(), diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index 7db5edf2..d0ec75a4 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -100,6 +100,8 @@ namespace Spindles { protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed. set_state(state, rpm); } + + void Spindle::deinit() { stop(); } } Spindles::Spindle* spindle; diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h index d977fa86..5fd37e5b 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.h +++ b/Grbl_Esp32/src/Spindles/Spindle.h @@ -66,6 +66,7 @@ namespace Spindles { virtual void config_message() = 0; virtual bool isRateAdjusted(); virtual void sync(SpindleState state, uint32_t rpm); + virtual void deinit(); virtual ~Spindle() {} diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 3a1305a6..a7a2636a 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -49,6 +49,7 @@ const int VFD_RS485_POLL_RATE = 200; // in milliseconds between comman namespace Spindles { QueueHandle_t VFD::vfd_cmd_queue = nullptr; TaskHandle_t VFD::vfd_cmdTaskHandle = nullptr; + bool VFD::task_active = true; // The communications task void VFD::vfd_cmd_task(void* pvParameters) { @@ -60,6 +61,12 @@ namespace Spindles { uint8_t rx_message[VFD_RS485_MAX_MSG_SIZE]; while (true) { + if (!task_active) { + uart_driver_delete(VFD_RS485_UART_PORT); + xQueueReset(vfd_cmd_queue); + vTaskDelete(NULL); + } + response_parser parser = nullptr; next_cmd.msg[0] = VFD_RS485_ADDR; // Always default to this @@ -158,7 +165,7 @@ namespace Spindles { // Not succesful! Now what? unresponsive = true; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); } } else { #ifdef VFD_DEBUG_MODE @@ -167,18 +174,18 @@ namespace Spindles { if (read_length != 0) { if (rx_message[0] != VFD_RS485_ADDR) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 received message from other modbus device"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 received message from other modbus device"); } else if (read_length != next_cmd.rx_length) { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 received message of unexpected length; expected %d, got %d", int(next_cmd.rx_length), int(read_length)); } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 CRC check failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 CRC check failed"); } } else { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 No response"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 No response"); } #endif @@ -193,9 +200,9 @@ namespace Spindles { if (retry_count == MAX_RETRIES) { if (!unresponsive) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); sys_rt_exec_alarm = ExecAlarm::SpindleControl; } unresponsive = true; @@ -218,12 +225,12 @@ namespace Spindles { void VFD::init() { vfd_ok = false; // initialize - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Initializing RS485 VFD spindle"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing RS485 VFD spindle"); // fail if required items are not defined if (!get_pins_and_settings()) { vfd_ok = false; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD spindle errors"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD spindle errors"); return; } @@ -246,38 +253,37 @@ namespace Spindles { uart_config.rx_flow_ctrl_thresh = 122; if (uart_param_config(VFD_RS485_UART_PORT, &uart_config) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart parameters failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart parameters failed"); return; } if (uart_set_pin(VFD_RS485_UART_PORT, _txd_pin, _rxd_pin, _rts_pin, UART_PIN_NO_CHANGE) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart pin config failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart pin config failed"); return; } if (uart_driver_install(VFD_RS485_UART_PORT, VFD_RS485_BUF_SIZE * 2, 0, 0, NULL, 0) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart driver install failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart driver install failed"); return; } if (uart_set_mode(VFD_RS485_UART_PORT, UART_MODE_RS485_HALF_DUPLEX) != ESP_OK) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart set half duplex failed"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart set half duplex failed"); return; } // Initialization is complete, so now it's okay to run the queue task: - if (!_task_running) { // init can happen many times, we only want to start one task + task_active = true; + if (vfd_cmd_queue != nullptr) { vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); - xTaskCreatePinnedToCore(vfd_cmd_task, // task - "vfd_cmdTaskHandle", // name for task - 2048, // size of task stack - this, // parameters - 1, // priority - &vfd_cmdTaskHandle, - 0 // core - ); - _task_running = true; - } + } + xTaskCreatePinnedToCore(vfd_cmd_task, // task + "vfd_cmdTaskHandle", // name for task + 2048, // size of task stack + this, // parameters + 1, // priority + &vfd_cmdTaskHandle, + 1); is_reversable = true; // these VFDs are always reversable use_delays = true; @@ -299,26 +305,26 @@ namespace Spindles { #ifdef VFD_RS485_TXD_PIN _txd_pin = VFD_RS485_TXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_TXD_PIN"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_TXD_PIN"); pins_settings_ok = false; #endif #ifdef VFD_RS485_RXD_PIN _rxd_pin = VFD_RS485_RXD_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_RXD_PIN"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_RXD_PIN"); pins_settings_ok = false; #endif #ifdef VFD_RS485_RTS_PIN _rts_pin = VFD_RS485_RTS_PIN; #else - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_RTS_PIN"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_RTS_PIN"); pins_settings_ok = false; #endif if (laser_mode->get()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); pins_settings_ok = false; } @@ -329,7 +335,7 @@ namespace Spindles { } void VFD::config_message() { - grbl_msg_sendf(CLIENT_SERIAL, + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD RS485 Tx:%s Rx:%s RTS:%s", pinName(_txd_pin).c_str(), @@ -382,7 +388,7 @@ namespace Spindles { if (mode == SpindleState::Disable) { if (!xQueueReset(vfd_cmd_queue)) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD spindle off, queue could not be reset"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle off, queue could not be reset"); } } @@ -390,7 +396,7 @@ namespace Spindles { _current_state = mode; if (xQueueSend(vfd_cmd_queue, &mode_cmd, 0) != pdTRUE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD Queue Full"); } return true; @@ -402,7 +408,7 @@ namespace Spindles { } #ifdef VFD_DEBUG_MODE - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); #endif // apply override @@ -433,7 +439,7 @@ namespace Spindles { rpm_cmd.critical = false; if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD Queue Full"); } return rpm; @@ -465,4 +471,11 @@ namespace Spindles { return crc; } + + void VFD::deinit() { + _current_state = SpindleState::Disable; + _current_rpm = 0; + + task_active = false; + } } diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h index 1c852292..b2b091ec 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.h +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h @@ -39,13 +39,18 @@ namespace Spindles { uint32_t _current_rpm = 0; bool _task_running = false; bool vfd_ok = true; - + + static bool task_active; static QueueHandle_t vfd_cmd_queue; static TaskHandle_t vfd_cmdTaskHandle; static void vfd_cmd_task(void* pvParameters); static uint16_t ModRTU_CRC(uint8_t* buf, int msg_len); + void deinit() override; + + + protected: struct ModbusCommand { bool critical; // TODO SdB: change into `uint8_t critical : 1;`: We want more flags... diff --git a/Grbl_Esp32/src/WebUI/BTConfig.cpp b/Grbl_Esp32/src/WebUI/BTConfig.cpp index 05a7816e..85a2a7e6 100644 --- a/Grbl_Esp32/src/WebUI/BTConfig.cpp +++ b/Grbl_Esp32/src/WebUI/BTConfig.cpp @@ -86,6 +86,9 @@ namespace WebUI { bool BTConfig::isBTnameValid(const char* hostname) { //limited size + if (!hostname) { + return true; + } char c; // length is checked automatically by string setting //only letter and digit diff --git a/Grbl_Esp32/src/WebUI/Commands.cpp b/Grbl_Esp32/src/WebUI/Commands.cpp index 7cfdf003..d59ee19c 100644 --- a/Grbl_Esp32/src/WebUI/Commands.cpp +++ b/Grbl_Esp32/src/WebUI/Commands.cpp @@ -43,6 +43,9 @@ namespace WebUI { } bool COMMANDS::isLocalPasswordValid(char* password) { + if (!password) { + return true; + } char c; //limited size if ((strlen(password) > MAX_LOCAL_PASSWORD_LENGTH) || (strlen(password) < MIN_LOCAL_PASSWORD_LENGTH)) { diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index b9a34b8c..f9a00fc4 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -1085,7 +1085,7 @@ namespace WebUI { MAX_NOTIFICATION_TOKEN_LENGTH, NULL); notification_type = - new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions); + new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions, NULL); #endif #ifdef ENABLE_AUTHENTICATION user_password = new StringSetting("User password", @@ -1121,16 +1121,16 @@ namespace WebUI { #ifdef WIFI_OR_BLUETOOTH // user+ to get, admin to set - wifi_radio_mode = new EnumSetting("Radio mode", WEBSET, WA, "ESP110", "Radio/Mode", DEFAULT_RADIO_MODE, &radioEnabledOptions); + wifi_radio_mode = new EnumSetting("Radio mode", WEBSET, WA, "ESP110", "Radio/Mode", DEFAULT_RADIO_MODE, &radioEnabledOptions, NULL); #endif #ifdef ENABLE_WIFI telnet_port = new IntSetting( "Telnet Port", WEBSET, WA, "ESP131", "Telnet/Port", DEFAULT_TELNETSERVER_PORT, MIN_TELNET_PORT, MAX_TELNET_PORT, NULL); - telnet_enable = new EnumSetting("Telnet Enable", WEBSET, WA, "ESP130", "Telnet/Enable", DEFAULT_TELNET_STATE, &onoffOptions); + telnet_enable = new EnumSetting("Telnet Enable", WEBSET, WA, "ESP130", "Telnet/Enable", DEFAULT_TELNET_STATE, &onoffOptions, NULL); http_port = new IntSetting("HTTP Port", WEBSET, WA, "ESP121", "Http/Port", DEFAULT_WEBSERVER_PORT, MIN_HTTP_PORT, MAX_HTTP_PORT, NULL); - http_enable = new EnumSetting("HTTP Enable", WEBSET, WA, "ESP120", "Http/Enable", DEFAULT_HTTP_STATE, &onoffOptions); + http_enable = new EnumSetting("HTTP Enable", WEBSET, WA, "ESP120", "Http/Enable", DEFAULT_HTTP_STATE, &onoffOptions, NULL); wifi_hostname = new StringSetting("Hostname", WEBSET, WA, @@ -1158,7 +1158,7 @@ namespace WebUI { wifi_sta_netmask = new IPaddrSetting("Station Static Mask", WEBSET, WA, NULL, "Sta/Netmask", DEFAULT_STA_MK, NULL); wifi_sta_gateway = new IPaddrSetting("Station Static Gateway", WEBSET, WA, NULL, "Sta/Gateway", DEFAULT_STA_GW, NULL); wifi_sta_ip = new IPaddrSetting("Station Static IP", WEBSET, WA, NULL, "Sta/IP", DEFAULT_STA_IP, NULL); - wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions); + wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions, NULL); // no get, admin to set wifi_sta_password = new StringSetting("Station Password", WEBSET, diff --git a/Grbl_Esp32/src/WebUI/WifiConfig.cpp b/Grbl_Esp32/src/WebUI/WifiConfig.cpp index d918e7c7..e42c0c05 100644 --- a/Grbl_Esp32/src/WebUI/WifiConfig.cpp +++ b/Grbl_Esp32/src/WebUI/WifiConfig.cpp @@ -115,6 +115,9 @@ namespace WebUI { bool WiFiConfig::isHostnameValid(const char* hostname) { //limited size + if (!hostname) { + return true; + } char c; // length is checked automatically by string setting //only letter and digit @@ -139,6 +142,9 @@ namespace WebUI { //char c; // length is checked automatically by string setting //only printable + if (!ssid) { + return true; + } for (int i = 0; i < strlen(ssid); i++) { if (!isPrintable(ssid[i])) { return false; @@ -152,6 +158,9 @@ namespace WebUI { */ bool WiFiConfig::isPasswordValid(const char* password) { + if (!password) { + return true; + } if (strlen(password) == 0) { return true; //open network } From 5f57cf7543ea687dbb8e54a34d95cf578e5e60f9 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 4 Dec 2020 19:23:18 -0600 Subject: [PATCH 41/82] Laser mode (#692) * Update Machine.h * spindles now say if in laser mode * name fix * Updates * Getting rid of crosstalk * Update PWMSpindle.cpp * Reset some values at spindle init() * Update SettingsDefinitions.cpp * Update Grbl.h * Return to test_drive.h --- Grbl_Esp32/src/GCode.cpp | 4 ++-- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 6 +----- Grbl_Esp32/src/Protocol.cpp | 8 ++++---- Grbl_Esp32/src/SettingsDefinitions.cpp | 23 +++++++++++++---------- Grbl_Esp32/src/SettingsDefinitions.h | 2 -- Grbl_Esp32/src/Spindles/10vSpindle.cpp | 5 +++++ Grbl_Esp32/src/Spindles/Laser.cpp | 6 ++++-- Grbl_Esp32/src/Spindles/Laser.h | 2 +- Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 20 ++++++++++++++++---- Grbl_Esp32/src/Spindles/Spindle.cpp | 2 +- Grbl_Esp32/src/Spindles/Spindle.h | 2 +- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 5 +++-- Grbl_Esp32/src/Stepper.cpp | 2 +- 14 files changed, 53 insertions(+), 36 deletions(-) diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 5a24d342..6f8daf15 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -473,7 +473,7 @@ Error gc_execute_line(char* line, uint8_t client) { gc_block.modal.spindle = SpindleState::Cw; break; case 4: // Supported if SPINDLE_DIR_PIN is defined or laser mode is on. - if (spindle->is_reversable || laser_mode->get()) { + if (spindle->is_reversable || spindle->inLaserMode()) { gc_block.modal.spindle = SpindleState::Ccw; } else { FAIL(Error::GcodeUnsupportedCommand); @@ -1290,7 +1290,7 @@ Error gc_execute_line(char* line, uint8_t client) { return status; } // If in laser mode, setup laser power based on current and past parser conditions. - if (laser_mode->get()) { + if (spindle->inLaserMode()) { if (!((gc_block.modal.motion == Motion::Linear) || (gc_block.modal.motion == Motion::CwArc) || (gc_block.modal.motion == Motion::CcwArc))) { gc_parser_flags |= GCParserLaserDisable; diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index e5b624da..fa4fca3a 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 = "20201128"; +const char* const GRBL_VERSION_BUILD = "20201204"; //#include #include diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 9c808c4e..4125332f 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -353,11 +353,7 @@ namespace Motors { auto n_axis = number_axis->get(); xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. - while (true) { // don't ever return from this or the task dies - if (motorSettingChanged) { - motors_read_settings(); - motorSettingChanged = false; - } + while (true) { // don't ever return from this or the task dies if (stallguard_debug_mask->get() != 0) { if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { for (TrinamicDriver* p = List; p; p = p->link) { diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 982cecca..bfda25f5 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -96,7 +96,7 @@ bool can_park() { #ifdef ENABLE_PARKING_OVERRIDE_CONTROL sys.override_ctrl == Override::ParkingMotion && #endif - homing_enable->get() && !laser_mode->get(); + homing_enable->get() && !spindle->inLaserMode(); } /* @@ -558,7 +558,7 @@ static void protocol_exec_rt_suspend() { restore_spindle_speed = block->spindle_speed; } #ifdef DISABLE_LASER_DURING_HOLD - if (laser_mode->get()) { + if (spindle->inLaserMode()) { sys_rt_exec_accessory_override.bit.spindleOvrStop = true; } #endif @@ -661,7 +661,7 @@ static void protocol_exec_rt_suspend() { if (gc_state.modal.spindle != SpindleState::Disable) { // Block if safety door re-opened during prior restore actions. if (!sys.suspend.bit.restartRetract) { - if (laser_mode->get()) { + if (spindle->inLaserMode()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. sys.step_control.updateSpindleRpm = true; } else { @@ -717,7 +717,7 @@ static void protocol_exec_rt_suspend() { } else if (sys.spindle_stop_ovr.bit.restore || sys.spindle_stop_ovr.bit.restoreCycle) { if (gc_state.modal.spindle != SpindleState::Disable) { report_feedback_message(Message::SpindleRestore); - if (laser_mode->get()) { + if (spindle->inLaserMode()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. sys.step_control.updateSpindleRpm = true; } else { diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 0a11cdee..64757a24 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -1,7 +1,5 @@ #include "Grbl.h" -bool motorSettingChanged = false; - FlagSetting* verbose_errors; FakeSetting* number_axis; @@ -177,22 +175,27 @@ static bool checkStartupLine(char* value) { } static bool postTMC(char* value) { - if (!value) { // No POST functionality - motorSettingChanged = true; + if (!value) { + motors_read_settings(); } return true; } static bool checkSpindleChange(char* val) { if (!val) { - spindle->deinit(); - Spindles::Spindle::select(); + // if not in disable (M5) ... + if (gc_state.modal.spindle != SpindleState::Disable) { + gc_state.modal.spindle = SpindleState::Disable; + if (spindle->use_delays && spindle_delay_spindown->get() != 0) { // old spindle + vTaskDelay(spindle_delay_spindown->get() * 1000); + } + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle turned off with setting change"); + } + gc_state.spindle_speed = 0; // Set S value to 0 + spindle->deinit(); // old spindle + Spindles::Spindle::select(); // get new spindle return true; } - if (gc_state.modal.spindle != SpindleState::Disable) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle must be off to make this change"); - return false; - } return true; } diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 41599b99..d93ca82f 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -1,7 +1,5 @@ #pragma once -extern bool motorSettingChanged; - extern FlagSetting* verbose_errors; extern FakeSetting* number_axis; diff --git a/Grbl_Esp32/src/Spindles/10vSpindle.cpp b/Grbl_Esp32/src/Spindles/10vSpindle.cpp index dd2284b5..09162c0c 100644 --- a/Grbl_Esp32/src/Spindles/10vSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/10vSpindle.cpp @@ -170,20 +170,25 @@ namespace Spindles { void _10v::deinit() { #ifdef SPINDLE_OUTPUT_PIN + gpio_reset_pin(SPINDLE_OUTPUT_PIN); pinMode(SPINDLE_OUTPUT_PIN, INPUT); #endif #ifdef SPINDLE_ENABLE_PIN + gpio_reset_pin(SPINDLE_ENABLE_PIN); pinMode(SPINDLE_ENABLE_PIN, INPUT); #endif #ifdef SPINDLE_DIR_PIN + gpio_reset_pin(SPINDLE_DIR_PIN); pinMode(SPINDLE_DIR_PIN, INPUT); #endif #ifdef SPINDLE_FORWARD_PIN + gpio_reset_pin(SPINDLE_FORWARD_PIN); pinMode(SPINDLE_FORWARD_PIN, INPUT); #endif #ifdef SPINDLE_REVERSE_PIN + gpio_reset_pin(SPINDLE_FORWARD_PIN); pinMode(SPINDLE_FORWARD_PIN, INPUT); #endif } diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp index befdc0d9..6b7d9e6c 100644 --- a/Grbl_Esp32/src/Spindles/Laser.cpp +++ b/Grbl_Esp32/src/Spindles/Laser.cpp @@ -24,8 +24,8 @@ // ===================================== Laser ============================================== namespace Spindles { - bool Laser::isRateAdjusted() { - return true; // can use M4 (CCW) laser mode. + bool Laser::inLaserMode() { + return laser_mode->get(); // can use M4 (CCW) laser mode. } void Laser::config_message() { @@ -89,10 +89,12 @@ namespace Spindles { void Laser::deinit() { stop(); #ifdef LASER_OUTPUT_PIN + gpio_reset_pin(LASER_OUTPUT_PIN); pinMode(LASER_OUTPUT_PIN, INPUT); #endif #ifdef LASER_ENABLE_PIN + gpio_reset_pin(LASER_ENABLE_PIN); pinMode(LASER_ENABLE_PIN, INPUT); #endif } diff --git a/Grbl_Esp32/src/Spindles/Laser.h b/Grbl_Esp32/src/Spindles/Laser.h index 87673f62..10120a31 100644 --- a/Grbl_Esp32/src/Spindles/Laser.h +++ b/Grbl_Esp32/src/Spindles/Laser.h @@ -34,7 +34,7 @@ namespace Spindles { Laser& operator=(const Laser&) = delete; Laser& operator=(Laser&&) = delete; - bool isRateAdjusted() override; + bool inLaserMode() override; void config_message() override; void get_pins_and_settings() override; void deinit() override; diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 1ade8aa8..23477494 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -42,14 +42,15 @@ namespace Spindles { return; } + _current_state = SpindleState::Disable; + _current_pwm_duty = 0; + use_delays = true; + ledcSetup(_pwm_chan_num, (double)_pwm_freq, _pwm_precision); // setup the channel ledcAttachPin(_output_pin, _pwm_chan_num); // attach the PWM to the pin - pinMode(_enable_pin, OUTPUT); pinMode(_direction_pin, OUTPUT); - use_delays = true; - config_message(); } @@ -146,7 +147,7 @@ namespace Spindles { } } - set_enable_pin(_current_state != SpindleState::Disable); + set_enable_pin(gc_state.modal.spindle != SpindleState::Disable); set_output(pwm_value); return 0; @@ -225,6 +226,14 @@ namespace Spindles { } void PWM::set_enable_pin(bool enable) { + // static bool prev_enable = false; + + // if (prev_enable == enable) { + // return; + // } + + // prev_enable = enable; + if (_enable_pin == UNDEFINED_PIN) { return; } @@ -262,13 +271,16 @@ namespace Spindles { void PWM::deinit() { stop(); #ifdef SPINDLE_OUTPUT_PIN + gpio_reset_pin(SPINDLE_OUTPUT_PIN); pinMode(SPINDLE_OUTPUT_PIN, INPUT); #endif #ifdef SPINDLE_ENABLE_PIN + gpio_reset_pin(SPINDLE_ENABLE_PIN); pinMode(SPINDLE_ENABLE_PIN, INPUT); #endif #ifdef SPINDLE_DIR_PIN + gpio_reset_pin(SPINDLE_DIR_PIN); pinMode(SPINDLE_DIR_PIN, INPUT); #endif } diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index d0ec75a4..f705aea7 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -89,7 +89,7 @@ namespace Spindles { // ========================= Spindle ================================== - bool Spindle::isRateAdjusted() { + bool Spindle::inLaserMode() { return false; // default for basic spindle is false } diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h index 5fd37e5b..6851544a 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.h +++ b/Grbl_Esp32/src/Spindles/Spindle.h @@ -64,7 +64,7 @@ namespace Spindles { virtual SpindleState get_state() = 0; virtual void stop() = 0; virtual void config_message() = 0; - virtual bool isRateAdjusted(); + virtual bool inLaserMode(); virtual void sync(SpindleState state, uint32_t rpm); virtual void deinit(); diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index a7a2636a..9b119c61 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -273,10 +273,10 @@ namespace Spindles { } // Initialization is complete, so now it's okay to run the queue task: - task_active = true; + task_active = true; if (vfd_cmd_queue != nullptr) { vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); - } + } xTaskCreatePinnedToCore(vfd_cmd_task, // task "vfd_cmdTaskHandle", // name for task 2048, // size of task stack @@ -323,6 +323,7 @@ namespace Spindles { pins_settings_ok = false; #endif + // TODO Test no longer required. if (laser_mode->get()) { grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); pins_settings_ok = false; diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index ae4a56b2..83aee938 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -551,7 +551,7 @@ void st_prep_buffer() { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } - if (spindle->isRateAdjusted()) { // laser_mode->get() { + if (spindle->inLaserMode()) { // if (pl_block->spindle == SpindleState::Ccw) { // Pre-compute inverse programmed rate to speed up PWM updating per step segment. prep.inv_rate = 1.0 / pl_block->programmed_rate; From 00e0e7843eec98e2c5e807b2d5b571b510fc5864 Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 5 Dec 2020 11:07:23 -0600 Subject: [PATCH 42/82] User macro button (#685) * Test Macro Button Idea * Updates * Formating --- Grbl_Esp32/src/Config.h | 5 +- Grbl_Esp32/src/Defaults.h | 16 +++++++ Grbl_Esp32/src/Machine.h | 2 +- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 9 ++-- Grbl_Esp32/src/SettingsDefinitions.cpp | 20 ++++++-- Grbl_Esp32/src/SettingsDefinitions.h | 5 ++ Grbl_Esp32/src/System.cpp | 47 ++++++++++++++++--- 7 files changed, 87 insertions(+), 17 deletions(-) diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 50b014f8..57919c06 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -50,9 +50,10 @@ Some features should not be changed. See notes below. // Inverts pin logic of the control command pins based on a mask. This essentially means you can use // normally-closed switches on the specified pins, rather than the default normally-open switches. -// The mask order is Cycle Start | Feed Hold | Reset | Safety Door +// The mask order is ... +// Macro3 | Macro2 | Macro 1| Macr0 |Cycle Start | Feed Hold | Reset | Safety Door // For example B1101 will invert the function of the Reset pin. -#define INVERT_CONTROL_PIN_MASK B1111 +#define INVERT_CONTROL_PIN_MASK B00001111 #define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. #define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 3421c670..f13caa8b 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -647,3 +647,19 @@ #ifndef USER_ANALOG_PIN_3_FREQ # define USER_ANALOG_PIN_3_FREQ 5000 #endif + +#ifndef DEFAULT_USER_MACRO0 +# define DEFAULT_USER_MACRO0 "" +#endif + +#ifndef DEFAULT_USER_MACRO1 +# define DEFAULT_USER_MACRO1 "" +#endif + +#ifndef DEFAULT_USER_MACRO2 +# define DEFAULT_USER_MACRO2 "" +#endif + +#ifndef DEFAULT_USER_MACRO3 +# define DEFAULT_USER_MACRO3 "" +#endif \ No newline at end of file diff --git a/Grbl_Esp32/src/Machine.h b/Grbl_Esp32/src/Machine.h index 11ab8406..ac3377da 100644 --- a/Grbl_Esp32/src/Machine.h +++ b/Grbl_Esp32/src/Machine.h @@ -8,7 +8,7 @@ // !!! For initial testing, start with test_drive.h which disables // all I/O pins // #include "Machines/atari_1020.h" -# include "Machines/test_drive.h" +# include "Machines/6_pack_stepstick_XYZ_v1.h" // !!! For actual use, change the line above to select a board // from Machines/, for example: diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index e3a8e1c6..28b6b3ef 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -106,11 +106,14 @@ Socket #5 #define Z_LIMIT_PIN GPIO_NUM_35 + + // // 4x Input Module in Socket #2 // // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -// #define X_LIMIT_PIN GPIO_NUM_2 -// #define Y_LIMIT_PIN GPIO_NUM_25 -// #define Z_LIMIT_PIN GPIO_NUM_39 +#define MACRO_BUTTON_0_PIN GPIO_NUM_2 +#define MACRO_BUTTON_1_PIN GPIO_NUM_25 +#define MACRO_BUTTON_2_PIN GPIO_NUM_39 +#define MACRO_BUTTON_3_PIN GPIO_NUM_36 // 5V output CNC module in socket #4 // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 64757a24..c25f35a5 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -80,6 +80,11 @@ AxisSettings* c_axis_settings; AxisSettings* axis_settings[MAX_N_AXIS]; +StringSetting* user_macro0; +StringSetting* user_macro1; +StringSetting* user_macro2; +StringSetting* user_macro3; + typedef struct { const char* name; float steps_per_mm; @@ -389,10 +394,15 @@ void make_settings() { stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postTMC); - homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); - homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); - homing_cycle[2] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle2", DEFAULT_HOMING_CYCLE_2); - homing_cycle[3] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle3", DEFAULT_HOMING_CYCLE_3); - homing_cycle[4] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle4", DEFAULT_HOMING_CYCLE_4); homing_cycle[5] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle5", DEFAULT_HOMING_CYCLE_5); + homing_cycle[4] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle4", DEFAULT_HOMING_CYCLE_4); + homing_cycle[3] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle3", DEFAULT_HOMING_CYCLE_3); + homing_cycle[2] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle2", DEFAULT_HOMING_CYCLE_2); + homing_cycle[1] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle1", DEFAULT_HOMING_CYCLE_1); + homing_cycle[0] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle0", DEFAULT_HOMING_CYCLE_0); + + user_macro3 = new StringSetting(EXTENDED, WG, NULL, "User/Macro3", DEFAULT_USER_MACRO3); + user_macro2 = new StringSetting(EXTENDED, WG, NULL, "User/Macro2", DEFAULT_USER_MACRO2); + user_macro1 = new StringSetting(EXTENDED, WG, NULL, "User/Macro1", DEFAULT_USER_MACRO1); + user_macro0 = new StringSetting(EXTENDED, WG, NULL, "User/Macro0", DEFAULT_USER_MACRO0); } diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index d93ca82f..7315637a 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -60,3 +60,8 @@ extern IntSetting* spindle_pwm_bit_precision; extern EnumSetting* spindle_type; extern AxisMaskSetting* stallguard_debug_mask; + +extern StringSetting* user_macro0; +extern StringSetting* user_macro1; +extern StringSetting* user_macro2; +extern StringSetting* user_macro3; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 498915f1..7d6b3cbf 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -63,22 +63,22 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi attachInterrupt(digitalPinToInterrupt(CONTROL_CYCLE_START_PIN), isr_control_inputs, CHANGE); #endif #ifdef MACRO_BUTTON_0_PIN - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 0"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 0 %s", pinName(MACRO_BUTTON_0_PIN).c_str()); pinMode(MACRO_BUTTON_0_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_0_PIN), isr_control_inputs, CHANGE); #endif #ifdef MACRO_BUTTON_1_PIN - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 1"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 1 %s", pinName(MACRO_BUTTON_1_PIN).c_str()); pinMode(MACRO_BUTTON_1_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_1_PIN), isr_control_inputs, CHANGE); #endif #ifdef MACRO_BUTTON_2_PIN - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 2"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 2 %s", pinName(MACRO_BUTTON_2_PIN).c_str()); pinMode(MACRO_BUTTON_2_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_2_PIN), isr_control_inputs, CHANGE); #endif #ifdef MACRO_BUTTON_3_PIN - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 3"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro Pin 3 %s", pinName(MACRO_BUTTON_3_PIN).c_str()); pinMode(MACRO_BUTTON_3_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(MACRO_BUTTON_3_PIN), isr_control_inputs, CHANGE); #endif @@ -87,7 +87,7 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi control_sw_queue = xQueueCreate(10, sizeof(int)); xTaskCreate(controlCheckTask, "controlCheckTask", - 2048, + 3096, NULL, 5, // priority NULL); @@ -332,4 +332,39 @@ uint8_t sys_calc_pwm_precision(uint32_t freq) { return precision - 1; } -void __attribute__((weak)) user_defined_macro(uint8_t index); +void __attribute__((weak)) user_defined_macro(uint8_t index) { + // must be in Idle + if (sys.state != State::Idle) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro button only permitted in idle"); + return; + } + + String user_macro; + char line[255]; + switch (index) { + case 0: + user_macro = user_macro0->get(); + break; + case 1: + user_macro = user_macro1->get(); + break; + case 2: + user_macro = user_macro2->get(); + break; + case 3: + user_macro = user_macro3->get(); + break; + default: + return; + } + + if (user_macro == "") { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro User/Macro%d empty", index); + return; + } + + user_macro.replace('&', '\n'); + user_macro.toCharArray(line, 255, 0); + strcat(line, "\r"); + WebUI::inputBuffer.push(line); +} From f518a4945c238199d5aaa4e63fcd93ec20c71938 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 6 Dec 2020 12:05:37 -0600 Subject: [PATCH 43/82] Changed macro pin reporting to be a single character --- Grbl_Esp32/src/Report.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 20cff520..ce35714f 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -753,16 +753,16 @@ void report_realtime_status(uint8_t client) { strcat(status, "S"); } if (ctrl_pin_state.bit.macro0) { - strcat(status, "M0"); + strcat(status, "0"); } if (ctrl_pin_state.bit.macro1) { - strcat(status, "M1"); + strcat(status, "1"); } if (ctrl_pin_state.bit.macro2) { - strcat(status, "M2"); + strcat(status, "2"); } if (ctrl_pin_state.bit.macro3) { - strcat(status, "M3"); + strcat(status, "3"); } } } From 660b46cc32e695168a4ba97ad0d6138a0f3705a2 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 8 Dec 2020 09:04:27 -0600 Subject: [PATCH 44/82] Sd Web UI issues (#698) * Updates * returned reportTaskStackSize(uxHighWaterMark); In a #ifdef DEBUG_TASK_STACK guard * Disallow web commands unless idle or alarm state * merging stuff after review * Handle SD busy state in webserver handler (#697) * Handle SD busy state in webserver handler * Update index.html.gz * Fixed reporting * Add case for SD not enabled. * Prevent Web commands except in idle or alarm * Return authentication to the default Co-authored-by: Mitch Bradley Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com> --- Grbl_Esp32/src/Config.h | 5 ++-- Grbl_Esp32/src/Defaults.h | 12 ++++++--- Grbl_Esp32/src/Error.cpp | 1 + Grbl_Esp32/src/Error.h | 1 + Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/I2SOut.cpp | 2 ++ Grbl_Esp32/src/Limits.cpp | 2 ++ Grbl_Esp32/src/Machine.h | 2 +- .../src/Machines/6_pack_stepstick_XYZ_v1.h | 7 ++++++ Grbl_Esp32/src/Motors/Servo.cpp | 5 +++- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 8 +++--- Grbl_Esp32/src/ProcessSettings.cpp | 2 +- Grbl_Esp32/src/SDCard.cpp | 23 +++++++++--------- Grbl_Esp32/src/SDCard.h | 8 +++--- Grbl_Esp32/src/Serial.cpp | 23 +++++++++++++----- Grbl_Esp32/src/Settings.cpp | 8 +++--- Grbl_Esp32/src/Settings.h | 22 +++++++++++------ Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 4 ++- Grbl_Esp32/src/System.cpp | 6 +++-- Grbl_Esp32/src/WebUI/WebServer.cpp | 8 ++++-- Grbl_Esp32/src/WebUI/WebSettings.cpp | 21 +++++++++++----- Grbl_Esp32/src/data/index.html.gz | Bin 128570 -> 116657 bytes 22 files changed, 116 insertions(+), 56 deletions(-) diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 57919c06..f22e88aa 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -47,6 +47,7 @@ Some features should not be changed. See notes below. // that the machine file might choose to undefine. // Note: HOMING_CYCLES are now settings +#define SUPPORT_TASK_CORE 1 // Reference: CONFIG_ARDUINO_RUNNING_CORE = 1 // Inverts pin logic of the control command pins based on a mask. This essentially means you can use // normally-closed switches on the specified pins, rather than the default normally-open switches. @@ -55,7 +56,7 @@ Some features should not be changed. See notes below. // For example B1101 will invert the function of the Reset pin. #define INVERT_CONTROL_PIN_MASK B00001111 -#define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. +// #define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. #define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds #define USE_RMT_STEPS @@ -129,7 +130,7 @@ const int MAX_N_AXIS = 6; // "in the clear" over unsecured channels. It should be treated as a // "friendly suggestion" to prevent unwitting dangerous actions, rather than // as effective security against malice. -//#define ENABLE_AUTHENTICATION +// #define ENABLE_AUTHENTICATION //CONFIGURE_EYECATCH_END (DO NOT MODIFY THIS LINE) #ifdef ENABLE_AUTHENTICATION diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index f13caa8b..41776f56 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -461,6 +461,10 @@ // This can eliminate checking to see if the pin is defined because // the overridden pinMode and digitalWrite functions will deal with it. +#ifndef SDCARD_DET_PIN +# define SDCARD_DET_PIN UNDEFINED_PIN +#endif + #ifndef STEPPERS_DISABLE_PIN # define STEPPERS_DISABLE_PIN UNDEFINED_PIN #endif @@ -649,17 +653,17 @@ #endif #ifndef DEFAULT_USER_MACRO0 -# define DEFAULT_USER_MACRO0 "" +# define DEFAULT_USER_MACRO0 "" #endif #ifndef DEFAULT_USER_MACRO1 -# define DEFAULT_USER_MACRO1 "" +# define DEFAULT_USER_MACRO1 "" #endif #ifndef DEFAULT_USER_MACRO2 -# define DEFAULT_USER_MACRO2 "" +# define DEFAULT_USER_MACRO2 "" #endif #ifndef DEFAULT_USER_MACRO3 -# define DEFAULT_USER_MACRO3 "" +# define DEFAULT_USER_MACRO3 "" #endif \ No newline at end of file diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index 149b0665..34dd1cd4 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -79,4 +79,5 @@ std::map ErrorNames = { { Error::NvsSetFailed, "Failed to store setting" }, { Error::NvsGetStatsFailed, "Failed to get setting status" }, { Error::AuthenticationFailed, "Authentication failed!" }, + { Error::AnotherInterfaceBusy, "Another interface is busy"}, }; diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index 09a54030..80f40a39 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -83,6 +83,7 @@ enum class Error : uint8_t { NvsGetStatsFailed = 101, AuthenticationFailed = 110, Eol = 111, + AnotherInterfaceBusy = 120, }; extern std::map ErrorNames; diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index fa4fca3a..5b1bf3db 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 = "20201204"; +const char* const GRBL_VERSION_BUILD = "20201207"; //#include #include diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp index d41fed02..211cffaf 100644 --- a/Grbl_Esp32/src/I2SOut.cpp +++ b/Grbl_Esp32/src/I2SOut.cpp @@ -526,7 +526,9 @@ static void IRAM_ATTR i2sOutTask(void* parameter) { I2S_OUT_PULSER_EXIT_CRITICAL(); // Unlock pulser status static UBaseType_t uxHighWaterMark = 0; +# ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +# endif } } #endif diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 41198dcf..029e3477 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -372,7 +372,9 @@ void limitCheckTask(void* pvParameters) { sys_rt_exec_alarm = ExecAlarm::HardLimit; // Indicate hard limit critical event } static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } diff --git a/Grbl_Esp32/src/Machine.h b/Grbl_Esp32/src/Machine.h index ac3377da..11ab8406 100644 --- a/Grbl_Esp32/src/Machine.h +++ b/Grbl_Esp32/src/Machine.h @@ -8,7 +8,7 @@ // !!! For initial testing, start with test_drive.h which disables // all I/O pins // #include "Machines/atari_1020.h" -# include "Machines/6_pack_stepstick_XYZ_v1.h" +# include "Machines/test_drive.h" // !!! For actual use, change the line above to select a board // from Machines/, for example: diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h index 28b6b3ef..5bc9da03 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h @@ -132,6 +132,13 @@ Socket #5 // #define VFD_RS485_RTS_PIN GPIO_NUM_4 // #define VFD_RS485_RXD_PIN GPIO_NUM_16 +// Example (4x) 5V Buffer Output on socket #5 +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module +#define USER_DIGITAL_PIN_0 I2SO(24) // No PWM +#define USER_DIGITAL_PIN_1 I2SO(25) +#define USER_DIGITAL_PIN_2 I2SO(26) // M7 on M9 Off +#define USER_DIGITAL_PIN_3 I2SO(27) // M8 on M9 Off + // ================= Setting Defaults ========================== #define DEFAULT_X_STEPS_PER_MM 800 #define DEFAULT_Y_STEPS_PER_MM 800 diff --git a/Grbl_Esp32/src/Motors/Servo.cpp b/Grbl_Esp32/src/Motors/Servo.cpp index 65e213a1..3702f06b 100644 --- a/Grbl_Esp32/src/Motors/Servo.cpp +++ b/Grbl_Esp32/src/Motors/Servo.cpp @@ -40,6 +40,7 @@ namespace Motors { } void Servo::startUpdateTask() { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Servo Update Task Started"); if (this == List) { xTaskCreatePinnedToCore(updateTask, // task "servoUpdateTask", // name for task @@ -47,7 +48,7 @@ namespace Motors { NULL, // parameters 1, // priority NULL, // handle - 0 // core + SUPPORT_TASK_CORE // core ); } } @@ -67,7 +68,9 @@ namespace Motors { vTaskDelayUntil(&xLastWakeTime, xUpdate); static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 4125332f..2a7c9f4f 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -115,8 +115,8 @@ namespace Motors { NULL, // parameters 1, // priority NULL, - CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core - // core + SUPPORT_TASK_CORE // must run the task on same core + // core ); } } @@ -353,7 +353,7 @@ namespace Motors { auto n_axis = number_axis->get(); xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. - while (true) { // don't ever return from this or the task dies + while (true) { // don't ever return from this or the task dies if (stallguard_debug_mask->get() != 0) { if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { for (TrinamicDriver* p = List; p; p = p->link) { @@ -368,7 +368,9 @@ namespace Motors { vTaskDelayUntil(&xLastWakeTime, xreadSg); static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index 2fe07b5a..dc19fd65 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -427,7 +427,7 @@ static bool anyState() { static bool idleOrJog() { return sys.state != State::Idle && sys.state != State::Jog; } -static bool idleOrAlarm() { +bool idleOrAlarm() { return sys.state != State::Idle && sys.state != State::Alarm; } static bool notCycleOrHold() { diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index c2d8d3c1..2a6fab21 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -20,12 +20,12 @@ #include "SDCard.h" -File myFile; -bool SD_ready_next = false; // Grbl has processed a line and is waiting for another -uint8_t SD_client = CLIENT_SERIAL; +File myFile; +bool SD_ready_next = false; // Grbl has processed a line and is waiting for another +uint8_t SD_client = CLIENT_SERIAL; WebUI::AuthenticationLevel SD_auth_level = WebUI::AuthenticationLevel::LEVEL_GUEST; -uint32_t sd_current_line_number; // stores the most recent line number read from the SD -static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. +uint32_t sd_current_line_number; // stores the most recent line number read from the SD +static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. // attempt to mount the SD card /*bool sd_mount() @@ -128,13 +128,14 @@ uint32_t sd_get_current_line_number() { uint8_t sd_state = SDCARD_IDLE; uint8_t get_sd_state(bool refresh) { -#if defined(SDCARD_DET_PIN) && SDCARD_SD_PIN != -1 - //no need to go further if SD detect is not correct - if (!((digitalRead(SDCARD_DET_PIN) == SDCARD_DET_VAL) ? true : false)) { - sd_state = SDCARD_NOT_PRESENT; - return sd_state; + if (SDCARD_DET_PIN != UNDEFINED_PIN) { + if (digitalRead(SDCARD_DET_PIN) != SDCARD_DET_VAL) { + sd_state = SDCARD_NOT_PRESENT; + return sd_state; + //no need to go further if SD detect is not correct + } } -#endif + //if busy doing something return state if (!((sd_state == SDCARD_NOT_PRESENT) || (sd_state == SDCARD_IDLE))) { return sd_state; diff --git a/Grbl_Esp32/src/SDCard.h b/Grbl_Esp32/src/SDCard.h index a64a4e7d..298fbdec 100644 --- a/Grbl_Esp32/src/SDCard.h +++ b/Grbl_Esp32/src/SDCard.h @@ -20,8 +20,8 @@ #include #include -#define SDCARD_DET_PIN -1 -const int SDCARD_DET_VAL = 0; +//#define SDCARD_DET_PIN -1 +const int SDCARD_DET_VAL = 0; // for now, CD is close to ground const int SDCARD_IDLE = 0; const int SDCARD_NOT_PRESENT = 1; @@ -29,8 +29,8 @@ const int SDCARD_BUSY_PRINTING = 2; const int SDCARD_BUSY_UPLOADING = 4; const int SDCARD_BUSY_PARSING = 8; -extern bool SD_ready_next; // Grbl has processed a line and is waiting for another -extern uint8_t SD_client; +extern bool SD_ready_next; // Grbl has processed a line and is waiting for another +extern uint8_t SD_client; extern WebUI::AuthenticationLevel SD_auth_level; //bool sd_mount(); diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index 8a1403cf..5e5e2549 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -79,7 +79,9 @@ void heapCheckTask(void* pvParameters) { vTaskDelay(3000 / portTICK_RATE_MS); // Yield to other tasks static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } @@ -104,8 +106,8 @@ void serial_init() { NULL, // parameters 1, // priority &serialCheckTaskHandle, - CONFIG_ARDUINO_RUNNING_CORE // must run the task on same core - // core + SUPPORT_TASK_CORE // must run the task on same core + // core ); } @@ -157,9 +159,16 @@ void serialCheckTask(void* pvParameters) { if (is_realtime_command(data)) { execute_realtime_command(static_cast(data), client); } else { - vTaskEnterCritical(&myMutex); - client_buffer[client].write(data); - vTaskExitCritical(&myMutex); + if (get_sd_state(false) == SDCARD_IDLE) { + vTaskEnterCritical(&myMutex); + client_buffer[client].write(data); + vTaskExitCritical(&myMutex); + } else { + if (data == '\r' || data == '\n') { + grbl_sendf(client, "error %d\r\n", Error::AnotherInterfaceBusy); + grbl_msg_sendf(client, MsgLevel::Info, "SD card job running"); + } + } } } // if something available WebUI::COMMANDS::handle(); @@ -175,8 +184,10 @@ void serialCheckTask(void* pvParameters) { vTaskDelay(1 / portTICK_RATE_MS); // Yield to other tasks static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); - } // while(true) +#endif + } } void serial_reset_read_buffer(uint8_t client) { diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index ce7af34a..678a43c0 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -8,8 +8,10 @@ Word::Word(type_t type, permissions_t permissions, const char* description, cons Command* Command::List = NULL; -Command::Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName) : - Word(type, permissions, description, grblName, fullName) { +Command::Command( + const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*cmdChecker)()) : + Word(type, permissions, description, grblName, fullName), + _cmdChecker(cmdChecker) { link = List; List = this; } @@ -704,7 +706,7 @@ void IPaddrSetting::addWebui(WebUI::JSONencoder* j) { AxisSettings::AxisSettings(const char* axisName) : name(axisName) {} Error GrblCommand::action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { - if (_checker && _checker()) { + if (_cmdChecker && _cmdChecker()) { return Error::IdleError; } return _action((const char*)value, auth_level, out); diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index 8e02a125..e206b307 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -75,12 +75,13 @@ public: class Command : public Word { protected: Command* link; // linked list of setting objects + bool (*_cmdChecker)(); public: static Command* List; Command* next() { return link; } ~Command() {} - Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName); + Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*cmcChecker)()); // The default implementation of addWebui() does nothing. // Derived classes may override it to do something. @@ -442,6 +443,9 @@ public: AxisSettings(const char* axisName); }; + +extern bool idleOrAlarm(); + class WebCommand : public Command { private: Error (*_action)(char*, WebUI::AuthenticationLevel); @@ -454,7 +458,10 @@ public: const char* grblName, const char* name, Error (*action)(char*, WebUI::AuthenticationLevel)) : - Command(description, type, permissions, grblName, name), + // At some point we might want to be more subtle, but for now we block + // all web commands in Cycle and Hold states, to avoid crashing a + // running job. + Command(description, type, permissions, grblName, name, idleOrAlarm), _action(action) {} Error action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* response); }; @@ -462,22 +469,21 @@ public: class GrblCommand : public Command { private: Error (*_action)(const char*, WebUI::AuthenticationLevel, WebUI::ESPResponseStream*); - bool (*_checker)(); public: GrblCommand(const char* grblName, const char* name, Error (*action)(const char*, WebUI::AuthenticationLevel, WebUI::ESPResponseStream*), - bool (*checker)(), + bool (*cmdChecker)(), permissions_t auth) : - Command(NULL, GRBLCMD, auth, grblName, name), - _action(action), _checker(checker) {} + Command(NULL, GRBLCMD, auth, grblName, name, cmdChecker), + _action(action) {} GrblCommand(const char* grblName, const char* name, Error (*action)(const char*, WebUI::AuthenticationLevel, WebUI::ESPResponseStream*), - bool (*checker)(void)) : - GrblCommand(grblName, name, action, checker, WG) {} + bool (*cmdChecker)()) : + GrblCommand(grblName, name, action, cmdChecker, WG) {} Error action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* response); }; diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 9b119c61..ceffdb42 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -194,7 +194,9 @@ namespace Spindles { vTaskDelay(VFD_RS485_POLL_RATE); static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +#endif } } @@ -283,7 +285,7 @@ namespace Spindles { this, // parameters 1, // priority &vfd_cmdTaskHandle, - 1); + SUPPORT_TASK_CORE); is_reversable = true; // these VFDs are always reversable use_delays = true; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 7d6b3cbf..6b21702c 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -125,7 +125,9 @@ void controlCheckTask(void* pvParameters) { debouncing = false; static UBaseType_t uxHighWaterMark = 0; +# ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); +# endif } } #endif @@ -343,7 +345,7 @@ void __attribute__((weak)) user_defined_macro(uint8_t index) { char line[255]; switch (index) { case 0: - user_macro = user_macro0->get(); + user_macro = user_macro0->get(); break; case 1: user_macro = user_macro1->get(); @@ -362,7 +364,7 @@ void __attribute__((weak)) user_defined_macro(uint8_t index) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Macro User/Macro%d empty", index); return; } - + user_macro.replace('&', '\n'); user_macro.toCharArray(line, 255, 0); strcat(line, "\r"); diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index f9ef698d..d58a8b7c 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -1221,9 +1221,13 @@ namespace WebUI { bool list_files = true; uint64_t totalspace = 0; uint64_t usedspace = 0; - if (get_sd_state(true) != SDCARD_IDLE) { + int8_t state = get_sd_state(true); + if (state != SDCARD_IDLE) { + String status = "{\"status\":\""; + if(state == SDCARD_NOT_PRESENT)status+="No SD Card\"}"; + else status+="Busy\"}"; _webserver->sendHeader("Cache-Control", "no-cache"); - _webserver->send(200, "application/json", "{\"status\":\"No SD Card\"}"); + _webserver->send(200, "application/json", status); return; } set_sd_state(SDCARD_BUSY_PARSING); diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index f9a00fc4..cec830b9 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -163,6 +163,9 @@ namespace WebUI { } Error WebCommand::action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) { + if (_cmdChecker && _cmdChecker()) { + return Error::AnotherInterfaceBusy; + } char empty = '\0'; if (!value) { value = ∅ @@ -716,6 +719,10 @@ namespace WebUI { static Error runSDFile(char* parameter, AuthenticationLevel auth_level) { // ESP220 Error err; + if (sys.state == State::Alarm) { + webPrintln("Alarm"); + return Error::IdleError; + } if (sys.state != State::Idle) { webPrintln("Busy"); return Error::IdleError; @@ -859,7 +866,9 @@ namespace WebUI { default: resp = "Busy"; } -#endif +#else + resp = "SD card not enabled"; +#endif webPrintln(resp); return Error::Ok; } @@ -1066,7 +1075,7 @@ namespace WebUI { #ifdef ENABLE_NOTIFICATIONS notification_ts = new StringSetting( "Notification Settings", WEBSET, WA, NULL, "Notification/TS", DEFAULT_TOKEN, 0, MAX_NOTIFICATION_SETTING_LENGTH, NULL); - notification_t2 = new StringSetting("Notification Token 2", + notification_t2 = new StringSetting("Notification Token 2", WEBSET, WA, NULL, @@ -1075,7 +1084,7 @@ namespace WebUI { MIN_NOTIFICATION_TOKEN_LENGTH, MAX_NOTIFICATION_TOKEN_LENGTH, NULL); - notification_t1 = new StringSetting("Notification Token 1", + notification_t1 = new StringSetting("Notification Token 1", WEBSET, WA, NULL, @@ -1084,8 +1093,8 @@ namespace WebUI { MIN_NOTIFICATION_TOKEN_LENGTH, MAX_NOTIFICATION_TOKEN_LENGTH, NULL); - notification_type = - new EnumSetting("Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions, NULL); + notification_type = new EnumSetting( + "Notification type", WEBSET, WA, NULL, "Notification/Type", DEFAULT_NOTIFICATION_TYPE, ¬ificationOptions, NULL); #endif #ifdef ENABLE_AUTHENTICATION user_password = new StringSetting("User password", @@ -1158,7 +1167,7 @@ namespace WebUI { wifi_sta_netmask = new IPaddrSetting("Station Static Mask", WEBSET, WA, NULL, "Sta/Netmask", DEFAULT_STA_MK, NULL); wifi_sta_gateway = new IPaddrSetting("Station Static Gateway", WEBSET, WA, NULL, "Sta/Gateway", DEFAULT_STA_GW, NULL); wifi_sta_ip = new IPaddrSetting("Station Static IP", WEBSET, WA, NULL, "Sta/IP", DEFAULT_STA_IP, NULL); - wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions, NULL); + wifi_sta_mode = new EnumSetting("Station IP Mode", WEBSET, WA, "ESP102", "Sta/IPMode", DEFAULT_STA_IP_MODE, &staModeOptions, NULL); // no get, admin to set wifi_sta_password = new StringSetting("Station Password", WEBSET, diff --git a/Grbl_Esp32/src/data/index.html.gz b/Grbl_Esp32/src/data/index.html.gz index 1cb7e416e14d6369c4cfdb21fcb067aebc0f41a5..05a4f23a132ac4058a0e7678ff6ac328cb5539be 100644 GIT binary patch literal 116657 zcmZ^~Q*>rQ*RCCRY}>YN+qP||W9Nx&+qOEkZ5tgs`TKqM|Lvo1*GbjAR@E3+jkyk1 z)x0MG021gw4FvkC$13e`q=E3=cP8TpC@rtHa~lkYM#HU#rp4BvC&3m^CO6?|ehFBr z9uh`33aF|pQ{|A zaXe}I2>@_)0roE&Kez|DX_L?VPPqa!|8{bsOuIP?Dyf}tJ}u=4zj_`XR=C;ib2YGE z?JAx!_wwGKYRh8Y7EEyXekIn^E$f~m*D7P6w{Ln}w(nv9Z`W8a)a}tMZd>Pd8z<>B1BU&P?gr)*+YVEB0*eA!=}`H{Bp|2YAivYrEsduQE`kL?;gCQSWvQZ*)R z`}|U_R8B)(oS422xPlRh3`&}+MBLr7ZUDAjLjC!-vk)XOuf&^W5BHUtKu65ufF;=0 z-(<$kdmGnHZjF6KWAwRYb(5#)W5nS2bYN@7M8T;Z{Q;-`uiO4V4l@MVb*-C?lrQ5m z9dI8;{%5>@>@#`&;b}AOy1##S`APR1*n4<MY1RPUf^Tbf=j8yMQ2#c_-nQwT!dY|Vtyi9$9%ewFSqayRU#s30o>9E6MOLIo z-qoaSZdp(97`l}Ma3|>`0RM4|cRhu1Pk$6})%Cd1iRpC2O+rDoaV34Oy$-XJatyi*kKG*4A-8f9V z{kcK2$Yz>W0p^j1Ycc-GVLNHQFN|ROf^xPg*8uv}N_Cqg1wz*eej=-`LwMTGmn;d= zXvecZP(A61u7?Qh-69_3h zC!RZ6APbq^gq^BCsd;t6dq~yZ1Er0i%x1{fG{*e{xYYF0W(3Yq$;n5~ z+p6)`yqC8q1pUKx71VX254DCaqOYaV(Y5|=-LnLJZ_~96DVFM5NCt~6NXKq(!?9K( zjn|q`3>wjR!Pd@f8{u=`G&*6TRh{Gtk5zk3TNa(I`E8=3AIW*O1 zEU-IPNTMIgTlaIA%ugEy$sne*RAp$_18y4sn)KW&cPo&y)ANwjycvWKkceG77Q?e# zwPyrO2`I(3SGDR^Y2HR*fNKWseIi<>tmH0To1wv$EvR~wa$%WY#D)qIOG*3@)YxO*7ye)B*f4 zu7XXpSIR}uu|qXKimuhG)iH`Td|x?+h+=ISZxKO=jO$BC&<{>Y;P~9FQ!72B3cI4w zeGgQ7agS_*YxR?{^TAZ380DME46B?G^INP=7w__YPU_XjXH5hJFSxED%7VxE(`3&l zwamx3n>4RzZ8k4B=`XGw;{*D)Zw`GUnD3sP*Qba>BJI4Xf;m-rlxj0>?%@}`&Y>*P zdU6mH5;EDo78%SQl=+-e}dHB z(SVn8Lh|u9lsUf}eXf3l`5J_*GZw4Rhrhn==XX%xOJ%V?`Cc00s=sr=erG!c{rEKS zZ$ehoPQq_LUi@HAz2FBBXwcKAwcxl|q^a=FMSv}7F%_$O1 zo$~sN=QvIWV6nA9JJeU#v72A^;Oa)m^1!wq(EOIE5h<(yqU+#EcA4i!k)^zptjTX& zapCn#hUd*~BM+)#r@&tX{cKXxz5)v7R`!Mn?-XArLL`Mm@$j(N-z^J#Y>xh^+E{7E zqg0R#w4^n#$#m#2z>{AD`0MgpG(~?C8dg!jE)oJ@)VhAgXY>L`CX{Tjwse@05PNa} zKaxT@g{P;VWPHUKJr}n-X&6rwj%OE6v$gko3c+$o-i` zG98+NQq&)FEmI-QDH14Jm)t^cWEU&w$q}_BTQ7=}CQ^v{_1+DNuEQ#{DaWx#gtGed zq4SMi=@%D9^$TJ7ls`ZSjKA1H)UmP;OeIzjb}i%G;6e2W95=|QffA>lWlhu0`4gBs zsz@m&{1z?uV)(+EIHyjnlvFuS2Zh@AIJHWnlPD%QEu2uNux4zVw$vfN3OKZ#L()l1 zNT`@ADd8bZ!Y6^6JlMtntei#2xCHYGZtiUCVVJ(xz* z$_aBu4RA&qV!PKoLpwD$11mnM>E&07N*21E4SS^lQ{FP-SvKf;1CVM&uDOWD)kn7! zg0&ZT_61zTkm5xMaP+cUZ#nCDCo2 zzyl4;f(G#v$E&27Ihf6gXK%hy8LYhRHtX!#whp5wh0W3EVeX>DU;I+ z3X{w7oJ(sfY;76)eNaz(sBDU-iNrwhXvCx0{0WOQYBXyxe5%xD`w(q#1shFzC9i~G zz8Y=$biqO_=@W-#<{L+eFMp$+#NZ<`v5*po0B=qp1Y)ielQhaI7KV1SI>PE|7Flg{ z4vJo`G_{fPo(_X;5NyeRd;5Xs6>$D;JM}&Hh3~8Z@pz%iZ9rst^a$s*kWb>B`0cdU zus?k|`x_HFhxjRtV;PSz2ezb#9I|7d91>4chm-+7v=}_Za+o-poRZd=(+;Q%FAC!ZR(^&;u*)Z zHzFNgZJUpeYnpK!=Pzw_tb7@w0?>aysZT!3X&U~}6+%y1{-Mhns=mX^tNHKP>IOv@ zHnM#yF#2qMUQcE5*s6I?8k)Xh%Nn}AaFumNPi&RFn#;qP8Zid?UsS~q%NAc~>IOft;~1@)#Dh*6x7CSm28LupwMEvqmM z3u7Msh+;eW1^8Dxh<(q~=9yt-NDA>n7$1=WW3fOcY zn~AC~kU$`5Qz2l9<}2$rOi6Q{W~VsLaCEY19HY`)A##5nefQ($ooIG~67fPCox=rS zssMGc5c-NP()j`di9j2kLjz$l0;OD}mrAG?l0YMBTftKa<}XZPnU!SRO-eH#rY`+* zeZck+(w0T!2@ZZmj%d)}ET;&>?g|f#C6jE(!|ay?VRXj~#+8mA$u)@w#Zq>#p#>I& zlZzo8t1l=i0K+I2^ZCj z!CkOsb6z7|9y1EA5r=;>W^*3nUmhcJ`6sfhS)Ai7Se(^Tx5v~7-AP7;SV=}%uEb+S z)?#qO=b~|E>v6c?jX0cfg;|_=2knojd)K0If6c|)(4QA1WG93oaG9Gan_>Er5s~?91mHQBBEWR z4xd%5&1GA^JV9hotq#9lrp;wqyF6i}Vy6y&Z>wHA(Wuh_J!F5(8DW0x8MZy)1lg5x zh?tpjh~-W*N_3?jD`Kf0Yq3_NJB*Q)atPF!Lc%3Zqj9WRqYG}-@^AMo&b-^!$D9Gn z6HY+O6L+pv|0b!^i~_6Dj4sxw*BXZpPu%^Uo)8kxNZXL6AYS)L-QQD+I4 zQ=Z0UU$Z`mXZR|Wxg{^#eY8lARyf)1CpD6xc zg#4aEQ$`gq8_Q@d2D_N6p%7hVu&gJmBdP64LlRB8DnWl{<>}aD%DyF6g>Ji1u`VYw zu0XLFy-atwht>^46%Ignp;CGvm>XI(MO7r>Oqls!zbk;h9%n$QF&7||N+SmtKG;zV z#pVub*5Km8#B8YV^%`lAOI^<;7)04vgjII}>5SW|u@hT`rEpQ7X)3V&r3Zy!MUhJi zIXkAnv^z#u=8)j-T$QNzdl*Q9v@u3#Bszmeq?FuaL{>#g4m~ELMRjC3y}b4+^Y*e* zvGfG%Ufnx)hf^fCAbW>ZWT%qmiNeSvIO04t|3pjarYUD`Z@g4Du-297eIV)D`0mk? z_ZZ@b6T(@vhu(n?Ss_+k$?Z~bL*T?q8P(r2@ZwG58iMHQVH}wG4T3j-U?t1PZ&_zx z;c&Hgb~xuii%Kiw`FvXz#4P4=a_?i+>?x?6?cc&gDRawCB}2wZQaWYsv|ng_+4iW&DEM#!X~6#-!=m8F?)ptn zZmmA?F?8`pFwzYW#+%27U?=FoVl(7U7qRMwWy02XnUXV=H%2G+JIf8^o8S3yER~pH zl?(*UsbZu7i|A>mDhbma8{bqCi%N>6kMh0u$!e%}Sr>jT&K3T;y!e zz=e{Q{^AzR&B-QBQF7_hKYB0W7WJn$U|sAp7pxYYV1^Dy*VK;s-7V099`wj`6^|*> z1raA$#-ku*=*e!-hs>NjNJ|g3Gc4N$tX={A_7&~8eapKu`!?=XP{DS2lInqE!i60V z>I+-$6c>``g5f}1qZ5VY(Jom4@TG^K0)x>P&Q(D-nF`ot|17cUHjY=jUoL@IX5O*^ z*I)-cVi}?+qLBNduYxr*k3ur0EuIYO^OTQ~Iz1J5NRKa9klDEB>fIl4+l=JtDOWyF z>Z$T3EDq!>EMod<3kA+vp!2qf2Tod`quTIFV~8mjH3k%0mA9bk=JZ!B+OVRjt~faU z>5Q_`GT^QPp-}@axscO|8?8%b*oK1E9X5g54-(RlHCim)z#D!SuOpfejQ+W?8tS8G zYBS9=CX0sEuXl)We3!ht=N^eVo<|=GmFDV>!FP4f*cbES+Gq(up8Bd=k?$%~y8Uk8 zKQe0_ynLYr?z+&>0a}alM7I(qlTaw7^GTj_iw`UCjPVc{Pkd7By_c@Z}J~iAQ#>M%O{jnh++41x8j!z>(aNPP+2efHdX}KH~4l;@`pU z!gzl(-hdmeT_VU|B#4Vjn^>0HAYOZQZkzzP9}ytpw~f2^?(Gj~Lg)UFi>xavRux6@ z`wC-~llbZV8#f2& zv|?J?C=L6h0xQI|%Fz$tkn7{3)^PazHcRV~cS#dP$?;T(1NjzHF71;}eoWqC)+wX6 z93X8QtRb>8n-xcOu0?$#Cp9`K?~jL5YSZMQj_E-M3Y;QWl(s?vBWf*ch(M=Yk-K90 z!?=>(NU7r)JB6%ZCgrz1fQ;8afsC~@gp5^~K=$UwBV&pgN#>k7miVb{EX7Kg_7I6W z-+%U=`NubphoXnyi5s@9Y8kQhS_p?rKKwNcmgVb^k~RcUL8G(U4$}BsZW%Gd+I69l zy{J#qtcg(#ol?5+B&Y#aMC;`D#hp=!CwSmcoCS4T9#bidtXqk`5!$3F>th1n-_(Sb z5zLVsteG*d71?W+40n}m2KOZZnD)w4a6@0_zUFR*^s^VRNv&ZytLsQ2tBk*l2&)0L zkF_YW#f527jH;GxP{Qk$sH9p7tKjkyIWxB4&>wqZ;bv=kJ{(8YQ<4d!x&k3ra6aMb z0$_(4CLHHK@sv&tN0vxCzHpS7Srv}R;xHK~mJb=42&6;gN#<9Xb}AORErHcrBxp!{ z23nW?&70x`7YQ3hs>7KUHHsX&rT4-BG6yMO`xm4v%soQiB!+rvo zGoODf-vb(Dqa}{EuTL>hZoQv^c!}!f8cvKTs6Um&aUEC7t?o|`&n86a# z7G>Qq#q_6HB6xqzi|S7>M)3kN=uy@P?@_OtEK)xs3 zsMM|X>6cIqv0plOE8Y&bgejKQOZHcAUS49JGMhiYTC_>69_uGswuRuM(NP4&rh$Pb zE%ou)UmH5nhl9z(^N9KLy12BF@lRdJm!;lybPOP8dASMkhfVZ=_=#m7ZPk=6>ke<_ z{05G58BYtU>M#FN1?1b&NQed%`B)6Je5}4}rH0 z!>sSJ(F`MH|ne<7f!Ft9bkQvu)Y+8kv`58Kyi zxVtOZ;@8BrQLppe3|e%fTjRKUcy@O-SIg;Gm!MwUb_CJ#lSuO^K6GQ=As6avGKE#ekXmA)r!NMU*@4 zi3!gIgX6WFsF_s#Q+D?rC0@Vz>re{q-#pn$LSZz& z+}7=qESYK1*pr7 z6S>U$M|+k_3ObjC^!4hzqZymPOxNJX^Rn7X;d$lmbS_l*R2BUqHW_K&yqJRyWgk|B z1xtD(BrUq0@QrBxs1bl9+7MyvFCp53TjC{g-F;8gP?HQ*rSg9?1^<_({2$H1|IjG? z4^4pr)&J0x|DzEr`TuEXsQ(X5`Tx*RRsN$^7&zl()G05?bmAG5b zkv!{?tU_8P#vk@0OrnOS9o?mG%2{(GhJ&cZN{&e+fozlQsX(Y+2z{+JH_A2O zV6+}rONpeesHLjek^TFOR~t*|+%;L|ic#X(&`mge^UsL&Cu#xrwhvj2oyGPsHV$47 z&OH}0$S|_$}8pB=oqS%y6kjOkapVOQS%$iZabIS1wO#ewGwD6f&Ds+y< z?57HXa(69Va05yOZAc+%y(i5sWmO*nVGwS=Y6&y>z(rFI`M>R1?`D3t^` z7EmyT5V)Otvu!vN%1coG&A5$h8PA5iID*=?LFZY@X zreSx=z;wz$CRk(H!8Z7D+bT;As%$q4A9#iLOLXkK>{|m15Gp6o-2W95G1k(i?2@=PScr`_)=Y| zwuHMgLb9(B9777QpKp~r7|TzvYoXW!1z(i-UY`eFHMI=`=Qrnp=hiZ7 z?}D(;Z30@AHX0zywBXcDiAJGIlIEdQ&9Hh%sDHUwC2~#KC2*mM*4*2H;WowF2cDDK z=48&ZHENOg!p%`YXT_K|*;*cH7H~uB;7qa_xnPq{6KWm&7W-{KTdTLZEi6((k;d)e zJy+1N#lf`A(Oms;(Ez_ePhP1byRe56oO-#+JYA5)13}9%S2@hoYz;Zs*$1bSriV26 z9UeC0jL?lFA9XpKr)TH*Ysf>?b-dY}CYHwqut^A3R|~G0k$W~b06@8Dmd#;lLHFC7 zY>oN7j1Sr1k;?h9=(OQ{jc1gR(1O81$I@DI#YV`@p$_$;k#oBE;XPMHOXl1+LEY|W2rXovIc6Ji+Ldz!-npc@a18V5`g1Di z_%+6(^iM0SNpap>ikj3)yQPh&6cM49$3+0xpJf9h< zpV?XI1=!i7^^`BbrcJi7c^z~<+{PP)%9YJSQOtDk@+|%$X0Eaxbwzi0Y*~7B$1nON zr|PAiMwutFT(nmDjuPbxSR z5q#jjNQ)aM8RY~J?N2wK4Cr1{^5$;UmXhj28L@?)_-xO5S1qZglOcWUweHxxn1RTm zu7riE9?zp1t@L-+huIwh(I^Hc=$VJDDNR0`Qd)N7g@hro4K()*+*=OCTPZ!!22zM4 zg}^=o;rc)tf~_l@(>QJVLV(CpBykm^KA5Jh9Q;jJ1<#c*1}>DAF@y#`z88VeRuQoY z&NtB=bD*wD=DuJeD%0K@3%k1`QoN?RneU+wSYCkfluB_v9UY7CvswiPVRtJVpsel0 zT@nw@J=!~J5>i~m^u9i7R$Ue*4pN*pGYzXqH2K0!YLq7{_zP|TqBQmV>ReLatVqW^ zY@1MF#lwfF-y6Xf$Q-2Z$bG^3F_5gAUY+?b7lpIPU=!&|s>_EL5Y+`+I%-<;5t>KR zvc(=BRDEHdQH>Yr@Mv@z%gPXPb;Mc&$U!<^yrNl^Qzka_mTp~~4X~i+<}BT$6jFo< z=&d=nX<0tYU5)_H71aylzJwXSt-h$xu%;0CZ8+*wm7<(Fjjl7y*p&M{mvM__s>iI5 z-C7{2(*j+=-Ij8sC{~iEMy3xyEo|39eKlz+duL~d4i)IQcc3C-avpdB=X$}Rq8}PKu=187hS*g<3}KYy3U{;M7KKoA3)$@R)!K0y zYyr_Tn#ImqWUW$w7tO->I#2v_$w`3uwi{K?b>F9YwKev`PI{q)!IQnOD#>BUcHdW{ zDstpsF{YL9V+O0%??LsJK=OlAb(HY)6h)WtGp#vA!4E4r8}Z3t#6@)PUj?-+UgkBu z2u6L=X+smAvlP@Dk}q#qI&mewbe+rpaBH2bR%B#=^MGH^Xr7D1px6f0MO( zigv|q0Cck2-NhSMlkBZ0Swh^Cd?YwXDjvkqEra6WY>AZ6+oMx=Y=r`VUx+RNFAhVa z2)dz^H4H(c9W+5zZum(~7&9qEHy6RR?k(gU@V6GsCi+&9p=n}0)(k>ZbV8bDy!T^X z?{tW)G!&9}uLreCQSbz@m61snA&_$#?P>;_AK!$oQN)AfwSwsrgNN*WACjsg(Z!&) zJJlS6nrk`|=2jmOsi;RmdIq6uW+t==NK^kD=+g9_gR%(T>_HY`gG<`55AJ$+d~R-xD}|VHb6tR=6PvZ$C9y%0A=_4!vDt6J z@oGhGY*mY%S|UyMMkKhm4ur0O@L?`gHNn+-=vpn4V7>sdinCo3zLOGLimk*>up=yz zgwv*kT-uA-f&8fuoBp2l4E)J7LZi|7qoH5;Vnd3om0|&0$u?G z!7%pn*TBVtT3-=VHqGAlE~xysTf+TE3lxJ*~yN^K-} zmu*>!FYCV(yBSZJwN`?jwh(#++TXic$dT%RINUx9} z`#WFRs$FdYXDp2*EltUu;AwElSmz{r>hQ2@=ix|N2&i;$6GLgXxx#Ws6MSTwY0G{q zOH)q1FEifM@|`^-8dSLuu5HoM$~C$U?}w6V@hK3P5FHo29iky8%T5{%jbI{H)s}&U za&s;k?^zAn3B1Z?q6ZI6Z(E}2Er8TV+U!XkUa zirn3Sg}Ir9xg}a2nU*W|)G#>OW4GWV4s5k!`x^Z5KqIHA=M>X=_c;e)*CeO1ZS)NI zqZR|kqtV@iO0QUBbkB+NRadM1Z-}TRxRwJLa1rM5yJ7A;OPW$e@bN zCE28HL)9cDu*x!3&MXzo-djR9m6b{FLY?mKpr1&kHbx12PwVvO6;w!Lss-`(deg*l%}%0Wqh7@nCa9b$InJqWRnfc~s!|cpuVIgz zA+YAjBut~~6ux1{xqJdf{8nmsK8Ogng+fkYoi%D|fsBx5HHN%JX3D?xjG>UkD0IM_ z&q7!y`HQWpvcj65-+^d`2#EGLi%FzD#X#odlYwe;&I~N9DOjhHQ4Qxad7iSf-_`Be zLfd(dk+i}27--fxS*BPZsPSYYmp6ymxDGy(JjtZs3ms7%M6$ij=9*Cj>$$5W`Dewl zvhCE*`DwTizr-+XQQyo6)4K3KI4ytS(!^WvJn2D(VUjxJpbhcA9FcZAxF_HK4m}By z_s?~JQSnMOK{3HizOq#c(BPB9wNyL;953jUhlisFyYdpK(qKRA6Y5fyXD{o+LhDpy zv8IPomyh||TbMhAct@USqJA6y^Uz#8y0R-sOirBt*UA&+ITb<{yrfX_9<3_o#E9uq z4l&4R5WApAWSddk^p=gnE)4;2EX`+LY{f18rd1b$JA#8~FkqRs7EJv^yqxPKx`B?i z`5%iJHt@I5%DPwx3VWFDA$H?MB~L8Ni`) z+)K6f0?mHx80&HG7PiY>FWORrZ`Ii@@eTi|r)uzS#A?6nhSKdnpT&7*!}Wwve%NH& z4&@w?&HK1`9{R(|M|4|oh^tk4X_7dbJZ^QB8r-!@Xcv68<-;H_x@2tD8vceuu^A%^ zbv7zFwMsmnPbr&(IW9_xSe?8XRqT}EbuL^Pf)twBQmfJgOz-(W_MMmSqk0qJ;dDX_!WX2JF*9LcTM-3@sSBnM0@c)uKsQ`8 zy%quO8=Z#>$vIp%jkscGOx}*Dm6CmR587r{&0L-l?uS1Wme=cOyv(-|J)C1*_W7 zy3YVM$<^~?Y(NgJPEJ(I+=hB*hhvjso>q0tLFz)WQ-qpdl9_6JGtl^|LJjPTqsO{D zqeieDkR4o-X?dutz6C|O{8aq1a}%?9c47+Crklom(z1}%i;wjt7e_s&%qHY2lMZ4w z?Lj?TsUgDht3D_IU+Oy2aNz;lOE$;$Yh2MfKJ*I4zI z7j2;0HB(A)Vgk!MXe*WI8aX+oWasdtZX<5V_=sL}it!h98eKVIDSzqd&cmd9P0yW* zWv~xs<&gsAbs*&eWm76=%cJlzk(h$LT>EFjz%@5YMr*MxJG}$XQM!M(9WYyQ2Fqblo5&t zUPiwHv*xH?&d&_A^0YaG_Q-d5;@Y>ZGyjxyd)mS`94lGID9p1g(wZ9_r_80r?6 zUVlp(J}o)#a~bidYuvfIN{1aDdhQTflT))Ee4)uFju(^dTkPJvG;jKaAATG+v@Bt~ zwcgFu>jOT&Y|Ez@zuPG8ovq&NzC5UASQO)f8p{vAICDJNP{drTp_50&;!7uSCXg{= z0aD#EDVAU4R-lNlt@pcZr-DppIk*4pi3zFA6a6q!9$;Hkm55IGpuuVC}tY zs<|9T7+oIl&Lkn;I2pX*$Lf1M&UGcpS?b-;O*}DG4z17O67+(kl;2g+)F`X$^Jh`p zkiFb`sF{~MSbr`aXE^gfSP+)D-HQ69N`*Ht$wjy1{7)6B%mUPaF)1h+aBPBGYHB4v2} zwmNPIY^KgGzIzV`GcmfuYLm{K%N;wWr_50)Jj6oEnY7I4qz;K78ps4OK{-9if>~T- z%A;PwQ>4B+Db||l)N#zA+dvLv^$R1i%!V{PbbE+=KaaFjFmjDeD22EC6HqWiUFtK{ZJ zzlyqh^p`tlA(?X>TZ_BVH*tBLd#BQio;F?=a`HTAkA^)x&Vf)XOUM|F0^|$4=y4fURdFq8HjvzjjE}IZdU7{7eLSdv_ib>P}e;c}U@KZauj|Tf8 z`kM#DntK#L^NNmf<1@rjIBJ#2C7MMN&1VIgpF4M10RNu6X3rz@X&_WMFnr*Zd!D>v z3$OUD1!~P=9%ms;Yhw`7q~ld8 zDQ@iARYjgY8&H9IaR9ciTr8dxj~-a>ZV_IK+&Q`cCmuG5o9bD~qG*Wy#+wLXJQTa^ zn|Zs_1Vfg|L=WcN&@1+P58|Jp&q8j>Y1OD>I&<`@YnlvdCXUT5 zbJio~x13`wFAXXqWYa*xQI=bj+m2IGm1`s>Y(U^PI->x7LEfcAsAN?GaxSe5g- z(u(@aP$DrpJ+g}W;f?sh)Y?SVwblRggzBKGe(;;MC@`fm9!bv_gcOT-ZwLB#U=D={ z1H`mWcYV^R8Na}=EHypsew>q1C*=inW~dO6lpLE$eHI1w7fJsaX;!G_tml=?6Y6EG zF9i_$V-B?0Ee|F@c-QK<+SkMRgZt&|JiY7Xp`(5F=If4=ap~~mut9a+d%ocVyg%$l zqu^>A!CLFguSVDB#lwE@{A(BQ2@AxL`o4**AAu1+D}cH0W^EP3P{g20@wpemh4!&U zy|$+gPL8WbLr!w$On@g-#YcMaYl+JGudMtE1#D5i-X>EkB&7&d^qPAz+N`FL5ZKZH znvTbZtxD@FDe8!FvDS#$8Uzyu&<0r!5MaQbx@Mvac557qg{|98lyWZWEKgD4GM{N6 zZ@1P(97AO2s75LEX8hVPeQd?UGG&$WGQ$6)E#me1AZ(f89E$2#%=x!^coF3S?w~n8 zZo;JbGhbqmg4hFUB!Y@>Gho4J8XKhVk0i(o5?hj0fLLd)204xhgcIo|LFrt{`Gf2Mj5DECN%w ze|Iekh|J!!KytVSSf!Ig1IqR7!n#Zu)zDbEg}D>=&x-^lJya<)7kxG-z+zs`+et!W6_oYCo-R-<4J=Vf5R)h zYO$?M`YSAgc`iot73J-ty8({|t&{>z)VD>EpjS~f4caUDBXvTP)5#60jN~$*vs=`h5rPhkeySpfn#Gp3+ zt(d$&=9jtGPq2k9Ck&5%d$!IHZXui>o%oDODHH>bESD>myf^n5t`gu6 z;N41SpO8L&T9|I_wqBbI=&Xn}r+}vbF;k`_kGt_kYjfKxj93GDr9dsvP2e%JKBo?w z1;++53th2c=vY5^o^}VtT`voDI&j(jWd~oG|65P{xzgk>M}$HULG_d0G0Da3m>PTT zhOctKV}Pe-QZ7V^+QVV7Qq52*(KiZPc088&;kwBDS$cb~+jPk<2I9ISHts^#Tl4Oe(jqdv&NwsC=gL{O!*`4w;Y^ ztP^nLaQAX3`J^K@lI?{+92WEP+JWv|H{zcte|K}V=1HajZ}*Vyb20zI#mYbxf?q zcQGFmt6ZB^>6&OgrvIn+`_97>ZD8>gv2AZX%xWFnT?}*iJ4ve`RWIU2`G95^Z{s9{ z`BRON7OcG(I)FU~re`~7toy)&e6MBmZj6wTM2R4wI*LR+7;2GaP_SU3kLDLe{-!p> zOg#S(_?Z5YD*YZ%f{^@}FmBi7f9&$vtNW)aKm)OyZ3w?^Bgxs-f(=*kR-G%Zh#Ka6 zE9w%4>T2CqNDtR+tpHwzf$4#TmhY>764)TUrHha6O#u_Q@zUb&*I=N-THjvpSC_+A zB;19;ZBwXDLdCmpIu9A)>UgaDui<^UrU};`k)kk>gZdp*(RjQ;1WTxkNqm>qOD zwB0-c00rSxG1ve+TQGV4Ub`{9q-x zNBxf%<%F%*CsD8~}6zeZu)EXQHUQd#wT z57qk5_8*Uf=&V{$l5aw8aDPm{lMtj&0_;m5(^Bq%y=3m7R~Cua-SS0H6TSowWxjrz zV}F?4nVSWnBaa5={9~bqv~XcHA0gRiMCr#rP8-wcQe}1hDhbF#XnI02vHK2(&a^c_ zJ-^xvD7fT0DDetu=DE(~b|MmmGAPwxjB{&t+g< z09WXIYelW2q@uitX~L%u6|T!$*=6+kD_mj2`cq3^Vgx_yE2+QWWG36$r?ih5x#s7Z zET;MTTsI@Y4=HM6uh(%a4ffV$r&k z6Ru^kiir*D`gVz%SD9zYUkOmLSvw#*LDUfX-_Z?$IoV+2-pM zun4{JvcD0{T=SoI^NT`+;_iy0=k(df;fi0cl6K4Rzfo2N->lJhb4S^9l+*q{0Bk^$ zzp&g%7H0Pb^ti9j=ZiQ6!CnDMek-as_m29@h10eH{I0c?AjFDjSv5`&#)IKfc*{>M z+5W;KwpkoLHSb^u>{Gl8bM-2Flk6A0hCIGes&l!^1#6XPt3qN}1ob2bWQaW4fNlz) zam_%IXeM0ph6UX^qTwUtqb>JeJSV=#g%B~8rFarEf^w+XE4ucD4zs*wu&eY+BLt2M~$k8z5K<7cazMw=)6|zln z$b#{_QSxSBh8H}mTCBeO;}745UpwHF0b?QO(xCvzjODApBpunXT~e-`WtQha3JjGmzZzaRz=H2$KnU}^75=yhe5fRh`!N)X`f-Ur92XOAUCj3! z7eL%PxpZsjl}m8A1PM(iNGLc#LU8~SzfDk=L zDS}_uI6`jmX9oW#(nwa1m&xP2KBgz=H!|4%{>{YUtTSF=3X{HEx!dcpTX&|iPb_Ul*4K9$vzSOi^$cdjZ>Hw+MkKa6g| z5HJoS$BpxF1W<91qg35QW0Wmt3_0N#lY-n2BN)Xu1CYaLF=ffb0LRK%5>yq~4A2ERe5G)bgAOX;R#6ppT zv>tcl-?#(MK`M{}Bw&Pn5+R*{5UP%lkdcF=2hJcT^+NmIffGSH2f%JznUJqY>ndiEcpk;DewCB_WWgoOp->0f-u-YViXcs3WwQL5V-` zFZzMMAqPHhuo5Q#$@YQmjz4xHQXSAdB$gGFzz~oPP!kRi>j<=ukAr34P#xuAU%+o5 z@g_hO1|xh3+Qcuh;3z1^v}1f|9crBsHZBYaW3e*!M-Y*Gl1)mCaHtJE`Wr9~p`sff zQ~Cq5tA)QakhfoxI0-NQ7 z3c{v^PDtd$_sbAF#UJK?3ltGliu^|sk#L}5z&?rEMa>IZ2FQU*3AJtjtoFcH36P)X zYYo*Cu~>KRk2~AVJkRF8(RsiQQqz69nWx1fzjmHH?W8TfuSR0~OS3$mfdCS2r=C+Cz-!^o+23N5d8967XRnGQ}8D*%%YqAZi6u zZrzwpRRE|?EoC0>QW$PUOPjYt-q zMFi;MhY=d-0cs=Sgwt{a4vp0cqy!{pi1GpoEI^-S;9#YoNTEuYS==Bn$~a1kk8T5s zN<3tO1U4R&;{Zltgvxf1j|cE;gw8wA9U^QrD$!X6xHk@MweOMPMrQFST`~CzLClBo z$|ri9j7e`5_)!id0wNd?NKk}Q2!kSkzX2G8vR+PUSvZJ0EF83~V8kab8=Np2sLVgl zah>6icU+6BRypaCD2$@3mYyg}a;IZl06=EU9-u|=5$F#$<+ z#~_#Jn8uJbjESNIp`<#$B+ecPBB(7a9(p$jN&RLt&kXJs^eQXqQjdYo8_4&NE-T?+ z@jWte#AE_MybRQ=q&TUUSBvOLt+dhsaTkZAN>bwxXWS!)detzKEU_`;(JgHqHl6zB zC3V%!5>~AU%@2DFxODZ+*UA!-l#B9lm8Z99F34Y}#Vw;gUSVj#9TdUS3REqV>Uh!> z3T3rOUoXmc@HiQU4^VZFJ)&`L1ELN_Y%T*@fUXJPFEV{u;eDwamrP5rN&PTMVh!?RuHP!{Ms(W>9g&;BBg&Pl)_O%Y z_DDCGW6%5?Gh)1G65{~nr9qz*&tEX3k{EL9^UL+j~OYsXp29gsPI-~$|~ z(%kQX`%q-0JZy@HC>}~hr%9pHJS!aki;vo~+;8Gt^X2=m|KrE2)nT2%bo1`wGVng5 zcpr!n545KHq&)0XMEY-w^m_X8EX-`&&VRc)WLMP9Z6Ao3VIn*FRDW%hYYV+;ej4#1U-8rz|jnW6FYE*li3q;2CnCP zju_zY&)FwIRxqDgN+gIre_Gee(%#c-i%kb!;vu37zNiM{X*|GrwCEkUwR^yHmS5>C zL6wz)iX&d7toj7aM_0GFlTrH^72KQe$OgXe0WmIF{vt~O=%1pqih09Fq|&j85`Ihi3N)3Hw~Yjgc|Qmkg%q?(L<@th76fHaSodSSvka^}ez72C;U z2g-eDLuZIv(ZJIo*rgErC=PL%xy04xID`otU9G^C3Yr9yKmxIHv4~5`U=nV(0R)tx zMQ{}k|13!!^drv0Lb8FtKG<(ENqp0#D_;_}KW0Co?|6MiKk{-PSXT~kLtzO1O$4qF z@D|*daSlEU&f3EfS?6?vFvx=#{EuL8Ga$Qs7j#k5wcQYDbO}HmM@Jn)NCmO8IRq~dx!Ze$KavH|T!WXn20g2p%nAXET_ zc(x`QmOeRV^M)P8h+?w)PYxXgbTgb(?vZ&$NbS{#Z=*XUoD--R$BjAsYsE2(mx zeCtg>&cX+Mk7X|;Opf$Mnj>R(lsFP5O4#Ewa@KzPN=^V|7{S;N6coaAex|9Dmz3(F zX1n3^qOgWgw-+x)T#*?DkDPR2;2))u1W~By65(QEK>Jfe?pccntwv@ju$XT)5B&Yl)tt1Qo*v#+i?3W}_XrxY3ZHB(9qx5hP-Va%I^j)QIZ;+g`(bRgiCo$@q|b3p5g)QijIVMEvU{<fS*ur@MlW+X$ z7xVmBK>z)g(yJ|KS9t^{{r1^B+s~m=EJOjx-B|c?d8D|0JZ0$i>4JUodoBnG7$=8) z{^a*d|EaMGG#dTYcAZvaVaQ#QT9gYhQO548*sC(ZIfdBWBGSt39*5aI-pfCg242R1 zuDY&6%g}ozpbJ0~xbW+a{^Ek}d7ssiPX2;|>0g&i@WY&aT7aPe<*;1-^?)l+AlyG} z5BqugWxiV_;28nZ5Psd^FBrhQB;xpUawv){*?g5B_7$cX{86FeXrUcnr?(k#R9{>~ z6p>gQ(S4Ptn}tBZpZCdbmCX@TAmCkZ7pMoxHED{76vQvW4iMVA^$ausvgJQO9KM;O zx|e)N^rc1@jx0rpbc1&Q7HKiR#|1P9CpxW z&%omX7S|nKL?}FE2o=L0&>8Yu%H9b4a5Z#=awvAugFha^KcSy4`cA*aU~qs z0FEiyPa{8n;SK&hiQ(zh!~-P`6<3hfz#mp)AP{st_WNs`cud&hX(b}KN?D}T?X27R zQ)?@opS;F~VzWHfP*-fJ)=-Z%QEun|SS7z@mR81Rwh=WrVhLi(Ebl@Rmdl^Olmm#$ zjGx8MB{J76w-vIgcBi6Gn= z(*d}201=gN{EQ&I&wu(EL$~h1twwm`l7=6UMGvbOk>#twuZ{1#;Pt&K>y3$xKS|6nwd*HSyB>v^XEBGgrH3r~ zb_9GO>@l$hGHKqov;-88cvImqaph;XjVlD=17Zs_q$i7vNn$*d7$z{9y!c#Rn#84* zF+Y89jHw02ZIBo|)LV&+Ymyn?5pbzXTe4oN)Y8Jkf08O{QzEDGu8no)b1sVdxDFq~ z44Lr6q-1=DCOh8}=pKXq5&&Zqq}mx0v#qMAO%r`E%%Sa|9BR`r&bI`*#~7;c#&>8g z&3t21N3GiELz5dpYz(G}q;jD#%8butrb%Sl81vHy#TZFw+I7sgZj;=2Y-E}RM^;Fc zKBCcYM19nvj!tA=3-8WnT`=l<R`tmv0f+>0s(N?A6g)S{C< z5Dw8cFg{rnj%U(1-;$C&T2NvS&0r{Pim6R6eOLk{%a6rYiJ44#G~w~N@HELz3wM6{ zaJVB&kJ~6disJd!wFr>UhO0$_Bn{2lkj{t};iq1{~da?@n0wUa-rCfmB% zb0i(`t!wV+y5+b6{R5uos75&wRt5K}wvljCo9;GM#LRKS`SptnKZ@Yv?#0D;aNz(S z#{C#;qtI-fODuy$Lb`f}TdY+eSV#t418mlpxj>_yMiL$ep+t>5{MsY(4s^wI<#rUy zbVV1JW~#i?$Qm-D28^Vjm7u|Z5H$2;f}kb7XQt>xlE#drh>_IJqY+)Nj7=m-G}Ux{ zT}9@#Y!5j?Kwl6GPr=Phl3VSr_6OFjfxznpxWhB-`9rop;QROy;gY9MlbTk(?PwWZ zw?gbzn9oKWkW*EhUa}~=0%W&{6|Y>`b`*bq| zhU(?BYL#Bi4N!(%Ejys&+YeymxIbxy$Adci*-vJf0qy%$vbQM6AO>CclF7?p zX~{Cwm!N>sL%hyVp8$ zdZ+WJpDl;#XM6dWxArVmbpV;97gyvUNO55-MXt5;v)=Xb=To`T!jsSGv8;+2b&L41 zaq-|Yq1Pcl>$F*06#xFa{3`A9pWJh6sVjP_`0o!7Ib4u$I*5zJC zv92p*2j~|h9X4?&tR)b$1${ zf3~yv?%`5*_aRH&U8!@ULF(?_h15wkzNzVB?=U=AD%<|UCLgk#C%T+}*&yf1J5ai2@ti%rc+T3y^Q-qDo-?gKzG@K9*~7(i_92VsO!o=C zY7o!adlS$3$K&cR$)nlBrqa{lPffi9uK!}QjR?uLun z=KIViTr9r)>E}rmVotgTEyUV0$)k=}C}6@T->V+k?o~?@)4QwNG~eQ4t$d;Sr=NNH z!|zkC*U#5gKuo>gSbtNlld)aVRB4Jw>y2s;sq$I7c8;1+V=B^|1}l6^JsEfD$v2jc z{N)|@V-;_O#G2Ta+x^vcxzwT~aNWM!XX|8tCr6hg4I*WWzb=f6%-QM|8!6Z9o%Teu0D+ZJr%Ly9p$DOSTwz~><)mr})HCBuIF zF(IQ=!dE|u>hk!=-=94=jk;h5U*I~#)(U&g9{BlSbF%kj{A6f97$hpCWjYuTA zU_>Y*gaM^MXc*E6t3GWMv%8_61`lWPkJkZyj6sXtRbMR?kl>`7 zRN5O z;$YBS*ed7GRC|0UPpGWIZk`KBvmUX5dV}bLxK$}ZT@{O~U2-VWMpb3jQGZL5yv)|= z%QYyd-+gXTLs2I+6uqU`a5Fp$Nl$QLh-r_8G;Iv-JZcZ_9JGXiA2q(YI(WB~I48cj zs;(KR4wEcA7%OJ|VCnhK4^w)s$pJnpJ`pHtZy4nxv@j7qwnHASqA$#W&Zoa~x5{@J z08y@xgfS>FO62I4#FeF;LQ_fx@<<($mgfFIpKWjG4x(~MxhIxxAh#1w?Si1S3ouve z1lQCHuU1*f2Jtu=LN8$GLVP15jBmg|QYOYF9ToVV(I9AkQU(FAXGma04Bc7M2L;Kq zrtm*LH#0`dOP+HYqUH1lB#=8DhVc*`1X^EAh9m}FB$;LXB|+3B=@)#^e433)F@UJi@+{37R#Q4Bi?QXxFrRP!1i<;^E9&;_Hi_|HWyz*e0 zgEh?6r2aF6_LP}xj_}zquag&weUGgatCFirt9jYJT0XYgxgScH)?N3P`|TQL&@j7O zq))upT-Vab)(Rj%H%Abs*VKU|;lod|09aVo%3R|P03w==aC zDp8^aT~wp=Wrc{o+>N86q*^nO)EiAHIZV!r-%>65udf09k5jOGFCq)gK~5eFvb<24 zlTK}uEcPO?-y~eQ8#qxoMESx+{TQD)Bkz9j=ibRK0+%oe9v8ghSe3_d4Ngd)56MVU z*{|giAff?2s1W1jW<*V-({m~eWrdbQ`U2p4@(sl)%!e}3`Rda|=hH-o3-b>j+C=B; zPZOPgiHXh+ADEdRf12ogn&^BC6P=fzCOZEb6P*uD1Ah5wqVs8@^R|4$6DB(H;mU4j z{i?@z!p|OmVEpmp92n$zLVq;4@yX%4AfOL8kOwmHtwbNR9X{^>#&THFrxFQF=}Lc8 zAZ}I46>+gY*prJRbVgVyQi$N9sDw|R&Eabt{8`0-I4JaGc03Zt-VcrG1CKFzp)sa( z`axNJIi&Whs1Xsl+l=ar~{C3>Xw5Tq4gR1l_P>jUta(h@N_ zj6^+PaAHQ0L*Bo|M?^!+A_qkAWl>U+j5?4me-M#+24g(*L~5n904eY}AfNcgZixY* z;1h@Ah=@F>31}EOr|OsFXe~7M&39oGIYWG97aeds!W)J4OWcu1%~C)o7{n*P!Q)gz zYG+)munaLAaZpAiqYO#&h^Zncf*FUM5XPzxLj-h0K7cmf1P;z%$muI&3n3B@`#KS^ zpEz2{o8IgQF#_U2U!Zj0jKmus zAeDSS=#;yf>d~(6lGpgxZJJk?SKDq2)<@qTK-Ee20|!m_0|{T($!ppQ`4W?y*ZmTc zN?qqhTX=Tvbwoq;+8M}qAtirG(}jb<=ss#8;Su!=GQzVyJ`s^_tiES4`5u2dxW0qVbimT@-QSNmj#*SqkyfQ=in+jL)M;FqzR zPwQ;4Ai@6?f%y^H=g>3XwX`t57Q2G*m*0-c=Hd->c{xYDBO&raj*art4h_CjS89)ir>^ z`-+(Gqd@D&5jjENt}T3m0P-=#P(ZTP2nwQG@S~9b-lHbi?%h!B=we$`VW5AIpDu{o zZo4V6TQuDz3OHlrN+9ts$r?-xdGA0>{_Bp!BGhwK=i%~yV*Cl`|2jXy67_14wj<0fA_= zRkDiMY>UePBXCB0K9#eWNGRA)NejtIM(giBHDsT*mXa8Gz zhtce%VU*y`(*H>B{=TcODUuQOkxY#KZIND2U$#w-PV6+NRrCA+&aPSWF!%UFQ9Ka{ zbu=92@z95H;NM`(6AXM(;>aH5{tz9s;Skc@p+myAkkw;>7@u8y7(x<^qqH zFAM-74Brs*Ngx%(TVkvff)sHgLZ}An8qvB?sR0c`tP9g{KiKK!95$PaI$jq2lb@Io z`yIRRYaYl=94{>KM`~x8BvruyITST~2y~TfvUPe<%TsxEn-v*aWID@q!NV(3NAUCb zq-c&EGr?dfmo|}Q@fAgZ1Ng%R=p3( zwwuKtUzhs>Sy|}PHQWvI-+~+De{zF9xj~=YAg-|ehr2=kCpV~*8{~g-gFb#Y$nWF^ z`R~!-lD(X6c2Rs&WOA7q>wv|!w$-aKFub=Ge z1se?ywz2i7`ebKY?W|n|eX_M5i>+mS*lceZ(7Up^&)EqDF1?4mIvbc3^*G8iJ=x8m zh4Oo1;>o0|?V*H`)pDALpJ__jjzJ-!xZWqbRW`?%yUFG@DI`eb4FM43JoKs?L{ze$ z(}K#tlTK5CgVS2)jH-kgu@B^r}hKe)!*+VTtv<*y;m z?y17rt>F=EAbnj;JsiXjY|YG%tz-F-xqNksNJAlrMaOZq&R(aBE6%>Fzkk!cWzENg zc{i#)vvN;Q>^%B1JzygCUp_ia4y+n{3wFkCUS_vx)eQ0Q8Q8chB^!^;UVgD9T&&%T zOYe*4G)>L(FE0H$?>Qx}(*ub@KMJXY{%nIUbr>?Z?5W0^G4yJ_UOZvOQEeDLgJXYd zOY0jw#P(^GxrW-Z&2oDoZI0iM9jADLo z-9vZkAGv@0_s~E7Tk9YH-S*Gf{QncoFC1{!@s{pqPgxf*6<#@(OPquN^gJo&E8Qh* z?W)$L6RhByO;!@-efUcy1HM!edp8kQgfR=q9eTW{k823GFwggLHyDw4AFD7J*#YTt zN`wm-W0+?|Il1QGhU5}4UV6Z{zDE3=uZXNRkVkQ#c9%Tc6$Kc0$sG(FtP~&Mg6x5R zgTQ#}0xx91goj*VhM5&xehCK{^=1&}euQBG2KgA@+Zx?q{D6@Yl6WGcGR7bWf%nFt ztp;jYjGutK{1uZgyybxxr#-w0G9ZD1R)HVo7_=|Ml^i4}0^(u}qXK^eup4`CFy`c_ z!;(|{@Fl1CmBoRY}DsCw*O0+D8jH$ChEg9QOYVrvU$kV_D}HF6AuS{Wwaj87$uv( z;`8etg(>>9v-~fzv;1zX44(**2bJs1VKyQaYtwNelx+S>85v~LCmZu#ZH%yMcU}mQ zw-CC>w4V9F1u1F^NPuTa#LxrvGIXA9uFKU`6@Q>s4vkQB!y!-BxCcUWE4>r{%fvG} zQEQ(P5bNJRnow7&6dA+3n^i zvD6*G%;|~+qT*9fVz@EqH7#{Vmk*X8p>C1A!Cm(e`|}k;K0=3&;`@F4)l2W|KVDou zgH8b~Ew98IN2&WhA!l8T=M_{8lK5Y3lG`g|z%#!Nc;*WS7$~e`)PT(g3Rn^8 zNLEe9VTYJu6kV;0>(&z*f9KH6M>N!0IM`w$HUs}6#m+tZ89OFnu&SzbCZ00TWadw^H5s)h?WQDAPaDAZi z&s-YB#}dE}hPt$ktf&k43GN@Om049|->Sz}u;cdW5@dH}v=l6ln4>3}a(XJC3?QKTiG#BVVjx$=qMf!H7J3{E zsQ>uabLu1oQXB&&S4uHX6Fqq58oxvfewJuEfZxpb9$1ePRj{hCM zWd8y?DUQ)G)1&YC0V$7FyA%}AEu(%63{5&=K7zZMr%+B#Gn@D(5p$brhEnZ0pMk5? zbWZaX?oUh)w^~M}$sc?)xf`5{$io&Ixk#Rv7vox&ym}ga5`~dnDo2jjD^d+$5D8d@ z5f+v>ST0SF>fUDTY?OVzox9_@T`uORlAxu#lzU8dRb&HntCbYK!|c5rEoQ{|#@jDFhO<XEka5xS8Nfd-ivM*RU zegw$3r}zXo;q{r}&hQu;O&u<}d4T={JvcwdS)4D%iuc2n8=eJo_g;s{+XO-}b_)h67f}rJ-z;0h z?2>`jcFIqqb*5;1rw%Xl=z!T)vQQ4R`Ib5=m)zh?4o|Nk1fx8?Y(J_WD)`ej8_Mw7 zJ%e^az8m=d<$2kSTevrmsk|l{O3xkNf($VBQEUK_+_o49$cLVX@VgOg z{_T6)Z449J-u-A(j6sbB>56!;F0UEqH+cc3b4U-zq)51P)!w&1Huo{WEWR|8eh9uQ zv*>Bq6uRNxXCeDoOf?&YQ6INSqocj6+4~seaEAhX?d(Q2g3;NIQ0HWy)IBIHVYhwN zrr-#uufn>HI4rd!RkhZV$#Sy``P_|;7Vw>p+v50fqaRFKn7S$2O zi?A482f*ldZ0sBmr9;6JccTsqj@l)?&<;~-D%WbFu23m8iIk_3VQMG)7KP|TRZ?vA zit5!ATGm2^YbW%PPt0M%+_`d{uN57-LZaNRm}ubbF5BI07QmW%|5cM)O1ljW(bgj1c$>aW?spWIh|bi7!pZ(Zb>7# z)Y#aKoYCb_z#J;2{qcRrH*VswEPjPZFfoqXsnIthv%3gNrZb8KxbJ4%5mrKFG zW^c>>`Q)OK>%D9<#~RMrFq%ZbptE|5kV$V8K0fIYLrWElhjiI4+kR9f{qj}GQu5NQ zE0Wa2}IVR9@5S z7u*-CcBD(AtUuYh=^Z|tPlF)3oVci)pg6F^Mo&j6%!G@xnY>FDfjZ+!kX(Ni(f}%v zf)k}~{7HssaPV}dq!7!HSgL&RvSOUk7zS5v7y|qal5pXc*K$)IcQy2hGhoRD5(Ca9 zJjj>@ur6rQn zM;`~+;zj5MOcplbx?~Hb+xgM;W<_DQa$9ulM2?~*$n)rsrOW?q6^ zdbMEnh$>_t!&q(Ku6+LuSH4nuScObu7hf2Pu_-S;c*1IY?dXJ83~U?ucIhh7Ozsl= zELDvL2PScr-vBhy9$GrB;CD@}5v!Ou?&9$I6Y z0fpr+QFjeEGc;V@$GkmmbZu+WeJPUeQJ3)BN?os{+ZBCZ?Qy-g6P(#SY;~6*>(#Es z@rs}+$l5K=+Aqwiji|*@EWd(-D0>XVRJ`;k;Qy*m;JF(`uz%(?ZfC~GM?H-V1HQy1dFb&o3_pe?&dv=P%V`FRME21B5c4~1I@h%)& zI~-ApPHV<9prm8hjyVQ)++LE#d7Gbmcy{Aax=T9e79Gi$1NmNurC;L5JuHTNm<$&) zIFaDec+JFaOFjUx+Fu=&2WH&b+b3BMal<4LA`|@%lSQFG_hXmD&-(yF_oL0O0BFI# zN6&c3Bw1)IEPlj!xF~xG8!p)vBwLHPvDAiZN;^*G>D%r0N8r_%oDa?NNpQ6Tle0r+ z_B(LFfZ?>;;ENY-t-k z+dx|cFTIcAD>F*r@I4vO9mRQV^45c5GStw#SbcYadswtam}>6H4hfsKJz;~75Hv+9fTh;8&W(L zWK;QT0k0(O^Rvao8xMbcbMjN%5(3}Y;6)|pHIkQkmZyFeSam?LCEEiHh_|mC5PyI! zc;}E})jXV-DE95^%LzEirOzAcxAV8L|>cq-F9u8gGr8sAuuZbDg=XT|1MAI${_Vo2FwEWcyZT zj$UQo=uAA9ED{hBH5g*h=(K~1uW6IHb`vZ@?b$Cd^C3DGj6pKwNVLBYRaHo&$WAa6lYz$wRai^uYMj;Piy34FGZ}+f zJ9oxkd4)$FYST+kyZB9zHa13%KXN@JX9s7G)}zVPo%fl85q{DY?}JD6QC+fpk{P}B z4q9Br?moKs^#E)hMgb!3uDMTlRd1);MlNp$pWQmP23$-V`H}7(yOB^nu}G6xWl|ehl9W*6)l%)<8L_uywvfrwt5=xYRiR_4MJ@`Q) z3VGu<+VlBcnafO*gBbvXPcaILAhHUGfyotbe+}n%x_Q-W}tlZy4c@U+o5(|ytL@u&N_XPei zJ~D?>?E#0;zMo;FI!MUbgPunUyA=f;Oc{tCQ=()aqu|`-p3}!}Md8}V<|*0rvc39B znr_w6z+P{PH+a<%t2~`l`Stkj5yQypL-xO&>Hf>v?e99BUFU39Iw8?lElL?NFo6)X zOOi!N;3$b9R}T-qJ`BZi~E_Y9bij}#W7O>eY&3?#wOQ(I_-5|Kq^6K^Ke7WxWC zKAzhT?!ghy1k6|pCuv{YAeb?s#ioUM9A?64n#*l=f1$^t}? z-tBy3Yo_f*9fDK@tYGiVIENb76eBOk+Fl2kgKnFNW9h+@-5Z)aT?&o?(@2jvJG;;f zv#&|BD$STz;a4Cg?QZhRQ!}|eHS^aeaeJbgTujcnB#B8Xc<7{fYG&L<->pwmM4D-p<`{iOWL9IShk+lJ^Uf3~T`% z-0yYmPIsIP=rIRfJb?Ol&a?3Dhf>ln^XpUdv7cQQe!_}r!8^;SJ_+*FG?$q^3D}(H zPXbD>$NBSRdvMPG1@`kd$=pl$nA+Pdo>p&sQH_Ov&HY(5r(fYEItOd;Ap>U}33MJ& zbD8PQaU8niNi@Gn+lHTOen@h+4Fb5K$9XQ%$=$;Ss z%P5-7ds|xf#5O+L9}4Z|>F_24`W zJi_iqXVCo|rfG&F$b%VY6I2sep9QLgK@VO49HHh-fPkkvK^`>u%yo%gzbAknbDFYz zkJ|INg!ZH3oqNnI1bT7z?0jOMK%yifp=F2u$^ z$LSp-B;5R!)D+ND*CKRbPf$@)n?zW2K0(iRv}{o1+#N;UjW#FP#PV|9S(`1wSr9KM z^IrNN@&Kly2a4=834+^%e+`_Zf(D7KdZ%>U(0_)AF7wub-f9s)ohxVJk;=~z*&ArD z0>JC}(tLGJapdUL5NAlJsmZJtB>PeXg^J6sXegtzR;Vr#muLQXrPg!0N=vIg2~{Sw zGg-7jQz$u|p1ydZ&~ZAQT=%8*D_En_S*M1B^RUq8BAbU#=0gr2Y%$aL(!+ zmLclVPVaL?UAFIgZKkf9jU;o2$A`45Q^?Odfk~l(o{2k>4yI8@dnb0I&-eB)tH+bh zW=6T+l!U)23IF{{!g+_DbODOG;@lw`JMiZe1cLMvv|dEJLU7FkD6CJx-g?~7Sy zHo+9&;ARdSR{aw&>(^M9=1b-FxS*Qs4mt19U*&6G>Zi%#1qjvR=?yv5e)nG2rVB5c z%t&s`f60LW=E$cWKEihzY3)tbO zo{K*p-@F(%Hc}957%eKF^Iwda+BpkLmOCkAm$VafvG8Z4`2pFW!=ZMa0x$6v1F%g4 z;Ji^ZRsmgVP%?}a@dLH;lZjb_UR{*TCJWq0p-*JJjG=xKwQBXUDU{x)mV5!~4&yT%G-tx{dB z_AYBkJP2@e#)w%r5)>Q=^ze$7kXpnHBg>DpSZ7>G<1#VMICd9nm8afJu?E{CtRAY6 ztGg)+^u|0rbJy27YQGwV&qf*M5T>sQ1Z+m3+N6Em*-oicZ0A%ex@bZFE#P66 zsZqy=$;g!3NpM0Wmsxit0xvNtV&cynj>0%3iNv54UOvTbQ*_NPqdl8lu&Uj8y@7+G z%A42Eu%VQ<(oZTMUyn*Jo${&4uiceiy;4yjQmjF2*H-DS{_nl57OpS z#Jh5UCS~X>m4X^OFjCfuZ`JJSxjP%P84=A z#!i>)i!J8157;E~!vNJ;?Y0K&+IAQIjo{yAXCHk#chMCl`Z(Q_!lZFGhr%BMS!r4A zkIWf@*m76hpzcPqQm_zA{B;P z@}TIB2W=vve#9KOKmfB#*7nF{<44zP@)8~z&h4;xRM}YIeamaAnfdQjeMC^?&5POz~P*vR%bPv%m+);R&d0>8%@sP7Ca=<0B1P~zS3ESyRT5camsh}8i-du^ zU^1Hj<_O7kLe;@ZWICtTn z2!)S(p1n(^`@4|d+(kVWbQgesfWz6v2t>PkiEuHFGmSs?kO_g)9P_c!Cbk>FKfBZ@ zwuud*TY$*kV|^hj1TgGkUzoOwIlC32!m^iWN$(?dAJXMD&iGK39`7$FbB!Si{P8?k zAa91Q4AJ&O#F2E#hU3hcPez(>h1rsCgZQ8PQJg~7;m$F01XNX)&f2YP<3Z2`!OamFAt<}}pRP&>7tMn_%a^ogQ8-x~GS;FPZS zuny-Fe=@(6Mti27qoZ?*@(EXVlD^<-(b~-;{P)b&n07~?F*%}!MmPbs4v7o+hdfn^ zh14d#h9TuNa@~3k7j(_>Z}8Ti0wV*$Z_ww|xzT)=nF(>P?F>-@#3o3j*k+XEyVpsF zXeh)9mgZbQ*DaNp2ayN-q#4$+8~T35Vk_m<^|{zYx~ea?^i-tO@(!g19?AQBzae)A1wvL7+T)98H*90XR++|!4KyI~IJHPvLnHevip?mHEzco_5HYS<#yAdU+p}&AMy-Iq! z9|9#cSd%_!Okr^j6YhC>p1G1Hqy~$XRfOcN3tbk4rX)@l?trwneteBQ3;tBQppSDo z1B8TT=mnz>!?!OKuHv6k0kp3EkZ>Laj^owDN--2I39bBKLz+<|T8U5aY2f2gBx`e* zjt3im^5dH!F6xk5^ZY%_1yCr4c6u3HYmlV{iwGLH>mohK0MIgaID#W3bi@?Kn;PeJ zK8%8k3%HfC*UZL7u7Wb40S%p%VuL6feC%Cuxy-HPonW3U?H63@U;pSmKf^r%b z%+g*}{5fp9J!Z)2c&(Vr=iemsdN9e2+Bsf42H@zN>G$fVSOAG?bbok?3&Di`S1nO z>?_9`(lCbtJ2HwgN@RHj_f{g5ubC#E_GU5_q`z07@+ZwY%`rnq?b4aY-g+Jtd-Bn) zyF&5BUrf)?24Fhzfy3mS=Kj83W010D&M_uX@RT`*L@BE#ty+zVM28^C?a@4ktM-|L z$K@y(ijS@>3;!p%CN@CVoe4>a>7H7Pf2m9CQ==JwMgZxhE&cnNag&m zBh}f1Al2iq7OBowL#nf7km~Gz9jQhSf>g&}EmDnEL#oj-NHzLjKq}SU4_*2Fox&!% z*vkF{m$(n-s}*00Q!Wc@-;C2eta3fZ7VdC8ev<(3b>EQlpLSJ`83kKhg6r))n;T+bkB*U!r%APsAz2hf;z=%2Gok^#W`sg{Icn%uG4L#Sth%T2pcYdNOe-|gn zPZ`xYlNnU4Lnc$FyPDwa>EC{HD8UzZxyyrF=hEPQld^h4|12ha z^QF+`XIeb;03Fw_z#C@BtB%T02Rs0boqr<2y{$j854Knut_Zci95+_~%KuMM_G$#D{Gd}Z}oismp`^H zu@UJK#azli=w^)jslOpbmEujwu6E1rN^EvD^#`m9I1Nw$Q?S&gJaPWiShc_FwL0dT zR4Natfz?xYNL49Qbx2Rz{}z``gBKk0QV8=5?JNFH?S@Kx5yPsFE}}*iw!{bVb-%&-8a1HBE&MHbWYxeU#c1oJl%jGKcBy9fzmqC`ZCa4z zooNZaTqx!ED!lXy3uFo=yWj)y7l^dP~b9Xom#_aww7uV)HLMJz@fbZWdTf^*XIne5+ zg=wR)`!g`iw7Z0vp&FtGHa1ilp?#GmhZ3NxSR59a_7xOKTOSIm^StRHz&Q8aUB z)53b@fQjcsjogy1?1nT(b?*0(k7H%$kGwv4e_YDZXh={rg z^9gK3^yJ4k?Pty<(CkZh5@DI^`EVXA;KVe^uv>hW&#{Cvzwt+C=ZE~;!k5~I zw0SW4;6}q)5Jm$_N|?;BS9d-eUIz1so%qbv^Dpk%+ZVd|^wsdkm#1%jK00}dzfa#B z4KYHh`2FhW^z@gPujB6*uZAZtpFEX5|9$k++xYwWFX``-ef@ugPlm6L-aH-HcAN6wzWH(Z=C@avu#X9Ej$S{1`eyj-mqB;O#)KEA!zV9Z zKRtdkJbg0Q+qN+6)$120N3VZlDOBq8>G8{-pTud@c3f?E{NvN(e?fA`Jbn7+&5NI( zpCVug{lHF=Xo#5BHYeiY1kIlZP1^*Vir9x`N|w=sHH((R4Z|QDPT|~f@G_)Ji5S*< zVD;lobLOIpDKlcI@At2Q$yl@21_Mq2juzudaEu%E&*;z}peq?gPA^*xkmSQ8l8ZEq zboG3RTqBN@xxU^=RhRROKHKVkYed1cUd~4o56DCiHVo5?lb<^izhU&l7AIE?4|3xV zzcq$aZsHT-V+Q%gVei+r8Me+t_u~TCpc|eRK^mQk&Eo3?MrpCs76#XI zHylo=Bra1l;I4{;u(g1!qYG%KKfj(tqf0C^f+IzL@l@{tS#H{Eg9ymt*!}b6c|(5_ zPR3ApOE-+rjTWK5X7$ftX+QL-($n#UtExun>MF;Va0V+Y@f>xvXA@wG;TET;C$iKP zPn^-q(_dBfAV+(7s{NX;$fW^2o@{@KfAFnOs*)$F*R@E@2<;OKO02d2k6Ver8u+V!vKQ~k20kg2W zP}V4|uL&s%aYJSaEF{HP6&4g`)`x?Cz_lTrz=ajY7b0)hb67Q?T^m*F|2tM z-h7VF`GNX8KSTlwXEU#)U(b76Tl!`^j+ooS^L$g^x}NLiI~^8&G4TWaz0qr=(1SID zY^uH~a|N4vPfkF-!7$ZbS5!;&ZhQggIFaCB(3u7aU*eHt90%ZJ-;lV)2}9@RY;g`3 zSUq8aV8ftu#bm=s4a#Vbl<-Tc8d1XHdQT)q9kvQ*uWoOp<3U<8v*UI$?bC3I+rOS` zR6@rCB~?)z^>S54Yyn|_*f6@RO?>DdZt}soc7#*&n5M?~d=8UH$3LC0Z{e8OqkKWn z%@A0I@W5{|EzN$oxoJe3gVPAzthDeRbA0K9$G}=N{CDAd!?fWP$6xE~c8C4Cy=|N@ z&n!dc$YjW(0W=7&HwSI7C!=yj#$esT(i7p#@@Iiai(~aBPeAcf8 z>CavlWVoZtp!d|$o?!?KC?QWg2Ot`VJ!td=)L&e0?($nh1Ez#eSPN+C1h5=B!|87y z3_Lz}KmqmRx36D6{rS!C#S?un@Zcr@iGmOC-r4W#ash_S`>n6*uYhl?(;^mUx6e$2 z5?}cQ)}{rwPoVNmdoW0GpwM`lBLxH(x;-)j#MFGS1TsOk+cf}W{pk43i+?|r2Ol#m z2qd=Xqf7UHg5O3(ijrHPeF$!Qz{`hQ)gLg`F^= z-!XG=%EE~>a6BicV$u!cGxdYz`>BSZOhL0>;D0eimnZCVk~|g@g0Vp)W*x9Cb=7?1 zT(FDg4}f;Krzfy&C&{**z_#_@P2RU+cd=2@37ozp_J4)F1Nmsk_llS^cMsj(nv`Q4 z5+jpA4{|3KV?-*m0LNCur}Xl<+|~pIx&|Cp%R-iFjL(s1Ek98|0huB?8_!J{cO!gp z0YiYJfVp=KvwrEr%9b%a-7rGYf-FZ6vYH{@lL&zt`ddHk_Y01yG=>#+g`P@LeatY| ztlE{BA6uzfQ9&Uqr9gW|f~0G938O}i>$uOaWh#m)9G6qiFRL{ny)V^lK=@=8v9r#v zb0e^7)aKGzRo!|k^v~b|am`$_OIC*$;|j2BAL#UhKH!%7!pX=7}jVL*nwZG_U{T8;$eSaDYt4c)SBwPIDbZaHO0C0Y(mW zXY;|f$=#yMiINPCU%^?fpn}QD+5OZJ6XD5`ZxEPKv-I9ikFExnUY$76Wh)F8{!U;wYfF;Fnl+KBHWOhU)-t4fB^Uf9GfVFsn*+U}Z;oB$Vu$z^5l!5N zl7>Ht1&Xjk4c$z=2{s}Lqc}N*1LB+qi5y1chq=KJ%k49v7woo4G?U-F%_@1)@XvU4 zhomr(+S>ZBdUXpor7iqcqlHhUE&N{I0vt`l`4l$d;pgY4H{6<0!-SFu7X1mql_-Q=}S#kU7S`aRi3K|R&+EA>Egd^ zSJjO(izCv`CnG0l(;1E2Sv1hK)`bcGjRN-X*oD8*pAr1)k4EtCbNui4CHwcuQ~3Ml z*EjI*&&MlMo+^ARBR#e1-jvpDAK<2O;P!afxa)29y5=Ng1;Lwi6Gk|&)Mz?}1nqYV z<;rNRm$}-OY^xd`stX8?*@@?&>@lZe6i2yH99g=_e2-w$L+1a49<`Pmhf_I@DqIjN zH)l&XZ|9p}K`5FR;bdHclsgGB@D_@IGR$tR*-iY;)@^xZe0!W*smGSS+>)HtToS|F zUS^Mt9)9r0OOA9@cci_08C}I8RKfyQFu~7$8D31H%f%Vgyo7NMbQDaZJ`ziAje_ac z*%+S|ww@y^@pL{z-lFib;6DlPbt8Ze72fx(( zZ(U7RzrxkpzYKeyafIfdf5{&HPrYg}s!&P{zL9}%WZBv8bx2it7lq-U#0&+b9qdUwI+-z2g9swIajk)^7-t6%Q_%B!^b zkdo5c1h&qpR-4K$)7Yo_jTt?pt8e`eesFzj*t0b9FLgkH2); zdVQdaUwYP%on^iHCCT9??JR?T=vf1Ik*jAd-C4%-U-YcWJIgrvm!7hHubhTM{q@n4 z7cUtUJw2|dOIGs(o&V4?_Vbh>#wHokQ#t~lGiGm_HY~h6t9HlA%~_|Lovi)++zi>4 zm74?G>gFcQw)WCfV#z~m+k4qTwVA=bJf4odlOKzW0JhmS%vjB}_=vpznKVwzWZj~S z>&ZB}9BkOFjyv7Zm=P*|w(;|7 z;$A-v{v2#rnx(-4!ar@x+IVzu!I%bgjW*bb`G>}~WnuwCn~xmNZP=#Pv<)aebD~QP zJq3D$jqi5pe;e9(uyNAHCSiducP;Cq-R?p+ZM&^C_x7T_J*^49ubSJCWkExb+vJH+ zlb5<|V#Ow_Nr_MsOJPk`5%X9=#PUC#K!@7CWnE$AOZkq&4G`+pOv*?POOfb!qcPsDTY_>(Rm11-ekVx_G4b zmR?icbQ9IuQ#7ST13Th5a|TN(?;UPd*LNUiCvBn@?%NlQjCm^RA8TYtqViR;zAl-L zqk8hucziq^H`zOE{C?Vb%66Ex_VQ?IHr@&3BK zC3i3l{bVwqqg5o1jP&}~U>GG@$!2C+>(B#e=ig6XpT2ncb2&|r%Sp?c9$1|YpZ!v= zL%E#P;rVsaS(UvFKd;Go5b7PL52Cd}e(5So+}>7?51Zi3NDRXI`clPXh#5)`+YKX& z=9u=m99dJ_;JF)VLYfRn6tWNir#z*BCUCsrj-s>9@|{hcQ5axX*hCV6ikt}Wkx9`N z8j2QVUyFtb^Cl&5>k1BDR{?Uj5@Z5FvZP4EmJa9Ch{r)L7}AN7Gg_h{`r+ZBijzx? z@FxcVzC=QZZ6v40$6x-_X4B=Jz$W@+t7y=*z3Mn_rY3& z3JoXo;TV|K0j_#^$E*R|c%l_%*dk1GL?U~ih+9kGjg!bbzBS?=3yvc633RZt zeS2%ScMrnb+kNXGTwf0l_ID2(JKNix&R_uFjc{P^?ii*Il?VFzclu_ysrTV#LkZX< zcg&*st^XHH{Dv-DLWznFoZ*%=0F)Uu&w8=?=fQ26UXOR?{-G%MNY28K)UoTL};n zg40**4t~lAUKU70jYLQYt8duQg<^c!sV7eyTvQ<32`g5Jin;#Bg}ZRcME~|yF{7LM zAHH5Ntr?uvwC8O*-w!5R1>F}^iSwY|5-UZpW)9~d;U%0`NkMd+{TeTf zxq{3W3zkKrP*-xUxxMOgltcre0&J#852!?DJ_v} z6<=9TjyEz!H>j6G=AE_hRji>zwxvv~S3t=-PC%vcO|2P_gw;80dFlVly?HxBPKdK) zHZX;5>w|lPEadun9;?hXE7#22xpI>ote)!3&Fh^7oGPo$%M2ubclo(C%&gaNxS8Yt zWm5$owReYv0sa3}^f!e+#hHtzS8i5-Jv~b3uaHhuF4*A;1S@l0sO+Om<+iXBU8joz zI~9-lb^8+955gO`GMUPZAyH`bL7R5xhIg!#U?6*em;6~}9Kj`yb4GA`ExDMYvj^e- z8}@%L1pu*2WKU(zf4I-4_4V?B7QrCnuOJf!7@5w~pXhUUgg#%4J~>uF*OiRS z{PXnX&n?!)o(5f8p~ z(N8=ssDz$5%W#F4q~{(Z?0XQZpD>{ougs2b-G)tZ-v3 z-Osz=y^>$danQH74svTch9`h=)oTSSF~m~qi5{qSBi~y_9C3SFUgRBJI_Pe+ z<`_scCjP=@j(?zoH7C29v4JU}nQz49(XkRD5S$A|;|DDGBdjb?*+yA5P02K$ZB|Ud zcW_&xy3!2vr-Nw=&=$iJup%Z3w$$^H_rO_Qj(g;6r21KhUe=xItPfYLRK@{jcu_9> z5vE^IdShB3_?fB&KU0<92Qhg%&=1aG8Zd5G5Dqqi5MxViJUTdgq;F0)RW5I)Vg)w! zgRQei2U}SG5s4tCN->q9o6sGLK47oBoz28vS+>W<+BAQy#x67I$8B z*3y?cPPJn1txB$_HCxQ_yixN^;=JfuMk!OJuFUEDlHuMn2u%LAQ+?mIW0nfHgKc-=>4;2)4hRn`J-5*N zRkhpFW4UpSHLjiABw?NyVwnIN+xh~O2Tgp@0jbfU&nBZcmOF|a4I z6}TqNY6>(Bx&80IhYp2}pQ^c(HBxbb`aPa(TOFZILPiLDA1bg?mbDe&GVEUWu*!pP z(r`H*)jGzl5UiA&cs(d;4BcmG6X%2dXPglLM;_e34hrABp?d0hQt#xM*SZ+e^lrXE92LA8@u;3AUthUgh~;>E=8X?W#NI!uCIZXnOqM1vrFx2uLIp0qdy zd0a<|mnr8rfs2UCq(WzIh|rllJQw|*UtMg(Av8B)h6HDD7zIOKw4viHnEGZOi#By* zLu36R#laBdLuj-?f@3Gb$ZWJuJjf;oOc?9Yha5+J1N|leXS(s|V2je@8W?R;PZs^| z6XlG@FgKzG>u(=H}uW0#TS=SaA1M|!M$0`;t|tn$->iDA;4B@vT@c35i(P! zm@Rc=`rK-)wuU*JkwXsGL!Mf=xx`kaxk2Yf1q3FuM{|1<4mw(|ss)L!dK#P$aP(XX zK5DRW>f%i4O4+OqnK~29vi-_pKdW|aYI2Ky7fgD6s*Urv@RreU#tUHihfn)l(jYsN;jm`l{|ZWGF9hvZKfN|K+=|3*q(~7Zy43hjv2?5 z@B#O5pUBTZ+Mn$*SB!%8k_&4y2Hm{#Sh=IH8t~E0N3jyQdj>F!M!Dy)lIB*Ii_}}S zd%2E2pj;%c*`J(QLq;M~LABQ`uA83T%?go#4}_(O5?ua}LIK)HqLtDx%vcK~dVt^2 z<6tOB{St|ylID&a-wz^SD6d>?qq$*f8{ffsa0y@sp>_rod^*D@G{zkW;nz8!UIXA7 z$Z*}>(si1{b$gr9rgeqdWg)ClxTr34?s#*Ty_nuOU+3gAewXDriC zi}oqBJ0JeRpkq{u@kk7pkUVr-0ZW}oE)I+2d|5$WspyZIjfIUF5oC_A(aKgFn)fBX zeg@$3&97}~3A7rn)j5U>TGOkEY;9pV!vSbTd;0X%>z5}QCy{7;iLr#3e$eMc55|JK zwAJ^LkcJ*y%;7#w#xnvhpn>qF;lK0Vn<1(-hlfS-gC2Gw%yuN5FjaeU*hR#vwS*Vo z9g4`!T14`L)rGC31LUgS=rv6@4&CGlRy^IKPDW3e{vDCJhnD+h!DSW@X;_*1K^9Vx zig5OuHmqz+SpI-b(l`8ppOl6qrVWM@oGFfTlPL~Kg~h-7E?4}!?-(Gsyvgcvv5Gs! z75>ayS{q1B9bikJQ%xm$Dk>e7FASSM^QL>gDG7XfK*y-*USyJNrl;#BvE8UFE8Q~j zdo&%xpD!wblv2P&dJ3V@%S+36jvUYy7dd{i13+K`Oh-;-oPlrji^pPT#^SM|NR8k( zwwxN8LnD()aT1FKrLp$%5y*-vbt8zKffxJ%zuqZUteESyb%g*po3&r0j)-uErj_H14#A= zGV0&FWvLY^Ob@zN-fh7JIx$6L7W zER21{ZBvweSX!%CV;haz6$@=76NfXP_ml-2nSvtSpuLOLR;RGcmp_|g1)VemM7+y) z1)#hapwPNUbkCA+w0zN#!vch7>W==VZHl|6flP{O?ZvS!NXA{s175(Gv}%rSIL2@l zQw*Dk+TTMZVmQmdy6Y)W(E32{i@5P=g4~ITi8vyBi5&GLPXwfwwWGb!H$y-VGeW}i zO5gPHL+=1Yn0!ddzv?~ud#am}I5_&6u41JU7P}@prC{-*6z_sm_Q9980#SjRNdxtr zuF!PJKMzVbO``qz*JAg5KSHuEzbl&s@6Yc0v<_gks1qMUT%p{Ve%LCmn|g(z`t%0# zGl@Pj?=WR<*fS5t*_H~Pj7wTfKb=u~va2i=I%Z`1cra}rl<5oLVLbtkYI3mMpD;5T zExSpHnP#%XLU2Gu`RnKCqVi{#g#kvN)T2p+)rNEvH^pk52$;PiCD-fxEhENQ= zk|*4II^0&kxYZA~`#)n7Ntr~yJ)+$ub9Wb{1un`*YvVf-@akq(r3Jj@2cQPL<>q{H z;g>&@f1`%0QbXA{p2AfR+(k%GPd>fy20i%vz17oCBX_3F7H~qD+H``h7g3jW3@76E zHqV`mJvhEx&$+=Oo;OK~v_%6!h<5YH`$HLL4%7C1r>8$5&!qI&sVLCpm6<_zoO>1k zbMN?2V7sUPG?`8!Z8q`eiu|3PP9D}UUy*Io@NinS&_w{^o^2S~KNDwmR}(UDf$S+} z!k~`AkrqKaA^HIj=`f(zE*}UGjk(-c8~X~*Bh5pA(^=5jk4`vNWkFXz22TSn4bJzVK=jAgB;|VN+Iul0ulQVZiZA#%2{RqrZFm@wC9q!WI1Pg z>P8wXjb=<46UHcG<(SJBMk0E@?Dm-DJB`|GN4C}bIf6@DeV`LL-{*ZE(A6Ut}|y$}Yo@g+4(5fwnvGF%@30( z^FjiJ+o%=9MZ$4uNKYK;5AdNRqe#hxrck+ot=e*{hN`*DGeeCw+vRPFC!d05#MvK5 zO&I9xr#8oDkynDDhAmTN#8J^^z;_{dAf1ulxB-GbLT+TE4p^iQVtDt4#Z9hh*XnD z^BaGZZuHQ>v|+X}*oeJGQI>5&8h4O^cRXmX9RrzUO=g4SdGXLkB;j+IcCPz#puKpd z0TZCbbxm!7Vqh*;lTHnwlmtXegeF0!$h)#;h{zDrv0FGD=?Ey&oaCZdC|;uXygV~)y_L?)Cu)a>|4+E7nv6G zM>@Mq=_bI2xBQC+i)2Nt2Q07FL^x3O2RQJXCYKrPIKtad~V0 zcd%FXEbF}`em^cR`*lMj z(IX(1qDI+@{4=@0t2_%WEezb{?uI0%8D{k{?cXG24-?Av&$n71X7I)Oa}s3*}) z1t{rQGIiIbaH=Z6Dq7E~87^4R`&<~_R?RS1N|T9u4X=5(wdKcDvbve=_*u23Gqi=W zM&}o;|2K!TRo{F9m*tipTqj{9POt=25byq(0m zqS!j|oufF$bufP+j&-UWhYslHkNdUB{AcIqMr5GZOu*ylz_zSIQiIPGUSA)fXL8Ya zBUgabgW_y4qYYyCldv+z7t?k-KSZS*bm<(z&2{#93$OnwfWk*w6^Gf5ZAWs;HsMR#wxRJtl@6CFSR8og-|xB6K%SfM`&w&=0?}9 z>uc6r1NKY)zTW#$o| z{((*~z;aE8W!d(WZ50*F26vJxNuGQ;X}=(e5NBP~8mfswmup%iJZ;(sq47D~jL&qg z+a;NPX*wG8BKjYAsPDZvW1#{Xq3IdDRDzKN<70xAycM|=sOt0TOC?%kf#Q-TdyOSR z)TiDWN@fwAs+$$gDl(z#Yd8H{&89ys-}LX)bXwrRxPu@H!uiYdbMAO9gL?YOURO6$ zp5U+)^7oS62tN81LXCX-5MvT22ca;qC|B)QBZM)(o(QhenY;S)_MYB@t@-_rdjBkR zoezDHto>^7oL_TUzlp5ha#_DqR_8zn2@kR9&B{fYyBm}(DhcX4xOwEQwk#Dp7%{rVfe z8Ka^IE*y&s;nz3=SC!m`&56*_(P=ryHF;n7OyA!02pSd zdPMkrcO_;IxcLW|^-r}X{gX83ccMj~$#P4~kny=(54wm>^fA~$?4#g!9gfm3u%*8L z=ivFijqxqDXXG7?wzPaV;=TgVo)}90;Xk+eJ&BcvgndizE9gX6Mrs28*Wbh}F3ruB z?c~0Tc)1V%J8%Vlx~YrX?|1I^Q%Se#UgdizMrI>sB>14y9kKeVtSZ5L&B@AJra#Et z$5GQ>81<>0Tb@t;bjLiIJX7ZyU~+@37f0Q+V7oJMge0DUPPl72E_dpo_-_MuC}hQs z`Pdlv$)Gj<&;gma#A0(pH}wtp6ha6+`bkCvtDQR zC;1W00f-ihY4s+2lhTb|Hl0^vK*uip&=?y5N)MNN`=hcfpVt!JJImJ*S6W;s3Ve_9 zhj7WSAWiCJnL3nG;=VfN*e9 zZ18yJ#~2J}LmLk^PP*2vwr5$M-LbXKcIV@+X76kEZfhIxqQdHU-$_hD`J+g`JcQ(ep_2wv8}s*PPF^%T2Kj zrNAIoKRVd&N1K}l6e%IDkx?AwlF0{=M5cWVCc8aF@6sQ;%*rZ(qMaM|GT8?aSdUBSUowka_e`=#D4R9O;Ph zxj-8$E)r{F$vR%S8#j-?9LGMUQJ;tI$3eGind3z`ur0K9nPz3(LGuYRsp0V0qGEs`0v`lUthUIg717SWY(}Eu=}0}Ot`1si z4P)icK}=YdK_pb;jK>BG?n}@|!Nr9qEha?@VrRsVk%l4gfZ<~7!mZ$qrJFqS*=q{_ z!#{q)uf8M=$W_KS$HHbymdAT+l!wUWiCRna{o8IyaO=oGQJ! zJ%f^{^1NL(Fep=RU_X_Q4BDwbvY*mJ1BN|bLPipYOW*6@RCVx+Hy~f?T9#FDolLrq zU*MMpAlJk)AUtK2FIzq+y?uLYZ`WRtQm7wl zMcUhJMeOaW6|qz+;`lGm&nwr%@zn!^#lWomT1ct&m!hB-beVhtSKwBNewrKY?d>1j zEtpbVA=ol@ddtLwtSsU}$$%beo%qJFD>ja8?cg#+>1B$~9yn*mp35W2N_UD(ELmV@ zTqbirOl^xEu{kVVCSZ{6kUO#Ub$RJfXPsDKb|dEu$tzfkfxg9brsjAwov-wk!@GD*Xc0}ND>!7VD&r3F7rwc_xbERQ*7Vbv&1mJG`rKAAbs>64yLAB*|f zKz|D#+yyz$JwAGkB}7l$+5pFfrkv7uKNGTxVB#YeI(#h08I9Z-ewzyl2R3AoW%Yj* znJC(Tt4U_6V*hpG5)DWSh_5cycg5v6%C>`I?a6#R3E|qV$U+%QUs;7_3^g)&HM}BG z8W$P)bPJ^{HUlEWA*S(RXCt1)la$zDr_{)d&*=qK`Jl|kGD}sEW8QM}!gK-B7i3mo zRxDe9jbq71NGR1dH7?0-0iyvUA*a2Pa;E-+sr}IUA&Y`!GG4}rLQx6=B!UjIDS>N* ze5H%Bmp3jNqnjJPs8q$A$#2}m#;giFVbqz_;P#!#r6)H(C5zz3M%~_I1GHR! za%QT>R7TP#)5Knr;Y?Y*m0r@yCqb=hoOGFjXvIzyb5Yc7MT^YwKkWYDN_HJ7sES)a z*OcfDcZoO-b(`+pr3GQGbWdx&)NEC+A#1MSF?=ZuIK16N4`9jZ8muWN z(X3!zGEhhiATE4HA0f{|nITU^Kdktejl5LDC2_V29u!PVT-WWb8tTvVN!r<mrA05mo1HTXtV&!!$;Aw#WeUm?-ZrytM< zroNfqL@-|`owhm)HFL!l3drno2)zxN6i$i_Cd6h^<$mRm8;V+zbRs7WF=D@cTkN)U zI3tm&%5Omg1Zv%(9#88nr{u(dWsA>QQ{50)I777pU_kkZsaHNOmSC9E{h>nG;d<(@eswt?i7_Z$|UUSC1VXTdY}@)GjQY0 zmmmCt5s~-=tRfvy2v%7|9tvRYp?LSCV`+E(`DJ5N!EQa?+78R?> z_gXc?lMC|w!$ly)-XYUUqMu1Yv<0K#315KNq4SAYOEu+nB5^<{tDM5l2edf}{0WfX zVX%nsJ>x9{KT~d#;ufo3?=l6K@oy39zYdr`1D$pN_Ov!|-?{Na!23frj4p$!SCDI% z?w}$5FKNL2;Rhc`1ult!y7h;@bur1Ng>f499+&ZCmGEbWiivSCKx95Wd3;jRVG@t_ zR}%MS1XTQOd2OxuNL_m{-Z00MCYN}(_=S^swj|&LhfzKUn^B7y+-x!BA)#U}$$}b{ zq2Sl*kE_y8qamrx3w_~1)ybCcU*Mqh+NW=wKG}l%ZX{$?EMZeMC#(e9^s;un6bN_= zW(;NxR;qG-B3Uq!qkY-;WMMRGqnfYm9<%^pK%c+RT(IE8+l)h^kQII^vayCyVKZrm zinKLF+W(h?u(Hw65nqH=kIzY*D=E%5hPzb0REOfF+M}oD%xiV{UK)!;w!I{p)|Y%V ztwQO`=eSbJD8R=9T+)<{4e12^f(2)RN!io)7Ws`~iKq>poF-d3bDS=G7c?w0O5SCG z70Lpc+NRl%%=d2m)M9tlB;&?-Eu3{B4nR3W55#RWvZ81q7DmD$W7+lFOGViG=jM3jbd!wB` zLH01Udkr_GC~uhwH-%Br5fTl7*o3_b7#kbeqmtGA0rvLv@k)@r%>brQ@LCogb@YZO z8c{6ZxT3?O5BKZW#p)Hv6E-4YZw_HcLmosj7h)h4JZ3_ z8-!B_YEC{${9CLN>Lzhm4{WR5Jw(Os$9fOHx_jHZJBN*E3p0R`4xXX;cB73!W&g#? z>~?m$_8!#Z#Z+Zapsbv&D)aoYo__)_SWaQVtoQAMJf(?wbhq?Q4M(A5q)89RsLQ6* zmQ<*;Da#2LdPpZ4p-l5oIkw``+yqZAWRwA2zh~-sM<+UF=NIGHg_Lg zAmv)HOmEZX;s&R9)J(lGC@T=Nq5Uw&tptO$hBLqii)`>l7Z)W=3D92f#heWEAIQw%2fg2LP!2ed+(%7dS=SsRz7Qr#q2!uyZkP^x zZJQVp@1qW;tJqJx^yd)?}L7khHzHRQ>EFND07+~Dj++!l>+`MwuTHkAyjgK z5Bc?-wRL-ocp0Oidof|d*O;wyCfgsEWcDzH`MJPn`D~sNS8)6WQ{Y3 zX~2jq3|Wyp*}*sfO@K{Oz4nUC?%27S_@hZl4Ctb})ErRpNxLMHGrU5^XSar>fUL#T{1&rW3=6nR_byUAg@Dd0QCOLh!;;FPw6`r_!IhZi*- zv7Io9(Z7(YD?}ps*H%~$e{jl)n-oHP>;K;U-yeT$_5S$bk3;?7(I2<(H^1L%MJ}m_ zudlCd{ofz0^f%hG-p)e*7MRgnZw5E3=Kg#5n9>iU@ki_Sdt+-N>X=p%h=Wl9KZSa^ zv{no8k>4qzg3X=bM(J@TK9f@W+gx=IhHLdW<@I$7!z&AwciJ=R-B3iIlh7H5$+ zJ}hdWFY^pOC`P{Isw$(6!;iZ0vRFRJAu~f`lpili#+CeAjL;?qz3AVYq?vpeXS^0K zBh!VW}!f4(eRNW}tn5 z&JyA#P@;o)aEKg7FUTfyE<=rL&~6a{Ka|_K48n+Mr6q|Z*K-V0&|J?HnroRdDI}Gc z7pa_*U6~hP?S$>0G5L%zN9@m(d61CiHwq#L8NjjlZOF5~n7hbdj`+`1UN9OhW;~=w zW131Z^2^GA`pi;#%GiDaTI485oTf1rAa${076u5L!f ztzuU&K}JOkmH1F$f?~Yz-9@-r%c!ADw#gb;6Yh+(il#Ubn~y7kB33msgR9V;Tfu*S z%6f#-t0FSy>MCtK`k7rLR7RK4c~`0k=M+k1{NIQbeVKxIR)v%bHWxFdIZA%Fn6fMT zm(_~N+M~C#R;v~J=SoyZjjUw81pr2gb9mXf7w!Y-C5PQO&_fPWFy|_wSwqMI%TVOi zB434Z z5BY}~VkC$OV!QO*;%b*^{9-xIzlbkQzC6c5Bt z93myFVyQ5(#BdHL?Ok1|zL>XXaL(}1i#wvvI=z$`f3mC(lj&LLC<9bc91Cmj4?p$q zcDsv~0|>f1s1bUiw2dA{%4pY}1@63>F|Uovt6$1(0P!(-fe*96n4Bk#vMFA(fN@`@ zeM+Jy=VFt%t3s)35g$Ym)=baHKQI#UE#1wA53Le;nmSnh-1t% z6P_^;19y&2-i=;Nn!*HAl`d?^)CWDgmN%H!d(V}@%irHLg(yiskh}v13vaCXL8PG? zTw-yc8&$HDw=`86lM0jwJWiizBPSedV>cqVMAc#srTS;ml3uN|v&spXVr}pJU{oSm zW+jpVew+J;p8MQMmC8jC`ehW&hN*zgL%fr@<9d-q9xvuMEqtXi3rusQhUXe=Orgr- z-Ds>IQ=2g|jBl;vVv7RZwnTIbEfw}I1u1WV8z#H49Rpm+6!sIcdH9_OQ6j-JRUo^O=^nrK+QXv$`$r{%lEOg?%h&u$Cv~~*SVggOe=kySN}qb0*%u}={zU~ zn;R~PHvzQETF^O@s0@)Le7xl{dD0K)9KQnQS$Gix8&wUEi*bemYWZ-D75*&l&pFg1p{JCL&KCwRz`*X(rjCd;; zr)bcopPw-N*mOM5f3+}vnu%G%-{Vk9=#dgrj&doFb1BDi3Te7ivgbw-`WOE_=f#k+ z+3k~sc}(7Z^aGYIMD3^`(d2<6;HX_h!G<;&qmPzc045TzlSAkvn|)($!(g6h2AzI% zFc6bLp(&Wkf4_t7DSd>u*X2Kt^xiiA`B?AmFim;*IL6yL%G84y4T~Yj2KfQl(9I4j zh$XZ+_xGvsDlA%t&qHRg`j7tbVIwf_FjO29%v7+hVh`=9xlBjMFw1Ls&Y9~~D;u*Q zVVinSH|s-D%<$!d^vH|Esm-{3acKouHmNf5O=Sn4*ff@C?|U zvXI!R2Bhc(%oeaA0xq}*154#k8#PJqF*X1|GMO%>8iA!Tpp?N3a=`)2l*$jE8L>;2 zj}$J~&d&;)jchQA2b&c(8v>l=f;%YZg)n%A{Q);W`rrdcyDIJ|nkt#Ezi_$;FpVJ7 zV$kv)*z4d@o({6>j5Qc2uLYEua=uw3jC@R}L=gEh>(kY01*`D$QRRgJ3mB5S!D5& zse`-MO2dXL(kcUL+RS)imAd(>W*^XMg+Z^cE6!tfgI{S1EqMo|Cv9nRT8?UKf+8Sl z!ESL8wSXk7xblq^m)=;zt$L4i2*GSpCwa0n=R{|&tx@0yrJp<_9X~E+viNQMQ12Ib zFXnEI6ZWp)$@{S2HCpA#FaI{jcugLBmXk#Lpds0Z{Br!VmzY!~mIdep;GO+m6uYH3 zY~t$?)Q~<+-RP4bMLLpD_-Zh@iXDqm4^su~_ePJ)T-pbTeM~zEHlHfq7R8-GM=IVa zDBcmp&jwwoc(!&wx#0R1;w|ixR4`}-9u(xUj_5j#QG)kX_kd$i-SnV3z=eO zaiTdOWv5u&%2eG<0~c^JyMT$IZ3i}hElo^yPd7#I+RMOuQv&bJW)`1P@EWEXt?G39 zsR;P?GQfW-0shlw7P~XRtC74;w@wl8on?S`N`QAZvwJNAyn4&k>GrG$`0g^mpOpZA zwwc}e8Q|3fpiZ}=BH*`|0e(~h{Ae>vhtj~u9v#x;=;Z#%(e!@kQNd8Nl&FdH=}zQm zdO!524!kyl_vZe{(e!@kQ61ph4ERs?M~{r=ZfDvzQlDP9A-#`o~BbS32n z+Fv(r$8*s4?;*x-Q;Lb!*I6=qq96J}G&$#5NDYGo5t54AEjm^z4HNZs&l(TW)BAhi zuT{GobhB)&Cu3BcfYr(EZeztq4wYyj-FP`f0{e>TuOuj{TJ+u>T4DACveKaMc#}`G z8_rN)%lKTRTTv=`hN>&0DzDEc{XWU+-;JgPC*tW7)oDm1rRYE`;or@WG2-5UqH6Ue z8H`6PZ*r7AqNB8z^0dR!D)zjBjIb(V8gbo zjYkJtqWmMu`iEX287+Z9FPdbA#>LFXE5AqXkYZy!B=L%gj%ofY*uRAjs+Q3!d2B7q zOTb6j-)q|<=SNMdNJ*)}teyJp!; zXZmM*M1IOmsj$7qY)P-X0D7RX6fR?sd2dc`g&A6}qS$df4JC{~`c~r&vg^jIfLrHb zFg@mL(`a{doj!M?SJ*1dz`~1i6LUj83o}^DDMVXAA==VDf6rKE=At)#5b)#S^EkEz z&t=?`<@4>1_T18b*MYLg`vOUhGqC!OECcrNE}3$kt$*sFf@>6TH%x|7xE*c|9CVqV zAoO+z1J$Tqc|Nx-?WeY-y+W`ySAGgjcW7HB9kP)1Qt5pvoj*3eFcMaRC+Yi9QK`@Q zrp>z;K(WVa>WRIkp1JOnbUFXtceqK)J8|WS}5}8)WcFt9oU5RrHr=KPdUF1<5n8+DHb=< z$aW)PH)VxHXU}~Wb}3alK1E_RfmAjVN3hx$fZ8m~HPG8!y6O4ldE#n)d4PEf;(#d1?{_VmX+;3?1Fz#qQ|>@70jcRx~!U;ypA zs|zT!8e%1wbu=CmH_dlahEQ<4dcp-~8x?A{p~Gkr5OJ;`2A4IT?HC18!6&q%wKMOf5A_y2K*nwi)EXTg2*T4pey zU=2yU7;=_}em~5OojZNI));la+}c>YR_$ z*IykyfBNFd06DS?cQ_fNnH)tYVN33kX)H92c5|5va7>WmO<_9AZi|NGV>AGSpJx!> zBZLS1PDbrekXvVEgb&u&=Ya6@48l*+2u}w_3geNW0=CD9@5hKAny5k!KS-1Ooc2MW z+y|3lDwC!1d{V3S!t$FSnQ%{q?p`&zdkS>-a_FAMPEU#M-ZJPOiU#3F*br4sFOTjX zM`5;z42M02!%>#9OZJ1Sk#RkQ zbR~gE10UO@?5;|NxM<%o_J{0TjQ`QcZvpW`jS&;Z~&&KMS zoEAx?sk2My4?457q=zW!m=$U60b~p?C`!2m6;&aawnaVwL_IKYSkDZYCP34{9+EUI zY9@uRU`RN&k4*xn88;p^;~ue-O91B6Nf#%~vXYtDff2NmVeakX@7)e`+}?x4w$|-J zo4b*{qcyj&P5zf9e}vyK>F_(L2$P;P$R?s2&%+Kv*cJncST*Xd3H?EL*f4a5{XjSD zUrN$GLRZjJTq8=_VRHbzeA3O47c9nr8$eSab8r8nZCfxefF%DvdvC(s#*w6p{*@Bu zT!|Q==-L-hv`$;LdwgHJ?bEXF%p1244@eXVjEevP6e&x#-fzG0MP$|j)CPc-?S7hR zi-oGp$SpE6_CTlnn5UN)%Ju_lmw?-z5!1AID?%kMN*|*Dvw^``C}m+T2rcAIh_2_w zW@d=#4`WQf&GYZm2mqaMD8#5^!{g364w|_RaxR~v8mklSNzGn0<{i0Gd zsAK^*He3;3I|K&9wK1VQG@GZJdL)?Wrqnd8h#%1M&>CVH@h8KdNIt4>^qR57%@~IN z?rRi@oYVmT`e()U8A2w2QZxUifx8Tan&+N8_`J~6Gx?pV@9(~j33xyd06<{Vin|8t zqTi@%N@cAGY4Va< z=J9h)SNDnj$#}|A8vtz$P^OV*5g+WG^0ZQPLSD^NKAUXX^Nvl61jKM%UK%VhA)dR! ztPtMekL&Un>2GSrF&$104VAvpe`)L+ae%_~+a+Z2-T|;{&}L4}z@}jZR4Ok{KWK`n zkZXrjF(sI2$97We0@npD-QI8JS4ip|P}eOxOal)`iMw)R!ayT%4J*Tr_TmRKa&0UJ zHo0eG5@R_nZT=DPz9qaj9XD#e>RFhas#o?4X581Ie9S%nU^u= zll9e%4-3LrJGN;<@s7g%;KmuB5Wq%0-L2x($Y`!iDwUd4kS3RU#c1o%Qy8JMw1+~R z6`{9sT<$%0kH*Glhdk52v>yqSAFX(R4rYMcbZGo84ta+b8lV7)CY}n1A>g5v?8PgO&W=c3f7Cr%ZJ?kV zp`Ui7W{i5tcHN;oSTI>>n*`6dYxfuY58ejd{N2B6v|nQ ztl4Bu6XIDR?|$0_UEr>0;%FIhDcXdvd<)xgYCADV0e9v(w$LejNy~UJSHL_Mo6Yg4 zv{Tcz$USg)qhSn2Af;Ub66DqB2H{oaA3W_SkE*E2Fg=`7E^S)Jx4AS54S~1g62n(SwW*Lys^oPKNPExQPddX9=IJ z0Uz)}HPK?VlI@<=$wr_^8?7xM>IwdLUk4wyCq_$Gs3-m(WlA2eBgWx98H}=730Its z{y5$JaXKc(X`LAF!}g>NSE4`6q)6eKlX$X%KA=Wd`org}gj2E*NvvZ)3j zivXaCL5z%4x1LIWBATU8*htJ0Gb=b4-z}H7=6oRsBMTzJRSaIk7OA%I;=ba=U1rQh zjDUBU%aFM2ju6O&e9MXJrd(zcA}(5-#`ysN)ZY>Ptjt)XF~s+{!t-(uscR7T5G(16 z_yK^ZP^KP2>Ark6vb+*rD^S?^A_j6?nsbO;ET#K^01L7X7NCN`WnrBi1eyG0?S=Z9 z6*Z`Ufj=zUz$ZxEMf1W>L1yOPcqm++cD~~qF@)W@tXs2~SOIP zk~+so$hr+$Y1Tqy=qoiKs|DpF&{?L?mkIJ&>mW1dhw6zQU#{&^;r2Dda0>Ou$l`S z1|=bL7a%48r>d0|+BO5$yXX#Z$80F6Ogg7=0HYVG>n8Wzl)>4=r$Bkc?V`ITj~_pL zE<;67!&#vNknYDA3tqX>jv3i2STmp(HdDOZHH@G72~|50?R-}n=(=MBDYr}GH|Z>= zaRxNqE7sJ~6I8YMm{LLLUp?GBsOcIqvly}$8Z=-WFlfz&t~Ih!^wNWZz>q#dMaLtc z*O+FE=^Zb0L=%ep%Mfi7G^%t2v4$AuRq>=h;xGLPa696?Sv<)n$S~KFO|H`fPSbxY z52tx3Di55(4x6*!1zUp5oK^xVWQo3IHIpd-@E|57)D%Mp@Qj{NcOGl-TnOSsQv2Z*fGY6lUd7H)q&axO_L7Tt^2UTX=f3#@hli&O) zZ2lBCej1Dr85<|M7&!zEj9tK%gu<4DiY*CKdJVgqsL@IR)TrM@jykPpor@*I9GmWO zfc#K;RBJXtqegCkk`gMXFEkdI1scfmai@ZAYptk*2(j%JDr81y{W~!ai&*2+D-b7^QIH-bf}}*Jl&F`(2$?QACd(g+k3|g)MNSl=C!n$<(#2Ag3zI2k zg?up!vL=5r4x(@vlzJ+EH+3!AXo`hvzgP*&?A9Zw>*np)4s93j05R}kb7)Rd;juI= z-6i=VESte7ANw9-NxtYW%#%Sf1@pEcke+rRE89RM&(E& zvcq;vd=u0oG79ML_#7!T4ipl*;e*q>{>EUW%-%mP|Kl!yK zwm+?r=^Q5sb3y^6FI=_$nV>_eu|$xBQOC#xljsbXFD{oq!^%~&#bJ)KLQROJstJjB zMBNUEs1DfzITVXU;v~gbmwsY=nF)RgE5Z~$kwf%Hs9Ao84$}FH@fpksU~&Qw{$o?Z1b$}4TbQ>O9)$)mt zooQ?h(^kV&%-_o4716IVRc_`~=10|&YEnyA$~#*+28hqeh*OfjsjhKvZRj1)f0mEK zEnTNz__#YC`*8M%Ms=*zVawo{mPYK&@W-_nFX=CxuKGJ4y=;&cDRVUX0pr7xTuxqO zsst;8?ve@YDP!VUZ7YCj9VmETR3ZgYPdk_Bguv;LS5g6B3A6snf*U}X2d4j4EH@5e zE(Rmq;Gii#XvSw+P*xEv8ovX=tzfoCq@yRMu)GM@>x6mpo~vNXNc1X*5Iribg59(V z9?K_i%o%Kk?9yuZd#u7t$*eJr2aOhz&5$@?OU|}KM7`8qhH1KxYT=Y(IS&s^DI_XrM(r+O#STPbQ5*44y9uudz&jcG{+hU{dvd4%?$m@9gcO|pdigq|AVO*hE z2s;<+TF1(Dt-}SowiWv7T8g%ym^m2LlPK><(T}1VQDqc*5L`3L1-p5#GPCG|+lT+_ znuK-{Me_t`%GW!pb+4oOX1O*7AO4<$4}a&u`@iSl{oi@;?(aEx_xBt8U<7wqwmcDW zsTkG3^u=c#uO|{~b}7Oe7FDjyh);V~Py2DkjxqFu?Ru0_vLy92OJZLo$PPM0k8wp1 zUdX|@cA$mVg(NvE!|@1PMx_ZM&yoSacHr~NDGXrKA|oPi?7qkjlL%AE;PY0 z&K-EbQ`cz^H3LC{%hU>_V!+I}4G7E-b#I zTp!p=i9Kn=VM2Yf+3hpHjPM$HTeDF0Zd~y}PvZ5m;&;#z&5JSa8He0@G2^ zm#TN5whSgUbUl;hdF-5Fdxr}#nG19B_i$c%~tKntTG*>P9Ds$fycy_a9tHPnW%I!o#s)vQaUj#|i(Z+%sWVsx5zWsuepnjR3~J(h zL@As>T{eZ}k=p_In~0jj2?I2vL&AyRJ#VQ~+YW=w6y5~qN??flQk+2w5~P6V7DGA^ zQH(!1rW}DNMg>!-Qmkt4VoSysoL2g#K?M0*bUN@){ z81FEjK`ZlVH#48DR+ars@_Mys6io)Y$y#<7+^{1NIf^kWnXZ`&bxkeRHASrJYN@U- zQe6WKO7uhIQUqy z%77Kg%+{7AOtC}g8@kk&SdpAU%JQlOvh)F;_oe4@A$jIUE+Qq&@^eu{nhPu!hCXPQ zMY!-g4&BVuf7NUJ#UB?1Gx?Wa;i{da^P7eFKlht#e*L!Wv151HciKJdJG%@#?Jl>F z=mYkohYRZ;V^W+)?1^!BR|tEqCszrKSXtJq6@&#COA$;7nte0`2h?E zad}028<$YQZMY6zQ5}?^I#Pu`@Xw6D!I08oXB{yN$AkLI#|G}s`BiqmhMf%zs|BTr z7qww4j^+`J1{=i2T-l2`8w-cEi>)~r=yA$JgKts1lLRvv;R0IMoIYb4TiK6hbJ z3G$td^!e(AR#&1L*c^=`j5)}N2Qy?4fsq6~Fod&bzE@7e*cQ|Lm?=Uh$~f2y;Z($S zl}rVK#rHrH!Vn*J@fcss*O2GXx*_NcADZa28^E_4XJd@$8C=X#0aT#5H6Rke?=TDU zz$*Hymp}7kkOwc}r%;DC34rgDDGtg%yJj+UA0pp-ROKYwv@xaX69N#&;NxsE=>`>C zA`HR3%Y2QMMI0(9$|5BzA*Nf*;C1jbCs1BfwyR=dF~O`9R#+SPYSTVNAft#QyYM@o zCsrmIE*qX9NX?OWuyvUVU ziDZ)qT(omU>gZw*6a8)Dpyz-~Fnlfu7*EA}lUrTIjj`EgKxUND9oM+;WFCJ|zBRm= zSbd~s*kFqWoMeteR26ZtLb5T@f@$GS#w+4ZJqZNmDC)?w@(^k&E*f!MU`(CRUWs}h z1{L6yY(=NzSWTw$ERTq1A<~qE^TbCLnrnxZJE@#&Fp4!C-4OH{&pE zDRaD#$G~EhfYJPw!&mrBp;{{Fm<{^|`UiT+s7qAqcZh2JHc>5~+!kNB544Dms26dD z#3I)7#jHh+lGw>ZRG^Hdz8f(wbIdE^hq?M3Wr~+sQI_uzIzKHKzQa$1{z-wupq*16 zlsH{>KJH_7@t95cmdj^Mw6z>i%;PvJOl;$Jp>6paP8Uy=jZ2=6_hP1;%1niiI#UL5 z#<0K--PFas2>x`}l~l|lp<{KefSKb!t90o|;Js@7^eJ$x)}`QjmVa31ZP3U2Nmx$c$V^68i*8|y5qr!36d<1P;;7xvEz=3 zs|K=3RMwVgp+GdtIFsGnMHah(F3jI@Hw1C985}$QP%9h|brHC6>M}XwS6pkAp*x*J zI6v%?44L^vz4ktcIp1{(5wYfz8BR*aYDB9OmE^#f7jX^-aDlzsuGlasoU1rRE0<^@ z@r-yo6k!qe51|)1#37p*cr_OKmT09MwrO>Xc!v`0P{&WSny_Ad_gUf{r+4yFO zdxLAJ2#ySkCq)jnX19UCCQI(j%gNPGZzk*Mfa6+f4!6tb+Kp033_V0q#I-J#*EYhx z3C2H&_(8K->TPDKaw2f_a3Vsfbfhz+YO0MH+hmoIKITAwtI2r$IWzW|G(BCcB4vt3 z1@C=~fy59XS?tXQnLVp;GyXT#-wvN{aJcE@Ef5;P4Xw~iBi0PW(Te$4n-3%>;EhUu zQ#1c=ssg429`@9U<3#28k~f(ug65D*QEwothu1=Me7ugRJI2|E>xt2ZNtl%3^#p6+$wX;0u)_e+8FKjm#4C zP*u%L1q{%&ip6)^3YL&;s~`C>ruxnnZWaqHEGK2rN`uh@`>a}BQ=YQq7D}q-S%=G} z=Y|*x*f!>rW%sJcjJ7!D=dl@!@|Yvv-yV8g6z498gsd_Fr^OUd2`>oPfZGtzyk?e> zL&$Qlx};qy*myngMOd2GsPFaAJ*_=F`H#nqR9AoK7%6ABfslqsb)w4c)VG+ZSIF%f z>Ieyv$DL5yFh*l}z4vyyFDMeC^*DfvT&n$GvhsM%n z=$TnE!}1h$Q9^$JRBLnraQa76>=#TmzJcLMR;^%Y&i zXOF{u5=VvmjfvO_e`+@cpIxDMH?LV*xw1tW8(4Q0%#o^uQzevQj4E;{G!&{}Zh^RY ziUHeiE1>bLz`d-%NI~k{uCBHW%gpDCe}OgDAsaVK?&R)5nfBvXN^EvX^#H`RdH zcf{H*uduSKEd)zS27v!(;HpqQj@-bnwv4J@9xE1n`O}|%v3)Nv0*Kin&xrh0Azd=U z+(QNq9ngrmv33_>`EJpj%TZ*+Kp=;0$2d`e3OXWoBqp?t65GpD86h$b6mCdRE*@UwpRf@utvg(q)UcZO5$`9>FUPrPn0J;X(;_CW zDop@~?LSqF6l~exomNERV81y_nGeq(8#!9_a6c$+^C)%(VHBg_Ac(ntem%l$uGUGi*vyu!QtlbKEcV!{9&XmQ zsBT2VstnTj-Aqay96t}$P0sXzmdcPbv)rh3=a@KZv1dr&IBtzWYlX2TKH*W`p+jOp zq9SIBvsdK+vrHtg) zD2SPA<3ajeKQ--mMimj|)n(m3oy)N3p|0|W!mrfhZU|g}8pL*0MYl z8Oe4TMZJGSx!(xoBF6}$D77uNdkLCw$#vK~#?H03kzh!~I%^9-M%`MiDF6#x#c|}Z zs#Bym3u8_TME^Cc{pDow|IfvjkS>-HaJPLU^`p?B|5of-q~8Xx1rK2AfxwUvY}+G{ z6`q7(#SIfUeZ2WOfPk-Su|4*By?kS)$u)cT69?!=PU;g^;L$j2jvC{r1r?4ZAW|;2 zXTE=Q?0g@hIf9j!vvV=Iingx%VM<3#j;oXLzeV5I^P3rWJ^=q}SuuCtRQ=a6n_MST zp`8S*so}DTf|Cb-wpkeafFW|u#zRBuV}c%Ja3~;PGu2Mfd2u;|D-f-ooq8a?sCzAu zj0k3)P4XE1q7MlxI#?gg2t839(b0^GU`DF*+AXnnSUI$bCYvI0<->1^#HD-H5Qz(O zu$p3 z2zZ2uGq5=hjhKB!STp504-QAMsYk1IvYEe2%}*Y8zaRfM8NEn>^X+S(zik#>fd&w4AHA?KVaiX8=*aH4(-9|&21Hf zpVKpNR)>rQ4^gNsec1uT6rV-U*N#amGZc$;lWlHc5%1cDx|ZSo05`KQDb5`m zM*E&8Hv5<$!HGyBX+u3Cd;;g)&LoPs&9SX471dlOJ&M^o0XaIAM~WG{@H>|qELJII z*#k(Nkt54s%LG^46g&)qR~~c)WEPgBe`hCbKQ}k_)twbs85AC#_IbdH@^Ruz$aE!8 z6BA!6gQGo)WS$&WamGi`id~_2v8Xpvrpnf=c2`<3Zq!t5XzWCCS(>I_9=*>h`z;QjmXFaD`M^-hwKMS;!7Y6=7A9CUNXCabCp!DrsO=$ zOi6*dV%v8Xgu$}eW_^Qroya+by(5WKB?OC&d{df~w6)-^ z5~xQ`TY$4E0tw7Ks~B+7R9l(cPL4+Ut8}@Mbh8LtM>mKUrPy?=CARsxv9Yyy183rX zW!MrDLo)WTj4=>HCyP~AVW8>sT0HB8W=IVI0+*LJ#ZBkfGwK|%%kYN^t(5&Ck^USf zFUQf49LtWnJ!qq>ER}>^#Y<4SEgU{L^Ek~s$61VZ#&q#U#DG)8vw$iMkyPQx#o4|g z>!9ncVf}sT8K1i-1@}uEfik_laMxy1fsfi2e*+Dtig5ex3c zZ9q%iC8jr@-8k@?HkeWDu-^zDS&kR)r6j94negx{5Q zu?>KoJ8?T=NN-A-tym0lAy2i9IIt_Ll*3Yzi>)YZY_7aar^&iFk(C>PHe@t8S%nUe z(HMxLJ#`8!uikyFq9Hgcdx-)aiAAF7hNgxbiByqts!CXTA`V86E`WB*gVW%E)y)^d z@hmoaWwSEzy*ESM8Fs-BRVPcV`9reK=2w$cL_D~1B4UbNOW4&G&YP_T1IsQ9{+yBl zwvDr8EMAJnoXOF9~ugk^J?r?Er7|kBPt-%>3=bVoF zVi$Xy-oDc=(KR38Xq2QWi)P*Y$d+Utv07tSnoL=28WRTT zLjFsO5F)@g4L!qTMC<`>0JQ>8L@XR|#KGL=`H7! zZnV}z%cc>P$>I%?o7QW?dO9Fejt380dc?$QEU302Z#&8HhOll#6nP@EN9XWD(9=^E zamuoZv5hVx;A0RNXcTyZV*`Od+31$#G!W>r-?l=^%X%nsnYdX~mJPyj<{@x2h#81FIfpM=wDE2}Do~0V=QjLf+pA#9Srr;N1^#SMLjKc^xgU%YO zJew@0h2GUAwje@81VqMlISdmrjJlkOQNp4K`y0 zZ5lSAvV=9@;45tS?85fRLY6~ZGA=BZ12u`_okImx{0|V~o}>164Dn2cu!6Do;IkW! zjQ}VzYmRV;;usXh4pv7^O%%@@q?V-S5cL7}y|$z!p{BSc2ehuM)>96OL>419H80a94`=IyNXs2vM*9cf%J-YW1t0?)M=IO27*vTlJrCr6WMA-ZE=K% z)HYVDAvobG49UYW^VRFdnpPl8@;O9T2267!U|!17AxH8~*ow{svoG2JhcOw5j8HO& z3uXlGzIH?0%swxSiP*}V;{>0<8^HL$X<<@PvQPR1NC|0`m87Q!=r1nRFB z>LixbFCH)-kyhp~!AS^dG5!;FdaQel;s>9jxZo;7lq$HFe;^z&I`0O?5xM%sVt{DU zkcH=P4fi>a7}n~6ceg9Q*jNC0Ox5=0v18dE7!NGgJVrLF2Uz zaM06FVbSQoV%mV!D?V*-mO@!<^Wd`01MrnobBK!!tm(Z6oKrNb9`dF_2?WgWJu&}@ zLx&@~h-VTCAvdFF#FpjR6#E`7Pw#yn8L1a@PmJ2}%ePG*dv3SSv{6($Q}GnO;9hm89? z$9%CN_Mz;KOzjp!Z6KTdYeZ@*n3^gC7Hk&G#;S7M6)_()w{612U;weTBQ_ZDdl?M) zg~5Q|W-#Cv1_OQ{g8@IwAL() z8aLF)uTAlkC7$kubVAJV%-jyIGWD7W__NNuAJ|63EHvkrQd3_tNXwK>SpHpoRz_Gx zrVYZdFxB+ay=^6UK_6s2a?{A%GEywPDzmOQ>Obk6(|mKtt({DpVt1&l0P8Y!D$_wD z%MB#AGCl5Tz_OhyR2wKd%CW36=~XWG&o}Kb2#R|d1l6Rz6r9Yo?dEn4PGz_$cAM4I zn968VTbxRA&JG8yn{;_uN@1O+E_39{LHthUBKb;m#7-shGLAbunAY6MMyE)~( zVOToB*=5v0%}_k81zd4v1ddU!3_>EEXOBN|!U{2}v2$s5%nsi!2QZD@j4hQ`D)2r; zDokt`tQ%i(eo=>GyQ#VKRpL(DTgkqZprM%xJ?cxC3z%r&Jc|vBqJEX;5etORLOHvi#FMmIKrV~y!iZ(o^dR81x)b%VJBmu1MI5otRjWk1AOSylc3a6pg0AKz9nPetJea>~+HJbZpY*BmHq z1hc#=sv}yXKyqF}2Wv}@S=XVJ9y)QWgou%C<4KC5mGG&|ri$7?1l^HJuB~M7wuFS` zQ^bK{-Husa78yQrG}ypjWVk>-#3q{~z6vF$))b}zDtTwbj>e*JX5?jkkgo##vmOY` zgy^iIQ|G>ETSV?G@-qPt4>mR+2ap}4qc@{lo+y4?u4}7rj<92F`GJW+YFeU!YAuFNm^zNNEuSD(RmPaO0|GWh zi1}>M<|KhWAZ7Ij7iuhdaESpm*6$;a!bacoBe^wZb<^vnHp z|3LHUpquVikSAnhzn-UAvIYuhB~;uh^Jq3SiQC0uS*=_i-po9EZf-MywdTlIuFCxo zOFq5(G!^f36=1XC%u#2ZpnL#q1S+S0>UbJ4u!eLuT;S}1b3`oYK=BgCW)9sef zmHN1Cd!pQZR3~;PRZaF*-v*=UWHY;5su++^sM)czvISEm_KC-wSj-l`NO>P}>ThM; z&t)>R@<>_Sh7YeQQm!b^zSwkuaI>Y7)Hln|`ny@wfn=7gVJ%oKa||eD zK^cfNLac5BHi7BptcVs@U4G?`)ta7W(YH41KBthG{W89CF`qnj;OA_m=&iU%$q`M6%*M> z|HF1fE;=N4@ga#-IO(y22Mdio)(Ut`0$Dl%D(g^%8FV$~63=DA$Wl_)dXe6pFm}ZN zLUyoVLKA6nEM=l$Kb3SBE$eNUs9>~N*W0IINWvcWzUa9)UojdNEaYkHyDv{Rot zg8x>8FJey-a_lR%+t8vfWk;E3YPEzIW_g1nLU;uIbUA+oQa@_LHg)LvIghN8VtT@q zsJ|IOFb7p`2kx?^`tpRN1#R$4Lgvmw=rt)+~xIASI32K%wtvKEf=V8Sz7 zB#23nEoR5~FJr49_Bs&9C$@aTDW=6hr@4SgG&ZpcMJ1l(9{9;X7Kj6{!SE_n;mKo` z)yBlfB3eK7H4;11kd*><;HExxh5lR7#exipHq26iaWLBvY2OmsCb$vW+0{3S!Wd(rL_sv{s>t~C!SlLCYo3eluXu{ncUuGZzY=$ zvJJ&68c6{5PckFI|BlU?C4H_^{_99QfyW6D+3PlQRQeN6r+@*eoG^J`sE z7+Omjn3bE=v-bfzA<$7o_f#B8u47Al$#7BXjShBGE>hnWS;T2BM^q-_F$*;C2e~19 zC9#m=bff?RXEugpBM^H$k;bSP1}Z&{lZP-9;|{vIFhhZ_WIBP%ri3A6gzWvJOhd7C zJTDVtdsT^k^Y(>UiMO?gmuyG8L)-wRP>G|EIY{&8xhwhy(gvHe(|NY!Kn@&u=lLw5 z^VvllQbNAx!CNCN1#BagSBwLFout6L-^}jj3r_Nl-4Ic~p#*axCy(dLh?}(-g|ihf z4l~vxfNc*#P3*&B}%!rPE@S%sPOqLHGivZ}mh?rEb zc23K4vRqFkQQhw{897RNW!!Y~F|Zh;JYYwaEruC44s&~yzuU6gJDab57_D2Nb)w2h zo@ML#<$acHevh%n${k0BsB`JyM|s6v_0285VUBogp8Dn$-;|G!h}@Kgj(n>f_wTmO zPCodZj+T(1W`LF+{O-iWdh9#%IT)qO8@{Uja`rY!098s1m6b9^?(&e2J4> zH@bZItu7yI;C)QySYmyQC`)Xzf$ zAUWl*=M0o-Z$zECPA41IFvL(*L{5q*(MT!9$)=cnQA`BWP9h`_R|Xj3xssQIT@0Nf z4x_KBu{?OLQcbe18MMAFl(}Zhotzi*l}a1(Tv!N3EMu^tm2sVw!#=!9ryIhD2{ed= z4w`Yq4wG%$IbUlmY9?6NAqKNgi$Qced*i!esQ^o3Kg=W@DJW9nky%Sa|IRol7ooMe zyw56@Z{G1Xd5l@dC2aHj<9pF7Nt@B0u`vD0LpGHhL_FN-t^5N{*!|fVk{}B4C>(79!j;h6#7?yx4J?hW8d;uemK?tl+Ch|ZNrHu8}fha7#^b^Fr6NUDJgF?k z%#ZVDU$WRc3KTz;zni+2N>-0pDJ)1rOaM9nea;UUiVTY%^cQ~cOcg2OcP5;VCi&r#4U#BQcdLtw({p|84IE3palY@MZ1y~EHuvof z#g&tdn)5h`QF#gHFugI$hq~ zy)~!v%})cJI!!$1BfnQ_BBoF|p^sOIf`*P zU-cfiKdp2><%olbspiD&LI)BCp;IaeGAg-1I7$^#xFWUXhaL;{oG>YHyW`P%?C7!; zkJfmcfV55=(#6vhkXB8$gu>oJ7AL?QN<4Nr2gBIJ_3|rLaJ*!ZXFHM^6%zUY_6Sb= zdrUf}%MYD2^;27E1+SF~;Q(EB0lBqw2(qA&VyVE=py~<= z7?nd!U~5kfKOdL|S1p%kA@a92-WDcskO0^3m(9xmJI+qR0xAOO^iK6!pUCh|a3!T89-W&9y{%2(Y#MRgp5%P{In6r4^jOC18T^_c+)f z@%95>3A{@II4rI7-V%71?_ISRMt<$o`P>y<7M(%4Fxzi*gzgj2Ld4|Y7)zrP6 z$r=4T#!M^0W2=ZTy&jC`#$@>KIG~u`Z3U2;= zLOvG%=sd-l~fMjs^0G8CJ<2Nq$nxYO4H&5FUM zx)n+S>Rfv1=DG|m*bH+cKuHvMNGVlF;iyd5BWFj{iry_8yqd&ZbwUD9l)nX zVyNH-5(W^K3^uHKp&ci#3-N0tt4_qm81Slw5i_wMA$yEi9CLc< z-ra`tXf-)n-BSG-$xGuz~=cz;){p;KK$J72+K{g@Gj52UXx z>_AM*`hn84WD3ogLj;0$wzVAtYoCcLbOWpKw6b*+9m!F~oH11KxJ_XK7|Sa89R4Dg zAM{+QsJv2M9#c~mOFS-^c?)Pty&zQHL{8Ka2Y<`Y>!4&nHEvC%82BsLKROot4QPSU znV16riML0lM9+})T}>qnxFwc@>;}xX>b}bSi+h>v z+~S8kOEE6BQs(e%Z77+8r5UtRdV|PLkOdSqvl7H|OXWV$-4ikkg+QSOhm%GcYj$F0 z_aRHnf7l`<)0QFpmctUl%0U^N*~&R0g|Sb(!eE)#lYlUY_~IZl6zo}uSqq41j?K*_NyPKp>@e*_(+`#ACsGXvlp9xdL?{b}ZF-juLFLnT2O)1%QORjbs@) z(ldA5H8_7_CZU{D3^qncZrk7~;FQe};oAnHaQcxrf5aRLmW{kvnPOW%XX@GXl*>x~IV(Sx3zaI1yOLd0wZ-Qf&X>M#=gFueaT)@z z?MD&0UFFvq&^+Wd^V;JUL?cy+lK4=Gpqp}D5|lD`JAq+$bX;(Rguur8fTmSo#h7JVXL_GHsW6s(mwED+^`U=ENt5bW}DtE(Dt*ER}F(p znd&;Rc77xHj)WuxXYF{*EJ(P|86t$TV%FD6e<4w?mUnM;tT($?Ae!0nDn{2jj`?cm zYEF9$Bv!Gr5~jXq8}BS+Yyk_==lQT04_ zxd}M7j>QI(Aos0+(3`_aS;f|WK*V0tDRV1U za$X$XO2l#L7@KkB^6BC#5RB1}Z3#z=Dp6D7(wSS_O3u!$1o2MLyVanTmx-J6eHMKP z%MuGm3?VmH^UY?t-mr1K&EbDM=4d&{YXr*|_PY)hdB;u*Q7+mPKmI`XltYWM?vR7; z#S)!Togl%A?Z`!U5@w*rLObP{sUSWg$L%;`it|Ha0=TXyaYM#qR}67DToz3%bBl-s zMk5F~g)Y4e)gi+h$&{T0M{M3p%G5*lp5A&M(dKsO5;qaymSYEcMJGe}&D0|Gjbj@5 z@GQrl88kvFA0KO<#J8RE?=eS#!jo<#_)fXuZspPy|}68Ro&4Rm@F@xpT99 z;qEwquFqlH?J#D(Jx~D+%$PAX2zC71lsT~gw0y`*#nvZUu|&w>+oFboGX>(WN(yp0 z%0XOZtEBch7+b z5V+fsoEETOkd#5b1O{|ekYouS9dYVtI#nT^V1z~BD_bK8 zb62(sLcoS+p$^3ieg%w76l#MPTUm#QVTsRlgAdP-NN^a@xMFtku|1Bs%q$#Xk*iKZ zR|R?F#6%C$3+iEjdos*WKd`ms2nMQLhm?Rr+%PS_h*&Jyi#(41q8yN97#w-i(gjnK zV_Oi0arj+#<_8x4pl8gm&jFJi9f2;E$>0=s3|8_ovE>DKKoyaGfdCF^qlT6X(mG6S zj7qq`nY1xIpuHXgcYp~5>teH6nN1Cg06}k(jZ&UCySdjDMFb= zfhd=VdB*|AiNPh(xlovH?v<$m7vegMN*XTTkQbRMu;OqfQa8|PEb$XB$8ARbKBzVy=8(hF5ihU1levpsQWF}9k$TxQwwR{3)9MOs7|5b=kT zqaTQ&VwxKCm3hM|=&++AwlXlst~8!4?{qQ&T8Cn`bj)Jasus#Pb~S#10-32M@+p%c zbALoPhw!?0>HVfmdH1nya*t2P>ax1?_{5^FU!xS?$EMQ2Xxi7xwzOo+?*t%Y_u4yhrc{b)<$|h7@R#U=TpNvKR?f&K0V3K2llC(+zy7L{QZL7 zFYw-}zF*V(_4(lHe!Wf>*{fx`Tn~n4fIB&VVxMK}x4)$4>3k!t|B(Fno@mLd*-0|| zg??|AX>yvZ*URW1Ghf^o{Bt}QCBqScu-?zFPbR~4lHIQt=qN#6o)2EoFpM|z zX*N55Hh7*rA3QT=3C;lgT;S)!JbClk^5^qsVoYa=5!|y+U)&g?0bQR8%PX@Z&=`6cV2cyOCjE29sPtzyosnnUAr>9Ap z+|q2H(xQGLkDJflzDg&X&EF@t$;sdrz|3r(rO9CUgZ1NR*}qcqbH;VfCa3eoB3b{( zcVGYQJQ*!Zzp~MEd3C=9?6bFDO;!KX>CJ_}IenO<_sO|n%M-$um$b4(4a}BrWWdAs z#T;?6mahOnz9ll?4EQ-%{xmp0PZ&A(Yy6Z>r`wa^@QlF=OFh`9eYGWclF*Utywa5$-IY>DP*!!q$cj!(2|==?tshTn`+-))e*3i2v{;4h zv|d_p_DfNlLe_eLGb6|N?aiUftId%Nhg7r?8@Tj^+5~0n)mxs?_4*X?qu)@0~(C3X35o0{Jgopx=J>i;rk@rBu06w6+3Oa zaIx3^_v>%|etJhoaYFCTiuO+@_t^|{Br|l8KBP~d4sI|@z>AvCFHbsZH!q#KO)gOD zf@Q#!6&#$ ze0uTC;{QqBet(Bqwp}{LPJc??zFJP{IP1WVJ35`;EZ0+gqFqz*&2AHUv4b1h++W_# zvy01YL9JY+^Q)hBTDhLg(_~uSt0i2?O1QEhWS(UAo0pHWH8r&1_&3?<^99`!^XU`9 z5lxQ}xRb#bphAqn^W^!!7}MWucyEbp@mdU1cqfu`$TOy@91 z+J7)P9=G&h3oDO1z&_*=^qbuy%}dq;^Salp3rNSJsp{op^VO2(QBhyAeAo9hy~WDD z=F{kJn3oe-*W|oo`fxbc(S4)re9tqsQkz&FxnW;^|N4u6oYQ%*xT5p)`+t1(YI%D{ zOS8xb&7KdQe*2f#umAI#e|%nFBtwzc-+%V?SKpn#O*VtGefM)RIv7(ACqjCWFim)? z*1b%uEJo!mJ1X5OrM0KR)g$^fAO9x}bWcO^*P00Gg8TY02%y7{mq$6aJ29X%4WG|9 zSNZlU4gL?((|?t_sWQ!9mMEu>^ZW?&+(oL@t%KYqil@aHOyqIxney`UuU^0U=I?+1 z;+34$qq;3Ks#)OS`}a6PZYNjkC2^@2`~cC{`42x9f;eyrx(K>c&+6(DIjc*D$>se# zonBNRU);}6TF@ZxidfBF+-`1~X0*h>l%`Lbynyfyp<`LJ%Cg4~CE3pFv_Vq?xtMPL})FH9w>RkL_IUzofKV>-5$XU5%RIz?V>Xi~<8clYy| ze#6=xdDVg^^q)CRt|#|tR>BFvHbFo^I>CvN@y~%O{m+3hVkFG*WP6nLjolH=I9b2# z*_bMQXk$b=Eqk_vqK7u4GROC9NtHgdvFT)SlMs^kTrDo$-PR|Blhx<#11_7aA%gvl zPdL>|pChhHZV_2hy+;OLy#AK9$1}A(=+}!+CI^Cg{doz}>(37dM2v9>68*|S_1UU7>D#+m z<4&!`^U+%GV8Xi-*1H*Vy^q)X?_=pgKT2h>A626FYeieTIOP_H`9}RnyL|ZVTDM=< zwfohzXx0g2uJj%bV5=~Aat?_=_kqvPKX00DT*igs5*$2FpFb<@r&zvY`n{1}$ zbvD_UY&~G$={iZKmy@fX)aK6dlbbQ1$t0X6p88o`U&_5uD=e!UV@s9$1$3ZsV!3`w z-rnEsfYLgdY&PF$N`)z$8R(buXS7Du8r71bL5EdCK5BHzn5^d$LYd9gdVWa*eS7|l z2PRUjn$sztb;b&XQbpmd!7`~e{;p7vhSd%KE4pbV`Y^o#&~*RC4Pny8Y>PbTi%Tgx5V-^ zGC@x1m0A?T0%K(COfDZoG_^*){a<{NO+Up9(0XHL&!71;-tkG^KShxD>z%XPJGX!D z=ziJEWaZ1fQ?uW>O0E04eNTHWdwOS=Ht*2UKwZpFXq{0^5MQ1Z!hf>gpa##Is2)ty z0f2TzIR^r&VVMquRC}XnSc)(cxb1STe0?k8)8eFPIF?KL><;+bmqRur%Cc{r%H@{j zw=Ry$vcgFnnJ-V)a#H+_PO4L&<7B?roD8b>+S8}yQgU=|ml-t!;;XU^iYm}z?bURZ zWc5uOmPEt!m&KIs=#2#u}#htwG*{!+&D*^|!&SXCFF|?wd!bhiZI#n=^`zW@m9Rox zon#7+z8*Y3v7cu$NwgVc<<19mz^v(-DmL2S69!f({(NwHgF>=M=PXMZsU|Aa8;&*I z;-F}x+CUjn3Cy5nza={pQk8?kl2|5XT{kI4EdSI{b$mY7P`3yi>4lEkd7!0D)Pssnt~e*HTh#xO|_&ARpslizn;*{S;MpBC6&6KuWuV~vCk*j#JF8v&#&if zgIix{X}n$D8*e5HBKwyHB1F8TvUlqxG_=x#S6D?^z~Q*gbhB9Srg}JehRT<*O zn5^l77Xe)Q){q89#$B3BHVMtzJToSXsX>J3`YnN^Wl1+l3CFFogwQ)7^rjvY1yZj= z*x~gD#QwDCXO-Ey!G<-zF)1UK@x=Wj#Q10F^77;dw(i*w5wf_MU%x%s?dS;l_|d=^ z41XN`0%meNnB0j6G)?L!|Fc;x2JeSwm+R#lSe(s?V*Kh0(23CmnU@bErzG% z8|Lcz=9FR36+qJgPY1?vLimKciHG4h<7W)+gbuUOc4ZmaS_%=vitTc?KBbN(3pSz1 zX2gwt!ZfaadHWVi^}~JOq1y+L`5#MN&4@$Wu0}~KHRNuaz~V` zzpVt)42o4;*#TS7K3bMET($jQr+jXG`-bQNHyhg!CQDn24y7z7GaP7eQ%BHwFXM7(EEg?EPqG zay8BT{xK|S%6G-`r@^pxsmn(7?Wf@vifgU?h-*f`8W@KmV40nJgrTrm6X3FoSk;=T zYPY1Fr*ikerq+|8z6f?rxyHs~33IL?pT%dwuHmTr=JJ1t2GpXHh4yG`X=JoGCB`Q2 zy4nUK&329;kEK<*UKB2B*-Jv&x7Q2)Fe<8Qb}E1K!!}sc{;}fHRh}5_{$*G%wyV8t zg*e6>ysub~X^7iRg6RgH}?T^gX#Z{Z7?Z0LF#YijR>{p+uG@IwFm^;grTAAD1P zbA1AG{I}?laeKdEmf=&Id7&3}V7!@Uvn6pJo4d(Xf-W96rQ3(^E3TwFu(wZLuhXpW zJmju*NBF*d;3KqYA~!ue8G3g008gcAU%FkE>TUfGK0bDxmX1Ba){p4+^!G3Rv%S=! zw{ukI3hsN)cDO#3y#v;xT8tajZRjCW%y;YLI-zx@Vv2JcrBwEQLOp=6>RkUCDM9i@ zO%3&F`^&Q*elhm=zkzOQFdig}!Dz3##7hwu&JwDN@4q4l*OPRU90ufWzPKR>+4}xK z0H6QIcgOYMiYWi}g3KnE-Q$23X=(R$#z4_fQ~2c-#^46EmC~1xHAi10ekT_5$jA>H;tgcBp#- zc(+x5-!tU6>X*k=zdEjZQm*bDP@Qhr42lz~?(bRo8depV9CtTY%d;C$5{K=HG7>(* z7Sm_wBY(7oo!ftPgt9pzH#=X0NkoGt*56SwH zi2I9MAkOh;DlV&|PF99+J0a|uJTMT?It0Q|JDMS6%loU@9oWNcHoy95AykQV>JRpT z_5R0aqY|OYxO}0sU#0h=b2=-TGd?KYu;TCUlX@TAPS)vs(IVG=c^rX1KDc_9-ApHg&;_82L+GmC$s(wzB8>oW7-ZMk z#b&x&m%U8P_43W2yZ-&=vK7GQ{_bwM&NlzhB;a?Y6Z)jZiZQ1dHL5Y<={EI$W9l_Hz&ih8k@4RSzDt0CBH4o(Kj9Dw)W$f+cCSvp*ANTxRbeS zfrAK&gSJo^<#A|Ae{or2=#+J0vtgjRdxVGkB3~ZlvVehjGUdcUaVZaoq!LPoMNGJJ z`z(ENo}H!7pAQ%CdY82=0x+fQ5syDFK9O&qw%ICn52~=vU5pqTzFK4_>*1#v$ECB| ztZsYG*!jBb-gWAVi0Ba@R%{=_tn8e=!Q@~0wqm(gfVDH$Bd#6jD~Sk zS^lF2qCp)sl0HD{hqBF89W#~!)-i+W2c>>Abu8yGBdWqXW>ozlHKAOUD?1A3Zl_1$ zTNi+~1B_`ZqDmj-5Mx&E5stD>IO1%&6Oo~oG68*I+DLJgI|;qORr={*__9>tLP!%u zk&GL@bP{1LWRTpUUH&tNHrzOrUv}v_&i}Xlnl5zhRQ0lw+GN~=2;C!9=K&-x;wrq| zp>2z?*MEn5an+gWWUI^%){m!J!=uCN05vW)NtRK!WeT+qj(#bUo)U%FcBf%$4K*1q zDGT#@HNB}QTM$k|&xzvgtP1UO}><+(1I58qDk@!^-7`FJY&lkn$RnEH6L31TX>4$Fa*I!S% zW{c+o0M=LS-kqi57pk58UGW6S6fV^>6?Ush(DJ??5^oCw+wU5Yx3wd5O9VlHP*mC; z3)S7Q5-s%@?&tsU-OH0!A>8c#c0QeFZ}*pgp>~PDX;YFP)`1S}=xo2oH0`KaTh5D3 zLTgzPDt!Od-d(<)uQ%CI5K05SkPdyJ6~qmoc5CDvIC;{%G}URT?W$VeVUh0_qKo;t zbvA!dGuPW%@w55!=fkW{t}z$6#=M_gL&7Z-it=i-)%G*A$AWZigB}wl3o&p8wK%vv zFD)Ck4+{2?ocG0`k{G!ajjUnYF7Ll_4#d^HH^p$s@kIJu7|N{XWm{TjDsew=^R^$+ zCBR~?ztJz5uIifup{v34xd~v|^HMkZE&V_@#?=@0uuEI;^l6_t%fk1FozfQr zb6m=+%B*&kIl4a@ecCh>{U(X8_?{Cab~k+|hfD9yhkAWfi}~<-4Dhn2jD<~tTE;d$ z97qoxO~1ubYL=eMrA?IjE|@C6yJY!?Bir3`*?#@C|CBs^+B&_xKrylvfc8@QQFqHy zwwv|jZZ^N#UnJBQj=Iac2X#GJv}hD2iWpk&vSHh{8CjFEZxeJ zR*XBecK@TQP?UBkK@aO^Th;DP7G`%{8d|DLt2a|-HT5KOpRV=-)W753zy55Wp5JdS zAF1n&koKAM_8YuB#*?01yIZrFaQhOCdID?Vi^h-Dp-!nzXNP!5baK?+zHKqoK6FpJ zVmhJ&Iza2;UAS5&lPsCe*KO<9?_w`&NME+y!cws91nbjaVcpvQ3NBg1wp_qPseP{B zdf0Ybl)(0x=t6+y<|}RF&&}nQJ-NY9ot$;YNufmmE=1umt*9u1qfXn2pWhPwLPUee zpSxQqc_;LM{%$|u?@F+{D0%R$&I=Tp{5(exM*O+>?;JwkhK3_|2g+aiL79}Hcso!& z>j&ks48`Ap@~R({tNIZQhsu%Ba|uM08F=+#EyFEku=TXyYr7AB>$u*k}%Y$C*7qNY~v9y6>4C@&Rl zjn!Q#R)p(r2$ET3?I~Noicv@}OWA&5`|IOOw>7SlkiB4ed9yo04ct8TjboGxXe7xoSG4^Kwl;<>XxS54Sz&XEF?!x`k@Gu2F9@ zmS!Gn*9^5Fm3kY#j1L0;#NNxerFje}St_{KIJR3EAKXgEaH@{eFZ)@GH<{s_PZM~x z5uuwA$(WpUwl^Ff+2CN%2iss%(Pj4!H~779j4F)x1KP0^Tk6X$*gAT^UY%Bz#`^>B z*Ll`ib^+&*1$%WHHO6~_?db519m&hXJwwj-GFTZO(GU%)&QYe_9&g)RL3`EovG{Oa zWKW+yInrt7QQT0v3`2SY8rXxg9;jTX#Jfhn>npb(cIWlSN43;*hq4m8b1zjq>h|2M zs5?Hmg?__mRaF;d4jcm293Ke1U-z|kx~q|GXTd$mAgMh*qLqHbY1Nz-Weyw$)f^uP zySiCbI_;j1<>(+Kznw!RUKQ6s<2OY3G8@JuzgRgii)f2EU9=5?s;aF>y8g>sEbqD z(FFAq6HxJs$5A|uAFaKHXbMdVuU^w4s;ho;B4sZgK~pw<%=Y?Bi`LM!%}Ghw_Hr*q z?HH}z`0-k5nTLuVaocR~$*$R6q3VM?x^2?l9XMx)>ng{O)=tZWRaBN;%2Ar4idM5# z0(;sKckKTIMB-a`d^rA4!?0COfdsGvI@uM}_ht8~8 zKeTdNdmi`6!3GWEqv5xVylgnpK3r|P(Qmj%+J}rEr;*ly7se<(xayJ}XRof++F9;p zFFHQFoqmFXihoG2#u|J#_Vh&=AJbOH&~l5OmnW5oHs5a&i63X^fVe%if_ZU`T5VQD zue2|+ZKE-dLglFwj<@$nVqy0Eb9W=;8kVOEhV>wpDjh}1i-C*sEU?a zidLkSoue<528A6JC)X|oHTrx$!CIF>>C}@Wis}&Dt=}rzQ0muT+e%f8jM|HC+wruQ zt{TVm>cr>&@m+hlZH<0==^bN*UY$_ZBE8DON~2f7T-V(XEHI^4N%ofQv%I1d?N#=A zvS=??k{-1eeAlyF(z~3#WBU!de3|c+v{pPZXitjas z>fzfDtBRwty?ojAt53P_Uw_td4D86ldzLS+RL?3RSMD_-1*O%ic$t3biTxG54I+rO z1xaZ(B7JhcM|ifUPfyTN3t7@(9?z%y9ggzLhi;<;(=zpArt!Kg+T^ zCW!7A^Q#F8vdn=PyGfd?v&I?x?0)n1^z^i8?p~E4ZVb9M4VAkfeQQVqBjYYjCYyvN zc%B(Vj~VMETfaR;v}icqp?4M48pbUOy+Ji6)|0ZZO@{U4+3n-2t7viTbd{_?X5PvTqSGz#o*W_anPu*uf{8COh|LnQ`ah5$l ze>NEHq+);e{1CDANI|Pf%H9I7LprU2?xbjc_8jpdzF0tpd~%#9dN``igXFYt>2sZ! zc?*UX5%+aCY;@ZzmvGU3wD!evUW!Hnu}I=)vV}U0BdEL8e%0sdJCjF zIpsDCNs-zg22PoBO+5sgArmq942R%l$o>G)Z+tHzqaR@M_w)Jfe-u znl}8JHm;6nV^`=n9PVIa`M~bqM6;AJKRgfU_^$l0fTm@4{h>7o2{p)Ejvs?_`56>>w%QqV$ z3g*Q4ef7m)`0KCbH(yMNgB!j)Ia!>qho?xkUY~+}v^Y&Co9yf5l!anQ#?Br6oZS%? z{QM;qKKWt&76EB<@DSpqP6u(_BZ@|rcE(XBmGE!iZXOiFgaILb) zS2L7%z*nL+?Z)+Ld3$%yp$~OjEC{$Jbhv+#oMy{xlK#7GS!aEtL*A`#ijYCLsy8Njso2^}KNtW9#4(05* zO~TOET5zd=8v^&qpd75TEWgPS;E;rR5!omE6hhR z$qb0RGs8A+Z$G)6FM1E*-3J}Pe?R^JN+jjDvCw6oLRTMjD*i&m=;O^s-7Y+I4Au+Y zU%%h>N*-w;_>_q0|Mnb*mWCL*8#}|%>!Jn~$NG2Y*tYoi-(z^(1w;4YvBy@DeEc4R zqvq$g_qS@0(v#XDmWS7KXtkn}!*|)SE6Ck3xIaeTq1~Og_gJ^NyMu>#YFWoDrS69z z{n#BD;2$zNVNFf@{G51~&Jhs-J57H6=K5rC3Y8YB7x`$6%eTgvalKsQ{7DuY)^8V{o;;}cqFysvlA9*a2NyT%<^3I} z`<`qzean%Mc56UY@thXY9zUD^f#ao#Hoz@RNJQhl3~>Ga56=MWC+ zay4GP%mBe?C)&Tc&w#-62`uucf*lYPm24-7x%^i0D!md30GI;eS=Yp2PJ~)t61850 z`OTg_Jz1V-!x8-;u5LZQJsB$g4_}@>)rozU_Ze}67vJAasdp!XAO7vf!E+koKNn>q z2W(DGM*ym2alYG~*TO)Rs;qUDHC0*HRQXa^*%7|{HOUm;tOOh{)!*-Ew$2C7Pm*)` zU&5Nx@Bi@??YbeM8J%fY)a8F8uKs^HF4ErV2^rQ--D_%ak?l<2@R!;-Do>@1B%_gJ z=S={2r=WAzTBqP*!_W_$2|czRGx54$e%mmvR7`S&J=v86cZsZ#$(AsSruzpijWF#W zU)JNI!oqRG*M^~0rdC+zdH@!ToEnUWXkQCpbr^6@#?73!F*c|Wb=VjWo^LscWka=_ zD#k=AOPXf9rYPkN^1M^>=^y$9IF#6MLju zzeqNB7h057ey5~pyYpJEkxM}!0vWB(d(G2XqdgmBDeF_BJI_6fZfRN>&4``oUeJ<4 z1j;rM=a74zE|za-*o)`q_Nd=7i5PD20T$-@iD#RO;q%&>hNQR$LE7mmLqAnzk8QF>3uqI_m*g*)}kuq+dX%X3+5~5XhepcP%m3>Wrv#Rj#l{U@< zmTflkDZ{_mTul~l=GXIc(;o5L&Hd%}hqEGeka~#|foG?F&7S!}2 zY3M{&*_k?DqyBtS-CWxro7eC!-*PcF#;Qvb-gWTg^(9NsepsL@D+Tevjpy|{FzGcF zUa$dF8)jA~uPvixJt;*c<_jVna-snW04h&>nw*}t^&|Cw8b%xcNo}aBzWhon9(D|g zb%6^S-&^^(jr=-@bbN-4OhXnBR&rC;xJmo^F=w ztT-$g+ZCgeGoH!m>#zR$_rLsY_#%Im=~pjJd%Rol)0%?z*m^%aBOdDnotT`bIjwR= z{P^T-Vw%GRa!FTP@I%!#pc_fnXSE>DDloa{4C;T?gKg;63u%JN5A4CY0Q>6o#kc?X=DROm zefP!ZbSNyg({b@G7!K7KKCQN=pDypSjAPdjgLVFlkk+98Od@-3C+oM*jOF4gonQTQ z{!C{zqMRsI~jaS=s{alD8cbyln9TlS?tcUfN0H0v%C_q;;aLd!0 zo2vevs$RL@RP=4TCmt4{`Cx0wrrULG^}KlMZ>_zroqcJaJ~xbXVmRKp6HYuUsMkCG z=XyySM>atHw3o@cX3~i%r{8+&Kh&KsjOkbrji4DHWntdeQG`O+F=Nj(rrL|_opkdz zF}NS;w@Uo9xj~LO{Sy2L3o)4*Exp7()9;mM&4vCh(ny;5Bzv=5|HRPl?=I$>iz%%I z!s$(y7mHsRP47AawrELBYFKEx!db8@j&v`(&Q`s z(48nzu%)-Ve7{~XzJ-xy^n$3q9Ff^#gAXS*odPw>B!iUKv<=m`HI;VwnK7S&rqP#P z!gpNue83bB-(AIL)`7~)6D~U9qQhs#r>ddl9rsdh*=P2%!I@CfyYu%$^5dzB)F@Z| z$@@7@^%{{-Tqos!e zM(cCyZ2h8s(!Sv_>Il|n>*vphHET#@TwCoQ)<2%kr_Vvf_bB(WJd?EgLX? zJ-Pbk^?$Lb6fH;&;y(WlcJV~vu+fYzgwS99=VnHnF02o)*UMXB3sglLLTe%n?viYt zti9>vVZOMU55|7w{b20yZ(_C5`C>43OTT`a-(JGO&Et2Q+a+CX^8~{A!8qV|EZiRt zUcP);>eoFB%`d+HO5^WIfwu|aPf~+F8fg+j8f8LOzY2HDjWR9s0(&stQW~7s&Pg|p z3`a$$HLMGPO^n5SG0)1rA}3qQ;K;WI4@&N|>h$rqRk*SVFs9C*9^`+l%If8!8!$eKz=VP&@v`kEgH;M*i`J z&(-szEL5@1CNKUkd+*vD$8jYJ{*_{TOrV7V*iSsV*+TFs(U+tU1Z7#Gc@YJ40cf+k zt5wwv5)v6Pw&(R>;?C}@*N&Ljy^p>7VI%g7#+Jt(*^(o6{{Z1}jcLF# zg$fNTae!={Tqzb$R=h&7U~CdFmRFvI=d($kxRON&0eT!OV})bDjxnk| zo}#E8AR}PgsZ>sTE5PV*G2O?9@Db14wVmz8Baj<-*5YkqwWR9)`Lv()$(H}@115HaF z15Fo^G={-pI3(d>njTv$&{KxpujLAfW7=_9%4K(qU(uhXTf06@y`@STr!uD@Q(b^H z$bYKR`{SxOLB!xoZltB~Dac)i#7QOo#`wN=eQ3*}pQm!@mAW4Rl0=z8-3V|)y?hd} zZi>QUfMK4xt8p(tuTF;n6__C!3(tUt_dPc1zrq3DR0DkZvT`pSt@J+g(J3hfYK+dD zzB-^BZUA>huxN*tbZnn37rdZy*Inh}oH0v>*~*&gXstqCv-CzZ>9gZ*+8&+OdN{lN zbV90ycQeybOKZ``@F_vnKMbi{8u z$~=rb{-!NK?YgXx*|TvE$v~|;w2O3yuw&!i5h&F_TNMz1)Sa)&qECiT{TPW?yt`_g zOI*F0$JpEqNH;Gin@F`ohr=`p`nBY-vUWu5oDFI8$ZZFS{iT}nZgJrkfE8}M?+5Vj z4}sPRt9kwD9$M8m_F!u~reBZY*AIIjY+)QGwRCuXvGHJcjQ@6sfM~9zUSjtXI>o(QfST#`tgASlny0+i6?k@rHV=xAo@WbG^0I+HP*Pwv2W^ zs7eeOAOXe~gq3~#_GIu>2?a;^xz8U*y(9mi*MD;K)SvY}AM`s1y#q;&g>v5+^+w{= zt8TI}nI)&IGvHf6FA%Q@(kDa@dfSb zjD10?Iu^Thp+{ffv$-s6h@( zI?N(@waN!L+7GAV#uZG`;U2uK_ppf_88M1o)E#2jr6ACf-Mt+gTVyboh=vz| zK}k$K$-`7Mpyiu4r5(-XLt4`5ovl+kZi4>2yd!W~58Pru;L`5IZ`Bdlc3VHmB@D}M z?MnmavR4DL4P4&cfaQR!U8&{!&tO%P#+0K z6gUqPKJ?LVxYMl#zQ=m-eDGNi_sKb~SMx`~XY41(uY2vT&S8K9(FOU;y#phMiAvm( z59JZ4YX`Hz1$ScR&=W*s=d6mTW;{)%;RGExD>@-uyh?hh&J(@&X)=`}vMoxon?s3< zt%MN-{t~Ui2*bL9F_Lj4?Bjl~^1ZNz|5vC8gFH3K~C=kV-t+N@dYD z7At;{Wk7&6E&#B`q5x}L0>B!J0jzP^0Bc?VV9iAV)+`Ay>0ewXbc%4`E+|uQK~W#n zf^1BKVJrV$;8I=BA*01TQ|0QS>#$hyiy~cXNu&$MeT{aPLOZicG2(UEZ9n53)D3Mv zH#C2tFP`nr?1$2^RPj_gE3c$1mK;t8$S$z3)3>OIfdc2owq(l&(+P9p+eqFoV_eW9 zEEvVM(85)e zYx6_RVKAPN&jHMV(HX@lzQeJ$mK9$3`i58UaA0%dc&k<4kz`L!BbC1F#Bc~Jx1gq7 zm+%@|SB+KuCWY7RTsB=zyK1fismsoPZS5jFsM*}QadQjyl<=YcxQ{ko}3IAy;&(WK7Mo(Vy zW}j~;ef^XIl3c>h#XKIpe5dIxCc67oZ&GhU{Kl*0wBHR zAL^3_^f-)J_K+~B&I7{xAM8JVc;i6<3~q;8Yg#`4Ydw1^y5p+s|q3^uBEZKh$il#mH-nlL4{@)npXLhfi4$V7z}CuRu|=x^a&jZa|l`z zf)qWNo-e;>A3lEn;PJn|e}-eVT1%P=0acLeQSR6-OK zrc#d8xdgM@E9EG-0@cB-_wU^L5BYqM^6!Z-OCUX=XvtP=oHO&qwx-kylVzBB)6L8!cP+ok zOP{e??A$oA_XkHShXkI?2XnyO51$d-$u`}kmoG0guxl&pDjCs)tx%{VeLyT%y z(rEDVw#ny|7OUUUl4^iRM~x;=YEOIm!^@YSzv`w@?CRa{y85gKuTz0l@ZNCRK=`z( zKWv-^$p_EJdt|Skp07?3Q!CNoPk>UOWO+UbG5eubw|Aqtaj$KT$NXj^>VLXrH*zK@dZTZWP<}7CTZWFzw)P2L>D(- z>4rNNrc59@nTQM9iDf&tPJ=kpSQ5sAFxd|&ek?c7=N5E}=@A9NteYPO$%Da|KKE+; z1)Yo^L3d&NcrXed42Hu&(yP6)XVCjR4uLV=#Ql}XR3@Ef*z~pcohvn*JJ8Z;ge%U| z?P=IZU7|~%8uBQ3!Q**21I07mco;ksALKyO4`$<9$9flo<5ks*ik@J0rM~0h*PmMb zSzoO^Vz*6uLFP z!k!nWdx)=IO(zvU1&F*GcB}FFI_W;WcsH$9h~&C+Ob~rrk(hDcHG%9#cTC+ROwfW9l&obT;wn>#=Gk7wDTaCp5hDh z3to&f7eTKs-lf$CWAh7C&)Mj`&quT!_riE@@FE;`8a3f1aeFe@KMH^!oWPdz&tXsB zrKl#bEj#%+LhcTu0O^)WF^)Z-KL}ndU0H$v2NQPczMPYMCfRTj4N3FqR1U+>FhDD& z-x-GcY)2^PF_6`UH-WD|2X+tCWmU)MKCqe(hJCEmGl#8* zzsjJ;_+7lUonq2>t&WIz9ngBbw$>G?>l|o3uD+`JC-I=)`TQ7`Wgmr(PUU+yZsGsI zoIt{T7Rt9<^uLOaBFaaDK1`?XC$O}W`|tv0f}j#hi*Gu0Afz&s)joTUB>;iLC>7#=FNk;=r zq1kD?@=v39@EMc~hCIS2PZ23^j6n&ShImmImhf;e98AxV6MQ-Y$(BDt{kt)q9J^3w z^*Knf(en*wjh8RQ*XQBkvjH%nqB5iCv*H5Locc-5cU_1Q_%5mdcR`jI&8B*ivT${E z96t9jLs=CCH7vsUyEXwqHemgRV%9!y?IX%`l?yBzRIcLYD2b`ck%gH~fnlWNWS>NY zJ!F-zw%CHAEe2{D$Sd#(o6#6ZzmI)#gVu&Kr4gp9pHEfkPUs8arsEWDatDd<107L{ zMmopG9 zN#R8mqUfk;yTC3p;1(2W?8}hKuUr$qdezKA=PF~(N?K`%9ImJVGi#w z2IlcNx*G%8db|KT)POi|5=dqLY64{A)2G7Lfl8^ zz10lnB|zm}@~=c+TVl9+l_|y5t99UYg6j0cGnfoX8IN!WN+Wm9XAsWP%wg%U5o2yl zr||$Z{Sei3pe~dvt4wK*cMZiEn!t}v2IF8zB|YRxB( z!T5SR?~pXJKaDwhnQv4kn*vqB=&q9&72g^VYXKiVUXChqZIXf^kUTnu?to23t%G@J?rjbE9avRPZx z$+ZEbx~0BnVS8x`-dKO&bQMmzM)@g3a2<=L#X3#vvU)O1RU1m}ER>e&Zu&t}czmt6 z41+mLwHwOuEVJ70rXNzBXY6amDIh4{VhB~cyHo9mQVFGUhh$7 z!C`_sV8hTw*2i*N3aWTcOL14M1cNQ;6%<(oWo*o0>N?X`)$X_#U>C4d0S@yO$0KAA zUcGun!a}AepRSMB-6k48=~M{W+=Tn1a>-Sf=9j zPLyXHa+FP8QQ5N;jSYa8)YMkLWT@$%((eKlp$FJEGq;`QG2`vqg`^5zUPMAkP^ z&yRYa1EuQJ{9$-Jg+IiHI%rQDz(Ntv^F@9L`I1@1E5N4ddc=(DoxtkfAM|XSG3>cq z^_o4& zYDX&~AhDiP^i@|%OE!^S#aO+lIK%d4oqI#7)W|cv%9`<>)oZANS9YkbI@y3R;41nN zu;dATIShff@4*aVFA2> z9>t;AsZteXr|JckN_9v1S$pNzu8Wt3!K3=Q0f#g=DPZsP`>q<@RLQt{j9iw*YS+t@k$C6n+`?v}kqY*W|?LDMd*+p;d-|l!QFQS8|94y?XaRPRh2~ zbPpsCp7k^|JWx1)EvKS;*fTk76KBLEYW_@{gj`K}moo>zcF~PtS;7=HRu|mBqhjP# z4V#z~Y*^N$t5-9xq-9=0^#tUalk0iKx+fB>lU}CGlar_Zu@3FQ>V)}JwYt91?wTk% z**J=ZQT*~{YB$pne3Jft`Ldo5`C|vz2~h#3ymZFtwUn{4jTS9~v9my@BubYW^Mg?^F11ALhD${Y=^Z zQQwqL-F>;DH(}SFJ=sSiY}J3>8}LK@DKT>7J^R&ry_+fQZ~D(gplop^GuSZ9%p#c< zP9lga9raM@s0E`YwCq#DF5Qur261Zm=GH8xE_I%BXYFS7TBGWJ>T7TLrT^TwTgGEs zqvKCCyure+?!eqW?LB{0*8sJ8n38qcnIX-l+LH=1;pkh|7=tyR0{EMoGTLxa`e zZ`}6=PF%gpo^!Cy*89ADVGs_Ip%q(|)@&tATkgf98moFXfA73BwJU;75B^Fxo z@Ah1fSpBT(-|Csezbp5$3lO8VTWgKZX#LiD!=JO)T~Ji3eAiFK+4V-XV}4q{rGI?& z>XlA^L5IhQs5zKcb;I>V99o6f>$TlVtCVU<+e_oG>y0F7U(V6ZZN2jp}% z4ehspu@RlS(7}s#jxvQuVS@RA-Nm_+e3p|k&vcwiZ7fA8Mi zyZZ-14V?gi=vA&&eDgJ?rbOp2@pd-BuxSU;_#_&PPmJ8$TmnOE0TkSUI8+gi#DShr zdN%A;$B2qzT5$1o4k=PmvED25YEZt-1H9Ec-{tOM8@f?1Cdk}Dl!_S>Kv|AkE^X4o$ z-`Lp53p4j2wT1VhM0373;rMjVgTa)pmQz-CXrQV`#0a&q^03Ci*&%wMeVz@1gIDUa zs7dEs(^c3qc#eZRCk6!=sc*=cxZcglI42yaS3H$!?#C4e=C52^>58#o*zj8%EPQnZ z@5`Kn1U;4-@6e-B3{A`(ZKe$Di zbg2qDP|z6Fm~|cN+kuTPT)8 zD8g_i$hbEr$~b+uVdnsFsAWRMgO{ABK+!Z9(seIve`AV|lzo%K@bSMd^@p7McONDk zPng@8!IRdENa#5Vk;{SR+gquvc%$G&Z>3&?U#FlU^;UM^mrCaxZbU~%vk4g)K{x;w zPLTK7!Q;(cp?hij$ZMd z=RJAXD5Ds05f-x=)vn0+DzmRbp(KhsbY(q^yYnS1mwYMxYuD)+U+|l z86aQ>3IJN8?8$_<{rwehPV(IAfp-e@q=(s{!wnKTwUOi0*H$VXUl#8j91er=GbMKm z!Mmah-gsFog1(N2(eui?D>x{tJ&{~j3DAKOqM`2dIGA{4^8H%iO_TRxI|2Y@rf;Ya zKeiGa(9ajik1Mj3Nzm`37rWPq$kg*nTB7zNO`VXNs)ZuoX3falENo% zpS~3!i9u(W&uymqs@!A`Fn`msM0+mV$T`BQUEo-yr1iylq+=Le4Evi#33nAu z>nzL7m)Ubuw%R&pQ(2!Aetx^TaOuZTbb-|nzOW6=}yY~3>#_q*8 zhtjKIeu$+F%$GG{v7vY`h>=(Gx2dImQr%{D_x)CzyG$xwp( zg}Mgw>_^lubblTVS&H_Zws<08VIJJ|7VExk&lv>clVP|X^S#eXJsgRDtFZeF#LcOR z*HNsBuPl@R*yA&N4z$i>n*=1GE#`3PZK&Xysr;B2~ALHQ+ySHFAg>H7E3)Nx> zs>SenELDr4R4syD{ABo4w$Z83uo~BUY2YEFNi@MR#=B2|FyavU#d&gLi2X1R^SgY< zFnCf|V*x(FnvpdCSGN%g-@>E9KkL=IXV)V+)iao?JqP4L(UUU~8L8KR@IyKz9-mTytUQ`BsO|!^4Bw;6OC#mm0M{);6VoUBWlo*V49~HEqLHRMzk0*Eb_ganGQ!0abJao3;>qx6Jk7$As zTn;(9m>-MV=zicwL@NT9HOPbj)KSj1vuV#af)$Dj*8<~h- z@Fp9fEE5Mz+|6=C(~?*6jDSS($ziZsZ*2SWzuHC{be>GSthN`J zhna)UB7eFKi^kZfHLNjWOPTSdjpYnUY~gu>(s%m2VKwFtOK;B9hvVr1^Ur8Doy?|1 zqNX;DL=?}Jm9v|X;(J*;i{DxNyQYsXn%Kk3wj50|4bv-IF3?1|@c~+P=vZGTmEzFV zFo5KO!`T9cu*P@LVUf9HPF;D%t0IdPu89nLcR;n}nM$>*h3u(a$GD)^>$TeM>U#Z3 z&m~IPLG)Z@s=iYQ%ZWRPCYr|@(%>z;df0qtN+Nd#^NRAJ%mX$^6idLUSM%QjG}dvG zW)PSC3cj`ObYy?50RRL%iU1=Fte1jeAz>KI0dGv!ibC#}SjN?**0D-jXt8&IW-97Al54XFopQaaSCiRRmf zzO3^d*cc#KSsTN2_bY2tu1DCQs<4G3Mt!nErzP20#QqgFDeE)HUfeNPP!GrHk9*fv zZ@#?sa_{A%mycI}+qUb`r#2Cm3EaKqBq5G?@_NpM!|P0Yg#OT>67NuG`e*|kD$0#`G3B+Rp~}- z{ll7?Y4P2c#=FYe>QF?&Ga7F{tyXs{wGGguI`#*5uiUL@yy90L!@35$wrU_Zv63FUEe2%Ben!->cyq_ zt|j48Uv^L^{S|JXMF+Itj?>2?<Jrkf_E*)SOvObe;EFU@chGxadWWV zMEBNb;rXqo52CfbjrPMSUKQ6ZIj$ngmA%$=?6|@4>S`GaNYMtYRY{JKFnc!CBY;{m+^Zib~Jk=bbc2`a!U~Xw=ov} z^lUIXjDuwcOC|`woEiG&RZvusPjtDm_v8t^tlSx&!1F2m3i%hIS>>t!q%y;t-LL=q z>#yGYxxI|aLMB{ber^n_k>=kodPs*uQ*5dAoeeQH@q6c8#R>Z%3`X9a=V2g=#!<;zuBM^sRH zlP^99A4Ok$H}jy_4a!Q@s&BP>QdqC*T2L3Im+kxZaC_MXUd@FY9*bYS>MTPpM5lDI z{t}^ULc!?c&kyf@h#qMN@N+O7gsVm1&l#RRAx94w`7&p#bMyEQJ^CPapH73)o1e-9 zC#R^9>XcM80((|{mhHY3;vmT5LIeUM@?@7nZ{mr9%2S`Gh5}Tk4$|dpL#l37fk-Gf znX(7d(WJM+j=L7V^eQWBswE0TqHYzetYuqTlLq@rw`f}%J*z0D*=?*)=EHk3Jau6t zP@`CS1ElcG9BPOR>v);B)HTOjs=POhK=1^N@S>%-6aOz7pcUJ?Sb$bK8s|YB^ZougH|TZbDHA*+Mb72JW|Z3^ zo#pEr|JLELw%V$%kE`wrnI60F z<2|ZY?K@NQ_m|vnb4bS9Qe`fxlsHw4 zKWhMrh3SkL%HhuxgyMpu#vaOqQTDUD#k{?#$S7;^N5S|Y4)L-Foo7YkvEA|x=kdO+c6s@0b-Gad4ILOlP5B@QGt`|&YBtRD@; zn0(zC1OR&&mg_-Y;uOaadiz6cKfSDJKDsY!zNNdGv~>FUPVe(@+)2-V_8;HqRBlmX zSCl7lEi3+wy-wu@aXoJjBQ96`n~yt{o3N#)c=80}eLouZJD80Je^BsZ3>muNJqm;V zxi^hK&P!l68?tC!79C)|4raz!8rYEcd@vk()6+P54icgl41;)t^=?N|OkLm)R;>HZ z4yYh@@E|xk9gIV|sWQRBR5O`k`@v6wc*DCDj8~?jsy7?M7um3Ery)B;gvK^#)T{7f z62#9?4nYYPpj%yZDot}LLK=pVK%-QD8lgL`cN|9}>df~B#~#R~LlCh+k($%Tjr%tq zJzzM)R`G^|5lGypLE;_avG6n=oScL)PDny&5+wt^qnJSHXCVpg$<*u5LWFNJLsu1O zVK~K}*H0rHC>F+uSQbO{`>(V2jNCyF3sh8(b{A@A^kKoq;lWo&W!r=f$-4{zhR^9iS+ZzqSQT)|P zd)uU)O!MaM9zz>UZ9D-F8Xrh;3F0>pCRKOKuDcIwI6e3JxGk_eZ#n=s?F7BKw2^JQ zk-Zsgi0JrO4WGFotQSdt0u|Y-35FY=1CTMtOF|HZ%IjDe21+2t4N5mno)2&kX}>#m zzqg{>v%QaBwL52mI-iiw<^ios0p-M)Wk!@D!azc9Cc!m9WQ+gsll$ z5Db$@e$(1v?*9UINPxUKj1vzg;Gui9;4>^c2t<*9q3;s)@Dk6=z_ohAz*s>b80n6J z87}r|$ZH=XwT5-!&geaCZN`l(64!An=(Mj7!V|43Xb)>uSBQwbY`ld;v>rf5ws zI*z=R-U`t*WKn~0f~@Fta5z9foDgzpP2hI9A0(JV1+gv;Kb;K_VfQ|I;PE|6jf5dy zeK=N-$4&-gB1fWOH4-9z$c0I$)|w!cQb(Jh8)RytoR$Q*+wp`2_Id=6&utHJ=`P|1 zp~|U1y7O?#Fhj8fN11pk=cHD-nb~ftixd*Un4+>^@PCWoHCpo~1MjC=OD|v?%uI6U z(%Ml2=`d%!u+Dm2V<)-pGaH^HV@5*j{sh2n5UqU6UxDl>1=xM~MW1 z2MB`ZxhbI!>OR8&eyG*d8fg-?@lm71FD4_dHE1Go-iVK2WAq2Jq+qq0?L|iiA{5E{ z=J+;=IuI{Rmo*>Nn}|>EwVE4suYW!cMj-8g8a$MwRP-Ta22jC){0Z?Bk9DJ7Lp)*y zN&jj{kJFekSi-;10^osYKy9SN-8NOMx`1|M@1tNg5N1f72eAF zitnv_Z-t~eBwmLSSvnQe(ZktP_$3ZN#0ybs1fF^|JVBYHi~K;f0Fr?|hO?v%7@J9Yje)JbXjuc%lv7HSuF(`eK^y8iep&;BbR5&MAl%ooIF@$01wABLQ8A z#);5!As51QnXuyAtejhRz~;RIbFHm4&6KuoAy}6X5*u~gC_;th%*3ABv0h%u@A~`H zDTeYb*WLHA2T?q)(MDUe+Ai5DR={T4h0V6bV4GzJ+vd??d1&-UAA83jQIbG;=Io$6 zVHvIjM@Lt~09BywQ2r1)Aas;NWDFXkQJ7B_9ocdN`()8^sWZcl{E2xOZX5!C1@=UC zV`LZ_pTGl%bKJhSOdc?eFugpIgpOH3*%0$;ssct6sPiXBaWouuD!1h{kx>$T#`Y_| zx{ZUPxAM@6lYcV;CJfIQiwVXq-8?##dt*Ff zh{kx)7vK1|fS$ww79FLK$JapZhiJYS&xQoBTTF9M#uM76IDf(Qyf=a=)9qp45YeHe zrJ~4;SDC9p+i0@`VtITV9AVS9&{~eF6HEplDJ@(DZExX>p4s})sfb^=o~CPxG)f$T zlnsMC1?ndvi1_S|;vux+jY`L=2Xf%2Gdb@NZStU;;Znq*Y+(j%;NjqT_?b5ug7gZ~ z3W=Jo5nP$m!xg*Uf=)F^=M(S9Y$1sdeU7HHa42xe>=a-if6Kr2e0W^N8J9b<1XLWZ zDE!4mq)z*@v>(`G(+KKzyqtzblIs#}3U(%2BIjgK+(`OkoTj%gu;Hr#a}Q5Cb86nq zEN@;xDgGQ}M(^%N0LK`ow`(CV=yrs;STl^Tct2DdV{kmcfdZ`?;69E{21kQ%h$u?8 zoDx9@B1-*o3$%j46yGVZ;B&>l9VDlRyg?tMnJb|EfdMq}ASsvzAmkroSA9B98jsaV zCaB%Pm-Gc4J>i4^RRCTnO#=XMMnA;;^V#}Y1WO!_(OTVdF%Adac*Dz0iF z;k-e{ezk*fLbw!W4cwb_=G?)fXL=MeISMMQ9;Z@HqSl1V>9!jG5i*b1}2q18rv9mNJJC#3!K9Ty_L+M%V$WsSg6?h5}X1!f^8 z;=5}fKDw8}pPoQU3FAYcwq$M~9Ug^y)$kPd$WT3D^X#jL;587wa(5U#_g34r^`@Ax zVrYjzvC4+)TxXxB?1d3m5%)3quC^a-4GoglTjp=l8px+7?&Q>fvB2Oyx($GvBE5R}G7r>{!T4yMsXb3bLVFx(44^-A^ z>@}4m@?Jyd<5f>&)#4(i3cEDkhp|*6y}u2 zA}E(p+r&Opb?J#n@*4dj0Kx+=482$2KS`1E5yzG%@;d_%CbXS}_ne?+Bo@jlqacoP z$4HWxI@*;eofdYLGFr>J?YXKzDFgD4P-M|i)sQ`6{Elf6LS4q(M}3{PkcW55B+r(b zwc=y3kPgCxO9K{SRm2!sAf{I|yE{fLc#rC{m^U;V!++Sf;9EM&Lc0*{oP7WoaKBC^ zA^je`s5~K!GC1yx&!T~R#ga7^;t%IMxlm3hXFgbEnhuwyFgO`w0(<_8&!2@TH2|5$ zQ2hJ%ZUgT1k=Gm#NK;@(A!uYo4oKXreuo`u!;{8I@FhlsLy!Fq87k?A|M+}@UBSQb zHS4<$EaAovJcBKGyJ^W1w^y7zJ4AyrP(j~AQV~q2=xOm8YI$)g^pNlh5azyI?a)9h z$2pixhLfU6=tSsWh=m%bFzPQ@nd=tUQC*Pd^PKk6@sRJ_oYH_gz-#omBy6Ah5M+`; zB0qGL)#9bcc*CL}=n?SdgTtSoHxbr8?o&hy@ltpg8e+!o7HXW7FYEqS>4p*f&=``sR)W*5)fjajh%yuOT zJqVI#bk3hZ8>EPV_Od=_n4<1`fC$e6H0p|9X~`RV<$*nM;~F$E59;|fOMySX1suEg zLm8aSl>O9u>IkC^k3l|1a}Mx}{5C(Bpo{1SGjzQr!_+ClV3jw_3n(iIE~_frOVq;mE{OZ{(x>pu)ER9rDSQ1&QeqR zNC9GxPUe^xcn=@%!L#|jVSR5{-<#I=CVuBL*1I2(kRo{Tky!uRBtE0GACO98SVma3 z26)a;t3M)j1%(=|*3Ag8`WD4`PVE??B7!fi=HV)21;#xTfkArTzfE>3Ra%%*G&t>( zA;DEPp$`muO>FUmY%CXHFEz;sf}YwbLW+K7N;o(=X_H;3DQU?S^k$YUQa#0yU}_rk zixvAHeQem*iwl2(28)tq?~olA4ZdE`@5hWJH|PgSx`qW9WYO8gQhOK*zzGxl$XY*d zB*X$V7zLKz1gm(8?gQlA&yLNfpxcwhNE;3dTToUUb_4P)sH>w2K2E|RYzG@+++>u8!d>Hs(&?um|#(9@0_tyF)E9kSKXm~^Ubd!bN_@s#Kc2Q2CU z`_M2120IdVmXSJP-(b7#V2r}8{KbOX-GO(jQ2iI8+--8>qRHTn?^KL~9dQOCNePX! z!)%KEzXYKcjOm&Of-idsB{=(^M0mXQ0^p5{K4uxB;+BT-V4B4p_%IujS2hr7sts_$ zwf+eGzl;uX*yE6WFb-3TH$J;(;$on{3qD1KfmcB@oa0PC#$g3xUS9mc_s)Fmkre#Y z45yEbw*C)srvo0J0kWM95@8VG-967lhiYVidpRJ-2f#E?JrF8F-6tv_#5o0OeFPm_ z$jcEQIwlf~g|#jVIy7KeVvL3nv^E2>dz(wd8h*CHm4PP6=8P?EUQLzUr!O|mBm~kh zn4Y_*ITfPqgpFM!P5R0ysCZA^QkIs=JqCr2kfzMU4SrcQlv;eA5I;6J9ft1TYq6R+2-AF~Li`nsrXEFdTuJA9>vKvUtG> z%#@hYkT6mmBYrw#LY%R`@-*452<&S{{@Fn0hC@X>jk8RB4hi}~sHH=M#bsx9Pxr`> z?_IOVB?C&F7USHd7A)8`I#|Nc$;##U1npG(pg+PR7W#t{0G(`Ad`ynrsm#c%Bwn3h zTb;^a3~W{t`K^Y9o#;xoOKCy6q7g1w&4_%bM!^)wcXCA>gpZ=t-1i&l&s73}wM2H7x(IBRk!V@xT09Fp`!=D5ZEffuqtu0P% zZ3GmZ2HPYTzU@=QR=g%LgPky?ZOnOtbejXeks_QiNGvbpwY+YGe*li;A)vBe>>(l_ zP_>acDs^mcmly8?;kIm6;!DUK@7`D(u6qcNiaa8QFB)9%niL@XOxDw@P12cjRrqR5 zXpkN=iOTXyldw#3wT_puP>L?d>@BJa=N8`YOyd%*i3j=)LaA14^0a}+QG`PKxweB7 zedv!td=<(eYvg3FFzhaw(+*Y-%-wN?NPLX<2Z6>P&XRKgL_oX0!^7o@G*u?4T8ff2 z&{z4w;mrb#-j7by5$X$8+~5^nyU_XkQTjQ>hzBMn@=+x6`}71-W9`g?Q4mo__lAE( zszTuTLXM$<4-`z9+=hk9IyF59C-cZ-B3#T_=R%c=rh%Elu)l?VzC z>6CP%#z&tj{@sv_3Z2Sbp<0oZ?lgLiIy5TRlmZla-7)rRJ5mVxZ)n?O9QVk=&uvr; zdpU1=ODlD>B3wI8<(a#u64>Y=;R!QKmQ-!>#Z-4Ij&G{oA>GDuxRxXeB#D5@MDuBr z)kQFFy@|Xps?IScX`kEW%sWe+G}6kSlpF0z@FGOp%N{N@q6=DHgV_s(30&HV3KK4? z+m{pu=|G%QIdK@eR6EM8r?8FGn_JeS_)Bv;7Mn@+MknS)mH50|f<{~?oY%{d=I_VR z*hNS+;FZe61#dTu@uUTF%Rw;}o~^?_RedX(ia4WE8 z@&mfB=3$5rppYC~B=OFu3q~~F-!Y%^-{#Psct0Zgl&la;$UXy{<#WzI8%zak^^ zt-TMuyCm3+kI)Q$S5Y&16=q9o*4B#akl3sNZG)f`j_{@kOi)oKbjr{)aq<)jl2Z>4 zt}F87+8deWkFf~SKxGZU;>09ol1bbl0ZVI|h294nH#UB-;jX*Cckk}qeP>8P({tmXC$?O5@W63WaNsy8K5!Hy$vBD$_?%WbK%wqZQT|XF z1rw!l`z)v8i-Qt%Vp8$(Fh-um0E#}`2T9=JKAjnGAX=l1!B`C=n9vY$Cc#k51y+aM zDJE=>=S|QTBvw-t5I(6_A1$wUFNM+G04?}O5qe(ZeImB(K+BJy=qDR$EA=(8B?-(v zN}PAelNMpJb)C0JtHwTG-a=x-zjSejPju1}>46Tqg4OF=;%2L&f~X-PnJ_()J{aj0BhvH(d)}`K=l*I%`2T?pFbrMyw`SDUs=NVEs<*u;vL579+dR{e1NeejdH1Ts5ctCP?(rHkZMIz z;e#%BDs^8IK+f+hq7cC8wEccXBt-<CE7oVO40pl<*<+bNmL}&f{j3Q5{M2<>Q(sJ zI`B52p#wCA+!Skv9^Q9{QPd~T&nO{vW`8okv`EjuAohB__7RH}OJo z+NKn?AARiIbzjL*S7+s&oC{wLN@f#{l%RN^>{WH+lF-{3(I?9eNJX{UpV4WVxWPOb zD%)X*;dPW#u4=?G3VfxtCOz0Tybs4BOi_wVL%iXN8X_Sj#?1(gKv>8rLo$h` z2v>eMnJ`&F>Rzn3N1Z4;YD8Wj@ZP)wk?FysGuTgOE3$$fP>b7CO=<90g>5eaYgLyA zPSXt>atzFxSkrDEg;Nw0`)UkcGB6@;l^;hMe(OGl)*#>0sfKeyRGiRb5h*BieLqO= zfytm6ftPU`A>|pzLIe#&aEwHy)|rEl@OIFA#p7TC9E|!9$0=VNA{nk(L^1oePRVxCG2xNKRr_j4{;ZtwV~WW&~23 z-vg_Q?j8g&RwrOJXug0bI|ce1BUV~aL*;Q z=?ukc310Yo)Hx#O6C7h2YQYWRk^o2}$9+iQQWDTLSfo_Ja43cxG0Pc+jhfQ!YCe$^ zfFI9N`!d?030k6LKDb2S0*!E`ap!)h33!fC;q2U9s{{$SUOER#6?r~rdw0U5pBEAt792xOZCKQ76BuAVRBJqa4Hpc$Q-VR4$r)-4L1zm&NWPHpY zh7vzQjyyOcN-_|k1VrRHjzK8fumceC-Dr;k5gM*Gs2$BnzKx+t3`=OmBNTLCoMw^L zgfYcfO!g@0HH9Rzf)noH0X=gJgH=*5xHhRzD}^SMaSHQ0u)`JJH@tWqaFqZJ(Q6`f ztd-$>UBE;eHnSGM(nrVcMzDi)5d;nMDY3C49u`7Kv?gdTxvCru(}B|oZmG3s3ul@W zZdO5Qj^vZq+-B++vDlbSunG;r2S$PlEKxOU_0jTyQ^p8^7W!Hd6d!6LRz#bW=!9eO zw%v>8+#YB0o}d;&5q8y2!Wk33C^pZu=*C7&p1>Z5T1@$`wT&F16}fD{N2v;EpUzg4 z=#a(g1j8?)QdhSEEJ&qWuN#Yb4QjHSi zehen`B2?F0OydGF{b(#CaB9(t8ENfL(W@T?;4a;Gh&w(zNrABB^X^XJ1-$ zNi($5>?|fvNaYU!I4dRmWR}xRBfptUM2GxVn)=;GRa^^{r=9sY8)#`U_wWEPwmZ;I!gZtd##HrvguR(EFy zMqICNZ)!!G+l`&Ncou)#EpD>0g-x~E^(~rdqb9y@wybj8UaKzNZ(;e(b_0fbiP1E9 z)|Eiu$>ZEDz#+z<_OY7!KiZEI>f{M>AJ7YI@+zx_L!S{f1+I%#ZEr!eSN zlY&l^4Ug(Bl@14xsxF2j@mB8l3{s`T(~zq$qN=fPTB3ftq0x^7HCs^x_3U0w{9GHr7k#PjalD8D$KCe5?i*laYqm)Nl$8@RC9 zEmMqdH`+!yWK0yGP$r$?VQ<1xcZn4f<7(C$ynJ=ou=u>yY!Go*=}V|uF}_gHu-VZm zU)w;hy`yJi4k^JS-7zOF0Mf3>k+&Q(Y}8w|h7vjF#6vclvj73jh)~J=Xf4C`PFD+@ zYz(m8Hf7?SP1UlBtjulNYlLlT!IrtU0K-;86*dAd^J?#y;&=l$hm66@>zy+=A@7NG zFAUPmZ*=8vdy_O#<8HZF83IW=lh+GAPwe=x?K0Z@h)IN@3or)r_@b*uz@3n@)}9t*zy}px@l-mSsL@ zndTKWD54wn+U6!Rx0+#2sqNG@*%k(LXMSk#G78utJvl;$)6aCvHrnkPDRQ9M@zzK2 zgX74S4khkr#?XMZoeNS;-CRAfI`*^`SF1~@qh(4L?V3rb1=cb4fH+qt!mPrS8-&!d z{kTO670uj~K3WEIOS$WI*NAhj+t^g0Ey>cvINa>i4v4Pfqbf>#2xOWa0`n$D5*fF(kw8~i}BGmS-Eo^^7u71Y@KCQq(WuZzEpEu;l$EcYlUlsmtg zzb(_XO=q^L@YfXrUbft9Go1pFq+RPeBEYK=WLh>*fxl;?#B252CN@f*o}8XK+nP;__n5z9}Rz=PGZKy zFKfZe2F)*9(;Zcx7!OhUCQupy#(K*^-O>R2%Tj$;EPm^S8H1mdl5OXvNfA#E%|E|} zirCMyXqkvp!=1=c{EeL_;HgtFBf!4_bfxcDpsAT^b~--nl=A{G($tycwUGdc^Zni(mA=VX!OOhYQLU)ph zQdOlkWoDk3UwTd+ig^vVv?8U8FNZs6Zi{(}xt8QY{3uLvQAE&HHLvYnbM0m-$jHv6g#->35OrH766tJR4yuLbCBk z%hh3jvXgdb51gvQ;O&O)D-(C5CZ|Rs5TewG7@?1{RkI&vnLUXJj$-qDI5g?wSS4v!1&Je(H*q)uEor1%KxkP4l^M znJov?RYn6n!wdc{7^;=@#dvfkc9E}GF*1jdQHabwtaKx0OXXFp60=FeNNI2K>xr~6 z?_-o8nK1?tdW$59X06F4C0FK{bP{H93SE_xMyAxn>$S|edQol}qs`4t_ZfYLT@p1& zv(U(2c3PPmdy6c0HnEKMrfBiZbTY=Hl|@DlDEDN%sYzubrwsg?jj)xfrJx7#CfcmE zSafN!%b<~Ah93$hVda>iftr{>h>^)Gqd>e6I=B+c$d(pE0zKJ`VXl?}t`xbpEK152 zf^0e(U654bu5Am~8h2!=F`u(pYRuQY~jn2|pnt zm(3?-Y8V-9?v$yK$!nA6SYw=^wN0TkJ+EnYZF}-s3z6E?^le(Xw0Q{Ss@OlGFgK4449(soPULJ2 zOCG;#o#!Z?qVrNoeahJDQb(bSDAuNlrL#CTg`%zRCyXeBN!mr*%%YIMa)x|}2o zTLydd)pmAvv=wr=J8f;1SXtC`HVv1(Md8*YveJOzG#vdd>pm{TMxAXt z&1OvrGM97WEKRw2#|(L5N;x_*?;s$hE9%6nSzmyTEOTGY6g%c61G~gi{ZjgUPGT{W zCK;#cMKoz{ezD7%9=A6qKK9!Alyv;Uy3^m$2TaCiS_t`S9^;mcOi=5T8wOK6NAAgeh{>TN5 z-4}MYXe%`vTVxyK%zwOvY}ZiTL0F^%)J^5OY`H{cz`6K6#Zsd*rvG8R&RnGKG`#;eTNx?V$mr=VTr&?P zh&`u7W&zg(BL`?1wWWyd7?|rwA9`8JV~NC~rLMm1G@WNXh+NR9!ARw4N6V5&_w(>sZ-|#uW-5%I4^GIWLnJ&d$IV<(Rm#^ za$T}ZtAj*Y~zpIl!1GQ(`%&dbX+hVc1sc?d-zoJ;wBAoib}~n@0DP z?V(XtCL+$KJt{9T?&lqOht?<}6BW-Bu#LRMMuaR)X7k7^)=plG&)DH6-`d>T-fAf< zb7n6V73|wFX42x+&XVd&N*1Z7$Dwu(V}8iLz*CsUIN!l8Js&T9=3BaZl?j=DTw-T> z{^W&k0E{kr13)ETF7KtR=V!*BbFLRm)b#>y-u;3}dcS~w+qs(cr%bYDSMZtb+P0aj zxzHzCC2(HcEtxYtFYXv!kP|wOU=-3%_c~fEsE0arIV6Bp@m9EZPP+K zBeleY1+QXRX{(J#EoHS0HD@Mj`;9Xdwe=>GiQ0IyQ&H2C%~vvMsOia3%1%T*k2AR1 zaAl^}-Z^R*$*J+dQO3wj4G&IjL>hg}yZPDd)#{C-BQ2G+`k9xo9vq3Ts)dgnj8BPBGcrh)V^p~k)o(@#SxaW$-3XxSls!(-`>od z?FOG*Q6RSb%;BkPDu_Rgof7t4b&UBoGKrG_)X+~qFwh2{-byb;(}=r(Q%z&b3<9}h zdQVE@aYNmQty><-Zn`AAGOg1o7g!e$P?($BTsz*L4}iib9-I)J32fbQ94ykK>!_IB z=O`AawCSeXm|L@z3P?_3_3X5oH9kcL#AX#>!Q|>C?#1XXwjw-l4aDxQZ02np-Y%Y( z{gdzPT=b0Y>G{1>sT#F5o5Cr2wC$8smiA~aAmRc_X}409AAbJW77ItP;hE05Zr7}= z1B@+v)!j!Go6`G$&%JiGo4HQUM*4a)$f0rYtAhW^%GqVTRM69y81#*;28SEj+^*G( zz`mI!D1{D1b8K2(RZVlw^;W^bp*uiww%#Hzae4$1Y#MXdtC`cL6DD)e8@KCriA*J_ z5i&g;s9Q$+R`Fq~5sV7^-C4-_wXsv{epfb^y=Kj&_Lz+?^8+J&$L7w-ywgyG?tmi7 zbX2dq2VJ^z7lJqIP1eh|cZ_J@E;?>T|2ADX9mHry@=G(bfbz9yHm&P2y4A$C*x`O@ zFMO3M>%N!gUj;;kASXV;IEViOfTU(}DVuYB>U+j69h@HJ>oQXJ{ zp&VBuCHYYXF%v4niONo}U7(1F5V-me^a(?Tv%Tq!m$ z=x+fKx4vZ{UJg)n*4%c=1(%@)Dj;ot+d!Hp4Q`pYyzHD*Dv+fe$+2B4dCS=)^j&OT zli6^%9vvT}kk}M)H8vYY?19FniqBDV+(K8k##^*eXGU-P`h4hEq#3A?C(OEYUgrxhDt+v7Ku1KDnhNz z;Q)2YXuT!o)`YKrdf^Bz%HC2WdYOn9bheL1WFIK#H*=ox8 zO-d-q8!ek#Intqol9cwWz^x|P*5dj(yp9&bQKwMO&}iia&fCgM6u;zB3BjY?t~tqq zswc_nPOWepX|BSi71^kipu&@HFUYgzik=tZY1ymIiYJxjshe}|C+xU05-x->n`TZ? z#CPZR$+$*I%YLJ21RT@@3`!!=oF(CrndRELk*oI7j!EKbHPdggvB1o>RF+In6YJUv z)x`zymNG+ZUU=2D?vxa|z2)9v4I>J%wj&6>k)=qW z#&%V8K*oEw@85AQn>-;C>^KwOXR#A49{Y}U8_aDR%^Ee_Tq^4d))F})vqq6D0xMt? zvD>mxz04$(sVG&upp88nN1>EJo)-(F;8In)2zLsI-4ZxdM+viTMyzbPLu@cZvgCDJ z0!;5hy`JPBjxBNNbmKwBH}JyWm^yUPQeAvF)i!E1<1MReNsW))^OncV%|hsv zp{?z%x6)%ZGL1QC@hQ_9SFiI@XkJJ2NmH@b&M>LBTU(a<$1-ApvT)lJ*NW)jx~(Dc zeea_Ou4`#xpjnr{B6Yn{v%}mkN#cvw#brcjT~K_Jd#pAmm-tS7K^sw#EvT+^+M-@< zyQd@6Z)Sb1yTY3-r+cmn4QkkkvUfv~!dwD)7fI$=1hz(r^s z9ihYiu@IcvA{=5<>L3!S8~Hc!O0b&C1N>s7ZBy*gKil#Y+qLJ>%mR0c&4xuBY72t7 z?Zn$}=k$Rk1|>kNT@$u-+m%Q`7|HRrZmp#Aa={RrH5y`5cJN+J`MuPQL1!{NXR{O+3x0Z7 zBjku%niJgxtyRtX(tF=+J(?Q~qfz?~Z)xoq^&%9tNoJdd>02k-2Qluwrhf9Wy~Rk` zY&(xyVWK;hY2NYNcLe7)*1U4Vlxpae-di-+i>uv#Qq%{k-ymLtYLV$JOTU831 z$fDnCH8<*>X3P=En{Z9H7{Y8KR^evKLfBgJ`OXylJ*Qu};kAq*GUp1r?6g`^#gxXR zM*HHXwC1+DUDI*|$I26Hz5}M`GW>J1rb02M{N4=sn3MZtsbkHw-+IXqkoGYvkcpj5 zBTS-wyTI**S2|Xd=GwSy4w3o7atepWd^kwDCLOY*pKPP$xY?$8Ddx>*XZdwZ$K8Bu zY&eMVcEj{qjDb`r@rE6vb%b8ay#6i99l+n?XCN~=ia=Zr)h zH90TTWYa1}v!hc?J2;)qgJ797nkv_KbnK0S7lYAkB!hs`3ajh(hvD>j7>?<-Jw=cQ zv0(>)UnzT4!g&Z?bOB*Ia|~M_$w>0p`)KqQFY7C)2xt)4Cng4GfK$r@&T>ee%E~&j zOJLOgT(FS&PMx@cF%)zV(DPzAQZ9f)-JJegj@_`wGpDfNC8uN4RjaKyOlHHWm%uLT zZ!AS4ns`P^dy3J=b#Pls{<#@P&-Ek;O5U%Vk=X2H2{4br_+(>ax#6>zgtd`eh7)fx z8z!OejRr??B%)41!~gd4|M}m3{(t}5&;Ip){PKVKAOG|V>$|8r=nq4$9}a_a{5~8G z()w=2+2X%#%cFDKKK{><7ofb=p`1$u zO$Os6r=xkETxC1rJvO1lOU{1gP@=N^h$Pl=hzp=WzH}06;YtE4B%)EU?V!;_7NJ(? z2eUD+b3Z&AP>KThG|IRHSm7j9G%}Z&2smq-om$Y1gqrg!H{v5;b;B?Y#u%7NbYWXg z(EF!lSA7%;9K`H4dH9nmZuN|KWq>t0o_a$>mTXf?_yFonzmLHNbDCZvUW3teG{U4D zN9RYwFpF>(_uF>#)gQ#!ZC|#xlH@^jg>2s!9DSi~n(ZbBGum#5yS>7Xx{%MSuG9dM zFnqS$0OmB@4-camBfNumlwCM0sB0Rvv*7~0;(Kha4#Q5;VG#q1S*~DAL1TtMrd6MCigsyma<n~sb;mtqj3T!Z&M(Z5d zHlw>ILTydFE2!YThSs5vzkU5%`1|#nfAC&^@%q_@1f(bV6;Dd^RqX<^r&zA`Ij()FKP5&;55FW zl3!y3nX%V#Qh&Vm=C5D>TFwGGPTv`GI35gx5oW3zQfh!Kus1;Ya~VsrQ2&xf^)ECo z7!3RaqtalE^MIxKng?${+s96Dgw}v*tVf*6>=d?>x3a$Cdn?~t!Tc~WCUk|NhNH<8 z17JhTzr|YLz!-j;LIoD^_we@D@cs{o6KNYTn16=eQuqN}0l--QYe3d--ux$L@8Ko% z9+vF=F4-|q-+%qTBi;gd5awTWB#Ahp?9kpd@nd89VoJ#?k3mkNv}mvYPw41hU;n{b zk_Y`eTuCVNC9UN*S7h6ir%KFU&h!coaWe}rECg{lMn5Vi9WYJMyc+l|Y6O)X8Q)=Yaz`axf0z$J<@oyZ0 zW)EZfFU;rQzh5&8gu)s}4KYUm5fvAw{36RI;ni1i3wO-6CQ&jVgU2$BA_RU1CH@9p z{mSeEGrT27aWou4i~o~o@lQlazCvzDyal9*M>4Nkr=otU^B+Fe@oyptef{5XEnfeD zQhZ=o`%XptfC89Ln&9>a8Z$~MH)qK?TTCT55tuMKWMcn9%l-S;|4LTTTa<$i3jPy( z`Xh5`d<&W?j{d*lNMX@`uC0d1&xuJF#C2WFwFQh=5Llp~OzeMwI}a-V10oK{0mM$e z5Qu{N5OGYj0XT6Bv>ce(>;DW;|2=Ws-*b<*hEWm{x_&`InS31$qnJhx38F;=HA3$@L}IOJWs=<$I(%kBu1u8g5a-K-uwdE_?p@xy7D{Xf5;QR zqHREe0Z2QQ@z2PC{sh&3hxh|c{B|Q} zyjK<&&lS6kgoDI$cHiN-Xwvs49OC?WFqjgNp!qK{bDkZMk*|=Y!YqI5Nz#!&=M61? zZy%H=nkR^LeX=)4upx=P{uiVu(8#X|H@{TU7tyINU@VHh(+e;&X;{1G8&D1azqCg;gC9BE=I?+6l`iF*AO=?^czs<pH)Mi!Xpzv3`e2HDx=czy#CLIjv`8lJR9d9}yjJAgD?+!AQSDam?(p+Gww%NJtg_ zH=5?Z5Fh)>CAOrcxjIQm#Nq!hS(e!kiiWy~1MK^*$a9;cEeI6@OI-W#(Y-vSg1&+9 z8pemfy<~DP*a%2F|A2xzb77B=997RhV%MC+h=_^4o4=s6YteIWwOw0p)|_B4Q2BQx zmZYWq%KI`UtQ_%gkSr4l{4KApEKzrsfR%ke98L(y{)8xA@$bRjBOyWQiSSNlSb-0J zL#vAW@2^nS#VS9DFgvqFDhXSWq*eU;!66AAKxBxnph)sFWbKvv1CVUdyrL4iq0uc# zygg`g$~%Myh9D-nlb`_ClH?6WN0KGJ zqBSAys^X7EQ0$MmY87R&ILu%~uk@FI zoDV^+kK$*v_`emX^M%SWeT3&u{ctKXy6oRxC${*PNIDb``8`neUkmOAbst99$D3bb zMysQv*@UEp4in)oU@{=PrmPT#@WC^}fq#Nu6A~;KC0G*Xbd0!8F(z)W2om}`?EXst z+!xRo&gL70M4Dhl@M#BAX2~Hf86tkALIP>}4B%hmk_kbBI48giMJr1#@Y1$W<`KO6 zS3(g|5)5C&c7R5LMDl~zU(-8YakXT`fKb6WhO(zNU!a53gq+8iLuh&eN_c-F6T;6-CL*gS zp~qPZww5TL7I&93S~R544NG;=D*2>p!YacGBkTMru9r*xq&8p-Q`GAS1BDLpm`Gu#^1l;o=PIW3snJfDth*d&^ z5=-!pM5dJ-DJ3eUS%DOfaK@VM($uU38t*ff|GscZwAj*ADp{@M~ zeEV5Qg!&s6Dsgx0@87!(RBs>irgC0DK`2Qt*0Q4F{`o&hLSxmQDf)|f#4sFPa+%nEfq|*r>QxXn4h;LerFT3 ziqAc}TBTu8PI}>e))A)7FEWOMtTOTiVLmBFMk5QhfaAm3j#?6xxByb1qPEj<^rLZD zIEvrV2NY2LPH7|`CcA?VjY#vmcg^AI6}t&(3O;t zoa{@aD1FHqQ^o&SAnO-MGw%;hPN%dJu;|Z_d5=R;Sy&ONBFZ}zACr%cXa{^l7IKhP z_8;FM=7R>4Um!rdRd@yR;f+1&1?ec+w+sQvTfMPYmHBNS30=b>N&nizg8>qz|61j> z6)8|jIz##XZ!=;q9mMT%CQecY{&^i~Hj*f*OvxWhXJ*cv^PTU^-hIA*8vXS<+Bs$%6>yjiw>C&yJ2ijHjO=Q5yaa9OH`mW{ovPRHHa@uw%x9_ z>$oL5-`UR7q@Zk^8RRl06KKoI+;Qn+rp`WPXRWLmXVU2PxSxnN{U^R|a3l4*h^QI>so;T^|de8SX23Rss2%?4PWIiMY>$ceEr&I-sa)T{$(ppCo-) zYGM5|50^I)1=m*-(_u%s#xMIzy{Hp_+p|pe_n##;JIq>kr1+KzDYK;?iE6&B(c9b; zj)kPec`#E+)2`5K$yg);b@_%AN^eQWS0=8?kE&Se>*Rg8WkE8MmlM~U0R3^Ia$az4 z!bdSShA7!fJXge)pjZorXH?K=^mzF#EYXa~w#Mi(n9PH2xkG94giLbVd4pixr6Hpy`Pfy79D!ZSug5kz@P zV8WpdwPS<(1-;A8k62;GZQ|iT+=qieauxt^0uBv@Rg7LfTx1Ubcs`d*&IH9p8$(Pf?5ClArbfr&Z zXxP6e7*pj~3%b9!v+ns0fP*m9^jyJz=(+_2i-0I7K)1Z4@XRD)XD@-x-Coo4Lq<5c zz<21RphzyUnew3nq#&`sjG;V61~tY~A?j~h#k?T`jn$Oo(HB8aQrRA@p5?RIh_#p(GJzE$DPC?(h_X8R9X zhOJxXs{Fjihrm#yJ&Xpj&>d@NgY89d)u1xRAA;w_a>@qs=zPb?B6J9ULE&>P88hp8 zE9mzF2DBZ#9VWbd5abAJ#SAr|$>po&fB)^# zA4s{2Phi%9+rwJL#0khrFneT_k6YW`9KLvv=}*mH-LZzc z!2%1zUFbSOQDgFK(LU2gwQombhsbhP0|w6qspO@2)_E;wSW`4{r_fNmr_p6v(qk=p zGcXYhMOnOo;UdUITk@l0T_sDVup&7{C#md^D=P>%kVS~A5!OJIwyZT&IB|Ip40qmz zRSJ8wlavrjiqWGH2$~rF#8;!E!oU->g>fEal)^t~(e($h*aX@lRIqb90p-a_a1c)D zwLTd0nm0j*z4S%gY((Ox=vYNs*)80Bhg#i@e4R9M@nW1>!rkRCrWl*nQtltyh%`bb_jsX9)NmUHd!1&Ygz<8#qIf&t5^J^Jlb8!x%w(9rmqEANUZbz# z@P-u_8ZfdjRL}&`i8dB%Ij{dq!#CP$d9h*5*byPFw`rw28eELBQrY0378(e9tZH6T zFZ}HQhQ(m7bnl4SUeLK8nHaS}tc+(uuJH&G=%WRV4#tG(ugcVbl7#Wk%2X5Uu2tx- zeA`j{dC-nZLosf%ryR|iszyRQ<4(}WG{RH9V%;>%;vdnKg6|(_8k<{-AzHpSLzDIH z%vsnV{Z=vnPpbRrS`1HwYQNJ`?bCLJ6gU0#c8105>T<)US`I#0E<#;;KAd%aJ%2inF-UHvdc#>CC zGq`e(6vl~_j=vPtM3f#?Qwy;$trSYJk|p`nlncdkO$Acc6}t6x9ibgtr!PjSYcWds zXp^8oww9-wM@5r|+n3=rTZ6DOS+B5iPD}bzz|$u#CDP75Uz%xY3dXX`?d_Wnhy!R%urJ3;+{HVY5L|KT2;H$6|oK0 z34BDB%ARsTHeQ?HEn!TNYpFQQZZB0!n^Pv(=x-xjFzcBZWVS8gIkq%k3l^}D!&Tpj zV51Q|VM$Y`+%r>HOTlmExs|YSa-OA3{2mi3byamHuDjKh{bHV31+VqwG|yq4Rp@u+ zlb5NK(3)G|M1F!h6P8lyQDQgEYU8TQmm3kGj^WW#P41LTT3rDoud6e}*~zLZ0j8-c zR0b1wNpE!`&++rbjGF9mrbTJdGRuF`5jcbuIv(11R0+l?z~h zejb`OAC^J!1(a)@ltDgvaTbThh0q0EDi|2X<0YISoMi&$ z;B6vj1%XSV215dm#2YEiFv+gFs{=A#ox2sty7)$WHCDyu3@TC0i_y)n;Xj>E zun~=a-}icS%1zND*51X8KMTQ*n=peX?(-BNsHt@3pKs*nfzb`}ZL>{1wyA?I9=n-O z5vhC}=X=U0^Mn;3@`|DNMwA+K-9)p7S^Z82(3K?lELH93GlH%;Rni;`tG%RS6Ghtr z-r|%dct^1;t* z>?AR!&_r>wBE(bTUI++s@cc0=v=hRdwlQZdYzaL3BY15DAZ2lMBr^;t4 zVou#>x5(}Ej;4`ABToAqID*Z$EgPQ>l>^>&RaIuz9FXe6t-HsAl&)sNQ=G-cJL#F} z=4?g=MeROzrLA7q7pgEO%?;%ygBAXXFK+rPJ=QFhyJj z)_I;BU#6!Qu3Y$NkthFqw#GmaKA^^iiWADjEf-@6y^MV(c`#OARpc@6u`+h=IN_Cq zNfghhqZ*;dr1l|oWy^+JN7IzYMrY-bEMtl#x_f6w8q>%xu2Tuf=?jEtW|8@ziKto5 z#{oOZ7`@^M+Bya14_Rr@6)y&}w-TBykx-UjRw^T(8*4hWCD-iGVdm=${B56e5YbfN zYWH;S9Ww&N`lXiqv9sZ3QT-$#Y2_yW6y4S1QsoXfxUft9#0JCoR!(yjNt^OkA)SO0 zy;XEe#~oOH{}&4f-x*-hst`XDLF8pXPL}{a(~;hCQfH|MZsm^fneU1#CCpF6?F_YEX@cw+3~N zt|LpV8-?|dLb5V~E@n;!oenzseC<3UEPnsqvaOvh2s{nF*>KN`(m8=i{mwW(k|K(n zfjzz{z%J1(+w{3t)->yHZch>vhi)oE?hwoD1Bwf;%dXzmSJ54M1A1&be*Tqp{99rU z9%?!_Un@rYg`(sx+lz})(x!t;kTrdmFcq<=*)}%rjFyfhU2YI+BX^hR6L;TB)8?4G zIjLQmkzvL;)J%ugSwttaQhVo>N$neF)THITF_)_GBb}fl!U z=%)_qjf9}^6{<#u;9#ER3t`A}-KDhbW2b0JSK&^PW%B%wCQ04{UCG=jw56KU^`sH< zF6Ijz{7||gQt53W>NDOw@;wF+C5BITIT@wsMmyVsS>v$Vus@yT=xsRW*LJQI?4rJ% zNWVEyKVF$Bij;HXI$CL=>gB81L|cDHSPf(YFEQ%K+q^VbKnGr@Gs$3#`a9( zi@h&nhdC1$<2WBZKf)*>9z_>z^>yzxLG+0J;g%^pXL`z?^d z+#n6>_W5&VneFUxFi5~#kZK*q(dBZe_;72ERY%hW^Zt~u z1u)O-)pU6+rQ}=!~U9TB%m$zoiZv&`HQ3*#$vs*CqSq9CQZ9|o=$bB zjNczbZFC=26C?|h$1v!sHZq38o0zi&)rm5oFXdKWdW%9;UBR!=&QJS^cm+~7D(PP* zP1Wb{64!;PQ1uqEk;&GtDl8(z0o8(0tl;gKQ8Uiec2;V0i^hpM{mawXYjQI1#UkKL zw4#*=CBkNaQ^h$yI!J0_r0+i-%4|JhOb}<3XuJffn7|ZWx4U$G(K-Fk=*W$8caJ`b zi)Pz9F$<3C+;QAA`p0P%H{fDIX?}22Bp^?*dI%ENLZZB?PSOCku-QvsR_O{~bRAa^ z$#?3ToggEU(KFSfCwRP(Z{nRl1_O=7a5a9so4L__MpZdVIl zDNcSp)Brz6F?Nuz;Ny!eNjGrV*WJ+iZQqi6%9+%jp`Ifi+FiJX{0V`A)a5f6u)I7| z^f5gdgQU>!YD_eOFwZIZEyn3o-WFrx{zM^=#+YT^`PWPhfztMRyp{R^m zfUF~nCW&69jIZYD1&wHsj+st*9r6i!m#nU)&}^oExKclkVfgO6m+uAVrM-={IOs(9 z7NMlA&k1Qm@TCBjRx{?j3zMtd@E$;sRsGl95)FNyGd?%z-hXj1nMug`9@@#>o>Jt3 z{ZxDqZ+U!D`Knxb&Fz8vNDjciQj$2Z*PF(El6>027#sI{*Lx literal 128570 zcmZ_VV{qnt*gyQ*?QZREZEbBEXN|MAZQHhO+ud?&+qR9Z|Gn;c?sxZ-H_0)XWWJvy zlT4D|@hcJr_ zJ8|1CZME~Nn{jv5gWM3m0u41$pO?eu?Ll3)ulVWDr$vfs^Eryc_qr{$)YjF$%7(wr zI@)y0p_Ahmah|yzn3(f&Im|HFcgB33Ts5^e_rmR4*2s)!p^YpKehf>YUOL*z@P1k* zqUq~>DGbI@NLp`yJrePCl#DQ^@6{F^p^ehPV=+*y+T{f+mwWw=eP926z5m+E{WI;_ zSy6?wy44x|X778#+h>!(F~8fk*CV*7Ch?YYGuPD5{rj`SDld9!cRm`I#^cN?2(9U! z&$Gc)-;Yme#ZyPXx$d2{&j|H^W=-p|PV*Sqt1jNSMJuq!A5PoajqjLo`fK}fZ~m4z zLj0a1XHXDL)E#A@QIAt5QGqenc$e}}c8h*_Stor{XmAmU?C0mzS7FfSSPOiujXLnL zc&S-mE#qm&2J26v%}q<^Uve5h{||Q{akU`-(`RCyNpqhr$`u`qVSMuRqJytT%-FsS ztl_GzNv89yhoZ^)9K-!z56(ySVqJf~H?rQh)xBRc=823jd>OBdAimmaPaAKmHn*=x zP@|6?+^|=T!p%9D?hG``k$+l2v>WKh2p%?g*3}r7ZtmSiJf+Pz>=2tVKRfs?V&1mM zHElgJy*w|h8`lY0BMZ0r!W=#~zS6*a;r*!HCVH8zh|F}=w6w0g5M0}UQ%PRB-(wAY4)0Sx8*(xD7&m%t_3nsDf}gD z)1_@?iKa{+?Fb;Bx>h5S4kE}Dc`+H|D(A;K!ewX+<@Zg7$$7VLM<_`;M93v~9p^u8@a!Ped5|-PjydpCBMstB?YZ(VKIZo76?y^!wN?KF zHo5=C^;X**-l!14lYAi|DYt6p+Y8db=kh9;2-9`)s!1(E>!eL5xOv(!?=5RJ5LRS+ zI;Pi;Ze`)4{_OGz9UrTg0Skc_&yP@Q75}pM(FKyEl^AQ_mjS7w46A$VIf^x3t$)EE z62lCbQxqpibVDdX9z>y$?aJl1ccqMXX*sEY->GU+a87$B+8@MeIhW7wP$$PPK`FUY zv7sno+t*9IGvJjM*Nibtc{aC^TGbMN41+OVn7A>}X;{+Zx;3wQyZ@cjsZ&XXr_&di zbxdQxUrg=f_-&0ANH-sLEo8h<=YGNQ*06Db2j51@Rt=1>7E6salv)JMR3TGlv=<2UQOLhmmkuu*>w@xT(FJ-ULK?MCMGij94#x)vWVmGPOvi`y z ~VL#FZFs0w_oXb2xa^4AwT1mY5Pj<~eEnTUTFW0cQcHAJ zO58Km3ZH2g9$#Sr&6kIWsFR=NIQgTnbL7dGZ}*o{ia%W7uI@v{qPF%gF4*ns7-+Ks z(_2>6Q!BYBUpOya%ol$whQVXXANR*Qq&(ud6MuqzHxDA6L-=l5I^L9Q6X<4*PlUf76;)LUrAT{O=ydZS%GAeX95tSSxcvHf*o(zP_(PHK8Sk$v zr1do#?0>$O(?-fJe&atQpBC_M_|^?!&k1n~UShVE@oF|V!>G0Maj#pbR?>|?#L4SS z;zgum#^$NwZ+0b5#-i*~1YkQ1mBQW${ZZB4*0+`WLQM~y{1&a(g<&_)WK>oopypo*T%n|$+I?&Sx% zSru4w%3v26D(LEZ`@dGa1u74pE!}%VpIKDjXFl$##sZ2-z9KHvG9|Wgeg1H{-XCy&mwQ$rNO3^HLI9(V0*dQWzijL6%FNW-(uO z`bYhmqRr=t^d#O_4w9}wj5@9i{v~KBLD*PhMpc#Y$x#fuLDD4MuC3(NDtJzK<<&JU zJyLQcn=SIBwqNJhh0-r8HQ(gkFRFL`6G0|cvp6WpN`FmQMq64$Fj=j?Gy!Bp_woREbpss^3a;ACW$iWU3z}*)7S%N9&PytH zIOw{pv+aLX?(8^}x%`>CMOS8fd6M#v9Yh)&10M=KMXlPJBo;yeXTO$mf1F`-LkPRo z(*7F|Av63X;y_)@F_aUJ5sI>gL@3DiNs<8PC2_5M?wTa4tWxKnT|OtC^%C~>a4UxwHb8BUueTYty~#? ztbSe{!kKUwy@o2O%j`K_>Kuh~V$paLi5oHYl1%tl`w<5!WnGTG?&5+sOzG!kk^nl5 zAO#XubHdM&98<_=vv##? zQ@Q;a@t_LH#KSnL*%As0q~^>RP0DmkLg^$Brg4n}MS1pD)Jr1bD5yE016j}x=o&7a zvy-C}Ny9m$GD;1M%;EYKP)hB==IboG)WQml5kO|uoIw@BlSIxLBk$?K<+t?7XAIgA zFi12)w;UA{Dr4N!#2eEV1i5Au`#5xIfh0*e2fm&1rqYxds0)9DwYdTemOWLa92V>G zj_!d{XL|Tn{^7lSQ|9Oza?4aTaCb&*IsRG+=3Lt7J2f23z58pp1FR16O9|6d*W40Y zM>Qw-@T+)GY?^H}JRm{2;t&mRH$L?zbPMQ*9yN|XIEPxI8Kawi zBbwMK{Hxs5|A1mN3Jz_H&{joe9_ac2eIlCRYyg*CqD$C4Lj{RX!`v}XJ$JBZWYu`2 z-rR-3m0O;|04BeHz@{@^7SWjVx;sX`Tvq;Bk<-T2CSWDM(CX{hkYMvdNTe%;tW;YZ zxIi78OT?4g%RHeUsmooun>H3m`Na7>@=!D)P;gY~Tz?{o?J54MK(7}C0UPH|rwF%B znygqRT{yLpjNg%xjA~S+K&C0pWZ7ag0yz+HH2g&Y*FW&aFL(pI`F+9$diBeJ$B@YI z`o@oDB!T)jozVpHA>#krsi=rh|*ef$EjrQTs74Av#mr~YY zy?@CKn-0|x{hUDJB4q8beh_C)T>~faNqK3VsaD{5_w~m+m)0~>%fNi>Tp%?5#{yI8pi@5?C3$iS(^xqy$ zj8vs}Q($J+^l^$#aEn{S0z^u(i8H2dOQ(ER9J@7_T!9L_4mt+SG4uvVUpBH?|sQ-jM`yY02yqA1ClI1Y$8rpS_gef4$5#JEhB;W64-uz zp#T=7f3pbI9I zS`n|}JxjvwkUdM%?x6?E#vPS?HF;mD^7wyMJqEYQVYSL$hy%;=UeN=Xl^q*56}L2H z{KX3rUQ4?5|D*=gsJo%|EGfIi_99h2Dh~dCw_W}>#RC@HKutH~npnME1N!(-P(s{M z$Gh z6o!iDD*t5%ll_!rObVDog+k`0ip3TwQleKsD#~)4lH@qX-ONDcJcQ#8LgH`cXQU|N zjJXmB^AqCm04E$m6Szc>_%-V!t_#u|2gcC>1_Fg3P|8(&8lHF(1U3N3FogSg%Hk-- zX>RiFkTB6!`~bT7XGljL1NC5PhcF+^;1V4Phca*JN~7*K+HtCncmE-d<_*55s{0*`ojb%K>t93E+n4Bh$9*= zf7$#%f;A&vm^e!e$=n%-(<;H{pso)<66$4-!)cUja}d}5UC26PbB;J-bH*RCIe-mV zA8>@}AGn3AkJ!Vs#_#_OiQmVzAsHsx5Q`DU5{ofyi_Y4K93Qadzw!{J2A!{yAmVsXIhH9z3+vpI0+?4;fTVUW{<)%YQpA(Et(vW z%(E7&fpW%b$H|)8vpqYzkM{{KH#?x`0Z=?&v*FkJN)*IefyHXeaYWGns1-#w@>xk zSN)%F?Atf??Mwdl(R};9SDvVT`>MZv8s9$6Z(q}YzQb?d;kR$>+gJ7NtNQi<|MQJ~ z`&7Su>i_x1zJ03SzW<~6_O&>$>Os>`RQG&|S#E@+lQd(Q#Ki5gI0osIrjsn1nIMJR zWpEZPnjh6kcBKD>;-1928#F(P^X^C&KH{3hV_P&nBAhlp5=mK~fc@E;zE2pE{%vCZ zHko{zOukK%c%gQgoJjg{D)pNt$(_O*G8+QTyD|9@#06=Ft8gbziv! zmqO6<|(|M_jBrX7sOLC_G+i>HyCyCLl*i@p!6MRk=n=U3Cyqo#C3(N_N9_$lSw&VaaTEZEL;fPsxzC2FF~L?=|3 z=O`_^?p-1p7kdmYY;{wRAZudQp`yn)PEIGql?sjDlVO97f=d9-p9=av%LupxG27mg z<6Fy*ybP^8BaC$Ygz+ZvK{)Z62sjKmlZiY!Lm>!tokncGe*#q{_1f9=XByplG|!kC zsuXtx%qk?qVpQ}lR%jpX(izWZ9jp2chENNxC0Dp_{uSfLIR0^ zH|%v>%akKjrl(L~oxx%H;-b`DD}j;{$O1dWa9tIQ<4E&EFLJVf$ucK!S^@ZeEiXE# zGZmmCS)gp7tolr*_Ob#ro>3s4oQ8aOO@U)F32MJQN|Us*H5yEgSIOjR8cp6;$&$IQ zlOiK195gzVJa7u-Xee)ACIU*ADDA&z4$-b-qO(eAv_$4O7Ictm6gOuNIM$HOkGNWO z81{7M(x$|92{4o@wrfy4KE)oV0L2|H0&y5jgboAvOg7dl$|ZIWIt3`jcUL)*Pv`LV z;<+RXeWRNIZ{eVM@rcyr->S5+Edun)6}#3Gd5z<&I!S7U+BB4X8((`DO`bT9SJ#<_ z3f0eUA30=tY9D)WAT(j7@#Ibp6Gm$d;;`D#=2#KS9?yxiz#oh2IhUYd^DJH0hISyi z!4EFuZEb{9!J@bV%!~!Iqsbk_Ym4RQvM$;8Z?)N132fR&gAM@Lh9y0v8!vO@);>4DK}odl(_*d zn}%-FhDchyu^J{csY1LxKq`ZWj@3YPmWnG^TE7fN7{3Z3MP(g8IPipkF$;EB63)wp zHH4#yVGKX507N;VrK-_XHvr=R76h!R7YMO56t&ryhQk%siA*K{6}4z+C7})+_~MCi zur{@r+pZmtip@%Nb1pO_%W+cO>$i9jCCB7Sy@I(XiD)g3`0qRgnydot8*~$wh zkC>ut@Jfe%1*ACiSmSj`##5!(zf{B3ep0==l{mg(8+HsR>z<;k)=P;A|7r)f1p+Oj z^*7RYqq0mV7xn72x0EXo#rIku2b8 z+$`ZV5@8~QV_-^0V<2xV`z6$;OHt@Y#c{^}&Xt@L5VB%2AGTRx^F9LNO6(GxhMl~O zX?zuKw!F&6E)Y~xFY%ZLm%Owjr)pHWN#odc#;y#&(1$X)H~5LZzBgy{KQ}9+T-++d zW~+rBqyP42Wx3!vTtI-Sg^5)S--YA|f`$qq=V2udG~L-%Pk(EhoU^csBl484KI|O% z36e`L%*D@wO?)uQ#FDUzmeXJI_DN0iVVuKYw`8p0%c^@$rjkykz|Wdg91i4QUXWoH z_yIhOSMz$H8ea%o;4$V4&fV_cCX19<>pd`a8FSYC133ZPdmV~! zmeLfNUL(Ubt8Sywn+5WoAw_7vUIS2|UV+7yyW^=I?RQpk1a3~q%HJ`LQ983UBkX~! z%vm{OIYMd@xc1NyCba60%EMXQR;z$54k8-AMD+JuCig>mR7F~_tUawto0|=Oa$rsM z_nE!=HQ+2Z1O;xbTC3#9RYy7hl5lh=B0xytGp68nif2zix36-PjmLEF7>PuXJXmDW z#8-`9zNaOg7DR|wW2z#aHYlltU<>03_GKrwEgr6qi9+7^MSgcr2oK$O3O;6`%q^O8 zJ2R&KEJ#fuju4;v#=?*10HFk|ipC9I zU)od!Ws9ATT#ZWyQcT)(HYnKZZ?L5*qv*B3-%1^47U}Sj-szhGP5QE3NH-N)1S<7y z$K9`trjW(M7*2fa4MEn-glWH9mdkY5Q_mR)DnYjA>-Bw2a*X*xdngHV8c3$Q$(5=g6$0az zTfe-&By-QiQI8V4dF>eKWIhwYq19p@uE_a&<(sy!{@NREU0`5o%`UzL$8n0Vk}rn| z`)QdP3;k5CVq1oO18RKEl*4X5#@ty=P=rzBd{p#1vB233yZT-uk9;#P>v>uu8DeJ? zW7&PZFz`2>Q$K33MwbGgx>0z5=uzEWl0LJ?Yfyd&UsM4Qh4MrN$Oo#1MjDwfRzW+C z=i5o^YOoy6qTXgQE2j}ddP4o!!e?i2UX|N~WCNMy1a&c28svO=cia2@omTD-SfLq#c{KMY=>a`gOZv`zB!f71OXiMxOwgD8W~ykwAiUuC2zW^ zJCj=7NR{fAu(en&>Aq+Iyg^b)Af?KTE3ySmjT?{DUcFQW?ZS5&`TtF$p!}WY_J7lq z{coC?(xm?{jlA-Anu7mLgP#09X;hW}Hw~J~cN!{cor&tQ1Bx1a4GXp(80|WbdTbg- ztZ?b0kC9_^cbHQFq7}Gft9rksEC;e7RzoOpq8eAV3glA^0@}5M*3%uO7P(?+*CYTb zMjFJ+dQJF}rii4jI?M}2xXPG6!f#58_ouIFp}d7}){>$Ve%qUvX&Ws6wwM~op0Cm_ ziZKUEcLK4ePNpZ(T&Apw(kwJ%*jyLY!#0W(spXn(q$HqJHyWb0EoU#BKHk?alj9vO zbi$@Zd(bXgjIfN4nPXIxOomL^w@qCITyVuh!7tN}iH*kxX%=rSK?AI^(PNkTs{InD z{P16s8fvup2v$lFE5g+D=EXd=im0Y9*g7?sm?)ytzu)pM|6r6!vQr-8D2l7(*OJy7 z*oY1WytBZMaYUmU?vULwO&O7n8Q>$f%nM3S4%`1cb+O}sIo?$l+Y-&IE^BaSIqK#q zR05(FuW-|byXY;LL!`YoRNK>F-)WS~v;{xXD{n5hCIhSkGo6CZXJV5KtBG&(e5^E9 zOIE7k*DaQch{jK-hbvlAzAs}-;Cu>q6}H(N>`t zt^2(L?`?4yB)QB((37@=l+=FZo;Eu%9U2bAXg1)}Br6rWY5>HHYNQm)9tZJwDCc(* z1^>>Fopc^(qPB-~wy07;GqMPg?towGrEBEAum&PllCF|RSZ!Zpi_ zBf7~bcWTC)ARXmCba2l6DnK}(eVDH^bfOimtnD0$#cC(IR;8XaN!mIggj zC0m@7EJwVG$MqGN9W!VH&`$}bFai0aVYs%vi^ppZCOpgn$*5CziOd)FRE|FrX^NxG z`bTJk$i=i{`)4n^}qgZcL+P-Im(%r@89-4T2=SGV1tPZPZaG zNz9iM5aASEeQ?k=vZal8NA*G#p+ypetmnH1x~# zp$i19WA*05j(Z}gt00|}^RMChswKwtYVhGXX{7{NG^&vLj0Rxcznlz?Aqio!QB$RVQ)N-$=mb!Fu5XU}qZ>m|UM@qBMSosW|7e zScc0JiFOE;g7#4U$PvK>9y%HMNI;%+ACZ2%Q_jm1!qaLe=UTqf1}WTM(7jIT?A6*P zk)r8=(Q&py)a=OX425SSH1!0{(XamF&9z(`Yr&Cj86s04@2!l$>rLMakIt?)?eYY~ z%vyHYO)&a_RiKmdQaxzC3Yx|tZa;`=^aP@!F=`80C9s`k6z`~IBnzoP?S&02hEu|| z*9oNsFnNNrS{2_7b%Gi^DcY#bhTxDTj~`qeZ5&ePj+kH)PwU=I%-?#vRC8&QUp$v6 zoz2aAGNW~sxpIZewd6gi4sDHz`bbA^d^0LIK53c$G&711o}y{Sx`)NZ8f3h=9b7$L z6K4GNSKzn}dJCp>;PH66hOG_!j+?l|h^vuxIJz^PkDxYg>72K84(;}<7`us)waE39Oij0qdT8!mwrQ#GXXhvo!J>~H)_Iz>XvB9F)bV@>-9q`CV!9M3Q8|-mmP4)yNj0-# zI5CHh1=Jg+dRS(Rig)2r(j}kRDse_EY?eNCgF^C}dXx`=%L5?#GfpF2Puk{$Z=MA7t z7{>kZZkY@=lc!SEQ^zBj-z5_{L8-8{&TaIDZo6g7AhXfN&!S`9)+Lf_V0jUm?21y! z5L(q38fNG7KTP;Mp!@kIa(6%2dhQUdDY>?qtnAwvTC~99;O0t)3;_4wBa@mZ`0Dw^ zwO_-P=Z^n4K70^wm^B!8V&suy0y*cf1z5p{30rREbwR(-D z0wt5Q6jH2wIg4h#)Z0-PV!aPSL)be)&n$d#YV1jk#Mh(fV3V{m}Opgp3- zyuf)to}t#5Ep=sb2eY9NAi6gmaev=Ck85V5&{GLya1P#0QmFmj&`6}G=^O+$vs+0Y zT}=teqEJxI!R~IAu*^Iv(8WQc!h#4{u96Xj_6J^K3!pd;hD;YsVFKx&Lvd3R zAx64l%g91gHi0CAZjrwTwjd2BzH^4Bz7(B|s;u&Ge)MT-vxo;=O%ANUpf)74VZ+i# z|8zW;4KAgis%)doQp^GSYn`Kb2&RyWL&nnJRM><0bMiU4IWi+S-THa0zbPLVN6R{; zf-<~cdo?;v^WsU)Vi;Ktsdfn0IsD)?+T#LEb0QI6-TsR~aguT4pepBtl^&lH1{4NR zZ34B_nsT08mB{&6RRT#x`3R`8*Gx$iktyoW&*oWSyI&yxnn?4Nf&xcEp`{z=*b_f= zA94p3%Z)zoB+Xn^k4L+SWsS+6L}_@bfZ{{dQpmX^; z!;{a=WXAK4xiE7D8~R!w2YBEwy1Sn6d;j>aY?w@vFPg!r==W=(f?v+|e}RBtC-5bb zuTCjekx#XVEc6%MK|-)@n$cLimu(HQ*mqmii?DawVFUhKeL)Yg17FB-*!V9^*|OhU zmzFtf^6tx<>i!o+7!za=-eAm&ChQG1KPJ+B)$Xfs5uex8Z~TIPziqVDOL-IAUfH}D; z@QNa_`Yojl4#}zL2X`E~S3jzrrbYq&t62p1Rxw!^U{lpB_)K6&Qu803gTT!q1;7J; zw?qDvYgh>2mqUg*R?kEr6`UPrhiR6d49W@qr)e+s=+WeM*6e`RrmFA^WFR7kr3!Yx zIFXl*QM;r567MIG1Q+hcRkC`8dD?;t?nZWLYiY7K3#(pjVWN#0m50q60Wgw-`m{tt z13q_iq%k?JvD(3?ggl=m@L!x7yptL4N+NnO!Tw14b{8LWf&#RJt5G3FgdBB&Ym(W3 z2Ryd1&8(0@5wtCW*grW2i)Y^<`1fUmO8Zfyc^`?P5_%`ChOc>{j4Lo_2wU^0O?a+O zUKfTkrpTcsHEZi4e1r0SgZ`OG|E6R_jecwZ{xqLyQ`4K} zWky-!xQoWB_&iWsyJbEyXT4io)qPXD#S!j5C~HY$b%XCBacT*;!J>A@@=^bp-NwZG zl?oSrEBT1(@TBOir0By{$MtpN`D03!bY1P8Woh9`2;r5s$5LZCE8hz#ooM-C!M_Sv zLW)U2q{&cjd^O&xWjn4vkOyrt`Z@`UFltw}r`3u~lJJXUPeXcKh>??Ah;yP0fe=)& z@-uYZvDT04C&Kjm9!&7?<}P(zAfvN3bFzywb=ox?@q>i0jYbGo zb(}?7Q&G9!mUcf}rMr#)jDoxG5bT3HH~rqI(YG;ajeZdPE3te|&rWV?p3%P=^IJ$} z0g;yVUQ->X6Ns~(acEK=zd1D5=3o5BChlFVuwt{Cm4uU#HfOUt0bvR0RW{6q>8fcR zW!?YJ6MV?~KTYb4V{;S#(kvg@RE#Uze2GosR|@l_rfdqo=g@bTF76aX(0Eex0@GA1 zzY@Ad(q=Lj{Bol>4UObX+2q-MBn!20z?t~V(D{!W@hsaSWHuRo#k~LoZF|8wjr$Z8 z&$a}h>JbY>=iK23jYTf?C`mrV{XqFwX1g)9*7Uh36Uf%{Wt_LjyfQPS>GlF9Xm0Q}Rzj6^KGxqOEY2hw)4cnn0aLr3mk| zXy%`-n!CRpdjb!b+I6!Bss375o)`OOcy?%n(@(FRKM{UQY?)}Iylp5Y-gK{KdmPiW zU_QLw8>p>~HR7dW?dKVpMiT!yGv5~oB-W)(?GxhXX~H_#e-WrMn+Oeb%g?`4h32HJ zl4o~kiFB4v|JsTAN(FW^LTn{l@puNe;W2zpHXAX?IOrHkY^(KwMv=H zoCV9V4C*hVZp@uJiBky=H=2t#{x`nDVyJ}% zI-Y=j+r6+isOppg*IR62eTQTV31zNWKLdcX$YF0+qsrPeoqEM(S8(|QnuCzj#=z68 zCqAWbqU^yTtxQJnX(N3KFWFuu38RZEx!8?5FxrzXi_eH`r+vr<+J#qgPSj_!apP7Z z*F)h|L9U^AXxscIzOabfv}||g)5k)m7QFl8GtB3j3d1nJBrA%&AOc0!zKpX%A_6nn zjaYgHB4Ihz+Fwv)%9?sEQYyA#3S7E#D}XtC|L8%8B6VgMc>dn(4$`q*^~Ly;A3-GC zK=w)lgHa=B>eJ!{I{b~$-X`2qcfbOf>lk(M*TGfbM*CrnGkT-wg_hh;-?)w+ilq|x zoPcs&WTVj*b|RpkbHa9EkKRz5oAH^pSUjatD47H#2NqR7lH{~X9 zO_8@Qrm&#$$u;I}=?@nQi}>~;NjV;rTup(qdD~*P6rE1$qPC{YiAVkGRD(`|?%7gG zi`$-^0g@+Rbw!1G^7hN&l01js6rKbR{ux8(Ff0W`& z%P4!sk`9vo?GUQy{J|l55Rg(07}PMcrrFeL1C~wGE{WMon=7_i^R#n~@zG}h7u8Gt zN@V=#^JjK#8(hn7h~#v>^vg&IBC$(@Xfy7?o!grFQ^!)zf6gJHuM_ zU5Pn?MVb-KQY>`W7p^IrE8B1pCRoMCSy4lYva04K>Kpx42dz!ziIkP-E}&=qA*csw z_Y(1jK1<_^$JUk(s@s~nI&)WSyPKia$TV}#38O7$OKz|8E>K}PK)7aRX@8IRVb}qf zlJlNB1(lq>_QNRDbu6fs-6g=dF>$%gc_0fdUJ#pZ;bDcd%7JTiJWDZ&2KfC=S&-NS z^(2c&Z82c+vnk1N@Ism4y91+pkd(RO5FrbveECi$J@WBMQzt4%+8pG;4W&3Kw5H98 zK@1v$HstlvZ;8^;bf_Q~=Nb-IWxR4qXFkCRI)Q7yLy6$0`;5H&S*==US4i>=J>6-D zq_5GbeUUWw-ZTK&AD{^%>o3z>FK)vD(!Jqh+(*o95!NVsPKCvpHvx`*cyW8_xU8afo&iVR z{zNnVO%e(`u2Fqv#fL2}V&2rWt4HU^pV2CV_L0L|Wr>}~U{0~Yep`@MD@?svWH&9SS!ZeV4_5O~$d0{>k%6gyJgjE86l0%z zA?Weh!>IrQO$pRp(&}Y?G%V=I={A+QpeGS}SHaV46#VIk6D_gIh47!mDKM5eQPNqN zw)8S`!(J8gt^(!tnTban=U2VXEoL+jbN{eY^6Z)XB9|vA&Ppi?c;jcQ|2fe!7vLQ? zS%*y-*GpxDVd#hZmG*I9Z93q1d2k(}x^F@j*HmGf$K3x#(mHm~B8!8Ts)Kk`62yDA z;Vk9h#l8){xbjmtW+R!fA6tw$(=u_qyPKrd9hov-7qxdeYK}p~P0NRLD$0rPj{(K; zBkfD*CI^M1!)8J8Cj;O1SX-D+O zY~;p&D10D+cVhCyKkpR|s@lK3a|26tUL)01#X{-AHcMe}g~sS@UAv3`y2QcXks`4% z++EaZ_3Sb3*@D)stFC&$6E}X3lMMEoPe8N-m-nL;IB*Sp#GRo06D(j}bPsj_bfJ>= zDa}r{2HQ1jS*3#>mZJjtgvo*_B(U1OVx>b(ps2{Biy0;KKJmnUDwy8RY8*$RAvyUW z4>{Z>l5Z2W-dnS_-_Vyxl|`Z&Nhjh63u!>~uV&+SNSzUgv#Vp*IH>QIj|zmFAKb^V z{3LOyx_V=IB}YW8f#O!kp&E->1bDCFDP0}T~_D7x7rUpqGzwrizKFIjQc z>UlYXp#ob;PwO&+$wV3G5}Z~X3?*l!BT4a}RLuWpLDq}ar7hQ_sZJ4-sX*Gwk5m|n zha=w$%Y*4aDBwbc#+5T_i3I&(0cBmD4;$LJ^2w(+U7aQ1K!ir@mZ#5-5iCllMSJ8i zYB*m}uKjey6-7^Y`&9m_JFuSeF>P)-g6Grrnc2H}dUb>sKs^G_O7f0S&q} z27ae5nAM37{@CV6UP|#5`&hH9?=e))*4L&&E)hs6F~25QKg@o;BxU4zj5i%9+e0(b z;gu!hu%a&R;o95UT++PfV#q<%r(+>!b1A~6Ty_D}^nsCgKcm!IGrnP@A5+p?n6)^S z+Yp>IaULdy)#k6I^@>4?e8w2!Nv4{9wy<`{8M;#lbqp6xbL}ma9l%st^{zFEP~eit zSCEGWbTs+ZgugRbytGoI2)=sGJ z#&$iyUufi)7gq47DAWrJ*fHb9E18trgo`t?Ze)_~tY3Ftm6>!l8rfkd)yiH|$Y^~g zts`*5{|GIAHZwve#LXIln(h4?W7H;3NeMVaH|R5Jpf+-kAYjBTzsr+4OQju1{PD@Q zevoL~GSaiA;eaVl{`gUpwFbZHfBiOtF@}jugS=it%Z&g0(gzah%$PvO)oUL}kAPzn zH0)A0~>Dr4{$c2JZS|Hzg5-BN^8L~>|0YqaQ!$tReD}41ccq3fYsXLKd-_4%;bxi7=beD9`McL zVa~Q0Kxu~0p&`GzJ?wR;;UVLP_mADyABjUV*z5y7HBe7I8h%`eZRb8jt zUI1|tfnx@rkACddN)2&l*To!^_5Y~9^QUrUWe4Rnns8Mc$E$!0??ydRt3jiqmJqQf zHcn{mq|y1<1Rwc}A71HP+@;g8siKDHi7n5*ID0X$D^ef>JBT);U3=Gpq?Pm&7v!sm zIEluhR0dyr@a9DTJP*w*@F`&(-)8%wr8iz!M}vaz*?ZAyaW zUvR%>y~F@3ZWhmypmX4N`m!E4Nve~viR8^FnnVBOmh2QwvZBNz(_H{8=x;~n^5;r- z+CAnfSIe5~m1;s;9Fl5HbZ+OvI6Rfyr6c6om(fFppsMH~#W=mc)XQp#xcsrQ`GOho z;l|+Z3fPZD`TcFNfB0esbdsf%4*l$8mkE?L_a>f5v9{-PKU&?^U6MN2fS;v|4*s*eSIPZIF; z7*s-e1%|N1{qrMZK2#gh$_Is~OMKkSWFSy4UYEbimu9~jX<7tN{dgg}OE3?woq@4} zh4bNB1==CGp2*dBT(qQPJVMJ7_>i_~EK1M+HH}cPk)BV+rnP=6yS#gC{+ILLVaJGM zov<_IJ-S|;rMn>3R~2Szp~n2+9L@}cw$-?S);%}M8PM|8;759**$?rseVBf`-PNPi zvWIlF;M@jDpY*yYktZ)g-ox(V6h6IEa5`@z$Mq}S5Io;9)f=WgbP*3k^WI9NB)oZG zH1tB};?(}hm=I^{UZ^6RC!3}srYrTuOi*!Z+6#6{j{DAG7{f%zW=_s4MHI;5W8=Ti zTf(LbUAyhi&DPHWsK@V?=U|4Bg%7@oY?QcjE4k9ZyPFcAO807^!e1i$4J&Elk(@*5 zrXZj}xmY5kZDdHtD@_4s(@n-uzyLY8@coh-$%09IPrsHw0cr`Ub3NaIqrsE8xp}x8 zoh;7`cY)=I{$c|6$`?nU(bOE{em;2n8sx>a~Trwc>3{=Pb zYyAv|R38%`i3{z)!kN~&7^T=KxLfE4CPlLqtivnHI|!Me4nwlj4Ay50y~`Z+ilt?! ztm;hC`b8boYDV0@u1}uzeI^ann!DgHa{dd-nl*@JaigUbiMQW3bE%mT3Et=NAkYG(McjM~2m0s4FM)i^T zSF5T(hh&6AH&)%_lnhJ8DC6rG^o)GTJUSms&-jzx1VAbCj*k{#G1JbXFxwG*5uZP} zTAM_Go5}e~0tjVkXOhvEAO&`!={>@rtqBrf2XKrylW=zeu<_jD55w29Z%~?Kzm}p zoHac((>JUGD&<2r80q4A)af_h0{ULloJ=~9VaCY{Kr0R0RrH{eV_5z4A#MQ@@UdR& z!QeUkF?V=$_#o$|^+0s@!ks&CNIf6tngs+8)JdF4BD%&wG=XlS{XYO%K&HPy;w|0_sh9eR25-^%gEa zS;d!M)AT<42L)p&%`=}v(Yic3^H<09g*OM^rTp|l^S(T`B?HL+;QJm(GQC#g`Tpf6 z@X3p@RCW=EUDD@&qQADD<1o1J4!a^J;7lD}c%*emh#G4`4I>`qn+r;`QX$)NhAbG* zTP<$}W_ZD~#8U9(m*0OEf8l{o28@NAOOFB|GnOy^nr-rn%YWI<%Kg;L$GQFYUmVMP zp}~k>gtx~+qJ%R+`Xo&P!iqJA;|EwEG4oc3eEI3cAK_ccpH-M$-d=k|fq`*-5?DOG;2Q2s|1%xn7Tj7tJD1=Jlw4Xwuq@Py!!}BrW*2nz7^8v)K zlPkZ5Uik!vPms`bf`oz-Boqe#h@?uuy+Kk&?~oF&nUwUsVM_Fflz8Wp0z&j0r3ijq z;|RIMpKJI(Q%16WcuO7U^&vP(zmdWAx34B1A9rGe{_U%)|NZ5^fB)6Df4#U=#_#2G z@6Z2XDe&k2FbddIrA!SwW_Q8!GmhwbU9FC@3pRjPRkbfC&!5v&Faw+C#c}S_Kc5pV zbm3K?^>P@zSFMSHytiCIZkT z@nR@Gjtc-C6o3Ayl0pAtMh-kGw%a>V@|E122Jg4uIXzeyA9}fJG#;0Kl6t zMzpkK>`~5Qlarwb4ThRXZ6TC|oC2wYnxH;PBjgsO1(1wv#x}hn9X$eu-uR<2wv@1X z5LfU&VF>7f`ip1cZ}WLyQDJP>hH5}lwkhT~VL zDwZ&rcwfh3Fivhp5n>z!sN186SOVy^bX97WZbc1#GZL#;Ey#>FvVELO(;h!T=dvPp>%4z*!Ge*?xLRCE)93ILXQ z3Fm79xi|ipC~_nUS~42H`6dA!GK_|xj!+FTbxQsxJsM*zr1MGn7>Z&~V6#F{LD;m| zi;0|sVHIPigu?=Gfg*xRk^e{{5)M=h*e6lDsChxl069=8q1KOp)dBb_5%TkVt)Y7& z4(rbSac{Ss7wh@2bRMvW)clZd=Xtp(uDvHuJ86sW>Qe8azDR;0demSHzW6OZVs1aeqRm ze~<9*VRz#-2e1zzxC;TOsu0wT7lMtCK!d=Kh;&t05w{1KN``L~GPMN%M?-YJDs;QR z8NxvheS)}Ppn`gh`CJkD>Lw*r2Z#}!o)LEMXt)Ah0zOParWivi8)G6HM6F=Tt)J4V z3INrqRSmqte1d_bk@5tPlnzHx0bIaI(JlC8kw;Yd0NX@WCbec!O6(OX%9W4w5}1JH zdt+*mA|ip1aGMaC5T;J7YS=f(+%XI$bTkJP<^Y<|cW z>k1WDM-hHO3%rj$PD)XL6x<{M5dvmIQSpotGS>y3qtO6`lCI)tRH!lVEa0TB!cBq%{Cgh3I(-vA6kSudxwEF7dA77kigFwzs34Ne#hROX)-xXy4a zIMwt3~vrR$A$RxQjzlC8=?UGwzW`y=s_AR@j*F=$5t)n@)Z6in{7{ z2@@+q^Wy=-8C`w#g|>vGqYqv9w)=_9;)tfKs3&6MAX5E&1FCf&@}=4MW!z+yp43@lSwQl)460y){;UHVo+LC zW4`ZYfzYS{U)><(QPYqL~f#hC|9mp>lN8FAT50x zE^D#7UT?db`>zrFJ(QSK_9&@?Mtv^gnz4y7bwu;UPe#v2V_4vj8JG)m|s8>Z5z%j3$Yzrh3H`91^i(Q7A5EnYOZB|;aqkP_B<>6chH0~_ZV7lAs6pssTv^}T2H4&JI+$>fXo8~@8L*Q=6(;{ zhaw~8VN*mz@lYx{O$xo{S>gDfeAJ%heiQGSFW-OhZ$DhEj+-@1H(y^|M!`oE?*lR7 zq1614RmVe)NdIM#Ur%40g_(`p`L9>U^%ZsVTc!rJ+5sPMcYR&tUqf+Xi7j)6BAewv z3^@?uRj>CsdWZS^*V*osRQKP$x{$q6mWK3PRj;7=f7@b`x7Ic%ptTe|cU9}wKZjP* zLmto7Qy%2pM>gYsN_rVmFT^2|;~Fn8;ej4#1U-8r!O;wf6MJxmv)L1J2CnCQj2PhW zkJ%?dRx+PiNhF9qep=Vd(te}a7Ml*dz(YhAd{GU?(|CaMXxTgPYxjWZtWfAINtIQS ziX&d7s`@0&M_0GFlTrH^0`AQ>WCK48fEb@Ff03mC^iR=Q#k}Do(&;^v&;in!aynC) z&iFmzXM|+NoXnV!>A5GBwYmNxD_65!CMIK_Jf{N%AnhZjUYKx>oO!lE#dh-8fpYKL z&>7-ZH1Ko?b}7a_N@HARu5h(EjbQ>uS1WL(f+oQvkU*?_EaHLaM(P5YZ(l0Drk9i95I|?RyR+su=!WN-w5T-J(@W5$#x;U|t@0&c~ z${kqM(P}sfZbk#F3HQVYVMPw@Btw|*hXd3OX-M|*A&q$FWCa`@@PeccKuB<9AGe?A z{!f@tVM~a}q{K|5!>C?v8j^zv;}~m%R9uh8jm%+8HlV|VY*|N0&={uxgbJV#&(NlJGA$)Tf&ZibV}12WHusl6KUZFH}~69_Q9Cu|Ub~yO0Ki_3WT};Zx01Z>~wm zS#X~3uK@9Y`2_V zl-3aH_Tt5eD>B32afvQ0{G(Ko5CwuxJ}ZIr1N^^WN22+`Xh^`s_6ZGAKndXYg_SAO zm6?xoCqwKI%qMUjhx71S!Rv`J`9ZRtBhk<4gVk!oq_g;S+Zg=}ggjEg*yX)UE?B#^ z$IT2^qCg5jJ;Lt{)Z%!|mhyKcIV*OX6_nqkm1N-`oB3Ux{emP)jWh}Kv&911Cx?@g z*M-V_E*fn#md()}Kecf{qNZsQLE>R(7lK_vji~;=-8IZtCHeMH#*1rzsM?n}3~K8V z4d9Pgn_^qi6MU2B&tJZL*?$@LcZcie7^M)k5160)P(oLuISLWo{IK40Il~pY=lOh- z(bS8}<#9`w^uFDaOD142!CbBvPrbYU`9D1J-(ojEZa|gvuk-55BFE3qUw^fD0ziFo zRiV#flVJ}}X{meqOTxZ8Rs{FCsDpkakV8)r@`0iGvVX-B4u7qy; zHK#XP(46wHNam%p`T8)2O0f_HBv)eL%jeNr40DSoMn*QQ~9t0oNl1~1Dg88@0CAeSSAuqwcfKFI0zdhoD6A1VByW?S=e^%^Q z88}3MG=^WV@fVC+UJ`NqB|DboI@^9;91jB14E_+PI9h1O7y0d)_@|#rbxoO*)350k=xM}5h;jYgdHHXuQxLo_{f%j19A9jj#^%E9npmv zU3jV#A<``#{a@te{F0aSd~dZomN}>_Os1|QUp@yI>NCbY@c3@QV=IO|@P**#z`GrJ z;NC&%0DdRl?ZA6;!JN)nka?IWYwM)67!iGXU5`5QC z2s~*d2&R0I9e({j&;Ek3Mx1;(e?S2 zlkmJxjaI}*X|pUFUlYp~xKv47ivi;O>t~Bqq1`NcxoI-h+R5)%lU>~$c#01A+P8Of z-Ev%k{vMBUh*6G&iI`qu8wvNa>28xCW{&&JuRgs9lLS6qe|j+z23dx{rfX(JI7iiSeNWxPfl&DdFUk60qfv%LU+>TO}uH@p=d0y(wLEyFp}DNG@|R3sf{FsrkbuVL_l80_K+vV z@ddF6G~CQ2`C@mqKX7ghL_sgY9iCw?9J2j^FvO1pmpnt7)U@*LM$7Q}01GHF)yrpMm0r({ zP=?fZFla45(q1zq=)?MXGK)p){Lx{1m6L1`~hK9AJl6tQDFuhu1=ka=d=SnwZ(T{sj+-U=C`^2NtbJcO+! zcH~w(s)c(c1|lDa=d+8+bR0POMem0A7LNV=%v^13@+f{L>mfgPbSK_4uZGa<%TMrp zgTgV~ny14!68lt1ARk>ou3$0VjyES7HL|0J24qMLNZe*X#?*YoDvw9HMN>GdFPk(o z_9VVGv$ch*13Oovfyvdgm^x%uIc)b_sN+J9E@*spv6W2c)VW?GW2p<=nYGOhFwJga z?r3CZZelPuIFGx;S?cPsQcW7fI)hE0RbsJ!qE7JNfl}}eNR;4mI2^TV2Bk$OjYa<* z4#qHTAB}QLEK42DZF$G?A4cs1Kx)tT7!X}FC?Nl}SnY7ocIzPo-A8bCAD=j?vn4Y{6%&1$$4}{A{nhCug`B~S+g-&|(9{-Q& z?+YnY$?7)D^O$@e8S(;4p6rfAf@3&k(*n zW^7DuY#P|H>BJB92)W}>`CF5^*KJbw({CVkuMIx`S&E`( zlSdP;RKSK$y)iv<-I!J;rmwGV^J0hVvFcUnAAjQM55FnB**xEfP?u)EvHqr9Cu3XC zBs9gN^@i9c5U&6p3B6J6!FSOC#<9 z*X{emdXpVqt8pVqgGkx(+vW1fQ<;cwNU{ew^@Tj!4SQTY{GElF_{+b4{`F}gCVs0> z67-E|S0D+R+ZIOReTp7IDb@o>z~?b1_fF+urNaLHqe&QXTqwi&b!&Qko^o1*gyHXO zJN-C96~?e+!a&_MO#|>j$UHqWiWhoyER@p=?%4f5KYom1Vvl*x+rn_@kstUT5d6Tv zuMUqT;QmS-5F=8~Xj0IrD^M)09 z{fIuOKtj8bC`&$uJdNqYBmRKBGlYSPkOC2>$j4`Z!j!&0b%SAz0`m5VM?e#N&B-5; znBpXy>o8_P<|`kgd3hv27zyr$&nJXl;@x1wMkK~uG$ND{!hljBG>&QP)Q~m`+JudS zHr_yu!=VRlho)D^j1hU`1oH5GHfS|g97psCl`wwuO)?&lXk_?g0{{g0LfTCllP6N} z(Khn*4SlVSL^7moi0UX&STSYU*uz}u+za2f1{pz_phpJTUiFY}>d~VshkRkgE~u~K z^4JA@UmG8<^EQ~!pqgVVuN^`451e=&LBQ$5vMm`^C%P3ijE3=d}t6gzyE>&*fS&w zBF5RQ=<9#v4N~|Y--a2ZF(q#=4bd2e0}^$e4#RYaHlNm)k|Bvv5=mxNe<=`kN%{pJ zG@oYUJCD?|(+IO9@@SIMrz#TFP;=v&BICY2hpL{Wu?IHV-fK(ZQIQN#t;9G_!6!0_ z=7Wg}dyBIh3^SY5A7ZJM&)^E56(_h(1v}vxIZt~c=hXM-ZA+k3p6F4$KZp3@gmbY& zpQgNh=}dl~(iuIbc%CDAXFg5%7DS3VjDHT_FS!c3%GGZyNFRL8_t)oqCxROPJ{91o zs{&M%+nHJml_*hzKB`gr3_?Pm*~U>(QLPy%>W!vU9H!vKZ>bjj=huMY?^CeCAR%kV zQ9)k!aXcqkkWOurEcSeG*d$zqA9+bUMESyX`V`+JBad|Ox7o?Y0GBWY9@ntbRF}tb z4Ngd)uf8Z!*{|giAd&&TevsmoWJFD*Gw=k4N}%PCp#=CF`G)co=6#vyeEwmg^I@XH zh57sUZKCtVhl$QV#YE@(_sq1Ag{lqVr**^R|4$ z6DB(9dC6|a_r#Mq@kft8FuweK4h-^^U^p7wgycM36wzlF$ZMAPuwe+=4j*LzV>zto zyM_d&@|8aXh%ZX{GA{N9dmnLx&Il_-3K3ismGsH8Ied+SzkwJL2ZcVPj;Gw%W1lH~ zjWH$9DW;UpFsj6tS1BNMQT$5jOP=I-AIT=gu)JS?$@xAnMX4H&lbaFkt0D1Z(A9x& z>iv)poHuV%y5%57XnlsEa76G_>wxBlVdanU z#Zi<%S|TNfk*Eg@PRuCr$YZtm;%AImGgQ3ui$4iZw&V2p>JNUf9>AO$`L z7Iemp} zAw=T&T`wW_6Gtm~fSa8PMnF8f3zQDL(byY!(J(|LAvrx8;(_6iTIw)(=otKSvD+P{iUx zgrYD*g+G$oUWJ(99Vq*AZ^oO1nAKic(8_6q;H%?oiKwe136eX{)lRGoC?Z_sq*kMMPqy`rs9 z?AR+rTf%yU1=g~9YwX`t57Q2G*mtTZh&*e+y>T-^LM?&U>8u$ph z4ct|Gj5y1=XY_&leIq=I)HlU0Tc8NBrqc^!QNEe7RVlnU;btyFZ1>W|eQ=3#Eyzce z9V&7@8-C0|{K$Ychl?s7_R=10FU@&B zyvjHG98CTZ#H(vegx@L%!VeOyzmH%D5_eq@5hRe`Q&SP& zir&(a*8F%lAkejMl#Hp3h5Ipf{q@i4s&2ZNh%0P1-VdC@S!wKB5K zhKpr!9MZLG)sUQIN4z#ef@?~n#KQ_neHBtch`9BUI2zO;C2Qc+5MrnUB!t;GTyuMf zwY(u+LK#2;MhplH4k%--xtM`fKVsHVUZ3)7zg1EhB3idCKZnCQ4tQ&K^qPs-5+`+A`4kP4o8JQ z#LceZD7o6(Lec#8M)uF3%}s8+2rxU5`Um}mPwL`_s5~A;X|OSY`fm%7qvW9SGVhO zjTV{CGJWvy%G{IuJU%I!W5-M|{>i0FWLd~WS>V9G*libIzN!vKGKV#WPJ_V>--5vn zKN#E(2KR%(<*FfkxWNrS7~D<|}7m-_YRBFhE%sEBkrAczpW`wSO?M z9}MgV1A74@;lZ|*j;0UBwbi)V)y@a&`g^gi(0|Q_6#(9qO?}Q*gSo^W3$YKoF6*&V ztj%P94J}ijV~}@&u6D-?MpMhq3Ew};*_^qQQCuIg{c1hO_-5JmHY*iK||+3tcgAEi`6|9Xpojx8Bn_H={I7WW|647Khq>@|r;s;pKIIFz~Qae3t;S2jL9@Wv@_=*QPaU+&I$O{Z&(L<Smw1@i;gKbW5n=I6h$`H50ye!`TSpS)ghKhykt zjKlW7mGvP!`(Se?19s+`k2Tlx@GoHz$7=*673D??1`j@4GQH ze40ayNWF#2MxV}4T`Bki?4;EnFbz1(eEg?#U>akpLB!qXn| z<*a(~xX8EH)#^&z(5jU~BNW{xD6$RiT+m#hchY~F8{a2t?NdSq{Oj)~7=SKC7V!`d zv#SDP`bi7fE}s<_A1h zx>z5s)T{Y4_XW>=V&9fAUV3Kx)4I`3n~W~T`SfCpqy)tdxXX=+7v5|1jOut`L5(58 z_(`wG>wJgT6hy_xF|4U zf4*eMNA)woe@s7r5q$CG#pN^T6u{E*Zl*Odgw%VKy6-Y_BE@>DK*ykn`_(qPy|RXY z2)iE!g^MX*Szh|iYN!CYC4X4#0;b6YExdfo}>61hi*Pdp&tx;Rk6K9gMkzw ztGoGdMjg0=1d7`qcGrizEWIt5RSa`+Eu$xY3LhVxD+Ye)?9zRHz;kVkBZ8cdy71V} zTj=BUb%qBuuBeces>;Fj0O61M^jfncqz4RjWwTdP7w{9@Jyy$;YPBxOsS>S)4C=ag zjmNuqSd3k{MiM879J+XYn@_9VeiF4tntylxd&npE%)u%yl zi<`}bc*7poP{Hd@Ngfhp_uRDyI<;9;W8dnBOR(b(`4VJzWwjJ6kC>w;hH`qU?%xwo z=fk0=6U0ES%0)YEH7xWv7|`(X`9sfobR|fJrX)p9l2{}iqC%`eXj%p+FZ)F4kndP( zW3}0Acc{*(lN3}d{0x)MLo;+jfQmuw+bxF?T;2&68?mUurMN{ip7n5WX^JBe~ zYRTsFd=DDP>tFZa|M`yoT;%ZkFZln9Z|P64^<2j-=rQEUc1OE9)SyPC|*x`t7e>Xu;z1-DZ<*7mqy8 zHD{(TSUu9rRIw)W;{RvwUAWptl7`{GA~dsL53-Sb!Pv&(5OSI94wIaa>|8Q=e1t8q z-`J9o1q_6Izx}JK?v`3o>tX{l+1<>UV7sr?)z#hA)pc=fe>nHtY2f+FQ$?J!{0NX) z+wcT9=JlEF!|(_jjUNG&q>B12A){b-mAA}=@35Nl@i687cIxu2s@FuK>_OJMxJ zb3b8erYt2h0~ex3o~yk1Vg}?t(7m%W#NzVubUuYM%y)uI-__3S+0yBUVi`phr2xtb z<(TA_FcLN$T`mJ}&M9H|+Lc}^s^O|+%V9)CQI4PRVZ4}VaAc00i8q6ji23=}c$4*d z9Z`zVWDUQDl$CXvSzeAt!ZBv#MLY&|(jOt@=g7lj7w)XBP+s7X>#|($WGQ-Q{0L4E zJKTlrPOK-##S5ZTK;fb6Fw9K3CR=Gl;d;1o{8Mk~Jm?U4o3K%gos2=sMHIvQH_O&A zJ7=J!eMr)1;i~W38Q!p;>@%}8=6RGB-%3a2iVK_xMM*SGFv{p<>rpK==I@MbD8p;# z6x#9mYT)aa=Oq_z=Gr`>@|tKUjyt{t8DQ*3whnnn4IBjIL(hEpUJ2z1UyTAg%3yVTxXw0X4z%9J3s}1F1@`|6pdEnszJUy zQF(xM9dKAmi4jW8B@?tIC-S8e95V9w`ew4d*TgVEjajpUDt^uO1rKM{g$cBR8buM< zonX`eigZ8<9H^quL3vJ4144k(6BN%})F}7@#S&mbo(5CeU;z@^VF4DZlEO2f><1=e z)&wwGO$$2*LTOU0k515Jv3y#jr`Kc}CiyBR)DpawJvPKCT%Uv zQvjFRtTidv=^pF~P238ak!~DCpkt8N+1Rg|y9{m?scDaZWs#aDn8m_K#p_W!*g68# zY*OT{T4xt_9QwRqSj*Ks&DW;G12UBAaY_!QfSmj%z9LpbT#(3Xa>0G}yL-Ge%KD?F z8(-nW<=pdv$<)DzW5t0b7J59WMk2Za4`wQhK<)9=i>|*4X#kao!HH5g{v^Ym*Lga7 zK@Md|ELA>uSuxIN481GI_W}M2Nw{#yYbn%+T@7903|Ml8#DH@Nk1}QfY>9D;q9sNi z_?H<2kM+j~FDL*wcP9r07Fk~6Q%iJg2ezK}8dPeHhx3|P>}8g1(TQs>7PE-B^kTv4Ayr5~hM@(% zUHSeSu6()sVG%Kn9eiQPrlz#`;2Ep&wUZNGF|cjq+oh{SExJqay9n860xOms&Jv9@ zX%;g>PGSRB%z#x~X`&kq{=8-I&>UL~C@fELJ%KYrL((qh?QtVmOOx(Pfpm|Wz;`ot zy^?NM^!sX$D+kYpvlPUWM_3Vvq}?ccF>it;2=sK1K}>7dlc}$ z>JxbB1Ocp{Wr>?x+SSa-o8~j^gtrj8m92e_Xmo8U<#3 zja;+7mVl&le@HsTkZAcQQ0HqwN$0^ZqTK@$ekOkv7-^Qni1q+PvYsvfI!r_H`~90& zFJ7GB=CQT2^%d)|1pgKO2p)i#*LWfkY=7wNlsCu4kl&t8zCE4%Jx?d2 zJEZzGo>RiQ*$*msaki&!cFnlFH4PS9KFfPN6}V1t?fIV)3sO85Bvbio0k0&j^V7@e zY&`t&-SJOhO9*^rix=HeMYjE!<*A*Bvlm=DY7F9#bn?yd==GFx|(LS<>dl0y@gD{tetz)uROz} z8EVr@PrLY=AgyeTY<&(_lHFnYBD)hzB z!e5WT=3yWe;_hl)x~tB1TMgv$cJbM*X=%X4G>{)@?XepPj9-Y|*H+hsRn zQ`~Cs8r}t#r%C=gDIM;;Saf$6Whv-l)0{M0?VPuk{97?xilO?f)@niNM)UT#)!pT? z(mnQMNVj0*)X2FgGD~f|Gq)SsMXklOiW?RIOzd2<6J5jFtx&nR6W+q8aM%#EgUvvx zJxUC?8nTM1(qfi`!Vp&`LryzrO7JAPI}~gRax;?sR^cw8B>C6Kj>y)7pQKG8ZTv=S zKD{banepvl0RZ7sjABO+Ss9yxv>z0(n4+Pi-wl;jZ49PTy9?W5$$EJh_Vm?kJzt5< z25I+JZ`?MpSkmxuRrHtq(UI^MH$)Y51 zl*EwhiS3J|_CYD3>FseFj_L{OBJ8A8rUUt@UYjld);vo zpobiE_5jM?IZyrjA4*Zb%&$+$$9{HM_z5e<1@A1Q_#{a0rm4*MNx*QPJ_#tj9Ouuc z?a?{^SJ=fTu#c!b>vPNDlH zplN|4NP`(>6I4@}pG#B=gC1rcaD>`y3LAL56XZdopQ$dBE6`@ypA`R58i;)U4wmx$gmLc+~o zNlgJgbzS-ntO+V=Xwv|TE~n_(j;0NYoH?UlcB3r`HZi@Nch(k{{=y35No<4$0m?d*cCJFIVQP zQ;H*pZ-$5=p{6FYUXbic?(GXMzoMap&RV9rNLZfvvJP;^xc1U(THA|C0WjP_3GHp(_$kS3%@k*3XoQx{rt`Xwe=M?&o=t7Ihd&gv&TN7ycEQaO zIIQv~VA8L#F5ye%_Bf-O><&5Y(O>0jU+SmE;sprR;`t3Z)c){M*XEbAV7ef=G5;n9 z0-(2R)&$j{jV*F;7i)Zo`;}g_!!yI@w9y7@O2jpw&ZKD_d!tK?-&O~9`1#DiuTO7Y zjVoI*2)3$qDxdOSjG5XQiy4(VDP)(lQ*^QLXP~(r*`Q-A`51VSw-|tJ?g8hGqOk(# zVuO-lEC}YN<)2K<8uaQs2b(N#L;S4bSu9Ef((@ykZmq4N^dI;DOK_Xy<+kjx7lhxm zwG67LDgI5)uZda!RkNXWF$g-@Bci7bW{$`W75Uq&MMrS6Lw1b?%38U)T8$2CNIVE| zbH;#KHxd*a2=wrZmXKP&3?uW8v`}YUxyK~}P;u(c)+&#^nPLsLMp!*mAy;>k7ln&? zdgiXLQ@j0oJA5+aD<4;>$aH|d8f;(-ilZg1>t;KqQqfMSRJ727{#!JLc~~u(5|h}UH#vCTg_lU|E^0b14nsbWm(^DtKV*` zU-@lSadk}RCgi!aU8tnF!q-yvO_kUE4|`AIZmS6P)djRgCc-T6QbO3elM?UB0h*Md zvs4Od?2L;$L|1u9&xq@*4xPtCI_U}b0W}_BGu-zv>~JiB9|Tg$sR#Ycd!{sfE)0Ml zNp#(02FhXDkcHqWr1WpHRdE(EyeW?lUh}&y93${Fq`sp%lkd=qxJx znBsLG|CoAD9X*6VmF%{!g&!>#-7r(q>d#RDL1TTFKhF|-;R>SCv+HazsR(>pKb^WM z0Vx~JStv`31f*e;_Y;l+##R-KhCLSxMc0*y0#qx?LlfgDB@4}ra$e$SVX_@Sg+Qiq z?n7iU(a!tWY{n!%j1p7WDk-5lyB2Og=xcs}bY)EUXkREJsd~%YbHSN!Xp5LThI%Rz z=*YWt8Q&qF;43jK52GiSvDM^oB^gQ}ip?>do47JK!R11AW5D$M3j@{sYVHdOLF6)U z7Px#}`)Yblid%_zn^<5rMRhU?B^>eh@u>@oNnm*k$HjD}2yAFuM01$3RSj00q9Ae9B~ka`c_h!O%J_GAlNjw?5#G$o&FTXSEv| zaB1xp{0-o5t=UDF&MowSiLOoer08heyPkX#^09uV3sZgLg6KZv{^ zKz&vnO{uu^0}+Icd7N$v_t)r&lli|cbdISckI3swjXWID1Yb{YZ8YqM1gnU0P)QB9 zlW{%?ISVTyANWC}6V|0IkG1ku8P{8#)&(%3z_Y_tVoW<^%o{VD!;4mzY$&=!8JN@{ zHh<5&*o6xMa>ynHcLGLsk1hnQ8G7WqKtKB}Br1%uG=rkMGiVbD^#XI?-T*u<8QCLq zjXzqm8ZR+Jd%0Z}h6?-VjnuH=8Smu+`>yR$NGkyV0cOpxwI-208VA-m=rw}9#s+7h z>y9Sx7s0L3`0+=Z%0LmUfv!2a6uzp9W5J{*zYpxeB?#6OLV*w<0DCx3Erb)Dcedez zLJJc?Z)m$S>KDde+ttwb$u6K9#t$9CH$V|(&Y&PZH9@niW>aIB#UaBb2L_DJ5V1G9 zH|&`+gtCS4r!)p5K!R%EWZ)_bQFT5YC%v>tZ=hP^@% zI`@O1!>;CP2wJ6l*|zQnK`RFZ768yJcBXqT05sv(V`WFV08c=$ziX|j=7X)Nro5(_ zU;UbDn34zn4F550ZZ?VXAD)QX2vmx@RA)77%=Jo8W1d*Jy<^%n%W)W;qr)O=u&@_h z5l5rL`IwNkxk5kE+Br-OlqI(v~!F$^z_U-pm*fzurGvC$f~8^E6qHHvLwgXj$)u=ZGA$O-`r9qbF! zI+)WbkPzlQKuhW#vHOrN&vC+as`O?*nJzU(DDW|x!350~=)n+8K1BFPhfFt4?d5c& z2@jZc`8J6EfCT05NC?zw#)z0b$rbwU@jvKl%Qb-*YPgoW>o9A{ilTZe?)yLzB! z*V+z%d^t|ofrgxhx*95_X4Gh^Yn(h&l;=yM+#H@|M9J;Qn#4L{~3WAxK zx|hz7(I_K-;mn8cnnI>0O%n}}s;p;6nJG+4%Xd#pzNOI!PUU~N(xVIj4Pu8QI8uCDpfFt2h}Y#X@XpWSR?1#8D_f}w$~Xoz zbXJNDB5&}aQwiQ`v_|zJ4gFPyiYnD~+{K2QhW!xAX;_dCpfI`wxjYGjOC&SSeeZG+ zccvnrfP^A@JjRon_muPqBs)=2cv;b|t(KnJ-S{A{om1Ypw6QKG8cgQMnRqkU+6Q_Z z3cZvT_g_n(Y^1ADs9Yn5L^%q})2L{-43Q@1WZ^A`uW--4vS&jY=1@>aLO(`HEN|f6 zN*m={rhuoto6g17-^}BYr=Sj8$AMLs&6kpuS`6;UY&!;YMn4Ht> zcJ&g2lr?jXFo9yGEHM&FUNvdfN+1&bfGC$o?F_Ekr#2p!BX1~vBpnSEAS{Wb=)9D< zBB??R786Fd06>P3H>7YXm(EDhze2I&G=q5&bol4Z@X7&ZU>e-4L`%Q^rXebPQk{8J z3fjw}pb|qWMLCLkIi#aP+s87Rw1g<{{m5|?tbXO@9z{g(ZyDFBe=qSI9)CKQXKPCSo&rh z?_r(mF}84z>+!n?fUo<8oE~SX#hR{`vse|l{4c#B-%oOT%0g9Q@5^jyJCqWlqGfs6 zG}@j1-P;Ag;9rg%e}%0gUL|{*5ar5k5$R1p;D~o7?HKCq;cPmy(P(KnbKLV_veMr0 z6IJ=UPHVmssP=-yph|57i8?vEKoVvDY3KF*gdEO#!j0#DJ9n2uFV1QVo>PXgbG}K! zW<&Qh-1wF&k;~6CdFY%stY1Jk%-B-xFux3V0GPL+hX5XY+|r*|`#UTRXN1~+tP^uS z{S(g2=h3N|40#@I=`*HGkv=|-v56GAvLD#ifbz_COPuRxw!7lFo^ys%_vilUOg{7< z?O0S+D}8O6 zYUDv@1-@J=pUneMp?ithcsFHaY{9A*V6?8l%beq46s4tB!N5}cr%y=X8_k)P&cepM zuKLL96MYRQ1I{T1S60S`W%FfggsO*FS0x^x{vNf<`P_EL%a!ylOT+5IwSGlgi@dtF&*D0A0ttDa(#1qe$G;Ntn2%?VNZ{xd3jW znS0wYm9ssH+_Y)r`fzPmhastRCv$upYkQjG^~u%TN?zqw_Z^p_1JxWTvg7mr{*~}- zpdf)Lvm6yjjr~Q0fEL^6@e=yw#@86?ZI$4l%(OJD4ouQ8F?&CG-lwHDn|>l2bLuYa zOFQ$uxn{dwFu}K0wA(PWrH8j3JU!1Hw19WvNR0#d7X^j9p1Nc2x{fhEabR`d!F@Nr z$Gl**qRa(3x5Gc$YU(4JFqjKFZKpRa6R zvjChWv-XQrFRJ@ra@Mu;s=~-L+|ViOW4aBcZ5bo$iH2> zQu~m$^hTeYV7T!7U|>oK(*^eGEEmIxw+z^c&)muW;+(#Jr5n%R41aum^6uxui{Qc_9@c8w!=hDxAAO7?{{QdHm`0wL?MZbS}_2N}n`S4BH z4Sbb*fuC}#PY>Te8@@e!_k3Vk4a$H2?#JP~-`-$C7ZctczJ2-p-SEXPgVwHv39n9u z&tAQKe)Mj5@@%lzHZkqZ+gHbjZ+~McRO;mU(d(a|g=y4wSZ#Rp1P+h?v$ers4$vZ7l~i%K(}R*bmc?%rFNVCQXOq`<_3X!?|PQWk{D2 zF?8_2?1ziy)IonXM!-nl?O%D*v1V=z27CiJyc|!xBV4F|1_$l{{Z=S)ddX@)B%h{% zT%=NsS5KEnHDXJdo12wbbvduvXG`6!k0@Zz>*Z)VgKfg|D^>(D`NBzuqE-rx$y5VU7q|vDeeZU@rPgUM!P!Wi&R6k?B zw}AC)IMpv9!e6=5U`WewS#heL#qxFP7%CK(@Y0((b!0j9=v;x5MjL4QHZLtTcSw8N zt7qvZH+Y+Xxo2!Y9fo$r0CTLTSDHPKJRVhJ+eDo>;!0KO6peGE{(gk}9EwQb4OK;($!UtCU6ETEAbq4wHH%his2Tg zs3)@26_4%F>yuwq^&m%keWLxEuE?bUJ)UfR3BT~AfN^i_@j#-yERWDSkEMlNO0`b@ zjO+ZGk3n=@M~{;Qx8X)v5BadsiEJO$)vx4tH%D8vmJIk1ovV_jHKDpwv3{sXQJ@dPfcsD7dC?Rp8b277t5b<^&1 zLPs&odEnoC4$t|4`aD0t4HV91UP-^k@b&t3IF69p!}EMw-??7u#s?iH{&MPi`p0Uo z5+e`R46>>Ew#*f5>peLE`G%^Y?z*5_toP~{fR1kx91J?sAmK|ma`YGvoa{Rix7faK z-<)2a!39>2m>^g%=*cZwFj9ju+2c<5C031g!t8p_Bt{*U3TLnG?xf>Enlq#6G!yO9 zaI)LKUTRcA#{(r+Q5^MhRkqmz!N6w2_GNAALjQ1+_s+CKM9ot|P4)8`Ad!xLI$__# zF|kMaVm~)*U>WRvx6afJ`}X#BHP{}U1nASDj`x_OiR~W&Yf*7O*dMEg1*bTE+uXF8 z?AzU4<(RpF7&5O3L*{zCLUg@701BZu?wT_{g*zi45VlA1`!hV_u?aReb-1L=rfyD| zyE}4_wnW?z6>%p;xFV1Y~@S8vdhRKmohkbGil-twk%Fx&! z_#7XwtLaDY-@bkR^Sj}zXZm2^19uOJ;%xUnSRc!t>{XecgAu<0Ua(AySe)G`6Aend z;xoW^9d4AcbJwiFAjVZf<8dw%h*l_`$V?5>LBSHp*jP@h3Lxu;NAF(!`?);$kO@Ih zH#b!r2>SR4%S%iI)_~mSZ{NOt8x1|p*ZLnCACsJI8Zp#N4-kUIw{;DR_3#&V!UTCo z%tIoJM^M4>9G?iJSF4|?A1vRGH4J4kn*9R*g}bzju+LHQSRe$03rGw&P+RQ!_|86O zm&YG~>~Nh-NVJwl)K9Q!(B2v1CXZtg&V1mI z$58<8ovKm3^g&_E7}jbSptwMm4G8(m5br)jKo$MH8+RN8$4{KXGP^=ohp5|BH8#xB zm6#(;sajS+A^)TRUDb$4H>@1yi#%U(r$MV!6jeAa$DUNyYeIT6D%pT=>L_CFoL#3z zV3w%OC9bNv%R4ll%S0t=7G=Q&;M_*bx)p$&qdlm{BA5E>91sRW6Y_wu4FC_b%OW#WKww z(#`E8MxK$gAv>M~!D1M@WRj0NwpXouSkumrum8-%Am@x=Ua-!542A_O z$RDAY6FSFb!+8qsRjGojKb9lWL^_j4Kg4wPB^r5ASv=-DbMMNbWs6=W9iXs^F2r)W zkr2?)UQ}EkrG!{pk`|V%iA=Q-3Fxp^Rk|y3VP1q;dc)Wr=oNi?>>w9A#P@|@3O6Rz z9{wy862c0t>PGCH4=z~|Me*$v4hV5M8?wtOzhfIkGNR$1;p`4c z2_m+%^x4A9+p5Fo-O~d6J7UE=z5#6q`#BulxboeL9708$cuT8$Q zaA1CXxS=F&zXug(puZK_M)BE5uIf`=pW+J6sg8?<8zTt)tyCLlGv`k|kIy1Wz}Z=L z3)~5@cdeIZFV|?nKJ~&wdyVQSvUo#ER2dVPS@YKMz2ToXx9Yx2RbId9&H7c>12|l` z^Jvu|X4h@!=7HL|E!9r77cRQi7}cIe(}Kl^`mfqEUi8~S{I(FkEyM>}h~m`5C6}=X zPvjzxw)je&;@?*Aw-x+t1;OdS^%W6*x*AEoo1)OFnR-UW<6?C-l>Egd?SH+Dpa~RUNCkq^C zQ-l)N>gNXh8+q*S*n!XB&j9|qqY?aliT{pXv%k-t!{@tS-@)IXkJjFKis&tG>nT59~X@cx5tCZeQ&eZC6SO61aH$#7~#NDqxl#TwBJpXE2AY@;%b|-tV+1J&Dd~^ zW{8Kp$DD#u9HvHbXzB)YjDbz_NbB^dwc0rB!f_PQf|$7_o4REq-30SuUpx1w;}ToB z9T5X>Aq%Lg(JF=A$Z=)ame;1Yr>U8GYU-;^$!W{Nk74`81eZobFdSaN;j&38`6__VO|5?P7o%LVcl#qT=8 zxNvPYEnif5eScZyH8NF6%@{RXbxfrMWA#le@#vSD|E;UZ`d7G8>z7gQ3r^7d=U=jh z|5LA8Y*)zL48Cmx-?o9TW*hh~S#Y1G^Hbl>KZF0p%6waW-&WtZ)%Sps#Vmm$W1Tx(k@s=NN>{;#}B8;>a|Exo~(In_$q-85BJ|tf)(t@B^KH(GxcD zlp)3@8OKvP0-rKaZx|L#yo^<=X{K=2Y$cJ^?WQnfnPv(Hmf1=n%rf`lC^6-sHLShl zpc>2wUmj1>+D(r|CIHK5RE<#0HT#IX{+TpR^JLwkjO^V3)z*Dy6bzT+&CTL&Q0?#J z+2s=TEDuVRil6>h=i2pj983mVR=w%Ww=`y0ia#6p^J?l`KlT0`Y?+#=!34sehG}j+ z**|AY1G+mKY=!(orEMBmpsFoL_ROhRhE}txP<&wr6Aj(i%?4ZFb?Cn>Z9LdIZeWuz z!IO?@URaG5bkndJT5WGH*xS=;@cpXRhAa~rg4`NU3~Id8q=pr1tR^KwO)P~qSw+la z2?5K$J%$dou4!IjQrHd zNm?x|*=AL5wd0OdYGUz5sDUkQ=gI!gIl4u;I)9?~R-RMca3a;(6Eu%S13Th5QwB>h zj~q@?*LNUiM{S}O?)z7jgvlxDA8TYtqVhkoxha{0qk8h;cziS;SCq={RYSjY$IjW* zWrmwK3$#N`+juGpT|1}4%PAW4EU8yjnQ(pG-I0rzihev@F3~CyM@D-68!(I@ zspzb^dS`Qc%uWNieWxCHKY7=V ze`J$KnEgsH2oh-rHLDlwI|o+JOtt!oJjO&#YR-P3ERQIA?xLIl4>#eS01e)lb(`T1 zK78y4wOU`e`Eou4AL}FTh2StiKR^4s?Yldx(b>1}?z-l_y}4=ccRL4_-FCa#91PGg zjyU*86a?p#*G_J6qA*`v3E$Zbg?Z@)G7Up}kZ%3C!&fp9i!) zzS670=p~5x{&EnFux5^!M>He$iyi*hcEYCtW(e;YONmac$P6vZ8v+1Zbg@82f_XSv z{xS_F6@4FgQ4xlKHOfq-oN6y#@WY$8PkW*;>_uS69J&cefF5Pt#sDEX0f= z-IQA|!Z&r9IAgttCD2aD^CuvzlAIUqonoAnh|_)U+l$GNvK(MsnFTRRWL{F>kQ9U} zg~7x%mn4B-dpJ;0_>C}rOp(e57*uooQ7(LRxeUB{WfiK*M#XcNyNL?(Q`B}Jl9bLA zG6BV#DzNtBG1=F0%fdmC-6-(gyo#`%HSlnOLvLQG#t*c20UjRyIB6fs@4M5RHn-sP-PH?3;su0%ds$a!PC7QpMNS zHRFwp(LLy7$h?+jzAROg#I}-o@){(0PXrViu$01pB=Aj9%WL;v&dvJ;aub}bv4F91 z>Ytn&WWzQ$(^O?_n5kx#_LUQ@U^S`}x0eqlaC@vaFEfyM#?^5Rm;Zz}ZQ$-xfHG9U zkJ76{qJaK?D)}42&)~wr^CX28;P4Kk{a4)Z3n%PgjSVZoau@cIr?#5eiLMi~%}&K5 zer26N`<{OT*A7FOF{B+DeY3{hoZ&qscDYVof+e3&2?uGB^O+Ic0CO&P=rBR}#fB-Q zD(}3-V5JH_l7u)7411rAU;#j^9QjS210C-7Xmc}vpjj};{(JxmwL7_LCOVJMxkRcS zPUxyF@d9v-ou2W(`jRfp$RBgL=G|Q&``cp|>T06%_)qjjJ3_xH)jl~VL6?qfoB8L- z>!0hai|N@-B{0xowLxxQO0~#&=E#9!IXGhOSg3*gdaw#dP6egk%xj7}*dY#h=e!?z zEl>##Q`XfA* zrt$91Lk>+xSN$thV#K7@W4XI4y0TO4O1ihiE&lEi&(F?BB;<}e33 z*zl4o7#o-qX}Q&~JUS#oqJgs@Yy2?tfB1z33fsubCX~$6*+u~hzJnVQ)s-;NpAY7B zAXRkL&x#l*5K`kKc{c+Bj#b};%A59l(TCeoEMp%tW}E)ct|V;rdc9jyO^Zsc>N&_>aX z?~Fwsums*O7Gepk+GAy7o)#d`g+HEtM0ZWX1ue{2?v_$Ip;2S{RuX+%2_c21|3HN* z_V!?2g$wGJlE<#(i^|A8Y~@EkcN}9qN-&{RoE$1KQz3po+lb%K_+}J3GgUG;^~L29 zj}A5aBewpQSXlKbKsjGhW!!nKPC}74cCQ-kKCMZ8Y~b z+>5nf?&l#oGoMY;K~7$+)@zxuu5t?J0#UPa>uzrw=Kdh3!1_Js+9|3jP1)Rs8~{F& z)4`m@E971%h^GV2`QU_@`FM~^*S(C~y#5$bvd0&7b)41%rOz+v&5Ze9iZb$_p+}fv=bJ}z;~HyRJH3el5HVq}0kFZnL&NHR9YtU2fyxRe_z0jV!$?;1oEYKRZa95uGgx~Cp}MH7`Jj2eOEIHyucrha%Q4V z)O1>nkvmWftjS~rt`D-F8c0QM|NHOt@S!Qk={vWnS<@?gtjFWF*%Uevq)N}%p@J%9 zX;%gHgNn)nw{Cg5}2oYEH7e1^v%si*Rs|}|dB>AIR1tsaBMDv%L z?(o4rqK&?4v#YSZZUB27zA#qF1u7$=%w-8Z_n8DF;yYnD7*~2bM;B6e$aQf zx48<8;Zv}WRGSACTiN6X_ajyb6XC44)39PGbPtPRFLexgHSw+y_kiGo3d_JielxT! z8f-PUcx1t?se9&aK@BK^0k^0ES`6?Sw8jTp=+zlml3C53P0!uld^#S_oUj_1SFqkO z#-(nQsomcS3&!Gg@opSnE_Z|YHp zQ8>mec|5}(f8%m`BoU*aVum< z5QD?O8}gzR9kF2O+i5D=)~j0@>koGv430g7Mk^$`wF8V9M)SnOKXSl?$r=5SBYbc1 zIPzOh_ID^ftbuVL^=Q)Xk0=(8Ui7CuQ2~V%kt79**t~@tbet@>GG<5%q-fvMjZ9X4~3c`944EGn^T)y9yU^*saW<#%n4Kj{tM zu1}oVqFPP0g#G;C>R-%#M~c5xTDLqpZ-#7pi-l_5Fu8Vgd^Ibs^DZ2jH58iiw+jjpM$)L5eqKUf zJ<_84j^5*JNn#gC1C{&b$aY;X0A}#Y(Y9(^hPL$`?Bx>x+w-+k*nQ^Qw-)fehE(9bJVvTz7XE)xD~@j6ouIH;ff76V>0FtbO87T|Ix5fMc6*bk_7ay6JbUo z=~t=RlfwWaU2R010q;;m2i5|T1gtMMC4Cx4^+>L1diB7G-ay6MIqEv}lKatVnu#wq^HTiO^%P3>b#pJPo$nj$JKkuQv#Kl7%0t|7^8da#jE)4jkT zF-uR^4P?0SQ@$|b@6mh=A750mC8ey2GzIKxFD>}uIdX(moaONK4FCZI7@nNU7zMZ5 z&mN1F7>mz_A~k})vE|s%JQ|r)itn*lP#SA5J?@9Z1zXVD+tS{UT=r7HH+BpQD>8E< z4ETnZphuNJ9Bgbt$#1|LSeO;7%||FH{IM1nR!yp zG-*@x2TeRrVW0niJ3AbjSe%a4ZTh(lNE0M&^94a>z)Jk{n?nx%OCWu7b3^gmC-ial zqg4faHNtEn%m*0Jut$(l7k|0LulhL{R2ah@(rTYhz&H;2bpFNDDnQVvAupjRdQ5=X zvIkb*4hK##zRi#cg5AYMSK3J1IA``pF<G05e!&xOzsZ&|UVn31jK6|Wd$R8)6*(qE{dh37 z_w%0q@vvq#DQ#hsqMVEANAcbsCsyrePl#P25jtPYq2zVLO43gF5><*~&>(6RqTyzJ zD-a7`vaIGtf^;OL$WmrTRycqvzjfMmj19un;Zr)IPQE&5V8lBeJOtHpXEdwPSdwN_ zkF7|)yye)#>$;>IfBHxN01v*(WSDhhWu+gVT=}H*oR$hbC$eX{=U#)4=Kdp08cDVv zVaiy@=*OBirX6mO|1}B+t&+&Tz6><)Oe0Gpl)3GIYsfGB1)@9c~ZzX>Bew*S9=?aRR2Am0;oBRrvN&h#!~wG)|E30mrRJ+M0$wVM0$wTM0$wRM0$u(DgBG=4LZ=V={_IwfG!?U?s=WG4M`o>sGpIgEhJ>5Y4e&&%iLApd4BOd-1WPq7bN+gKWK?EhVO1m+X-(01d)%rT#`OrQnf@HN#F$Fv68(b((<|XV zIVxA51oN2=B}oAen30Wv9ZA<529N~X^6AZySbyk(2b!yqpLKfmF}(7X9Mo*17vz}k zt|z0A^g|fT)i1GO3U~FAN?)IWuNW&Y%*IOoS)*oSCeZl41;^3mCRtDOnw5kC!HZvT zb<58W{PBaU0|`;@ep85LEq<5;iI);6+(7LhE*cI?Lwe-leSi-p35`lFJcY^?Y}Jxm zt*V+!JU5hRvytDXc>2j`Mx6bjJO6>szG_Q+9iXG?F=YX;Tp(9Ncs)gQ zjy`Zm5|10_XHzJa`L{ey2FtD$ZjN;@a{{U-c&on>kAp!CA5T_@m%1rhOSN1FnP{)1wP~19- zmoj7a4Jm)C@!%<+k>3x?%O2Ly?(+~fOHm_lM*iuYKeboCH2mnzo!?ikb>#W5q5ZxV z0ViV?uI(^*rC_W%AtZr41*oUNO#vwJASY$#B@BQbbS);S_`u|(P*{W|pgUfQ=^{%663CCChDv0<0MDmNI zs}suL0e=XN@REjcX8fDai_0O|(X#rgWV*;eJUU0-9d5xV;pjaO0yNKGir78tD%!yiRYseUmmr};yW%R%Sj5N@uk zkA7ep>yD~sF~z2J>#Mjdnc&_;Fds2J8{zBdUbD^T)}gsYmx4IAc?VY!(kts=y}E&` z<)PG4PlUp0cg1g2@<0awW;9F5lYENg~8q7qzBp zWZ2~xCJ9e#*1lc+Y;T8WI@j%zOu#f9Eqjst_kGm+zC30A_bayHS9`I9Y7`}o30BhP zXpIFhi<<0JR*0P*duzzSB6=-13Vc{3Vyc&J`nQrz->%;D@6>c$;J~;8 zFYx^3>$5ZNOD;ir`te>%H)77+uoUw5lKlui`ej0meEJZBb$LA% zT&2Bq^p}l2y$4J4`yciGsqfgI`XX8T)#N$9rm}t$S-+*Sey6PFz7P@~VAI=$ld^PX zC`lj*B;PuFQ1f@uK9`|#P`{{`xoOC|nfykB<$rg8(j%;^rt76;YCknh?Tv{XQ@X)e zdvLo8n=^vdr%~EWxRKbezu}uPDvIF5vA_#%i8C-;6dnC@9uEh z8w>`ncuc)Vm4}3` zsrME8gq^q5c7UM+CzG@3R0B+IkaWzb8zwAwCXSHA zGtde5Hpk^oJrw_K;R=PU(AORt13wwG${#uq6PH+QZ|R1<1wVxl0*FknLLxTsgt-Fb zT?h`O=R2_Ur&ms23%o_Ix%iV@gQfsP%f+~Q6TV65)m}24S7ShjUir`{8v%+BmpkyI zvMit15)L`@=MiUGTqrW!iSmbV$yFiBWvM!Dk4fUH!r1#Mw~43r_}qzHc3z!pHLFu^ z!)HLBHLE+b8eOdh*^^eox`18F)EX_VwpVWlH4DBp%zArPYqlEjwFRY{rdDg27BmJ~ zTFt~(8f`4yXu|%MWiZ!Xt7QhL`JCI^gX)Fx=WA{O$ zsR29yVD}asS>mWe4~plJf7Yw>uXr*n)3Trw%e;U|FtuhA>NaOq8$V%gtR@W?KblRz zJblh;U6`A0V^(V*Dy-J5+t#|fSQvgc8}RoQ8*b7x*LE#LEtCq2!w+p23pD|wU7Qf= z*J{ix3t?!N#An=#rdWpkD44m~5B9tLV0*g?MRGRRpqd>zlF0{gi;TM-j8=PWkBeW8 ziIIJN@&*rpDkR`@TnC@ywu8Q4(DajMl#Scqa!6johGI(;f$Hd%^ZxBC^k2uB)xI1v zGN`I90Wyz1@}2QCSmHiX{hXnV6&4A#v1A>u)Qy`5H4Xy`)2PpU=VH)mna23iA6O<@ zz48#x;)@jDiw8_;s~+2juimUF5|Vx^Ne1zbgt1hh)MhF(=lls;}i+NNkDo8B>vqPxSN|;ksbVG361^RQ+Ej` z<^*4&QU#cvu*m)r7oDZBh-*PeZs_9R9N=fqQFR$q51gKQGFS>noe?Bnl-HZoIKKPQ zETX-C9!7#}bi^K1*9R>&hoSQ4Alz71AtY4e495l&?n=-H-ud}VnoJ7*!_J5y+Zu+z z1NxU^2X1TcOewt1iIXaU4~v;~bA%p=!eMM?`Erucu!L|@L=fqU$gjZgg}H1=4OhMw zz^-iLFW!KBDX5!f!F43+K77Gl8Guw1tAOyFRmR9$@*%bUunqfuAlu;$&KB_6k%YlM zc)(Wz0fi=@NIez^`2c@101k)$Mu9=km9gup$9`M`LJE}z+f^k zD?JxdYWbW!??xVxsN~7KW!Px{9IIgA zSmrKHQxKn~@a%zec06-{xVo7_@uZpu@K zI?u#Pu^S!dUEaW44D=nQbF;)79*khPWJ)Lao*t&NkYA{il}H+n?9s%DGPp%vytH7I zs8$$}k>xSx9;_NgPLg3+_7gFes09wE(AZ)*_GsiR@V9YE@tlUNousy>BJ(qI9~5ci zP(Ucu-(ojx1Gv=0s6&@#;ywlR1~jZL)px`_DM+@163*#zJoVuQr^v$Vc3)YAuH>kk z$EzW?Ole$t;L?SJvKV?q06R?MI7!B{3NaX&)6lq3w+=i5jD-BSN_u+w4~F(b{f8tqk@avMPYFdS#ES?i z$51i|VvK)~WbwI_g!p?Dt`|imNw2}k+^7gVVc!?k;HG|&7%0k;)D=uaN){1kt7UtW zZIM#_$(gYpV;NDOOeuFmhBIaWm8ZE@fCRPHY1Cy5qBT2J;3BKrf)<%Gcv$_zwN(6L zP-VA(-U`t{>I%O7%QoG+PYc3w<$>mUC2SSXAwylRe|tpg#;Qmym~)Pl-a}9hKN$cx zb|?&Ou@P2O=RI>_K$fv-8`sdhadQ(n3mKUh*YfqW~z<=XMLTcDw(=l2#K@zZ_j<&o1E{$TzyNg1Fj9(bQ5 zA}z8fNbz1f!Uv|_Gsjt+e7bpxS_ufOh0nCafu-MP&%7Bh>NR}KP(0J>j75-MX)!=u z7`sqfFqyt$B?-7&UyhO6JD`VL_Pv*U)J?`)Rqx zeU68_J6%U3b7Yf)=^X7yqPAP2e2d!VA@k%GrGBa@Qowk4`ql%?xM2UD;M4tbdh2Y5 zq3P0ndXLbjz)oJjQ|MQbK`xwpd}GDj*z)8cW<16Jxr_%7OoB|!gwP5#1$#V3CvT{m zptZPi!rBT!=|w{3KCz89y;2CUc%g)v0{bn(5k^-U7W^&Tqs!rkoZzG;lrM07kwND7+4PC7hzcanUw9HFUR_fM>~_Y5v8rr0YDm7{!JmT53QaI- zUG)m}lq)JRh+3mOuJz6?{}^@8px5X|17D+^YEM6IG?~WNAMSCZRZO@lHS{1RpE*G^ z1W{fRSx17iLb(851v3RmZ%~O3BXB~i+;N5H-)JoCp<15wGnqAo=4!)4mx}l7Bv3GU zas*778yr%;F!E21OD@ATVY56|D9))A@yXBJGBaIOtL+vM+58i_)6=)pL=N~mZZ_0e zsN^R|+#?IFA@nw6A|5H+kPy2_mHUN5t|)3rx7*Zhx*COkzg(bo6R^?CdRtu@@a6yiRI zUoO0U4BT+>r3e32Z6Yw+s%;_z zcHCFYRVw^f>^6GlQaZp$Ka4Emn^7;ykWw)qkfJX|>+CX9C=C1Cb}z+j@+DCW@${U$ zeQ=3Ewm-PITG(e&39G|sc)}OhlhE8e)Ju!mnMW?iWF<3Lpg=aqo;!tIa_C(KD7Jpj zz)zIhrjVto_m)J#RV>;=jkf`_VAg2`U{4zZ=YtcgwC9cT<0|yIR+ZFWh3;SB z)hFG*pg8HdM&CJkwgVT!KnQJE!nO)0tOQ%mvZkyQ+;ayo23UicDxCHpE;)$3U}EnY zs{HXbw3BCgdgn_%8VVyh3)T8$cYvAZK@6!1)?7T}k=1Q1qN#?_AQNc^inI+y+W!}Y zuCdUt37`E`FN{%q8!5mqhPhKdN7~{!(x>;t#FM1`Q5uW1Y5P$WoiFJqI+-Gj&tauR z+1QFoh6GN(7;-W~Kgj{H4;#g=c~bqDBt4az{qCP3*WRe8UjM6w5YfS0aO&TmZ+NWT zv^b!gl99Cs`fRDymMZ3x^L!Q4NM}VasG(-Y(($MEY&cu0*q=nE8~{f|>pX6ldOtXb zm4`G|fupga)Nip3*X0kB??j$Ix1r|r zRyrd?6(}c)U$<|Wjn)AwGCtLN_^Y+o?(7~^f*s5N*2H^(=5Cb+2IKn|FVksuTGk%a zv3~)W9R?=sKj>+P3>4AEhMsBw2@NND0FJ!l*c# zETb!dOi&)b<&IaS|7D8I8G)m)Y{lJO$!>+DZlF)^{)36t$VFqqDdQ>1ky^eP$^=L0$&lSl}0a7}!H^WDpd+u@N=bn|~=(C@bQxO1a!#S)h%MQI`uYCB;kE zK_%bZg2{;Ss)8yQv8vm86_#U|G>09-K8NKQFEuc$y;5CdJiU9u2RXSFD?sapRV_H( zWV99{K7W^7vnkhc@V#O>iHbe4cU=mbv_=K5F?(8Nk912zi6EPHc zgAdOShi=Yz<-3pQtCTWxp5%))upt~XXa!C2{c1Tb2=7$X%mOY$_ihFM{*?4>p%=xL zO4U^=boUdhMrf3+qWY@TAWZ3-$@IS!>H#u6(WHhM6|7w@m|_`u5MjD->@TYoZfj58 zFY5Jr=xHRBu!pZ{(TkA~ zoG|376`fr!KPqg0-Nx4@lyYKcC6NY3hz*$o2*bC8`=1l;U9=z6goENK1u?@#|M$3_X`+S z;-~cEMn`9FKS@^-DW=ap#X~Rbh<=vrCG)b2*4%r4xbeSML+|PIQ4%ul8j~@UmBQ;- z%o3G&i?qCjDNs+(qDI-UH%wrrS81P;=&e87S|xf%jX3%O5gu_Bu_u{2J;@9PKKWY> zMR3L*z0x=&T5N^JzqKea-o{2!=vF6jI2Glp0U`5C%=ta;j$9iQBgO62o0<@~0mnDH zsiUsOBRw!56ltiav?OOrp{VR(oh#jsVO2Uy^c!34g?oe_k*CtD0jcMsv$*^Q)B3Ne zGI$I7oA8Em?R_c8hj%#}Ypxe)sEre89_!U2p~ibcRTMQkOY(5?OdDaa`LPp_r;1|v zgHj7EYDq6vtyt%%it(5aenKgUEHM+&0Keh>v2mX|o?^Kuww?sRVi=1NImFA6GcMk@;ORw;0fy*o(i6=mA21UiBIOZ6(L2_e;nK%40@{EC!ZRU}$)7D$PkCoNaR&rm% zL>W3$&5!SuF5=Za*8)%D`=WFnWP{BuBZP1s4L>gFoQbq|NF+VpahPD|2lTgF0Q15> z4}lGe2FT?pLjkpXxQ04$CU+bf#r$$WcsS{pEMTTimbRGTzwLC}R!MPXwa`YNs zRpH}LC;}fh?BkYw*zDt!eT;Z36nVKpUvDv@$$UJ}e>E{Ai-B3g-^1t(=%fr&4pS*l zQz=Js3f_Du#KLgq1cAdsUL@T)VK%#RvdB@dn zAr;eso*@R!ey~3f$RKamOr5#kM2}@I!rN={uZMcC&A&d?d%H}b z8GalA<3Mh-y#)=65y%#We%R8DCM$>~v?X@|r|~Mx0k_M&aI^YP{%~O-FmDZ1921OK z{EuvZurXZ5oo*ZXwPyCIGb`i)dEAk0y{8-Hp~zQ24GF2R_F0S^wbLB(UgGfl522{kQ;JHr9s7Q=5+yA$OesHi-#IAb0H+50x!Gy+?~rl`S6Kqs3@Kz9f9gv0!i9 zWiu}~ zn{Z+b3V8DzOaReh`18^*+@`QyC)_W9-N}ox8EYVNeap>_D#8c1vp+CZE>=;m^8wvO10>V= zqa_ux$7&emr$sY|X3>yNR)%<-c@j?{5!2W=n zAANFxqg|JF6iwye>n}tX0j3dTS_oR&DQ_8CO7Da0I%5t7%4@+|A(g|8YPEpM$!g`p zDYKe|5taE(r%XI$p_q^nH(0#6=EPDK=g4Z2lvAk7i=&)3>|`kAk^@qPQ7&0Gebwv( zS*^&q0p39Kpbf+Z>RaBc8_q*s+q5>IrU#rJm+xwVj*clCX3(34@4km zm6y>td#BLnWO19-PtYpgTj{sCWME;k0pG7k)*8PYz3%A=Lp#*wNAEiytdFAD9ffug zzn(x1>8GJr`y@z_jwBSmR+(JI3Pq_0u>#h|YLClY8vBv8N+XJ28!O%r#qB{;D&EW} z-W0`82Q8_1E2DTz6dw)RQt@_1@ir9~awM`fNZ|F2w~Qgy&A}%kaNUT{h-eN--YFKg zGFLZK!3o?>PGDq<+JpsQNr z;BU8+)SUocP2^>|wX=Y4t^&NB1H8SRTx$v7)l05Sx2IXaw^jlEGza+8?c~Z&0I%Kv z%5*!*0=~Tp@S_~yN88DLC=PrWi8;O<9Y6ebG=G@=C}XI}ov4QU)4jH%`NQl-W#F|K zyf+WG9nBwRKPm%!g8_g0aNE)RVfLdkz&9E2_QP#Q^M~1w$^hSDz@I+cb~JyO{iqD^ zZ3g@(u^nabq|qfkVLg1uVh~#UL5wBE?Fu%i&?RL$&%R<{m}J-=^58Ts#Hl3A*s0CojfDc`pSPO)|i@jo;>F<;V)G? zA9N#cuBT&EoPgO$t?ufYj~pt|LcH;Ohy?Xz@4u3usA$rA_h^MF_RC6xu05OH(rP$G zeXZ)}EZvG+$um@4n^bvyj`WAzR{w4?%{UQHo~cel+ETI(#2o(Juo)xn4=9*TUy{N2 z#PUW*=@U9idoiasEUjQ&oIFzLmp_P;Nvg%nPhtNMk1zQwgY$VU;rCJnYMj5@!+ zBg#LatbgbQlF=L(^eiMZGR|f`-nc#TOcYwp;TEqzbhzigf%TjDplVioIghP*d0(W^ z$L{#8W6#h@!qJ>wJ&VPGPJJ}7eGD^r7*xz^Of)F)FpAbOHku_dG}LUHk)~ZE57UYM z*&30ba$_oNZ!ufa>&}24c35(oKS;bcN4LVnUa#7*L%;e;7-8#Mk2grp8#BkPpZVVW zh|f)>(Momt(h1&RtAK&aS&%{ux)vAFoIr{}Vb ztmiWBR`6xJsl7C{-*wnoLb9v5F)ruI|A)ZQRi+iO3C#yhmkoDNwOW2tnPN~e#_FKi3Tp_BOiD67=x zbkoLtte)6oHTA?^Q%@adQR$dQV>h<?-IJ#4&e?$sOG%dVyUUeo(oS`Mh8 zqI~5KGjg7}nFioJG$dHy>2);;Vsy)PHw6b|WyU*E@6s~WAPS8<->!K)Q~@87%RWi` z*W)PsFh4z+HYWH4$TH{S&_NR4)KME9^=hHWpYs`(V(P#K%r9j$%t#%Uv9_X>N_dKe zjZ~7|NYqVPKJBw-E(^aBD;=I9p_)J}n~5V>Z46{;Wad`U_glQ_fwsdA`eOP+5zM4{C|Hb`=clpL!25WU$gqx$|XPKz^oW_l6 zQQ_LHb*!7gyomI0olf()q{FbwYbSrJMS;Eqh8zye3Z5U%# zi_949*W;fMmbcEurLzqBcHMJ*#~$C{DeG`s3`zZ}HK02>6ga^GFdYrk#58s)AIQXs z9L5HIK_8SNpvOA^yxObK?rw*nMgSwP#<-aahVz7Lu|!kviU#pZMN~BsEyHalDHA== zIUlEQzBzpP{MEAoa%AVua5_daISP`%rraUZSZEsU<`Ng+a6<|gh2gEZEbNNp*K>-7 zHVK0FiQs|0qfw(PHzS%Tn~ae}7= z8h)DGeNMt`5*ZEqjD~~c&Mw&xuDP`f8U`5Ed{BkYIr_@rTYE08nS$0L1_7uoBs@DL zJRd2|R8pFkO=(BAJA&JwY^nmCZHV5vdmBI{j9=BPws~P{wT7kDtX93P)tc>kJD|TTWm0QnF_VgQp;)U+S-VuuYMS)Rgv5(I z3#)5#S|F9C&L+?wbY^N%4?)y1D^lA7$QWQylyV6wszNZeMLqyTJuq-s&kUFvK-0t? zqBJe2MTM_mNI15O8i7*_8xLw>kJ!lsfVn+xA;L^E!o)6&pb-souY=z^P3X9>2Z;@> z)q*xVfwimE+Sn%lWXTus9gq&+ql$p^s6mE^R)~jPgs>q760mC2T@Ct!?yzC#4*P*_ z*k4N0E}$#uDXbAh?JyibFSoient7LFpbgklAak#KVOb{N1&9QJYIg~}G*MQ27y5<9 zt(MjWw6rdOl&C0V?(HHraBwD;G7%R*7A&QMpohgKwGN`c(?#@Kg#R{-09ywd>form zT8n^fG_ZSf55DhqXwWp~hDOD#Hi4`)b^+D^{=zRP1%q;2G-Jaf;5TR!3)XzYf=NQcXf|nTaEgKOniq%_pga6%HffCTIjeQM zusJngsE`N$X5F^d-NnN2y9vb7y2XZ@8Z72nZP$Wf?Lw)rIQ#$L|6Z^3m zGh7ADCeqUUaycDg5YAly-LyKh-4<>o&5LFiC{WvNYG!~dy4Ag^?KLf&4qW6d3z67m zyQM`xc4^%L-Panl1$LkmE$UsZ1N2L%#gYNhy|&l5=rnK%TTQ^&Rl8xq5KNqkHq894 zxr<%3*cMRhngQ-0&`FnfvCF$?hh5Z_od8TxH=V29W*6Fpue2MsyIRn0; z6`j^aqe;*ccrZ5FJJ@&l3+oY@YVTdp02_z_>azjkZ{jB3fQiPYuq8k|6z+7eJ?yD7 z!$k-A&^6LEBWwU3U}u1840bjIjlo{SiH1S}`wZkfxe{yWmLbP%G+^EBQ3ty?M@Y0l zWcUY8gC->h;xo(=QWC zZ2M^;)+^++A!NdW5<&$(vM!M)yObOp%)PAyD7 z42%vcHZo3fCJ}33`+$&^*`d{MHIXl9UI5}?8u3uHfMDqqwz@N`(M49EIcYU4#;4FB z&BlYcLdt96Vr#UZbQ9^$v@SY~1POu$Ei@d1w#!O4p^@FN8k`_B%k+cLj^(i`)}%ID zxJxx*(Kgx^mBzxjRnf%2E(V8ZtI@qhcStb0nloPnH~N3+Admll_TGfMZEHyv{VOp0 zUR!Jt3g&?_qm{8w-a8rAanjwlef+c_5fa&fNQs2x*si z5ZLpsUAyM;=-`GRR2+|H7Nc2@P<@D@(&4991QIR{G-e>P_><${1u#D}Z^$6?Y42EKBlu12Og=NpFx(DzM^g8IQAlJkFMdaatF~`(sDa z1}o8@W>IJ$;iHYiIB<8uKf{Vd-3MZP@V#;WEC)vo#Q6Ao^U;>XsNpug50NLR_FKR{v_6N716kxPZS$G9TZV_Ax#`AsKa$pR#F@=%$9E*PPpYA7n@iz~CB zx&}2O4`_FNzLAtfQ>rC3U&vjV^ygtxAVwBMgm*D`4O^tz!i)Qg7k8O47cm0fFNT@C#TGrWnVZRKb~ zZl$Gwt;OQ{O|A>k;2*g2rjG@D&hPV20Q#o`P?rESA^>$;0kjDKYp~_6!&n9kpfNZ!?|etAaGI3WaRtn#y=HgjED!6L3qmZzGI!y5+) zawHu_0?>{Nfw?E;ZHd-lw8uho4qfRTXbFpPuW79pXAwQH6HJVM(tdSd_ zqJ%2w3k?Nk2?nx#Jg8v1wN|u+2(j%JDr7>q{;imYMXYh#Vjo4{adM_NX^DcvI}j(9 zQIZ}Nf}}#HRH&Ez2$^kkOkO;cAIloL6~QP(Pe5f!q>H5}7ba888u?_y>l zQ|hVu-PE<{peYuv{bD63vs+Qnwwrg$acH}Edx(J#n?p0ngvZjf^ft*CLD>XWBVslt zWo+HC>9A16g#iL9$k^luT3G0Z1DMGXV<83fcYH=&wjxlrEi^moLs2iAh;ADMjJIu> z-S$;9fXen<$M%kCZ-J2?>^44EZ^Ne}_<@d~z5NWjp)#ZhG$=G;zgna*L7Fej9T z^o6VTKNoPwG?WOCFz6V$Koa4A>Fi?u8?0P4TO1ZJE7XKos+y39M>On!i0Y6nkVCOp zBu-L{b?GPOmz&_1up><26FEeGgqr1d7$D(a%+Fv>0E-iV@V5ify z#O47~v*vQ!< z8r88fhb@C++8VJp!yngTxTL>?UG;ZHy=;&cDRVUX0pr7pTuxr(sst;8Zqo$zlrizF zwiUp%4ivmEDv^??r;|%`Lf~wXS5g5$3A6snf*Xx64^02vvD`R>xfqOagT0n~uN9wY zL0L_(XnX{KyMx&tk&d32!ty3uEmG#qJMMxlGts*sLiDJz3-+d6@K`0G*oD9iWZXhx zuwo=qBq}~tJtj^Hp9wa^w#7!@Wseb)fY~fY5I=gUQZ;} z>{5g`EULUSBgXcup7!IM9b@PR+w~}=WJT(0R>ZzakR5ChJ;pUbcqs?x+JP2cmy+bX zlrJm!ucszdI&~JJ7?C@Q#i7L_BP}qh`-HkIEWXN;Ykf`F+rR{9oColLr)}6CY6fZv zE>m|P6$56*9T&V26BOWraObg(g;zOX-=$?W?x&;|^|=tgz>%_%>%6gcU#B0o~sqBAKm&FNO2qxzU~D z7(QIQj9C1BVgt@naIk{ckby0O5-AkdQ1%32_Q_Q*Y#aNJ1_L&aaBQYFZ7-MK#L`$U zn^m|*q-?DsgPjQ;=GpyCYR<2&aJ{><^$}QVAI3+Hc35!4MFPuF)0e7uptcMqHFQ0b z<$3I!VSD=vF_{Z=u%_IQ=qZALcvy!enab{obr8@%fQ%w4ifzV1u}|YaSz$Pl>4!=o zqbQ~7oBWMAl#m$})c|dbZpn@tOHl(-U(D9RJY*X;IQhWeO+Bm?kZPp~Tqr)5dUH6J z>0e-X!XOZ;1ZUo~`Fz^eUCex4Y~ZgG%kX@5g#@n$?WQ5Z43uW&E4>rlVL&saamNPi zVR+FP&*Fr7YB+F;1a!j&!huVq4WSF1i9`b$vGXSQ{#YakXbr*@cK`7!%Y1)}MUDfo zFqSs~z09-VaoyRB_`=xW2R9C6nSIeKvp01P%OIlpnZ^$*lbS$HT+AqgGpNg^kUVoc z0Dluvcfc?JGeQzh1n+rEVQo7Ma#MH{;FZ7-_oX<86eP&do?A|7Pe3vL;*fFxpcpks zp-Qo;y$&roUvOIKn*|Z%Z;{jFQSJr2bY7#oIx5nC5XWuSK6@eiJd1NE?yNh32So?i zN=F>pD`bORs>1PDeAhLE6tGMEvd!cTM@y#JMUEp}p6PjB!HUORahqANSTkU;A`0wK zjC-n5Tj*T_h4JV=Jw_X%OHV*YeaLlL2sU9%UNoTV7u8j5sa(_zngqsc%xBQbeA=6t z&sM9-ZYFu1dNj%|1KnjKy9;jEk%%1SoRv)1EQGqImg<@!)^)X1*B7ZSh*9aEX{Ov2 zlx!-guE}CuQ_FQtC57${E|Z&dkrWQ?N`>izq*A`oY`z$Z{DZ9=L25hvi}sOS(0TT4 z1|z|C8x05@+KeXrv9s5 z<1hZWD45H?{0diXl1;B?=6~NW^Xb)_s>hDK&A!v_VBfjTz|-F5783n|BkAD6`sY{_ z=MhI@>^~HOo~z_Cg)xiyFde6JfRB~~0x7y^7}^BK$>{}pRJ7`;HHD05t(!YF$16()f-nFr5uPta-1#4X1Gi9HomgK$i4%7+tqk_Y+}bI6dG z6-oRa+jNuMnm5U>>4LRtEaVPityN>z-6}$m1YlK#XN^Rf&UhCVl_1~QNT07>Xmus3 zfz8o4!kB}McrZf-5g19(14B4_=6eMj#V zPoWNP5&+(lF%acnT{9WF50URZs&bNT+L%)H2>^&=@PV65xfo2ZsCx5XFk18t%s>P6s?Sj2k1n6(HhiJc-u z1j^zhjE?z1dmpq~Oa;2QwN`;TQQU-Cxu)q)9%*DM3{&d}y zRLmoxV|A^7nd3mKbO|N!*|i@W8Z(;^0f*oMA-Xv#SjgBZ8iZ7vnGhB!Bm7(pQ+@*7 z90u}6WXm-IT#z%O;*$o?QpTl$NU>=+9*meE$>In#Cq)=L9+ z>=rJvI1O}Re#p}h#N}ad?D&t`;ee=%z>PDP$r-=qTB{7*2@e5(*cBNv^ND)zeGqfL z>y#p5%_nm^8KKpPb|eY9jC0?P5p(qg)(H=X@OApl5N|Z-WM|mtp z*(5fr$4V4)eM5zLgsJrdqaJa`2;ExDMTxkmoAB4XuEjMQ-z;%&a19l~k=x=$5yZCH zZJ@Wz(_8Z*x%~BYvKaR`uBGO1w;5f#Q3i>jhbW4;*2VJLM))^@_y-X`Xf{i|%}rHK z1dbjsB9ux8Izy_i+L*CrUYqG-PV_@9#^bMVLjapba5yV2f&7+LOS7xnij-hDTEYBy$PAP&x8fVA<5bEI|)d)!bB#p>?fd@!ht9 z6(Cy~M}fxF*xAC(Vu6L_WGq_QVD!K~tJbz9&scH`CDrCths&nthL{T2Hs+M&^s2~= zwm9Y&u^G$qSR>zGoqAjr=OKuMthN9rFN(#X3E)ZAfzEuov3m<^DQRo6>$58LZJa15=4%G33yO< zh@HN$1cGV_aiYGe4(tupZ4R*=_*CeJJz2!V<+0WwlHuOq*Yu4>&mfC}M+ndWMCMy; z74EA@JWxBv$|r{@I46dPRw>{1j^^MgMN)&;E^X;=EOaQQ#?ocznOQQ!@)UJZLFk33 zp+vTZmKXy+5IWApZ&aLm1*Em1llWe^q5<>boMW~-fq3rvimqYY}RBDTU` z+D*Y{SLpS_YgTryYEi}p*4qjeKvjXM3P>?Tl{pkP6skdP3Ajay0o!jYpy8~9y}X1- z0qVl8uCWZu%%`*e0eh@NHj0*b_pLLk+u~b$uR2y#;)MO>mo#K+(%8l|=7^?2j49%Z zRhy{|;s{m7sz7Yym6Bo~$mTNYf2@F3f3tc^rfRCOq;847sSd=xBi43#rIlS{Ay`s! z1NeV#Too$jQ5g6&mQnSqbH#?wfBnmEw(kW-05MzS8IgZiN|%hV@Q{H+2Q*@CtldRe zKArQwM&?L>Z~Ly0ZE$i231H`jbW}nSYALakDRCU6Rn=&<)3699#~FsE^Vwg2m7~ar z0YDDhj&Y&_6?8=GNK9xOCAOERGD74WDBO^sT#UDn!T^6Jwrk(9@&s&xZQlj>z#0iB zk|9Nki?9oNUMyyyrr@?*-b84O*k=`Fk9a?6emO2g!+f$VSr##IRcQh^Z2yU3q+rVi z@3b-!2m8%g%6xbN*_iB^5y8kCoHusbilfxmoR0hI2?9wb(Nxa2)r>O>2d*B|hO%F`#{7L82m7inCW04d#`e z9%Kp|Lb<^dE{3izhz^=3RJa^Hm5l;oj>MI7Y%cmW!RJG^i>jkx>Oi%at3DoN+|5hV z4rf#mQC?lv{S#h>MGp;?KkT;^0qmWrL9BzcmYdReg>L@j7yn`W;zPP8BO>u2T?)KS zDnumyz(1LO*i=w8vq{s4>S(5DaAU~;*?Np-m1MOM)y7&OuHUG&tWHH{vRXz_@1H>K z1A$!R7-1A;w#9ZY!6sO89X5}#3+-(r5E8M@+Cq>~w^3_~hNWG_aTKwtGo&~RV@?Z1 z|83a&i)8ly&&`*RE|wASuze%*qtKxL?%1CNCfgvNF2v+2+PWTxDFZP%u1?PXmSbN`uO~eC0Q{?E#XNjdjbF!Xa-Cd-c9LdI-7cFb zI7RShn}x9tm?GzF+&82?7U)4HhXT@UuKFnlFD_?r1)|-vGY`ZU4X-7V5w%%llRQMf z=tIJa0X9c7!bsFWbTp$fn2{R1c1!FWRt|ll%T1BEis^5P#HB~IArcqX#`oK;7TA1E zN3&p%u^$a&?l=T2h8#JP>ChHqCm&6LjgA#XZ7I`II+E<#4656Pk)L+PK)@qJoPo`8 zXvFL*!kQ`1d2l$2O*2?6(&hARYF^!ES)p`uRIJptt#KPI5IkjC5F;)x%$8zKIoEhU z=sQ5{f*@1^We{bkgfw;12rD;PziDaZBJ0P*0@G3s^DS`euIcNPk~E7sc&_fxP!UQw zBZ3%hm6so1VUSsbD)pXrN_If4j_*o8*6lz%jHuQGGc`IKZ+%L2aJGe-NZ)EOfLG06-=^>4dTj_B34AFy$xjnE!8hxXv~=C+E#&*>RBt3%F$ zhbYvRzU%;EiqE3wYsaKJGZc$;lWlHc5%1cDx|ZTQx{NvE0d8hrQk*+BjP^ZGZ1%B0 z0uzx$(uR6O_yo?ool6vPn`2v9Dyq3GdK9yF0&;XJj}$X@;dd@KSgcaavImejBS)6O zmIy)V$Sf>L|ISX>eqnCxt2-;Oawt4J?el;gWpv_8$aE!86BA!6hoe1; zOMZP?Vk0se|1s2@&If(vjpsl|=Oy8T$X)D5x|O|JiXI{P%n9>dK`)vdIr zx{y_H*ryqPJr&Wu_UNw#}I-HkY%CGES;c^pmfFhYc5*b(UuN^Aq?<+HI=Vr;D8r#+EwRnljg6zl2RIk^E5nwM7?QDv zWsHFsI$5l`3Ik22H^NybFhgnx5V*X(8E!hqp3vZkU4}nYXl3jViS*~dyc|bEax6RQ z_Mn4uXQ?FYDqe!pZ2|cJ=W&{Ojfi2e*`ph_V5ex3cZ9q%iC8jsy zZXmo(2h1o>*aw1E=o;+|%18tbH@wc!`8rw5rnBpULgdqhupGFttsnrgM34%93m#ac z%s25JGM0ed+Niy-bf5PvO$tx2>ogcQWIeGZUVVIX1+c~Q**DttTJl_NPx0j<*oEg& zX~(7IC2~X3HEN}?rV}-p$jWJlNDq!_;S}~Dirfn;5*Du@HVbOeEXhSQEbe8pkofU|Fq2?kLhaUUGk4*rfWCnH;LoWTU`zSTELtiI zwOt8v!yBjy=e!ykQG6#6rZG6niB4Zr3>Av&;7&b9YAw2GUgQLN_ihn1hP7 zGCGq7gtCss4Hx+9iZECXaS7)g^nwiHq%e|*R`g65g5^~h9!H<>v*N+j-*j11SN1qi z0cSu%q%%t%OnyRcFISbNUQk);x{zaW>)g10Q14AuPFGXS6z!^>BvbeO&Ban{J+y2Z zQJE}0AcbkYHmoNEnQ=UL*wP~=-o}FJ81k``9B&BgMnsV(GJ6aTF9bY2Wf7+=n;6>Y zG6FmXfq_l|H$WQ*{K-bQET@4$mn9c|sQe3Dw)P9$%=U1*2U!d-i6EY#aci48_SAMP zxECuo7)1*?46r^wxt!E$!sh=hRlya?QT!n8BV^%7@Y)P9dEmPeGNDrjF50GkIoz+t z-{KRhHdt$hf+AWM?o*ZqX;VbK7g@1Q*f&Jm6MLCrM?L0rOcA{5ohJf|fPn$S25HX# zQV1?L)#2M#NO{>0MJ^LJYs$94xR|$*?+W&L$L8w2jt#Z>xq^lX!3Uc)a85ebKhhp+ zH^Gm+Aw>KHJQ5i9Du7}y6u?=gp)S*aIPp1=QDzE!Ayyw?4$e3XfD`DfvCETWHZJw9 zF0lm>A|fC%uFGMVkYUv2OpFrFO(`^8LkwD3v!-|!Hgz_^w?lLtfp4%GBIwYt36&+R z0S8}U!)F(^PZqKq;*xP;u^gyH6z?1=sN#Qs5ceE)zGH}Ia)cF(y$7G&aBKuXkyUeq zLlnoLFm|vzYH6Z);vlsob%&@Au=~)QR@DZrLUZ|5;(ztlS zj3TYvVS&?-Q*(%m46NzB2jD4MRS$Vnp#%bC_@0>m#G%8HUBokq zqtz?Ti^U2cu#<=YN7&%O42jo#5NyEw0$k@+EDT5fRo4FsZuoSZNXAzoD?@|HF;KGs z2TY~IA|c)Y7J!9_@1zHZv)ZzQhgf_B%(4h6j+;(Up9=QpAMRV~9~$b9p`{3Xt$l|N za0%PnC#~X=CY$V90mJAWlNgdU*mhaNB7kfPNHF(F5c0tIMS+iExE%=_Ee0?O)G|{+ zpuwKO4kz{^xYV#p7YVZ;fpE+Wowf*2aV+?^!y!8-H~}{3EjbdooRH$oFu$)3Qf}(A zg}ky6^wc;W0z;XCDFtJ|eo&wi+zC$CFqt!mDtKk!Rgf%z%}~zJ9y09n8uP`5*oSgD za&=k^b%1R4uMw%OKx(D{SfE)T8@tMJSHyhKT(t=kg8{_Wj@V$p?_@CGmj(lVo56rz z8VvYd3sQ;pCPV3Dfx7IRkirt~I0&L3EsZ0lryfBbl$@I9R z0n2KxP;H>-D95teqSv|HzuvOLASmu+5Y&+RQeZOIwwtRtIF;e1*lpHRV=ALfZE-5e zIkz}yU1#%)N($>Db(teq4&rw*7s*$mBX%m0mvcNqv4W^PRtlJvdS=<0dh`M*puS5z zu12j%W5q-Hv8r_Isq_3a-NQOh+o;wZxpiMbnaLK)UBsPuAH0Wd6`;OsIA zQFAMv)&kyfW(1B=uM9#WooA0fal#5Qs<8`ccFYdnE(b7;-JC6z?o{A?h*X%^FjzN! z$N5Daj_sz_(pQN)ZSPLbr34MlRp?P)!d$>a1Nba9B=Zho)4&p5U!cu4U^}TM&F-+W zC6<_KQAq|cYJ=mPvQnkUNiEuBQ{o7Rit%FnAtU2hZlT!CgS@htp>vt7f)yFjH8M;O zuIdJJ2QJHyLnG35M9O}Mv$CrEKfbMEo{GW`6Jd<)YGoYAVbnYOcfNr@@}lvXr(~RnZzraV4ISsNII>Ets8P(+18}=@>BX z##+Y;moR+71l4EqmML$T(Ac6Sft3NF3TiJ_&YWyKivZM|T7yF{xGF#{%QXBlQh#jy!97@9B z{AEZaEXo8Tg?!io1%RO_R*a1VwH8AsEFH(%mQN6?Dq~FC0RfvL#Ee_CIZ2=qq^*|? z_dh4dv8~dk4EBCXP0lW8h@cbGK|}~|1W{t?5O1Kcfxa-8d9wUDDtt z*pHAI!fw%qE3Kn#(O^{YN*#rh72qtBe7tcQhYmErIDI`%zdBC$Pqdy6hUu;Zc|u0^ zt7(>}3xI%DLdC5zkIjZAal2S7Z&R*BtrERk28P}>ThM;&t-D6_Cptu)K99O zS+u4fJx&(HH$A@F>`o;_SSWqx#e@2om*sUE%>QHD7GE;9MCY@3JmR=%`y87dHljj_ zvp+!P89uzKNVTFo`(o1t!p)XSQs1mT>+fb+2a;K~hP7a|%rT&p1!W-82(h{i*d*T6 zpM>DlSc zoe?QZ61_-kixu%=HMDFT+BCx5-lmJoY>KF^zL*tf9?Y9U>ns1E0Fq88@4H==NCM(N{ZA&#KWve@O$Mo5{UmvAA27WfAx?^i8BP>y@q}^aY7F+hhQ65ZqW{U(d39`lP82{yL z6~tZ#;`qdtPrzbY40KWmh(u!(t58(pN$!E43;=XMi@#)pIPe+_uR;}`JZ4#KOnfY& z_0w1*u`>->DPRX~=F?E;KSX_+6WZ2aiU626A1A_>$T!^?hatR}l2FHz|y`+g* zg;_m&AFvYwp(47c;!tuOTjEQGi&7tSu$yv``mV?#P6|1qG7*nipn*Tg4dE+^g%l^0 zf)+TlF(ey-*yD*bM#V5t>2aJQgqaw3(A9+%3VbEg30yWM3?UqUd|VYYvn1k*1B!8t$2sRu9;=QEq2Z7yC$QBh-QoFhgk8fV2a*;AMb2=9qZH$ zrBr$!<7mv+ym(_K+3h5`Nb{b~9~72Rc(ox^@#HH8(e!mb^gnW~BFk3Mh#h6j>lB$D zdFZZRFp9pasN)qwlO&l^e6@U<*~Joq+g)1`9sl5uo~ANc-g_?6K-W#gqI!*MT3wU* zVl0X3KFVSgAn8?M(<$b_VvOp99aXlRX1sBltF!!Q+itHNzUFDP9)Z?{sseeQFQymw zdAj@);RBvw$Dy6`L%&o zfS_T5mY)3j!o+&+YwOt?Wb>eBRDDD)-VEVhM3NZV?MoetH;OnTL`vyRAs$tK$ zQKr2Sb?PchmaJijp{j_S3{j$yQi_vJG5eyJ2&SDxNFc5ZFvN2uF9*9AI%OP2UsGd6 z@LZ*uWL-08eOoAV&6Ya_FXk(iHsram5R6#HU_&e8I(H8H@G6~d2p=ZUAQC!g!Vx=6 zwr%Hpt+A+?U}1+C%sy=f(edn!?~0`YERFpzmvp3{NQp;gEe-uU;h>^uUWo% zhuh>aWF3~U&GQc*MW-ZfMtjD>_%8tEy;=I&cs-qs=dUGGu@Z-SJLJh&7l$JoE+SR| zni-_m1s&Sxu^KA9qA{yDmS$+gWfv4_cC&=s^8;-x5_Etfw1#|B?L4^czi(}-dW8{Xo0Rpj0lL+d3()pO#j09YNOBQvvOV%D{ zdzcD#+S)UiDpJJnL^vN!^1~$?BvGX9Ru>ni=la?kIF@|leBVLY?0MX6;oBRED<>N@ z7jXo2#43dw9fcW7^l8b_)7l0O#lv?s4Z}k$493kO`wEqPiS7NN3)8#G=C`+R%<**j zYfq<66VLg`@06N|DO67A<5?XN5b4}fmL94asN5PLb(uU?^B1jfnJGwfL7L20qX+Dd zYs1et;vizGIWfD?frLTmlv;v}S}qWdQiT+*NNxE?&xJ-#kQBJvmb|f<@B=iC75uEsAEIO9U z51lOJi@PSvRB9w08^9K;CfmsLQ(I}(UMm&C0lMt6<<`<6$byX&ORX*4R8v7|M&(eG zwzVgRUrbClS1(s&A@Wxa-U=jekO0^3md(ol7thGdW2ynFgyE0{@p@bDCODi~mOTYx-$3Se5c>95` z1m2YZ9F|skuL!)W_paIuqqugOeD2Dj%jLU4M(%4FxwA<|ez{1;(;Sg~>ZyCTCTH}E z7&EN|kF6rYbc$7M+Z}=x+geg&6yagCAPGASv%W<#TVAnQm*^g_6&y&2pi=7|bfgMT z5{(w$Tg;qz&Q+mRIrSCuowqs=j`B6%Q-0)Yfbw)}4fs4Fx(f4ySqrgSn=FAa_(D3& zv#6xQYho}}i%n^SMHuI97?34fGx2rKRBtB{fmd-Jv>&u=b8J3L$LU?E6 zj}8>5A_o@oi^gz*OuF5fQgbD=ppwero4m;7y8v8_NrqyR_rM}d7I*qupjk86RJTG! zKwU^L-9ne41)E`E1SpAObxB{U5J z?XxAYT4FZK@EBYWEX}*Kg)c(1xQK~&F=oom;-UD4vmcdMe1oHq7h*$*L2vF$TP^^t7^d6dlP?#+)%!@wiQ40vO9K`5gYDkRNp1 zsi?YBUJ+AMHcLD%n0X6mNxdLc-b7B+5(j_9&ugz_Kn-q9l^FPUuzz$c_#4m$V=yrX z01|JHT#23`>AM_D7;sB02iXmnZPk61`4{&x+skpB*7D_@_-u3NC0TcI<#EJ>#dFIa z@+`%;)SWViXKO>r94y_Sm&zMNc7iORteIC@toBqL1Km9#vrq^WYH&Ddq_JivW_BO4 z#QcXXLUL^x!f!b&A*>vf!I`a`BQhBK#CMo1^Li2x1`%HzWQKx03o&Z}F>M?Zj-Cv} z(Xlz;u{PUsl^+PC^e%fdF^G69cn=M^uOL^T0mP1_2G3D~O*XUe46Oi=aJP|cBS(7X zfx8ChPs}BhbB4*r2+3_5yab%G86te!Kom|t66cSYL&37KSR@Wo?BgP1R(UIn5j#yI z4%yj;|Gg96B2Sjs*3Y?mHa+FCQhd&<&(%Ve%HpnMmsM@?`G)hQ@7qN(>PVc1z-#-F zguta0V0%VZS#XGAgX3iIdWD7Ai%Jj*0h50x_B%{KunoEp?A{&rgSH%^K*Y`At>B&D zjgWot#OFyl%ZNvl2h%dkRL-o)F`a?BSW*u3Vc`;PL2V7nKncR5{*ZN>XdJ3_`ACY)%3 zMQF(f{VG%QC#)CAbp5vJ`u+raKnO*37BQc*?JDvgXhT&bv@Cr{Gv&5TjEbYkd_6jy z0=zVgao2{bSq+wc!#)p@z@1#%Wu9o?OxMaoIkg4x$srw;4UP-JrU*+VoLMLrJXfZ= zPOP2ZsC`F55`wdKJZ2Uo+~))lLRm5EYo))CsF(BGH#*jv-6;^w^D}lq-=*zs&vHZ{2xTxd4fvrJm)%#%D; zO8B1klIQQfSJaZn;ycXn-FssX8Xe3 zaR6PP!?xRD%zS&G0vebxLuwG}__rBzVgYFRkhhAhPqbo*fWx;%4FzWk#9x&Z^xwAMc^x20||3i zwhBVPhG(G;#SDH0j7$`2gBM#_hlpW`&vb(i&yPr87}2<5cJZ-2j=0P$96^z*Frll0 zym4Zp2N?y8Fu*+-=BOXo+HwRFRjxxyz#(p!mS09Jmh43y$A3`{NHPtMylLrznaQy& z2*No0t~>Dqi+|8FR@mo&$&QXd7t3OBiaTyr@-ngI1rIX2-oRZgbkReiHPacD7Cy1kg^`TR!ta_~i3Mi~(Chf{zb zh@o>)t!eY7IpmwrT8v3l?G`X3*&ylv{*Ww;#i-XieV9+jhBX?E@~2Oa@=?z|anqY#e^9)i(fb+R zJN5Soy-!BH%lpM5o#kK6v-zUeKSjIq(G&YLU%dH!G0LV(>HTH;>OIktmy@Hk|2zF& z&a?C+T`cB{qux)+VmX~%8~k(F8>IaKHDSG@=>$!W>FHEPWK54oivj)9qoSXoEd9Br|7d)l_ud!F zKb_s)=i_W`>6dAqP>F~1`}sJ@j(WP>$t1tYdV^X2l%_ws&$1_@h18jk7AI+z-q32F z(58MPkIOILe3d230nypN<4to)EO0CBq(31C#k{nQ;Goxkj9< z!J*FL(utELb zY0-Q4vvGPgouy;yIDd1S&aVtE^@1uS*Qo+Y1>@B(C$y>G<>?JKRf4-N|6V;uvez@e?!P@JYC*q$(vDcHlL-vZIuZH=Jaa4_605Fuf0KWaB@b! zd7m)C?=RBX_}|j}Z+U)u)ccGZ>J2U@>E*Bdyu81>Oqa|4`!rjoM)jzbCvCNGdDi}~ z7eD@Ra!aTDEG^B(WUe6zOI7}SL_ceXry`ye!3_k28sInwTvDd4ze z1S?Q^7y|nvQP2mwM>cO+2guvLXWJUtvT3S%6>WYsr*%}+mu%nFJuPp!vo9DM{T=Ia zB>S3TIm|YS+{pBCueKo(irLCFe1ZKy* zr$2r1;>Cab_>Zrfi{vQs;+HSK|L*6}n{?Sb-Ss@DgS{bje2p*HSL4kCW@y84kq%r5vQDe{oRYNe*EEwZ@v;-J!m>I zgN6<6zkd%Da+6#x=ES9*^8-X*qnEErK^!;*T?E5vW_5Lmg4HEta&bS+#^<$`&+n&4 zEnrX#MXYAeZlEN@R$9~EJubrPjFr7%(b+#pOe zcqvEEZs$|FLKh}Cz8qpji}7y|H~P)uMpdaQbgYV2S@!rxN%jkgrW({|*ToY{H`TLj zkp8){*;sQ^5g5km3sZ?|)hgahXD08$kkHor)VNK?W8~F_CS_=Nx4)h0H>~ZES1oWt z|C!_TD!I?{3QP#J2>=Sv2}}%(fA&=AfA)+417QIttFvtG?3QT8WbvkBXR7qRoe}9Y z@7NQH?%R#Z9N)1gReImf#>wnDB_Qv(TU>g3U!M_7)}L1qT%If-g8jf4PW93S#nnJ} z?Rfa<&1p9*_k4Mq&^l~Dm@=SaoM$ff2^s!bk)c|PXU`L3vPiE+|5EJ!KkG+Cwp5>y z-Zw9PqT}&Q9S{2T{Ig`wre1tqY3aq+`!__4ait~tRW#M*s9tApZYLX0Y9pKv5`BUR z?v7aRW=!=tPIf=XGK6lF%4|2PM4#81wsv;HJ@$)(`iXY=<*({+7x2m57|>)AOcPK2tZ6LOQK%i3-Hmais^bDU&^)nSzou{QZ`YdAI+=Vj zU1~~&DVz!Dm(erYqk4^cNztIgs-c)Q24y6RX+j{gyj)B#XrgaM&v;@Y)oQ>_#i}z@ zD3B@(uS}Lnt>M2UvFCXXk>c~^6ni~PE{N~G&(l+}Ga7;fXZk!m0aZfV^L*3pD29_U zOt~j@K<%keBJ1$Vllxr1T!qRyQ;hj^f=X^WAL~g!qzlOJGBe4h*R$cxbUe<|RrJ1c zDif%~25U54fsGZ1lh1F5hFf8IHZnm@<(1kL!vbSu ztSv61AzE6aIR4K+%g3K%1!%vqvZH4{&3AZIj875d-A3oEkIwBrI=Ww$6WRId=+x?O z-KExZ-F~LMmNUI}N;jX-K~G)GPiUV}OaNb=6~KSC+oXEOo2VWv(;f}2i*ojCsDWkL zv!%uxMYp90GikS7&QZyM$3a3Fv@NI z&YuQ)mzvAyuLD7p&4uLKfB~$z1b-S#XXEs@<^ufbeZ%xmkfPye)*qydjTi0MxN=wO zV<)s@D(H7XG}p$XTl9O5j_Z1B-h%4#cLJ{l+Nz-Enn}4HD`ADYI?5FsecwAivX66F zBsvVTbE6(1m<3%^Y_%y0j=pCdu1o}4yr9xn(4Ki zw{&em>T*z663e0_O^Y&nb-F5LC~zK+pGi~N=*=UTZ5n|V>FhTIwGR7qthnF(`jw68 z*sU0>;QQCp1bW8)?;4w=qbK&D3e$~9uiX66s1-##ecD{Vt!`UAndXLy{BcuFfsG9{ z`QxUVYD+g%m9OvK9ns2J{nPY}N?lDCHydxUuai76Zsu3ht0~*yHW!*3Z|3*L>tse` z|J*=?h%+jCyO={mDl3B^CZu2=@j4G+|LSo5h7{Qalid?|MGtM=H%pL(}H|eZEl8xiOW~{q>Z{Nue52`3M${!nzGH% zkObqo`#ibGii;oS9?3Wze@0Z~JWsNJwJqy(yY0j6;<|7|$-CQ1AkCmy#Z_&<^ZB>a z-_r4sb*R;a@Gz@uQ(H^rBUMMI?NsgQY-BYZKTU57(YyJ&QB-fsy;73B?U$aWd)uxH zMfkSdMT+umE+M4vQ07ELm5jaDD~AY5ONP>^gTW~$pQY3HfJJ#bU+X!w^oV>X95SD$h5sjzE6bw6JGH_?DvbTZQ(Z7rP)W+%kh6hl|XV4&H~0pzi?OINef zMJ+!gpnY>S;}3(fs%EE(H!oM4HSJe5m#*r@Nx1!T1$@x$5EaIzdlH;d1r;;JS5 zA=T{z>pQFUn++7%lTBAVYB8_fILVsqZHueH=BajdIoLee+BhD6lXrlyln(vbej{kS zoPJU@md1E)fJVQ8FKD`?>uIQ|z32A3cXsex|9tn(wCM-m)IVMwfgJx3BQkF8m&`JJ zZZj|R+zyP_(|j@~&SQC-T&5V}VN1G-e81vKdH_57)OEsUUDu&-tvkT??GHwwn-+4} z!IPomQ1|dus*h#6%Tlwg|K8|h-D&Ai43<2i+tUx<{6~AKS!d^{tvk5uJ-fyAsp=h& z3>qPB&~%{tOflas(yNsAnTjdSZIDsf`-DaSVbw_g8Yn^Xc|#5LY4yvqm%kf3{9jLZ z)f@KGS#PjYUE-yP3ug({`7hs53s*_DO!sT#b~?MJ7V^dYo(+8cx1SFi!6i}ti)7C> zV2jDXcUUu0`8*kqA^+%sDc!05?EdB=U7XX&UEI?-KnwPcO-z^PH;`l$jU2YB7o2VB zfCjFT*@642nEBgemSzWm*!AL~Y-XPku|Xx_({pV$g$;7|ZG>*>TY?ti9@B%`k-L76 z7Jhqk4t+Nb0q6sUn>#k}cBTHVhwiZI7l&29JgjV!}&T{r0?EwiS-X>N1v#> zPt@Hf>h9xFcY8GRiMabj-2EAdyEek?fP=P1nH_TK4<*fN1nlX*fq*^zx37K_LKaV7 ze{(v!JpJLT)7drt{M%`s+@AjY#ZRaCZ~5uZ|M~MLQt%Tg*pU?6C^ICQgavbI!FCmJ zm9>_|713Qebhf~(C1f83R3N>@>5xkzUdHyjUT^Vjh8CUOa-pwK^A764x(4} z+T4_@^U#FN?4m;ODa(+}Mj7g~3yt@UZUhzCI4>P@(uTMagI&@%r2R4;&&WPqJRjw! zi{s<|3?7H`Tb^7I{9?i0i1_pTGx_#;o1Mt^NtKpSvjIcHceDH`>3^PcOdz|>;;G|` zjgqQwQF^sJ&*#_Iw0YXy0Y+cT*Hh$0sKxo;Y|gA#6o~-f9FPz)gr8 zl~dewK!=AkA$G$Ti!}N5w46kh8a`wW1%}vr3fo&~o`K@F9$^l;r9)=XrHu{qsB<_6 zO++_!&`i2C(ma(_BJz;Alx-a{nQpB#&!&mOK4eDq_70g<_m(z+T%8U+2jO7I$g)PmsVfXg{}?S&uU@ihaHH}9nz%jLE^%v_01Y> zTh6`tJ6yZ!hU7;pWnNmZPP7DAhc*CcoG;Tnr(w$yYWrvXQh4hMh1hnd;b`?W87}Ff zih2#BsR?7@$}Al1s6*l3+tN$PNQjI3b#u-Oxqt~=zYzKBQDuq^y|2oApFV9H>uTw? z*zz)v*Xe~81Z%2ZePLI`1wy$HVTq)Bl02%#cW^o@XRq=F4fa|qL8X6mckkXEZCfo~ z4>Yj2Z1=3G6hBw}?Cu63jf~;WHc=r@HnFzvy8-d4_@(`>5eY{-K(|D1LKBKg+hL=& zcdVHC4%7YmZ$FHcUY!1HB1Y>Sf;d> z6`{f}-|al)tLb8yAJjr+!sjxeZ?pnw*QV+dZyb#5V3|u1qKWCqI-NdmSj$zd*y;56 zxSu!4Ev6#3n0Av}h%0NUD62=ZYsZlt7JM6+cUUG_=$_MSgt66iY4cX=0%Ki-kzGNA zLrIv6`Rg7#H+R9HN)LV3mSd7i+)vwF{|1B=S?=?9`gQcg>i38BH+!}&N4^tkgev?@ zcls0kz%YjO7j~zXE{J70>a-nD)3)rv<~^l~RbKLXk=#zEm;0dHt;5-ZHM)+Z`yF+a z%vwZm$*koN40o<4vpd?`I=SCYey*vo+v$s+nuYaOS^8U#K#`SK%oHp2aof3QX>W&9 zQ4?})6<~EAzt+XK&7i%}9VSs#*0r3>oB3>-&j~X(sQ5~$@)K+Dy(%bEsg^tHx41%g zY~SuOrF9usH~H01ZTh787FDXX?NW!t*!3w4ydZsC~VOd>^IJ~ zs+|W}7aE?moi{1iHr6g3mMZq{hpnQPZ#imZsa+0QbK7=X(1P}r*cM}CzWh#``EYlI zva8UcHL)IRW=btQp`iamdQm|Q23yq$zr7($Md(3Tvh6)o?2|gQ{&u&<-&We*c4s4~ zjq@CZlHaDFvhnBqzls*RcGMrZYps0It(BzOinrFvm)%;qsJ7y-wenTBRxX=o)bA_p z>ej7N!9`I?@0HT7{#OUBRKrHE>_=5o@WlZwe$m+Cmj|@?Wn+t99nfOAXYX57=rxm` z+VS`{n@1SmCus1O=z6*hYPdc3qs(bmvmwPI!TunjRg${@m3gAmb^ zbTS?}JTGT+=Q<|(bLO@`f1WRp;p6ijq#o(;z*}VYpkPTef0FfVr)0?QpZxpjD;e?u~aFJWDONHO^i>I}IB(hC4R9Wx#8y zm9zbgB}Y5yaE1qTM3ZW=9c@=puS)o}p=lQ-iSs;v`t-?xcBGGDnYm4u(z&6Y-8=07 z$``FxWks1?!J(`<+_Uv=C$Uv*U6$E%_*8SaXY1XD-$g{dYJj@a49a@LJv!KB5?!?U zO{QpjOsDK>xMx@0Ce~35-(Z)z8yQ!vI^3_DZWG!?e!a=4bEm1T*B>6xVYf+k6=pZt z;O;cJdfnlk9d*QWU1Z*C_V&9WyRzbNpDwzdshV7T8?@IfZ+ARZ)w;tyJK9EGt%;Aj z@fKA4{Bfk!!$<3HgICffDS4-55qWpFHBqVc9znc5e9Zp3EQ=QFx2;JH-nak1}WT760kiN<8rsfxZHO!F85oE zs(!5<|JHRhPoGk;t);YwM7KNk5j01`N9w6%#a4AyTh~l4aO;xkLc6cn0ot(PWAw6Z zsmhye>q<4~&srC)q5SUa_W^W%!$&W>uHp}#CsiNj#%P(B@12}N-YT5YBW!}U%T z1RmP$3(i{F2G8Xv#3yt$;zX0Kx}{)MTHV%OU0T{wa#cLnQnX1#**b>0XsM-WO|G_e z_N7>RC4XMUIk?CVUAUw+lOu}iC~_mVxX!+N*H)@#nb2NzRr%jux~@_0G>EVN_H%o= zRW)FH=`{^!r$HzcbEmRU?RF}7k&Zi+yGmy5Eq;e05IsDT5oX$YcbM#GTI>`z|*Ia!hW!?4b_(3)_nT6ci73e>B;WqRNH%Q zGEC~WnpCBG`)l??-TF|~y7yDJ`RUdP>g>8lE%qc&T6v>2+ndf)+iX*_Wt!_wvqN6oe&-8!PW9Xe_=3h0PFs+GI;*rulM*6n%=UHfjbRO!}dS#!6()(uE@ z>8~Uzy7s!sR-jw2_1azgY}Dy@gw(QXcZhA)w|52EYV947+-6nylU;T1yLH^EZ+^0q zf_p!--k{!n(pi&Sbh)d3_dtE>ldg)}Vt_m9QxDW%KIy6`Eym~V+xf+gonJI`?(f_A z<&K?Su6N!MTsI5LpX?!^-|eWRJI*p~SmHxu!%wP@lN=I9R69BZT#NYOwlIEVgof^4lZp7cIk<0Z)-Zw;h6{n|XFMqG5yLxu4 zf1KvWqi4OrS|X@t$NNYq2dcG(lu#`VZV^Q4M%NNVJv)Z$?Kd-s3y%(yI`#+kb&!l& zEn{wyD6X_o<_6o|&YI+Kr7e{rY(V%XnDY4b|wl>S+bh`~Xw_2L*DqCEcyQI_ z0|mBe%JtT@kB#EEwbqN|Z5^9#6}aiv>xtcVL}m>VwcY(6)Ii=IFphPx+3s!y`X)bM z7)ir0yoNq54(Q{ep%1^IkIMu4SeJG6`)km+l9I4v7n_A!1YH!I#5nZNj*e!dqY891cHO!XTAQQ ze0lQ*)nC5q4Q3k}tqB;?GvqeIFgUqc?=!yQdxJ%PxZ%T~9u4Ui4uogZLI4#F6zlQKlq3YV(@a)*B1;cww!Y^NZBvb@N~A zBEiPpmJBfMQaDZtvq=W`Ld(lk- z?yL1e4Ky5`{L;*QbFLwEfj^lpe@K2fg299Rd^CIdboP8iu>9`b5vP(lzoAV|t_eWBD%#Dkb{+GW_)0tr{SwI=bOXJBCtT!T&R>KNzqwWrxPq%se z=l?OjRX4u`=`UuH7)!+Te>=`Y%ODIrjJ4_LeNmH&WBq&U+*bJbk1;(S zf}zLo*mJ8$K0e0esQc;7{f(NWjHGdj)#-JdTBE3B|5JA84)Sme9*{Vk@K#^r=o)mWyqxKws*T)Z((jjQSUGb#ei&n?TH%D+!k9+6W zi~0R6=denav>U{k^{Cw&Q{E5Ssv^2-jcD(+r779knK4P0CQ!{qLbn2b@KWtKhI-i7 zMS3%TK+=+S%K69p98g4`z^4vsTLfuMMQ=_V-cJjb+k7Q#I4E;s91~*m=0Y-0iDW*l zWjdZBkuhgUBsTlzsIQnXe0lm*$H$-F=fsko|8hH~(H-?({>v-Q?)cAHRl$uD!cl!e zI?P7v!%?X#D^(I*C7~)wLlu>3yOy%M_nDG1oT#!1)w4-ehS1BebZJw-)W4FnTX;}eTwA?SXNWipToi)Rvg2Lg3uN$UT zTUy~?n;WoT;IhGRgwC}zEV!W0BN=XHe~}BdFG*2*$1A=dd0(BTnjy)pjMlik85^~v zstxrk6J4al^$JQ^$;=8Fo&HRnH3#7?tCs<|Tci%xMSHuZ4pS9HL{X8iG_Bx2r0jHd zT{Fiy5!0SZgziBD8tlFN=EYB*=e_#JH!ptv;vYZv22bpP>is-j-kxiDR`s0{Wv#Dk zwMHR|fE;3wj5@8;=|)>72sDxtBK}66MOP*5jOK=p^eAXcA%|pr_z1$sliB<=O?!4c zvIpI^Nn~NOKVV~yk38F)^^Y5C8j>y!RDs9L+R-J|UlcQ@dx=(smxr(y5NE+f@pvgp2`!mB-qooUeO4sWAyPK)Q7t2tFN^5f6a(g7dWT+y-pV^0^DAYQ+MM{ zGO8u%E)z?Sh*&(KrThDLFMjTWi4fCE(Wn$(P8TQ3`64foB}2R7HVW=9J$donzy0vV z-}}#tSGj(5X4=E`f}b}Ow1?LFKIbUKAm*b*L93h+Q#?PNn`VE8lnnXe^WH$7#s+W6 z=-C62{Dk^5Gs&jcv*FEjJkHW*Qf~2b{_5n#i|@XcN}q8qh?V+)4^`KIf+AggcN40SuG?rWM9qU^Ll^! zNs=rZ%yW&N5zrd+pGjoTO|p3N%$UzOeevj-P7Xx(F5#J@fA)IE#VQfuL|3YsXMGG% zU#I~{PrBdAV2~@L_kNdD?|?qd!EQa}L`5dGGd`=bY%B9hTk`=+p7!4HI{qbQQ@vir z!1}UJ2an$H6*Y)#>QrAfaXA5#I&Qciny-iYP4{C>*R+mkusWOZNlPxcZMQ{7eBXRo z?2PSqB{{kvcAbZH4HX}GR?w_-`;Wz(pjx1_dD*Mf)Mko>36>AN^ncV=1JOcd>CE=(kGhv%E%z6#Wux01NP#2&}xsIn(czGsC(5 zE|NG}`80n$U;N6g-QS*1m*->J3&eFA&(CM`{9N)n zidObSBSn1;wk`BwGbHXwHCmc#q#t?^6~eLdcAeeUJH}Tel4P9|u~h&vTPrYfVzVI- zk$j&^c}-HV2v=!FpBd9JNC|!2rF^^P$2}%A`0gq{v;I$B9&ynD7wtbYK35&p)6Cn? zByoK*T3^4H>8leJGK@=~9wqNa#g?$>3$x>IJf;4qLn}gb^%M<3he6RgLfAAricOa= ztLY8kd?wV7%;NKZ&J;ORpOrNF!EhmMk`qyP6~OzfN4wt0Oft>MKc2x|l`P2bO2 zLCcuVZtrsw>c!VkA3qwX4U&;{nmlh_v>$nnI+W^Za(vux*h3=V+G@W{UY$(GG_t0X zzgF(U?BCXo6^i2hts3urpIrX<;y+nJi5?__UYCD+>u{oA*w_p%1km69$8tgpCM@r+ z7V{gS*;PdwvSK0(Zqt04F1&H_Fr8gadqcnWzBhFEH*q-Gbk-ZXm0!P3Z!X|dNCheYqt*K4Xa&9k)2B;T*18f~M4bvkDnt$Kx}blP>wNKO?w+q`joCHk;Wd*AjN z=RbRO0-IPm8jB5|>-eK8%dk%8&ljh2f(`5W9GQTX9HSyCn55I|Nj|i#+uw-!F7r-8 zp}AgqMX+F0q!Zi6pLE6=<)k1TsA7$zNRCPWt!CO(pU2Y&!P{apSfcM4qwhJ;cNO`s ztA?NH`OJTNj!{J$?v$3{Pq_e@iKeV2AIdJPOHa1pS ztU6Iqfl3Tego4l7$&+VBD_JnBBQT*?eof!p-)2A$i=&_vcT=+sXqM}^LF>O7iC|1*6B{LvJS>a_Q*hHf&MC)RRgWe!5+AJVx zu0W7ne^HGbZk?7qm37)e;t-onv}q%7eX9*|^7-HAuhYd>M5G<{pFo(?OEOrUq%gxN zv@dx_?MuE3rI95UX~x7wUg{Kf=xOb`Ki)_r&iKTwW3JVX@fBtyr`GelG*ap)oLW;= z=qXI`sFF4t-(|rUWF@wL+&; zf{o(k%c=0DAQtC{taEyljOMUtY!IN*We8#61L5$oAwvE47~q%10N=gqeOHcF8+{F6 z3sf#@0z<|@p%+}7(Cvz4sptw>TfSZ{`HjmxZz`8;7_&AO?Hv~#9rxH`l;5Z(2V%5Z zw#Q0R1G9TkS$^HZ=WEj`ntHzWsrRqy|9Y~{`-~T+qDRaA>bNUkvN`NX@SX6K@l5ym ztXG-J7v)rbPpJ*?uAlusM~?r}GW}QY9QxzEqJQF7?_OH=t22J&f6n^-P$)*`z_gt`G9Me!Wnw&stRLvy*SW z`RN}&e$Sfqx_!zIb3?vWw!nI>E3BQ_WCS!|oi0|3oG$3ZCZkJYstLCii$JLE&56M>V~~)2Ev*w&n|{DqbjLBK;w2Zg~>?w5QIuqQ7mN4RiFkZKuoCFa7fF zR?e=pSn0{%26OuN-wD?#sk!~?CrH&#exjrCGe7-IPyasZ4KjqjDYg#!i<9rqX83o= z7>r8NPt$h7A)k5qZ4LR<$PfDRo&12c7%vzq$N2Vg`KllaF7bIRA8$sNgZt6=<>jlv`RMoQcz8d$ z*VtGw_rsgfje7O|G&{MyUrvtB>E4=;=IS-C^fgt-O*+g6-wxB0ZwGvN4;O<~RpKIE~F5>3rqr7G-(=Nl!aY6Un#yL)x zmy7A`s@2Zpj&|!H6;!R0J?o;7ir~(woyeSzRo>qyQBY&ogj3s|5qj9N!Zm^8>Cxid zDCv)Os&v-DG(;2_Gi^@NvwOomYFRzOzPnfLpn}@iXr0Y28m>5ctZ<=iiquunU{iA& zgo7>3t_VtsMyviS>xV~k|Jxx^7IEig*SK~!KyKS4wjblivn@K%;Wk~Onp6VfT}AEe z0PIw_iIWi(0jnsLV7<95x*v{ZCXEIcd6O_?-Ckp5P_iI{M*v`DmA(h0t%C2GR1T1= zny*Rd4ua|ibG;98^s1xZ!vc1}R&EU@49g}H2ElQqS)5Rn>5h(GEl!2ngRs{>USlSR zOnlPoZ(!S%)V6{zI-`WVjuHx%{_(KF>~M@QBW$p}J9JeK*eYQqIQZ*y440bABx?i3 zrURMpd80HQl6?Dt+@s0rZL;$@%R60h{b3V!oiS!TTelSDZLRNxd@?Q)ggah4M1z{6 zQ*(pAe~_v1kJrh9P4OQCG`6?3FKASUdJwM8H{)v6yt$lS&w7npQtsIAk8zN&zl#sG zSUlDbwN!NrfJn0$+t+>Hmo?il+kM)nS=*Q0 zv9mARf1uAlTL0W5A|o;*>jHp%ddAonqB1kC9vjix{H=@zoeC%8a;nWp4t>?A`2l+2lA)7w#!E!#eAje1FoF|uYKIGuIo%Q}W zzLYpQdA)Og+to0nf%sf{>)e5NZG^omqnpZOTCTmRl55Vy)}|+!#=%}qlbU&1lxl(i zP7gq2@hmk;L!@gTa%Xze#I{Z;VGeVgCuNRc^3PE!wNPzX8bgdju&2YF-an~z{CDa~ zLQOVw7l#sh&L`aG%HZe|L#ZM%rSujpW3lAd83qvS?llN@cTs}fy#a#VU5sFNZ<=76 z*C5#Dq6FKVlVB#kcpk(O9l$*$&6ER*Zle}dV-5}Pb)O5oQq?oW==7R7^U9**uvqfz zGTpr^GM!3?j_jU`?ffj$8E;#{_FK_G+o~Do2KN_Y@tWbxePsemtxn~=@?NcCIp8#b zxVm+ozO-`r34x0-0m`m6Q}Z+Yt{X-tGRN13-9;Kbp1 zgZR6a6}>*AkK%Z(cJ-=8(6p8Uj(?Zytj$SNEFXUwl9piN2z(GktTE$L}zXl>bD_N&vRi0FUV9*oN@ zzrgK^)L<;9Bb=Uxf1TPpQTYhh0cwhJ2yTBy*BYgVQ8>2iwNKSaQN!Q?(-=$ zc&v_TzH!5FzHp1{X`Ygr-SY(v989BzO*7}5V{@by)M@gNVeq}MjWNQivNUi?jCd+{qyX=wtM*ZZK_@wE!Ng+ z(sd4(t~4- znkqnldivxkv*b)|{!{WK5pVHHmB+I5^ECP#5*Q+EaS6=-_InLc*uC%!w}1CQ3)3i; z7^F!?dMo-eqNx_eqfvHVKsmk55M2B|!G6_AWI%30%4r_*nzL)Vt@eDdnd3`S==RfT zT>5QgC8pF6?L1Og$)$piP-a)8>Vh=et}`#J(xMFWD;U5y}<>Wgqu1CnMes* zHoI20<{ufVa)E&Xockvolp9)4xI~yC& zx%oZb{ZEyoTmruuP!f^OK@s0J9Wd4Hq~a|dB1gQ9epQRT4oq=>F$ohja(%{0&SDW@q1~{i;5PW`Pz&4 zh5ld~T7U2hcPe(PNRF3nEfzh&ugZSM;BWlesIT$0F-MwZ^BmD&5Oq$)Im>Unda)np zw}S@qfmgXrVjHvaQ3EPmk63cw^^SXQ1CV8EREcK4M3 zpPl$&ovpyd;A(ljzuy0aO8*roZOuYkrR@N8Gkv)1z25khEReoSPJX$v)p)T}Zp9DU z?QXXzzOzxcs{YE@0rIxNivsiFy<2%$Zat67Q@rrM;9#6T2yS-$uFjsM{uj#LG0}H^ zAM zj$3yr8QE&1N2J%!N!^olS#~*kNUG+C5$=hPcl>6%@K-JNNWa0U?V6L)I~!oc2gKIt zojcq5#<&1mrz;;<`X_lZ-1_~PhUE}O$5!v3cK7gK>JwSG@05A_NB&Q*4-@65WJulF z=od7!lV|jTdID1Ep~ai64HBdV%IZ)e#|omrQI?ZnCaIm!ptE#kSdGGO!ZH5`0hMA+ zp!j1lxet>|DtVoMKZ&Wri>=-`OS57^)<+NVNrME}M{eEy*gwtkw8A=X(wCo48PUkm2=@Iik)S4Zy>QbN39=dai;nNE5vBQEXCdtZm6%nfdpDR`UQX3{gp0veV5lL9T$}Y3F~}N?-$?hm zFH?KK7ff{$t4XR*a=(?VR87lNt5Q-JwKzFs6X5`=lExM#SlQySrpLURpHPgWK!!uq zDFrqroNtYYT>ZVYskc;L8#h})9VHgUP{AEtYI7gKd&@1(D@e+R>|evaO^f07ZIKkWZ*P#U z(^6-s&ZskNWxT`;G*0f0&t#m{7k1Un2FL7{WuDMVUtyU}(uJ?6nkTj3T~l(QCh4P- zB#lQbqHf=2+v#|e(a^R(f1y@rp!i}~r}!e!n7&)i7i`TOmbsvp^}WpOQ{YTMcb&ZN z^&3q`NFzd%dv}NMLBOP;aa5)p)4=bzTiqRy@6vFrOD=YES=r4Q(f?4 zJ!|nXF=T`FZgE-1&{~Fvn#(`_xDz%m!j{5vvxuA*tjnYuXg;jrge=o-#e3Ms$7YMO zNw__LRtvW%q>z`LUiN8{s!}h|(vckt!!>|e^`I0*F0(8NzQ=czuy{IfnviM;|%uSSR7`jsd?k%u){%tw^@ak5wKh_QEfh zUxssiVG?`NxE5A~^R5X>^7f4mF9nELwRe$|~ ze(5GtvJ&f0y3U&cBu zTORk^N*#S)Tv0BV)MPZ%ng& zp3$9H@4s<6VMdeRb@YL_U036ImT-klqY5eD<#gt>1^b+ntu?$!w{N#z8PB|->IvC3 zCl9)kZJ!vjPIg*pemVKIf9z7bSe}TUT3K0LyT9$T=wxk>jk5g1huUxEGWewa{o%t# zH|385ls}$AdDG8R>gQ>P{dS5@nrX+n;-YEDow|lx|7_=ios|8fKsv2Uwl7>y>GG)0 z=8ETP^{4nz=j=Z6CU$rNAMUL8_v!yn>Hp`{*YgKw*7uL~rg_>vHzRsLv-a$lb2wpF z`oHZI^04tMD{|uW+R1hpOycXCBTv=fH|D*^6Sr?m z@B|0j&{nM)Z?-k28}=v6!Ikch@q2B1zaxp^ZE}Rt_HDJ+E56cy&N-dEl~5BxIk#J8 z%ZCrdzbY=OT)l$T_ijbYjF~B)>y+ER?l-nar}humcjulb!f!;wcgV=j2DqAcu2we0 ztk;1cSGi2JeZI5oY+$`xS#_SCudH+_WR|~=c4q1AZC0r>_$$mMw|3^pc;`Hn)vC?+ zBx2up8YA!d4iE4MlXsr#-hUCSuz%N*ef9kpkF+HLRaBJmcw)Br@cS;SH){k@$KC6+H&`mc8U z=I@!g93sT{&Z|3jx5lflR`2#_%=Hiz?I`c{Yjbw>?#h<`Y4w%+@#DvjuKcAnJUU|g z;Ct1*&=60giZ}c*BLIPr^ z_hql|zsA*+2>#M|ZN9;&-q|5{FR}=!qroW&(X=USz$8CeIIC1D1&oW6 zt5$7p1H^TtotJbwfGOrWcFygAp|H-8_m7iUAP#>I^3cQS2%nFt;$m%WtqW%EHJ62R zQM$N(G!cBd^EfH_XgS}?P6e#Eh#1Ogt28T(&yEm*_In!y2S@7LtjYUaq|>&~56f!+;F+Fsv)q_=wcn73lugk$lp z`Zt65s6R)%?}NHY{h)#^;KeJS2GXFe{@2Z|As_=imkp+n%32Alu`pSJEcMALV;QKM zJde9M#3q=NNX#BrrjuKsvt`lyl2ygSnA740gKfSH9dA=c8LRI!03HT^6hp)L(4NY( zwQK{t^$hMh3Bu}~iR-TCfj^U>?%utS>Tc_c8T6C6MWHEpYG`2D81Qd1yGBz_li`qz zitgTe3U&&X>oqmIE<~GW^cL5Xaax9`MMPtRCN2{1KQ;m8E@8SI!U)4Hknv~+$~b>- z(k?(?<=ljdjh7Bopsb8XeB2A=ua$V@+cyPy%bVJ4;{FFMFM{TFVB5YQj!NG7dcoG#Tii+uYAuW;iI`b; zSGM(yujckuN>pU|79Uwx`SxrJD`a1q_}V)gbK6|F0G6oeqcx7ynHsGEboY+E%RCxn zBV1IyvvhCio9OjqZOy%ISgkR0u2CPH7*99!d4N!mTuCSmG4LAXeZS;CD<5mOw}fSw z@|<{#-s%PhL_%@&cZ6eL*LlUTgm;DR)SGR$Bi_B(`ltJk_SPToFSUq(3M>JfLM4(3 zy#0rzXvXEa58~*Q zQ_3VB4iUv3-JiVY|F6s6z--X=9?KG+p1q|NEG5une>_Q06u4=-K_JV+!h%!TZ)YL) zn?{>xHv45GQi7`kIlicHwpXf6hJP53f54Th}N+Timr@1JULib>mj9#(oM&!g$NO z;O6LIh$sI!jr8DdUy;-6v&#RBllZ3LGj18Z{_JTkCd2`HkY^{v`Ndw%mA9 z$`i^U@z$rb&VN%QhEmEm;5u60U(4aV=awiJ_vyBdE+VO5iVM1C%tfa^En={>h8-w` zsBD+|;OcUZwMk%cQrqBitix~vpYG7nqw1IR*M1&Y{M?DP_B`y>RptFy?4y2N`}hLQ z`{h?!KleL}S@}3$XW!udrmG6=nQEhB3v2xv+bVNt2UJZiww~+ytE4qt>-& zjIeR5KbQ37Z@n#eg_ioxv)+T=V%@Q+Q zrTz6=JAf6GONFsP@Nju9zbuuoOa*ya@AOv4GPAG7BwcVP#g%-BomEG&d(*6??>A*< z9rr_A*};5ME!GoC=7MhdO7}EvEI;|)HK|J9OePQdN&sC1i`=)?aVd*QQOAM= zLJRZ^U+L@XL-}G(dAIIP^%R{niM4@s;@7ycri6436^lm~dj@@oY#F3m1-*JJq;|KJ zLr+>d)m?w+b$qJ<@MT;gaaCqrPLVR3Y%hKDfCHE!J67L3%CqxA*Af4+M{jfMwGf2SW*z&9bZu{PBm znzuhHBtA9dGMX%Zx{| zpn#gwterM4+9%)yh4j80h$r_UrASgYMg<-;N}rPa_+|g_;NIQ!UpWo#@o?2b-(^}? zd1MnHxEu+(SU=XM(fe6HVOf!dyg@z%D7U@4xi4ZjbH#23d5bOCw0uF0UQ33I*R;&< zgDB2ib23-Wd{LmSH*xDt0`*FQEhnLyg=*aHu%_j6Yp!S zh4@8p+6ZMW958)0D}bhDj^ufOMES{4yu5MuPkr z>---V5NqS(#}!MWBcj-RGX-^IohwdGt=1w`%PqN3aCo4&xnFIBktPBd^(*vv5#T z$HZWjz2V%(5x%nR;H<1a0GQ*0jrH}1%c~o=c0&5EiF_7Hv(gW0&JM6RtgBx+OoO9S z8*1~5lI^4x=S{oBVivJU4P1dnwD$i5)L6w_Z85HF2LIivbD8~>Ljcms4BMf-VmHEJ ziaCt>fMag!({R$~7{=u*jpGV09j5zKfAFrd4R7eOxq(z@=_*$ucTgTF`d@jjR2r=p zyp8+y*F|*e7TKvQ{Z4t}3`2V|{NB1ssC~5zRwVjnOdDA8d}IB7uTP}Hl1cudH~o#3 zt*NQ?fdQ`(Ztm960>8N`(B8n+N6#}dNU~h>;hWdIsz+eFMH}p=J6|q8`tWM`FT*cr z&i|$OV)>UlzdrnBef8^Kzxd0V{J697Fk1f0mn-2?Vp>9M!to8BGgkaPLV=C3tu?_C!f?oRLj`!9RF?L^!f$m#e@zWLz2>)lz- zHN=-!`2Am3Rvz}&*T_WNYJLdUD%I-3X-V{o_#=DRG^otpd)#mKWs3ee#TlXe)wxp*+Vq+6= zI`7oQUN)o(o9uR|N}PFfS2v*~HYRUijZ@tevujcJ^$29v#@9#%_ zV6=1Y_U@GK)(;imN|WP6cuo zd%yO7;XJ+l^n{Y*vyxxV_X6L^I=h{g$!Msyvk~r!Bx}t}cAV4OC-iuGFi^u}v>hkN zc$}+r;3P}1*Zj1MM@gJ!^fQs4b%F^>KgdSKyM%sBGQTmC@+jx;d0CxQfBip-)kk>7 z>pD?h(W}4x=YRVj|Ls5h+kg7ofBoD4{kQ*zUgT9#cr{hH^YU;1+u#1{zx~I*{g;3H zpa1s%Qu6+1t#fW`7aERIB>Bv!#fyljFTBKFz@^ z)SV@t*Blg{l_kFKOyXF*O5K=N-h&iyxB6_Nm-?3!U}{upgEi#+&W?F$v`3+5nm#4+ zO`y091~`yf(MW@5pf^Oh0K9jUfZt1$C@MhjWbpSL+?8Y!$~|#<=iPERe%Lh@9^NEG zANdg`l6)gd`Mr?WH3g{mUQ-m@6X7g($&R#b*dX(G?%W_b>EfIPewag(!FoT}7Z|r8-hY!mpkIrDt*}ndu_xStzyN@H@EYM_H zS?M>*J(-$sB{ZNR@oLKNH=DcDF5u%#y1{3C`*tu5Z^51N!L%Esu1^K8j(>l84;6NF z-_XybOw{r;@^>`P?NFcwAV+mfbsbYe|L|jT`%#Zu+hzH|*%ODhlLa=Rd#cg2!O+tjVJZ_4pxXGsEI8vL@; zTe@RQ;$yrvDsSmdyQDkDFVx$fHm$vyEecqB8e5im{nWraPDRr4lGPEEQ@1ZDhrXc{ zhhgSACa`DIN283aJ>14V&T?S-69wzjK8O=CD&MAs zjM@}+hdVoaeEw;I{wMj#@%a@)?T^n_?Ml-fpp@Ox^{w=W+(dxM9&V5HgJ@xOZmpL2^^T<+tXN%k7& z%;*2>0-W*H&({Jt<6R1A)Ulk8n{fioWuAEspVrKI^<$^=O_8mu=fC=)(@t4(vjV3Urht&SMz?IZ_0sH+q|nCR-MxlXqr&#{i}%ISy9g&#*7JB?l9U&W*Xu@ zEayC)dZUxJc{)|Aw)0*;?FWUGCZ`F~)#+QuQN2`CPk1cQp_hsyUz(bGO(xpn!n>yr zey}%e=EwrK>)v|5>!=akB6|C7_>&hovD)pR9%pdpg@I>aI0TYrGq=DaX%A+a*cD^j6gEV+=>i{Nv)Va7RwS#$r2^KFL6_*6@o>n=)yON@Wl1Dp%X8UHBc zztbVkYqlCS%mYK%Z_gJ0^n(?nvWg$X>6=_3So+O*avA4C5A7QV8gPe)0|(lV`#B@W zFRyMq=nTxKE^0_UDfJB>?_OE?@S#?g#tPbi=vwcAg&31W9^AjajT!9#SYA)l`X za|lAp&)xEKlwTiq@E^m69X`=n2Oc_SUu)<0A?fCN%S#r#)a%}s`F*>p#-nZQ9W5RU zhobZ7CmOS8sETM8`Sy^nK*l1vJWr@z$yVuXqePw18_Mm}Xd-!{Gm_JTq`YWVyb+DZ zUyhU1Y%hLoo*S*mqkUc0iK4o3QEPJYPLVz8w+Q7$UjpG$w~R~FZ%iBACX!f+4*o(?Hv}bJMuGl-n`ro}Q>CsZb7OHLaqbPIG9Mq*)oA#%C&8T3zZ#OaHV) z!X9TXbF@Z{C*=iIYQGI-e2*zV8SkfPa$1#_C7Lv(ml;|$QuI%uk~CO@no@(hLJdz? zer-`1-(rYpY=}Br_`sSAE2k402>G#AzAqy*=omK&F+E94CE;XT{fRopMF{CFQzA5H zq`PH0U<=}p@93k>Z5vt-z8PuC!c4~Fg(Oic$?VN=w~Ttsqi=H(?0U%Mp~Zu(I{v_= z;}m#jxqsj?ImyiO?%vl0-tR6DX)2RuW{LN86E~+c@xE^3W}}V&;m^^rQo|g}F9MGAnMj=j9eqOXCZbN>)S~~*Gum_<6(EHWq+km7_pmtF>OH%iL0lD5>K=TiTfM`HQZ$Sk8X3_y}9Rv?NLeMHOHOD?3Ch_V1AbD9lK$MFa?93@D$*3q0>t?Eowx0*Q? z^yW0nJgh73sxT$hH99I_OU!KYFo?8OFn$bnVx4{IS- z8+R zc;7Z`VR=!%XTvFtme*II&R2SnWMX(hEZEnSij-BfeE0q;lr+*A1?0SxRIJjroD@wJ zMY8^h_PROdlD4bx^UK#SpSqS5KlhoVNv;wj4V6fHz05exN6Z-b0@51Jrh(PJQ1~c2 zkCyMRuWss&J{dt()^GZxVjxk4sNZG8G(#b&dB5SjCvtwLMiY9GjE<@Nrvn^9KD-zo zA%2g&MaQ&0VSGrQR9LYDwp+1Lw?r@gSYdTT#m}Xd)Bun>d4Knosl++C@vA` zIu2E@II5ic3hpP%>m9xS-g__G+pWa>vXN33Y)ir$@nHLNM{(LrMuk^TB4vC1mjdNM z$!nwDHw2iTFJVp4 znxM7^@DA`Y$~XKDu@oKi%s_OJ(WX#1TF<0ByLh&Yc(NUnnqXlzici?iKp9LiqnY@X zbjK7zGvW}-M0tx}QoU9)s86dGyeCCQFBIks9BGj@6s6n7R)j93kJ`#bx-CvE1uN4R z6iri5bf+-@kiIk@=pi{tasP+>#q%Y4lxs_+pxGjxAyTA?LW0Amk109*OJAH&>8)P< zgi~g6t;m-7(SLO@p@QjO`qmI^i6F3R`vBGD^nLx0w9_JJB_IX>b(D z3h+h+rXX5MA*L{Hf#V!y@sH6THfbu!oX<8Z?>Z6ZjiWkiT`v16-BOPf4=hp?;>~y@ zevd7?9{v!?h?te{he$?-zsft7n=P@4B7OpxQiN-)LGyToJ}~!Di~o#K{1;j$JporaHP~E zdMduQpL6&0Pi;Z!*Y)rIcUe)29btZ<1Tr&?k-)!A(YmD(bMKb_ zSbsf;i-Hu`&`F_Rl};+HcA&n16%aFCx(-4A;V| zpz)XujnZU0R5^*RXv1W+&;Iwjjqi6G-!~iIH;u%9@{`DXT34b)Z2Z7ji+PG;9@49K z5Tx{yOwK$3SdNqMQI?hB?`b4Xg)~?1|Bz(I`5lSiyO z8gez-14QO438+L#v=aO(msP~~pr^Gh$3BpW7`;nHWWqbqdS0H?W*tg`W9AX{tAH88 z>AcnZmZ1>+4>niKc7K_Pqww$`f+)lEq}CFLjo1XCG7C09(yTmJSnu{p)sAUyp*G~o z#rYB)V`)ZIfkD02(E)a<82=gdf;4H)#))U4Qo%7<=U7}uM#IuLA+8&ofn|ZTY4L*_cYXCfm9Tmi20!{ zI8cJh$I`2l5xbFzUBo$iM9UqxILWH2wIa=<%c>*;mwEiB)424@M9WjSWvE3*yv3FB z8jhaP#pekeWg{ZFQ)WRuwL?ZjaKtNa?N9>+{!9XZ7Ae9M_#uHDeN@8hq}n6lHio4F zYD5pP%89;Dv&k{_7+!N#&$%gX3(v#Bf<~=@p1ediBF4k=S|*xl6V5esG?-RsfBo0oYp&BoS_4*j zQu3b3AxmHxO)Vq7w)=V_t0xhV}zK5%7TduA-7pk~>@ zk!2dO31+~sVH}{uzy2>rDxc(k{nuk>^nLec^w)nqf?tIt2AcZKYOb(g)Rh?M6wDtob^Qb|70 zzce>_zCft*sZ{KFmk6n%!7i&*OMatY$)+HclV~FZPDB#g7bTe;?D~iYr+Si^%_Ub< zGNs?b_W@Hey1i>0?5x#f*Zc`a*Hv~{Fn z*g{}!LNDXVWOTu=5MFD$1g)coA&O6^b4*5Rs;f2B87VqwGTw>d?L9I|@}kURyL_kV z;H=xlhw3U>`a(%PRZc^Zza#yog&gL>5G#UXv$K_}-zvyoUbUoSHR4SZB0U2(LQ?no z@o)^k?e360qd+`2D|4{P3G#3CzK!{=u14`mO7>yGo~62U`oHviMCQOPM{E?)FyqLC zF)pM~#VsQ>WQ=n#8nM-QYy%_gCYcT|umZ?zD1WoT^nbANRfMx_8PTqo5L?}H1rqNY zQ_4!G%*d3Bo=}d zL4$!(bvl;)B`NY%mcjVFi0UJLZjnD%(HX4|*#oY(8acPW1a};(=EE_Yd6)(*k7^HS zlko@=_oyl^ysd8b&6>~PY$3xj4PRMrXHQD8EySlsMLx?;Fm*{LjItAHA-#%d8p!T| zd0X>_Gb*xi0gVdYsvnFRgp#*gc)QiH(TkR*liUU})%yW3uKu*P8JXboC`-k-mhAy* z{W-gHN!}4-R-z&)!>Zj9gH259rel}`1NiM4dyh{PuZQuTo{7LQ-$HPBnw^tUKz@(rL-l=(Sn!7m=6P#VBp$aGVXPpUG}8`x)#G zte2P%p)^wJO#nY3@9g;L_*J8G5>C<_QxjWKrxyH-W4-fWYK*Jm{ICD}=}2!N*fcgS zE@m`x**LMs&bIK=?p!p3#Lf1eEx?U2pjpZBj_~{I(8XsqbW!&3CZwhizbqzCZo+Q# z&YJLoJN~`?V>!Ky#htsyFCs%k2lNNo-Ix&A{pD$#Orodad*Z;DEMH}B=H9_;=sz+I z(!quM`B+|F>NE~RxT#8aH-yR|Q%8-n93?@k!n)Rrc;1_ii_-{uFT5{J4<~vD=CA)N zJ0|U#3u##BR7U8K@IB6*byW0iUgwR>r<`pnyR8u@P;TT|ut1$jJNokuNB`J$^y@u( z?a1XAtC)Rqrx(U4YgVfz?-pSfU=rRCn6x$odb=F6Y~8!ygE}z4M9;7Rt7BxW%Ic5)28R&~3} zt%`;pjNQR@yRPSjRhaeWVKvdCVb)b?^coA8qWtT>mY2FD_B-2@a61?5bw;qk#>O8b zjRy@&`I1H0u{L{I1J=v3A3)_+;+XU=e|eOQhKNDR$F)V#P7Rw@L_%t~2P7^Kr&hMp z6LIOYd_`aO4uWau{pum(e`8|*WG_o$ZYyLl&O-a~cdzOKvJYyA zg9KMD>!?k{vC!Mq??tb z54qDK%9swZEJlsmQ&nKcCn@0_Ko~vEXv&td`$Ai#xiU+1C%T1}MfFOXWM6`XE)9s% zbNb0B%Sfnv|JQ##9gUG=bXgr&r!=aElLQ!t@Kz)9$PZo|3&Jlgb^M4&YdD{i))%Sq zAO0LY35QC@h^#01W@DvwkoNq^u_d!e;Xf}__IKH6xFykKBN)Wv2`-giBOx6RFSHM( zf?r1`-7ra$bakSGbmxHy!`)0`M3M$B}akJmV8l2&S{0zU*gVi>G|6K-($7fD9A z8Ov-kL1NA+uB!NLoUdUixm4<`wC1EQreIUfT~i^BY@rNp^>PN|SN27)Zi@%y%A#G_ z^t+;CJg6m9DCZZZkx7Nh`cZPst6tJ!Bt&M^vHNWI<#VAkwALuWOgbe?2Hw1=%oAD* z=ID^2k_}@|WPWoXGiFr94!xoRjA#TWH)>uz&CpOJK21(eDKRD$oHawCp45MrH*}9n zUB9$0`gRp30%>Ll&XZ_e^&XpH*o2by5LM6AD9}D4c%1N9?4me!)g57D=NN6SQVGAV z+puXYgy{SfW;?u&T=OUH`#rIej}$uw)*|`MBCtdw;5$_BCn7p=FA0=4HqWuDJO)mX zjVzmR*1^Sqntu)-)wi2}TD!|EgI}AbR6DHp(>?p^J`&aAZku^O*vw%vJeib+i%~!y0)));xqZAx>u)4AHRzvwRdbA5o%fH; zyoWU8j3 z>t>S=^Nw9p%( z@IvfwW;*N9fhnm^0>N=_GO~!Lk$Ib)h;VdRciq!2g?mU*)81?Y zXjwppWoEuP<%qHHo{wpgEJ8%V1cxr68bmFLsi7Ig5Qe`S*2CoqmB(uvcRh~P0z#N> z5W_Qy`N3mvLJ_1geos?dc%#k48<+~N>@$((Q7r?}zM+^q94yM}#V-YO;SrE%BivjW zN!lD6eqP57+we<$c$eu-k^ryJd9a#SHs;8`(1^{@?MPy%X>F6_ByE)Tl`hRqnr6|` z&JxQtsHh|@po&hDqXdLFVdip*z;t;Q7vdpNyc3pyVGn+KuGglH2m@cePA%n8iQe1N z1>2Ue@PjhvQ|3+)%u*KXFQYy;bHc3SU=+Tck>+|vlE^kLD9kp_*neIZBLNi!5 zXeFN$=goCjByYB+o2{azL@fKTC)iZ*1>W6zvpQ2Ekf1K_a^VCNW_kochR@& zVtH*C3mYu&Ncl}L=WZt**}zP*SwtiO)+F5~OFU!uQR@eVAwj)t1p8pAG>Q>?WLn`A zvMFb97<;F3B}6>zkOe^bYNXr4{JB8Z-Sq` zy|=lx5e+%=c|ejr8yQim>(Dksj1aL@o1Z2$ap+|L$(+*v#77V4v*uRMW`KQIi?PPE z{%T!(n6<$>rY@BA_YL{Zp(S-9C>YuQz=VV0k+AbZXS&;+$sQ}JQ*SzfJkF^jV^-D) z?>Py%a=}rs))4ygh`Lv@{ZUuUDy{di@1oJ~{Eqk@i&W3C{d=PX3_c~q|IsU_hR-gK z&n}ODIG0DC=$eoKhxsUkdc+}(|Gm0GKKn2}`!GKHFh2V*KKn2}`!GKHFh2V*KKn2} z`!GKHFh2V*{(JRdxL4)v{LZn^&D-?a0Hw5yj;{z#dOIPVH`?AGf$FTHM>@L>F)kC* z)6s8KFrSb?Xc6%t6>}kl+#p10yDiZu)@2QWFF|eNDheDq3bxCZf8m!N-3Ux+xPhZA zJxR(6?bFRmZf7x!tZf_=w$)`_wCwBZY@62>fiWdrGpWb`ib{oJZbT#E>D#e107hb+ zJJa=^#e>_4B*+$kGo7O*HNB=9*7OM(uyK*ijcBS#PfcJ1Bj!#+WwZnf(Bv!&6|^=J z<+L8xG#eiwRL&_eHN9^4D2RV*xuZ4QSHnVWGF=A~kwA?R9M4RoALa)}owJfZQ1Lp? z+z_er6^(2cCez$tsq>Y6pWBU0*9T1vh=BMEu8W8Y?vP2YNU}v;6En5D9Y1$8W0>Ix zr5YspU>ncV8|Naa%1?lLY9v(#gv+b(B*{C_RB_u)o*gkr`Y@gerdlV5K*jfgBSUo& zE~M&X1?2BbvQk*EqHv9@`Un>W8&uWj38mMXj3z|x0jx5p&@l7BRricI8^gje7Sr-2;Q@&AQMNW0T&hlisR0i`>eYlb zQk+jz8E-{1K~iUxq?O1Nd{la?-aDD%$lR+3SO!_3jm|iq&iCA#KZYd=GzWX)W4Lb zX*91@9Gz5DGSPHCOjJH36XVGFNd-JPV<5O6ox4=*>B!N4!jnozs zmBj5ZYkaK_R1Wxr%Vnq}9~sp#Aj9~mFo3)8QTbZ{AC=zo?c5E7)E{v`hK1vFfK+RR zcOsw9Lv&?b)Gz$l#))m~#MX(8ifub5wr$(CDyZ1DD^4n?I2Bgi=YH?kc*nS%y&Ai> z8+)v^=KTF9L<}wkjHXa6ET*bA-Mt(Nn>mAjiMZ<4Ur$rjw+Q6$cXd=j4pr%YQ*rpU z`QZ=xm)w&29B6jy65^KJ*J|9sQ34R8c1z=u0U;3}53BYUddS{(Of_^Hi=^W==G#2| z^P?PaH4pk4`(#=fzQjr+`XYY=t;A&YM}1kPc_|!CTzY=|kP|`EWf(?{p-;*tq(Xq* zbc{it<`>grvijRO;z|=po*Rn^{h)k66OrLpIlvn-ebtH(k8;gFFDA(ji*W9ivg|1D zA`l)wBhUic$(@(E)+i{{3bbRwkQ5->cPs?J(!yGGseTy#?<)m~QrIVej+_2{gGOJo z!>`XP*c#ldep108@)RAj%IA3{HqZ<#;&oM1GFEmF3~|xx+Ac0-JOmfTyjy`Nd|b9RgRD0?52GOI|8p2ko#cNrSN0aBxID(;(75^l6jp`+s>RS zZN?&*V0R5~wT5vnbg~Yspn{q?j@zUGg6N=5RDf0dbw95z0zhy7bs*kKs>xVoH@~I^ zovsyB&9Xt5`z(D!F>L!&N;`v9o=KlA*vUSuo%6dkj0(SGZ%J~@erndenNDiC{Ep897pm5k0YgK|zKcK@C zBwm(*Oy@wRTIUglHycU?i~xw)tnAXtFkH6K`1JcvczM`YcywT{`4qU5nmt4fXpw5+9q;S7qiDdr% zEtZh(-19F=l=sjBIwsJG@)BM$m+j4 zuZQivyIuzaE|z0hP+mDXtQ@B+z2Kc{J}M5>!pLJJv~RBY zZSd!qo~PI}rX}x-zT|jDwA+s`-pGlM*%IQd(<5;rD4Hprh)gi%2Vh)|!z=gd@bY&K z=1u52Dr<3v$gR}osqliFkeX0`k1KC*+fK7*+3b?5TE@78w2KJWAadEc+v8Tq63|q( z*nAh+(!z7E;VSr?^bR0Pqku`fqoyrjhICrPcZ>Xu39V_(-IsIhjTUc!+qeRxuASc-E_Ou*Cx%u06`e|S*B`E%L4+8$IK~-Q ztJ$!#OpiGwj2*IFfKU!&omgqbK*vyG5hI9d?^uT8TsE#naM2dAQQhVE_+!b#`YAY4 z2_uSE)-2$Jo^(9O+QP`8S=(wQ%KA9Ci;jbJWb|FXEv+8igly8G|5mQf#L7!J&;D6h z8^{U!$yTGd`Gd@JDh7z=f3HRwggc$ahuCzk9E$*8aga$Kb(PD^ z9~BosN2&3I%8?QMm+I2aLlW2Z_at+a;Q?k|3UzuW2x}0g&02iWr>+!heQC2#hq1LQ z2uY}E9~WgqU(nTR=y`i?OhQrr8w4&^U<~AH79tzW#?_5}r_@eL3|Fm7=CrFuAhzQo zLa~(8fppa<8zKkhtnYtPhh#!u&ByCqL2#@wU z4QuVneLsDV`ETZTW8Ug0T)7kMoF=Wr{$4uY9J&>Rk{tYazcJ@qb#XAwmSrCC7&ZP{ zL#&s*p(eTe>&Ah7^eu>_mV<@$sKkYnK+{*D2K#U$jz3Qm$ZMxnxBz)u*B=erBo#$X zPv=vSIO)<+hfBXuQfnH}f=N&A#$DYM9-)feXLy*4_X35o1@j(odU*k{ep`lQL^xM+ zeX-u21AG6mtpkp*-JySEp~D980w*dt(^m&SI{vHppfc~?Vl%t;b=W}M|M7t{tx(SZ^&GR$-wZO#G*}8pKQ9j3$PkJ&D*UkjRHogN(F0PS7U@X1GY^<=PV0gN#tWzJWT3(?ufTin-=Eco5C&G3CFZ>j zFKFqFI_Cr)_(8LL8v09dW&Dw2X=P`H>~cnJZpPGuz<$Pv@VPZBmz)76@t8*Z`cr&l zORRh~f-Kk@adRRvedqoe45ggtaTgWFaqVcP%**P3lE|Aq^ zc~YA#g=l|f2jWWjEo^OjREFQj+BRm5a0eD-$QL{SWX2=9) zch0@@j!PGh!#!2Z+T5NLtJbe^K@x3BI3m>D^nmA1%m=7NrD=P<{D>-rwS%9tnP(b1g4e zmlyr-7d>dp3mh+ksEZ4(^Ap|6JDU*7pdDBB@RUH#fzW$KSD&y zzvk76ub+f8p^8GVbtxW~j{jvgS`fVtW{PEo?7)H3@gVMW%Q@GybQ2ES(8u$LhRzXf zyGKdG9BIR$>Feed1+D(;m7iKU21``6Lf8}T~au#pGLi$?a`Wfr_acv6i6O*!xg@quR5a*EjqRQebMUA9z zd}nAbd?R{!EWNQieyS#{I#1r4-NP3yM5DJ|5+@dx5Smx)CS|KqI>0>~dd_x|P;2$~ z!tg`GV271aNZ4tsa=xw~@-3`<_WGW*iJfFmxE?LsYpa*AsyJ9!Wp39u8ikgH-x5_( zs;oW*mg7O*X$y)+ttad{k14I}?H0z3{2sC!W_+2uD6A~9&&Tz3_6ij)ho$CjDL+V) z_e3|Knzdx1DObO>jpI^e%WXimdQPC3U@Vamf(d6CA*hhU^eG<$ch;34<@9vEP6-xG zcWt-e#|)dyK&$&+i0DW^5TxzSE01({<;~zQq_*p~?QxzrrVN|C)@L+29Lfps6Aw60IflU{I zf=qN08of0%zg&hceo=guPWc2mH0PR61BpozrBszL7>83uoSh;D+1V6P823*+5+nl2 zUoeJlj3~B%4H*y7jJ{Dh%##8F*9AH2+vd;H{ATf1VWGkU{LXMXl_;d6C&*y(w0>9& zYRak*ObcH(buMdcwrW+=K61hMW4;9r^GZs1&x#k0@Z6d+F>E`lAquIi2RJ=?8^3No z$&Q4r1g=qgi@FInN0Mp@bv!sTo1e5UCXp}w2CTb_3esa|ZuWg)+!k7yCit0ltmP%l z98Kxy-06nZfOXYsQ9>7+pN#hK?$Q6&Z6v8HwXm(|L5Oe`HbJu#dg=~D29rzF8B+b8 zrYIEYZ1EgV{(p=qRaltgnQ#`j{Kaz0O+-Vhqi3*6@UnE8PCJ%H#_OP&S6S&$n4W4Y zFI5_me6?AKK)6YpE<(*;IQUcvW}`Bp0SUX|oSc7EPMZ>Q zF+gT_f*K{Mn5XN~DhzY8T z84>7NOaGeO!nnW$z_A&Tc$AXl+#0gWocYlWEQuur;cv_Ofkr#Wb(8ZK?#ovBi{sNF z;mIG?wkYEzc1#=~34X0^XB;gu1e=zx06xWSL{Yn{y{2qJj6N7nPzsz66-U@|_0sRB zyr}S$ri>nC4d4#qD!bQq*g<=Zzj^Z2thdSQmstM=S6ul7=h=X`af$KECafq!kTo08 zz5ECoVJ+LMEY68Pq7m%kHVbf%A!HrzQ0<$#x8n2K4df*uNpcfy+XZtp@8W!CQLQ-@ zH4tG)tKXIqE+gRC%nWU)puK^5588#uRKzzm;-OTS7F~nVA(C)%bogSa7ndUk z*1^czKek-){a}4sO3MBP2QD z_x&8&V$si-XS6~*)hy_1_fGyr1L`vYP^&(rjD}h_>O}J;FUds1P@_9>ato(}6r|P$ zkx}TnFkK2Tf*ZlKgRne$9yXC*czw#H!W6%MWma$_Ww=gug#@}ysstgi=PtwY4a#w= zpCnQa@QmNjvpBm>K!>P1B$^U73#Ki z*?46Lb4S}ZIP03#7BWLIg`wQ6qd!XR73t-RAFSOpS?SnvSMS!bHDU^fv(kfVs|h_g z%w>u8Xko0LTJ<>`D1A87KwQ6?y37!4whb7QR#r;i-m{JgcvGZGdq@ucH+?b1z6tXx zvVP^*(Fx;PKJajvC3IX2^*SOe{ShMy0Q*dawf6)NC&$iZKwMb~d>YEmw@gp`gKgzR z#hF9-nqgPvWX}1Pe|l~+$hno1Pz+~HvlD4E+^Ge8Sy>2MXtp7s$`9fv%Gv)!Dh__NtQjAM%7`IsD_;S#bAW2G*_ zp0?%34p!{T;y?~Se2I4iVfZj$%5uMt|8*b>nBr&`MxZ6wzpzy*13X~)fvQik(x1+` z(Osw1MP9D5&41V+J)~|mAq2Bdv27S29I*UCO#v*X%8J)Flhb}ilakrhq_jdi)uh;A zVNg?SR)9K{FDgLakJX+Bu)`RT!`O6t$U=?OA0^llG%u(%EXlK}0RzyXzvP&W5c)za z+=u_`-_M%u7gd-}D7+ptI9eDqXwBc0Fe3CFmq8|;!Y8-pQ_@bEG%YU`aBEKZ{!}Jc z zqrN03gkY3lG!@M!AEPFJVb4qi&N+HfdU)^rqCC>Qg0>1p$;B@(_M`FG6 zo^>sZPU5z-8Sarmd{R+NM5r5iPH616$D;cbOFrW_hj;u1ax>hLJr z@L6VaE|0ub5p1{{hdoX{1~Jc)R>accw_jW^4TO~YBcBa=oCF5cj0`J)(?#=~%6?F} zUXOq)&&cTE;P=kB*2c36MpIfeN1{^7`{LR&XPE^3XB2X5;$B?=A>QF(M9iS%V$J8a zZV^n&1Dd01T(2Pvc^hP8AcWaBv|A+NC`EX>b%7+h@}X|DUWV{A1s}Iz1cGQ`*p6<+ zq2SVp^jpp@b{!P$3K#)sM%hvd*EzUquJK|wenJ)UMmgA`m>c--qRZ)+hvj}ZX~Q2l z)CewtvTcII9}aSnL&P`jPHvn5L3OFiTR>B1T3I{mc0ph6qjd!F>%I-0Kao_*Q?n?% zyTF|OlYfQgUQI8}T6{v>)S2gI_xjSmu)5eg^%0bD;71ygQ|Lo#%9HlyQ0}q((xwOu zP#H!!lniKMprFe<@g7z02GnWJd%6RI#Ar@A~$;AbaZY`P|)ZPs)2J7it|>0Ezz{;{B3hZ4s>+~FVt28 zl#SwumK(=z6!bucLe>nQPwfn&PqGn|STi1Ic9f?6>?9jR8;?HHt_~aH505AXS5MB& zN1U>4;%b;#_8|cp0sA6rqTNKFOp+mymy)?NiqZukM|Ix-m$wOu$h|!DKhoT%C}o+W zw;!!qd}jx!?JaO52pV8zs8xKsCIY_l;*QAdbO0c7X;nv88>LwDf!};Y6r7WcU`Lkuc;&W?~Sz9jPDrn1X9gA<9}^EhM<=578(L#Ua}!a+L7PpQ~C;TwC=5o z=#C5@7|!^zeEgf9Iq{GWm1|rJKi}rAAU^YUVEK&Zes_yVu#lIwY%L^R1wb_zUH916 znhX|T#=2JJgL^h%C*C!}h?oa;A+Smx(6H#ux1lx@IoH~yiIr_{_3J2(3VZf#M|$gr zuhqA=T2~%QdFY1h9cRLx2k;R^Qg^u~THR*3_Bhap+}|S$mbt3DI-{Q|0x3qq&Ihh% z(+<_CQD6B(HiH+SvxVJC=}Ez|6xeWOLciG0+vWPs5)*huzn@pOCSt!&@ZX&uZN9~g zFTRpVI@>@I^L`!V{zTsVpyA!1K}soDJ|has=5$gPB%4mH|l zd}2g)inG~`xxD1kY7<1%6H&fufrzh%wyB1+4I11+=KjreIPu#`=TQ<7&X~sA_pe#0*WRqB zA42&{|CVL^>%O$GT?R$dW)NSZfL+QY0e^FLY*0W`s9C0$k1W&$ zrwSE3B7FSUorttjVvzL5`IWg|#~9xBX`7VNZwyqFpo^E-jU5d?lMkY#_5u5+Q>Nb4 z;NKBFQ7(wihUy-(3v;48?eR6E#CYL1;aF5d${x(FvE3Wi%82Mi!|6&tuSHsK^5cBrA`{3z#83PV5MGrQv!LEc+Z}T)3rG?_vJaPP z3J6IIVoZcL&3y~g3y`PQ$#@kNire^yEA2ouTOohgpX6EG%reXvp6Rk7r&OW{2M@hs z&`aW7tH{)-wGTA{sDn_OiZ>AIVBBhDG{KEv!A2ut2h6<*IvS9(bdq11(M~TwD5RXL zAC-0lY87b_{5Ow)*X;Q7xms12Tjw|p43wXaKde53aUdp$OvC?F2-xwb^FP)#c)+dK zNFgeEI87GP!*X7`Fe`&*xdVCkT~YL7xtMp;*Dt!PWpGsu&`L7Vs?VY?) z&^KzwoF;LcKgwxH zW(X0xL|&_XG#vH9V4oP)c2O=s7)5xjkV)f1d5(PG#jkjgoI)0?9>6Uu=s z!LL4|9mKv_OQshHnc)ZvYlxL`puh8O2zymXHA=eQxBSPri2ZE^%Cx_ZIh@*l|k5Q3{@sqE{fpkqNo4{PTTN|ENO#TU*Xm&XYwK`-5wqU=Z&!knB`7tzNabIF*Z z%^|^WBm}+2vr#Ub2J*8RA#h~s0Hh6A$GYhrrJ!ePn2UOU#sEQ*rC0(YQqDgzudT8K z;D*IBB4HEpZXX&j-#2Mxj?2UOgxZ{6StHH}B>Xu<>z`{T!Xr;$Xuz z4^sdm1ZYxWJy1;*VmIM&vr+`cbb7sCK;lT04K?N`D9Q?xo5gKq@ehW74j?LA@F;#y zw$+qqXeE}Fi1;yWf+cdcSIZ48KMB?cBw%a``d)UlAY?>b+;bpkp-N=m^-oh^NQ_u> zmdRqW#)@CqFXu{L*OyhqJCEizAUTc_Vn{R6E7taK>JyoT-hX5Q~iJ~4&=vReKubX`R^18j3oUMJQeX}dDeUN5yGjAK&8aXolIERYj>n} zNK3~Cy8Ldvh{_+80KoK=sbJ2jXWp7>_7W6(ak1Z6iES&1x2@Fa;A>mlfbOG5evy}* z1-B7d4t~>H9*?j)aXi?_VZ$tJ0ie*tI&y!uQQoPjO^To&Z4p5;kOc0VW9+xZc z1!CUnmngeL?CrX^JM`Id$?6R0-8)z^qp(&n9F7|3$MfSCZ!5WJ4uxn|Tr`kcn|B#Q zn$LSdO&9b-EoVq%`o0gnT+y1I7V^W&*x3Z1uIkcy)4GwuAjo68(D&~TwLt<{6nnt5 z<@wF?IEwsZ5c&>BXIeb9i2l$nZs`GU#mi;11Hle7)3n&L7&tBQSq_L3-Bf)4B_r{@ z=FoSz(YW*&<_F@73uIorxtc+oU~Q@6CyP;jr5O`}enb2gLyh+Ei8(Ycnh3f2HMmXU z-DgeiLJ)RtObeZXe$*3u zG&^f&{Pav+>Laiba?IYX91ktFCiY?|pg2ceTim8K0j?Rn*9<;f7o;3(J(d;vz(1Oo4izozUc&q$FE=itk8> zDpFO_)PD7|tnla@=|v*&Q~mOF*RxM7q3)4yjQlIaVpq4m#}-Z~JOp$3=1!Y$W+t2C zfXIZb1Gq?~mJwyDX3H_^95O$pS_urupm^(%Cf#V(`LVp(_&(6~17o#)nqF+{Ff2O# zI&T@HFn`1+2M25mT9g`QhSr3$>|?_K4+Sag?tlnq9%p43w7vGf&R~aY%Bx^=u4I$v zUUq~$ny_omS_}xH7Uk={9JK!N#wRt0U);!^sGKm>_Op?Lkdv_?*L<^3hBGcfJ0XIT z!-Ojs8+=LEYhw&Gc3uVZFuKPEnWb|5dwl4Fgw}8a+oMQ&TRTeQYt`{`Vv1pUkKZm+ z!vBmu9@UTWVI~hIPg!(P{8qUK@VKnY(D0($IR_{#hrk1_LHnQOid+n{rSJ06SOR&HhBt zX)arh!nJSF_G+!uJIL64Vf`uj!vk{hNVbshz&a_S!?QV&{yX3|!M}m{5xJdW>jxa! zeKDaCbg*+ORZbk~Y|g%7F7oDVdBcYLQBLvO%4SLqoa?$45}nOK{@Rb7L96*rNkty; znWONlj&&`h*V${u4!iX}Ys8scPd zre#i5qU41ou_$CpZU@HfZWXQ7mVN8CO?wc6dB(4%XJ&MYv|m!sRkYMO%8LmSm3+-i z!BUm54XoraGUI*1c*MHvcd~+f7IN??TjqmFN{Dsd!qF?uQ;eJ_tpZ1e?b;A|v;GO= zCOqhA^@+Gwv?)*;q+;P|vx8+%QHW~dBO9K!KqMK3M$WLN1|>ENr~l$gSUMfmV3X=Pc* z3q+XCg5q0K!2M4rEL325kRzXr{f|zFTn7r^_~rj!oj|30it_`0H-hZCOnt-zmc^#A zbTst8eXR1m<(TuaKkCeEg}OD(_;3oFVKZ{~*j6iIjWuJKet-Tc;3RfQ{lCtU{}Bn2 zO8<$3>S7-DpOe%QmS6PzFL&*g>uQoV0q{2?*>Fp?ipZMbcUeB6-AEpjs#B2@beu)} zvV`COT))p)JHGPxD&Qa0$4lGDNXmO#H#hkhSP)_eyCm7;CA%Q2=hw1;d%0)+6bW`I z=bE2`3ohUz#?Y@!>?8WXb=Z8!M(=uCaQJMFW80ttYnueBR6sPMA+!AZnaZxW2(Z9f zCe{#PFmuEi;KP?YH2``Jq%6fUQBz&n)EMJ@%r+dQ-E-3wQL6FM zxuL`R!@!X5`p2Yjdybe^k`?nkqdSi%&ahan6fU;F?K?c&EFyp^37=>t@Pp7s`6uH4 z#}1%s72pbK>weR=FpdBleE5`%_9Y1)9w-UT02KYRY9_(t3Htd-{U^9?s!N|!iAj!+ zG)TzGPQ>|(`TD9^@Kvk3pHLf6C)AKWCiIVtoc2*1LPusz`E)knNl7Pby;)jRCHEW* zXibEYKSM98=%6(;+iNEfHEnsC6Sf;T`9Dg$)|#b#MPmPHhvB+vvfnyid@budK?K*l zk{~#tH@yu?j&Gd1c5B3}(-Tmkr;rFNvH>>-tgK$h-w~t$)>1NaS6Y3J$Y+Og@=To+ zNE_DfYl{nX#pyZg3W;L8}W&Qn`NtSKj zf)`y466AmB9v;8z5E31&(;+ckFkm%r694CeSE`bVZknV&Y=_WF&EnI(XGasd$S#cW z`R0+lb&CLq6a>w{3sTB}p`ZUt7dSw?tZrAhEQr``=j}nWARnov1?=+(Uj_%FOJ$VkwmiTIMVU8LWFVliB@hf8?^fW;sWf*@_YFY z`K&WqC9aTylw*EZO4anLDK#hwgil3m=SWwpaX=Fcir?Ledq2<-Vz%#pOaTH{H%}!E z^JnpFUNjq&>rjX!(7T1$=`37#0+J3&X{eiW?{x393{zgFGT*MUSG8ghWgS%y5e7cy z4xi`FCm{+t5OO>!f@FyT(gDq*Y!dG&ophab7r-`-(sqX zZ~hNuy)KzQ6TCtMpFnx4x>nFzA?4bR{}&Yyxv&gxwLWAK0VCn!K&Qi^d5=XcY?xnLTL=j?N0V;m%cf!BIhaGR(kXK zoheliP1}qL%%2huta*4mafHu}DdThDKBl%FyU(;bRkTTKtWUdsFuT*1!(?5ZYAh?4 zWinv21N@?|7*a-4fX1_sYKai!XLn|mCwn?Me{IlaJ&l0(e+WQ+Ehp4+wmvNEy9<|( z?Z2Ut(L_6b3ax)vD0s|-6?uz!_gEs9iwk{l7YfWWg8`yU|9L$wE%q-OYVmVL#YqpF zRlV?ceA+bK~35YUYQk%U3`a&6;hP>fhr`Zn$BIE7o)^T7TjN%l#_d z35SJ>q}}d431Hy_VB7y)BurAYz3;Z(J&9rkPg8K@_jxU96e1CGD*2*|MNo~EV(*{` zlb-*Q#!!pUc}mp|!kdUevEi*o+72L4NgY<6dn?wms$dUhOd?DtifqwB8G~*v5bvcfO;BOjwQEbUEl&SA-|G7vch3X=$&dA#(?L`y<|A7b3_-M72o`Q;@(j=~ zMZv~e=sKlbn| z7{aeQj>T!8Y70+H^8un3?fDRi1r5=-*3v+mXk5A*8VK#(8D|04elrU1lK4aI`9CMc z*>$W0pH9ir+^f^N&f8TG-|pBAJFS;8m>;H@oVZO9?)52N zHLR$pFY;z7Oc&-?=7yhu(zuzv9EQl%VGfR#?lLEcjsUI6n1x8g|Epxz zu@OYZ)R|wiuaGs3M#3;}4i^k!f}44>+*RF^TZ(ed`nJ_k%NM_BGMohYS?bmNJb=x@_ zN8B+xql4UUO+6&`nwmDt*b`RV5|=f{5FXPN`qe8oqFHJ@8md`p6uCUucm$EqW?(sY zuqB3iCdm?9Sn=-i%QCb}=LIDQp6uZ!Ba9mPsqxiqkM-R{*RvRUz(udb`oL(e6m$|T%%BZ3M5gW0U2 zwHxHO4b=bYiy`r@vn+Bkkj4=`%@VG$4aI4?*_|_x(J!fQVsjOU=&2yS?C%HTzonQaH_f(1xYd83$ZXu34>iN zQCH_Reav(ypXeD+Td2p@;yZ^}m>f0AcP>zroaI9=HzWXh+8}{86NC9Is$xeY&XU>Z z1kX|2LfZs+^cOZ<)sKYKx$R4NSc$xRTGCX-&YmXPVoUXyu#osgtK7$INF~z+;CVZA zoto{w0fFXiW;YGGeX6{79UG*gyL7STUk$jN@qq=gmCZjCJPA#;HvFt1 zn(m+g&ya`gy>zh<8CeV#x^s5>eVh+cZEN)~O`fE^vWQa7@J1h+^?z~)Iw=Q#AV$wn zs^ge!AQo*2F&va@MsV0THD}TrA)Cf$hQ_Cj0QutzLgmzD2uYxSNP{-47rdlca|-WD zFp};_|AJX{DA)AKZQA)rAs6xGbba(J7L$;~AGO!m0E_2w6jL2u-}|%{N?Wy1lq!H^ zGJI^R#dS@^X#8d)Q5>p3s)ata`RCc|>#+DYgTHNsOw(Ih{sXPsWDEoiToR~=1!L2f zt!!)j9TyQJgR7rZ+cXq&8EX{}l=3FceVO(K!e5tI<;>^W9JdsMHr@R*sKc;^CZP$A z4HB;lOczMw&X9CBA#-)@qMG5{FnQ0|79-^M7mm#dF6&lyJiO%B)Jy8sAh zR*DMqhe1R+VrRbaA{FTkCIW^? z>Xe39<`YDinvHXB@-#xGx`nzAmeo ziO?2zJHVdoNQQ4UFS6ZfEmHPU3e7W;V$GKPY>F9beU@)VHlcpA$2?Jv38si5iW{7i zb)n)}Xh9g50ggZFukGlZ&s@6I4WA>J-|i2&z5xgD{9${{cYon0BIm?TBE1tNG~CnB z-v;bt=LTa&V}g1`#=}(fX!8V#YWTr>GP7ft-qxWBL>6rh`S}PMy2!|BZzPteh<|~n z6#*0xIblXC>}qH{bPuS1G#V?_N(0sz_@`s?oioq}q1#+gZDQ@jdO zFD%RAsoC60y*saErMohAe`B_$MQ!FnsUbWQ>kdOrI#?DiVS{@IDI6|v=g{WA3Z2mz z+VCRqXY`Gm=Nxkw8lR~4pfR@^1}hTW2svDXD+b!fxdfsgW=;ml)54pU&spUS`e)67 z_`gLLrV?_1%Z-{^QLPk`U{GHg)70rVrK@raWCEiq!Fegv=urL-vK@3kt?#~jo zKSD|MNQ5Ik+^3OKS_OX#p&(njlNG`J<0tb|RQuV0r^GvWXnCyNMlP144%e8?w4Twl zt10JDybc&@BrAr|LKCJNMUeM6Uxdj{Pq-6{$!9eL;L)edL{L;wgkb6R5oE%^$y=2E zF*as}2a8AFCi_#x;iTtUwRen?i-R&JK+lxnGG@tnJahZX}{GocraS3f0XcklRoT>Q*+qh!fQ-y=$H}Bx#;%8Mp9- zPbNbV%Y>}tm=;RN%`_f%AX+TR=q7K2V3$fCBD%hXfhR1i+|JNmOj3R75{Gu#=N0+# z`GYYg@kA>dO2!Tb^k5Z*8V%4ak3gYRf1;1_dD3%sa_sboYTehxnv~%YXbDDjk2Y&= ziT9){Yrwy z{PlS`CS8`AOyFBl$PkYR(-A-Iz(`QHW+y&$df^^C)D6sMvW{iqxo8!0QJ)MIAHdH$ zs7DVwvCj1QEC)k(E}2fml~VI$v@f8x(0B5w(x9Q&uWBzvI0yqQ zehK;CZ(pXO?k%m4U0U4B_p8&5(k25N>xWbqf89`Us2!}}vZh0?qI54N|4vU})j}k$ zY-=m?@qlewLPaV&&fL5DCZ;sfp~Xov=`BuC>Cq{^PW4ryssgvl68uo?jTsCk*TA`(GRNt9brvp`I-{dnEFFsTQO24I$7CHlnpK z=zF%m*AlK(G%KzU_6;y!D4WJgr!$Jf{TMw{ zuO^;Ld7odo)WE2&B3D&`fkA4*U_r4RF8qt7wyIby$h26^;!;=Yq*}dnJRO(#uncOq`msDWaW_%-cypso(mHLd zVR@Ee*fTox=bZj6mdxtF1Qcfk6pxIo=}L0?c?%GN8t@y z_=DIDi_m3Js49slg4R3dZ0Kx92B%=+>G4qY_k$ZTs`Z?2Yx`-PqLTvPQV4A`-M(t) zeSweFVHZ?%)xQv#cnaPE=EZj1y^W#ix5ox!g#n_q3+Qd%Aaq#Nn?y~Ip0+cg`1+~8~9Kjxff zY=w|J2)FgQnR$^9l_1FRC@^!Hz#TNbd z%#*L|;tUqng|9F%)~KeMhU31}o<^YG@eBto0yxGrvEJP}Y*}&SpWg7J0^GrSFPtxF zy)IwOt+_9NAGg~_=iUo-BGPK$)Y+;WiJts@DNu_?$=!{ZO~ z_q1@)amB88eaSz1B~4G`zKE!g^RQkR^$9EMV{IuKTgL*j^t=sRv~y5PhwnZHax^DX zh1Wz}sdefSmA)^omkRZ*N$bcyFG?6JiCRJrW(YBvCd_6PxHPQJfGxM(fZc0JmtzlR z9iy`Gk&glxea4_YDc+64S1A?J%OmnNwx_pPo%bHF{}z_R zQB0`}mqkBrSYyBZ_z$DIz?}CexQg+QnnWCP`~$mD+1y;-r5v{tN=+te`&xS}eQgc% zL>={2+JwvXfB-T{iulGCvlub#&?;K6vP&e2zfeaTLIS6;pQ>FIG(GpA6g z+plVgwiiyUw)kX}$-9X;oLQsqnx$nh?`?L&vRA4TOB~Ck@^+0d^TwOT*jlLtb~Us< zj*$Z?^f9pyM+lO zy%0P~g~y5rrYrl9TAecb&QuQDg32D#N0kK+FpOkEb9kTF5O#r3zJwu_nBJwTj%$RB zN-5|da$7dG#QmnWMVl%U+WDAu{KHZoHWvEr8U?C8jiF+je;--yII{R9rKZ6d#ijjw zx2QNMdg$^6XmAaobc>M`e?9FIZ4aYkR|-t}O;O^`zOLirK%$I)EnOB(T!1Kg3I7Ce z+R3yYwE%x-h16?Ff3R?XGE5eq)(^`{rrwI+q~h=_do2uE8 z^1%gG+Mxxb%d#lLQDsD7w!KOd@_U03FW?5 zr_s$$E#elQgEym4W{Ynoe;9PL9x=Nn;mu?c>8I17ndMK{iqs*t2c9fW1ybdFwOo^d z-ol$nL1}O^k;>{~#P5=jWP-X9s z+2dygkCtGQ(Zdjn>WfH^e)VZkh z6QE`AQ&|T>=ra6K(q_pBWv6DVK_h#1N=fpd%IuFiMka%w!O&*l$Px*ejywrLmPqY7 zSp$!`zf+iFKd1+$i~*7S79h0i?+kIs{NFJSng2V+A@hIxIAlr>NC1yn(HS-6MszVh zrFs>BiQy0Z^(h1~_z6mCl&S$kOdX{s@NXom&zx)xxMJs0#^Mg;rGdxzsbohrq@jez zjz;!UAN1PFshS`TpMJI^%L3a51)wZ10Y`9UuaeN%-~c-+Y8kAxied$ZEOg*6R3z={ z!NVOR)8-nC4zqSwg-vDH3|&TVh?bt;qR+VyS-dkQ8H%o;+qgGFdR3sw&S@aSLm`vB zI)nt)gvHq`>Qb-f2D55~MZ7!e&(Y_wsms{`r-UuDpX8v$FnD+rf6=kwlbuo}7v_85 z!CY@xl7F`@ma5?r>@-G9;*XybJL(|(W(PfH{>q_`H7J33EHZio3l94{p?ucIhfF5f zmuq&^LpikZ3J6&>Sn6TOQ@=MTMoUO8h*JCevV;x;GxlF$*NSTM$3uFa47Y=$LW?Pr zCpoxBL(&>&>lJMdfiDHEZ;0s2SUKsG1Dv9#&J7cmo=#X{Bmr@G=*@)kKp8o8E(04T zkI0146)=qYQkkGhaB`EQ5iU%%t`5f|gA!&R;cT|kvTTA0Ync3yUJXGP@US;4^O}uU zs7=oWrAz@BhRI-wWmQ&~DX0*Zfy~l^2^M~q-`q}UU`<8%R;MHoSVN!Vrh*?k|D^H2 zPFc_yXe&DL1~bKFw=y9gdDifPuQ`m%MZ6d?;B%qrNbZ-aPVB|G-ZHEUJh{q6a&w4Z z>#<;l>HS6auYonV(ef(k;@zMiAKT@YkE!g&=eyb(FvQ%ec)+13m@H28EpzZINl`K;OBAaF=_^V-td`IK zY#UsAaARI)gji!#t^ye&W8WZ&E_O<0uu+9mh+@1ApRVamm4*_#To$I*EQXkMwED@( z8&@Py!|(+>_rKL7Td7-M!tmV-T#L<9W&y!C^~eD@<+XM-nT`OU!<<3@TRq zwKf7M0L7x8cQl0@)WDr=041xs2m?uvu zKDB6AW?m=^tTWZnJ~Yr*{I3UjW&VoMuIO*I01HUa3Twq_d_b$$t0iYHD2vy6mn8r| zL0HL07cX~!t}?!mpFJ&G*%1f3&k9r6Ga$o-P`6O@@@4rJ`R16>0 zTdo`Nqeu$DKxu)D;#lqsZqyn03&X2a9660j8a(QaXGh#HIOJ=6N|D5ufRD1TL4dE_vlY+w()!4@4a_7kZteiZ2R zfKTt(i(|>l7^tI?Epidf(S4Ag4-fg2sp>+Pnk?uO!52x$1K|dh^Q&St87);Fo)aaV z64dB#30<;g8x#UaBq~sVO?)MPxKb}KW8yQ68i3`Om&(CO)FhuuwQuwxTNfS|~bM zyhe{x*GU7|C2POK!j@s}Uxs{A7YGo1#jihFGDeC)Z?2vZ0H)9oTxZagM1j}Mq2nCB z6*6HpLz1^kUBY+lX(s?nl&PBSvBUvdIk!zyeuqO`$_L-$*%3eViI=bRpDcS7Jfey$v9qPRr$S=wBPG z^gfLDkZN4)p?w(-%z0cH2v+o7smxhsHv6me{R)cl7O4;8#B+W~gUp3ve|ps`NTw)4c0;F`t_%y z9wLdF8H(xgvLiu7`CainHIXry@AN{DIf9KE32c->*U4_Agd9a?o7WIIid}>QUptSX zqh9rk!}?T1?6k0RQCi_J2XN0q_?$Y#h{DT|y!{4w=S{55#Q_vA&^pZp()nXjRBbxXo*k%m_Xc$c7nS`+$=_?3>Q(Q3*Z|yj z4^tmb;+&kdw&>+*1$P5?P^WT!&%bf2`ZsKp?Dj*~DBBHWAC13GpfBcWN$~j1i3ihu zIUHUAyp+{Q`=MEe7p4tl$xd-XoKhjPAbVMvLqBHGmL|$dc1hBoA~;e**8*`g8mkE(O13|09MKP+woBYuZHTUBbJ=l z4`IICjT8?PMY~`m!U*bw4K#qTI*S*L=b)XhJdZ8;HimkkX-?S*wDbp+Q-g>Nr0mxL zmepCb;H;oYS8X8P&e=ZXgmR@YW(H+XZvM`7%KqKyzlyLahG?SVMkc=Qd%>`X@T+2n zxO;JgQlk|BU7htS454(&;$6HNLX!rRfWGluQdB-+WTmEP3EJW8$Zq}=eng|il0~L? zE53TY_;;}})yw51QxPyjK_gx+gpg$~MJ?eOH;AISd|0d~F6v#v*}?qf?4UQW-Zzfn zV>#Fd$Z9RZTpd)D!(CzpWw^+}Uae&F0P zu*5S3>~e$Jf&FA(Nv_^J0uxk=M0*xL-w<=LwBb5(Ye><7)!iB*K9wNBmDwa1nImOqv0)u#M7B) zeoTgs;Ww$t>c`A8KhbY~qTgK5Z!YLJKQ8byaEn!)o_L6*CsO_-mGVL=<%Lwr9}{B5 zqM)AB3mByT_B^#A^9oy%&GnOft_%5G7xKBdD~w%R`aF1K`o>H3lQ`9dI92rM=QD%R zaM&yR0t-4Az3|NBUk`2^Tsycn*`ACi50ikPllzmMgIkA$1fATO>`vAXZcaAg-|=L# zn5<7W4z5r3CXe9p^~o4tQT2P1hs9+7;41vF0gtX?i?Hs!$HFRH~mI5BzA{^DvHB{=p{&KB4A&ErpNc&w?SsAy<7pgOy+#gC>y1i z{{ltg*kfO>cZW8e{152ohm)OTN^bBjrMQnH*a~0@%~+po!I!L?srm~pALnE4^Yuq1 zK}}PWC^5AU1$El7Y=5!Cw7Z9EY;qr4j(f!vD5z7__%~eTwAiR!>v#kS9^h9{5irxH zMNOT)A}j(bIxalb_RVp!4=v(_F~(lSeflBB^c=$mc_iG`!!&(dT$sMyQL{BVE1z2@ zEja7sD`)_oARBlz9(j$^O6BO1y{Ex=}lgVZ)p6irFV;^lnst@7w#$inw z@pbYmDVK}b#?umhLk5A|ocx|%?^xK)$yc~@;aTK?Zo*@%eG@kU*mNJ@0D1_VlP{qW zkN6yRIGmH;K}nBzw54Yo0XcC`FL+bO4HM?XE&kdaX!lJngN{o%@*mJCvIm($luoD_ z)6=Blngy^Err}Vewy^~z z-=*UZBrX}^b)Mn(sh98J*U9k1SZ|WN2rZi7KB19?cPLS8@}qtHDiq1g9D#vqLjIhx zVMj*+owC9ivmf9Idf$BMxc&RPVS&P|hQc8SudVxLg7F00aShz&B|%NR2f{`xL}=HN zov_e=7|Mk~BX$lxQ+wxO&_mPxzw1`r4(-IBey!k^o(ptN@I zDQ)VvJe<+VA8iu*wF+`{@@xEN=u6)0ui{|yV)f8R;mg;lp*!4pL6M{#g@O9W-*68o{3) z0+vqx-TP3xwB;!9K}>_h&?_l8(^M)S*fdT-!yZ7K!m#0lxK0!J77tD%*y-fA^r;Vc zyWt5L3NKj{RG3^UB`E+(%<)iB8}x}7A+;fK8ZA}!B^-ylv_u{jbga`ss?k$PCUPr& zJ%ys0>JmM*w@5t6p{!Ed?@{eEC8UH1a5b0CVz0t9+{fu}I>ErOLZ@3D(*t9j{4cdz{*`ublwf$ul6yKx|l2{I5s2uqfCae1JlL z?7O=Mw*({>X2_QcCKe|Q%g!-4EbI%rtkaF*OZMrT_PFjCCN_1-Q>Qc|#X=n$dQo z$I=mdjVA6bGk+9{EDhkt^eyWONH#oW5_qy!J3&EGOe_A8lmeKIbHM^kAkC)IwL;FO zGKn<%07n)k^+UG#rDWWb8l9F#S_!-6_^`sR!mEYVd>uO%)jaB|JZ`PlwM6P;OQa_R z;Ul$q1-p%fw~eJI2X*Vcv!bAby`{sE^EuIWb4j| zQXD{Us^BzOFPQ-DiHNdo4jemn>$DV-2)#b&r&?EvH8(v4A6E+|LR(wFDYCOcSC6<0c}fpD0}So%SWT!Kjc~;9!{Y zVrOCWU_Q)Mwu28Wj4;TTWavRzhOmU;2trQ^3GT($;n6o3S{SA<3Q*hP0&)UW@%2;Z zz7fC<$FlfF0W*ii@ee{QVwBxLKlm8Ebe^cJfr&|(xO)OOYTS(BF zRN_7`ZlD0Mn1nv6wm9EA@p1yV7?JT#GyoOzdl6puc!YD{#F92&r@Pc2CC$MQ8*Nfq zHreZ$LL-(^lUbTTh$Y`|aTz*0>3uvD*szet8*Z!;))`30ldTYE_(T?IIO^&oy86;Q@sk!e_zViNPVjfj^( zCXR+Zfl%B$h$*Pz=HbgA7Izsqja@wXpH#&jX@(D}8d%}il1Rpr|4y~zF;7Mu+yEX? zhlCKuZ@hKx1hw@l?}teo<0w%I%s5IEA{z59E&A}SW=mZf^%!Mz5ECD92dFI*grZQg z%Hbiig+RF_R6Z2aNUbQe{L8A5#!8<26PW=w*nsHOydI+_!$GpgrDn;4k<@9|Y(5KP4DbZB1<6@8HI&?to(Z&UZ9|A!--51vq zO9US7ko9Lz+cRDN?6b5;2VmSM$A&-cn`y+UCazy9+4qZlk`}ys07{pGu#K1t^|2ya}IroaNI*#H$cDHr1?w5;VWg1?Lh07JgIOCi^q$|A9=c|Pf*kJ_2~=-c_xB+Y+P)rUOwe2{ zgG0BXe#dU8T<^4kfO{X`SWE&Aym9H?C1dO!ownDUzqtu3JmD_&gxq$3aGKfC(u<_* zDKNsd*lj~aQ^Mj+V}Pc)-9_4O!4VIfH1URlK~LTA#Jd&cpMNB{CE>*r+k0#!d&2PI zWb~nvp4(y+a2T2yR3tya|6SuL$_fz00ef50*tZ*-&{%W2(&%nenR>H=*QnTzBjFDp z!Jc%_s$3X%tNK@bp`_>*q&eTE&?=|)J{Gw0$>&B=PVWCX$BvO!4sOH(?bz=&J|{ns zea`Qa79{h@AE^4fuv%<9+6Fffdr8M#;QUM@nZuMf_~rCZBX)VpEyI=y{PZ@sa<_f* z1lTOZ(t(~#6dXP~xIQ&-d3YeP#6|m97>Y@eyWb1LyEkO_HCn-4vR|Uq*>6C+ax88*{VZBu3VSP4uAoIh}N;`9?2B zIO=S}l!|eB2Bd%T1>YZ}p7wD1<1ZbPJpT)t+mFOR{+Yw>4=1(4@Q>pF9hQIRv&Dn^ zfAvgscfpwd1NeXT$gx%3*gey^d^Uh|#7i!}&lG^pBxloMU1J@U95HrzlmR+$2FG;g z;FkvvS+0>umtGE15`9MWNi+Uo@M5WfQljb_*#V0R@sE`R4E zoNZ~|2l8F-7?GM3{k`-(pp7ICMB>^DSqnGCyh-PTBZr8SH_$|62F=>u%&Ilh@KzCB z*4t(UFSI1Cr#L1kqA|MdXT+E2Vn&L7LXP7p3ds;tupem8-5WSJTS0A$Gxygel20Guuo@K7Eq3ip( z%P_~nVnNZJ9LK|kgx$%#q@QbgQj7SC{*)Ku1-D^S0>8XF=F<~jbZAb)CHJLvY7l64LuoQx5Ic3?%~f03C19;-{a5_gUXn!8+&q(l?-cpUS+TYoj*|65Hr=vwkihj3D^6i^$0k@CDpB>bM+Sa< zY&^naUN@>upOZOkFLk=?$|r7#8;7rQn;W7Pj92|nW@?8oU^>bOZJzjxXNDj@}3 z>XLvf4D4iq$^^;K$#!zzd9{2-mAKcU zXs)(?^Fvf^<2XFR`1s6TvSpO^O}151C1cixcjQ~fP9S;2E0j-%)-m!gHEr72WN+T2 z5qhF<0W9aP$iTR>di*cm>?A|43+3e7ln**L`2WL6^^~e6LsjE>fTHmj=f?w21glWV zMNXKz>~ioXs)RmRhMS{Q>7)THe22n0)j1_PmK<&IO4tvD2@{jDr$2=cMaQOdlm`C` zE72x$n%>Z)y+JPO9djD*@IRu|xy8!n;$1v4`GDas`XpA0PO?XnF>-ED$ycp|h@_b9 z_+m9mq4pG4aXp{9m^3dS(vP(Eg8B4v`!7fAwJ*JIewhB^Pm^2uhPe?ZW@%KnP2ADg zG(tw1_aq4kaD8=yJT*J!ND)7lNELE**aX#&&0UeM!!$!P)*mJvJoBUm%8?^!lp**M zAvCa4}f^%ROx*#k0^>9Rq zAdeKUoS=@!cAMZN=X%~JDyngGgd}tx93yf#8+U>;w<8rLL2VabgPjf!wQ!;jm zbq3CAg^1&u=Gm+Icif!8{?6TS>rTQz2SVuS{@s=g>x68F2Y+Q7B znX7KW_D!r>DVvgai^!YVY|5|}9BWR|{XoW)bWc3Y?2{tC#o)Xj5w_QQK;5P36b<-G z;b;|6M4o2`RYDUWHDHD1b4Awm;;2WwuI~~B7wH9~WhCsWgyIv5lI?&V3Vttvr4zwc zMbbRdpL%8E292y&Ep(xnL_!W6-6@f2d8K_%O4CXEH4|tJgg~o5M3v#a-fRy$BBTT& z_AKRHq^)90kd9@^-ZjTI4kJgU;-vuR&cXn-m|{zuBObO6FU{OA(+qa!W_Vzy4&{q> z9}0Q6YPwnnM@S9*deLx0azBx2oo+EPJ$#zlA=dozwhI< zMhXqi_Skosn*a}TGv^AqjgiSgyj^v^a1w!ncHtbCNLn68Uej0J>&?CA+6zkaQb5VY z>yv$GENS(ICOkTz-Fn5Z5-$o$z-ei!n3poJ*-5f3x{^=KTD?B84idy3!&=fV!ER%{ zv@NTaTLh%+n2^&b;7>W1`Ldg54nm48lp;coty9_VCaVW?i8b{#h-YCB6QkBE#u$MV zR72BK&U1mY45_9UOT5KqS`iRwz0TCib{g`YnHnmh%R=UiK{?+W4I2iPn6aj6UH$I6 zM7!*_59jOK{n5}H{rrj`zAGlJnqTdgOFbm{s29z-A$5pVse}o3u`l$!=AQ7pmjSyh zAgZE>a0$mj6Q8loKaz;Vbq=n9NrPwc4Bg$~$l{D#l0S`OeJvokVz-;VH6j8;aD6`o zXQZjGG^~V*fV1&NH#wHsTvVH)^yhP$#GY59kCGPE;WK7o7?MgHsmDo7Db1SY0`>ON zigP|cF=>(4+klK&7OmK41Z{cd=QE>)fpA5P76?m~Vn+_{(A@dd0|tFOj1jwdp2win zXxX^q4DlNWw+&werul*_6l|-9S0Ic$ajpn#H>{xzvSDnCL~y&Hx+AuTeenhEvJ?=s zT^y|-R&9iE==AI{utn^O6%tzYfZ8=Sj1j7jqkHDAw~wDK!u3i6R|M&$s{i&Sv>LCD ze+qwpc38%U*4nF%wfn3QP|V|Cjo9++ZF^HNN7&sH_5O4Rc%OB<3u^>TreIkfoi8F) zYPi^at_XN{Xs(E++Kz)Kg3D)TfLI_*Esvm<2b3N5k0WtJv{r)Ip_0eK50Tptt%`&{ zbU;kHx4UeH$HxrOwbAg;1%Dgg>(ug~;XXHn=7r#M&M-qz&|^nth+RHl{Bu(L6tsj7 z!wHcq)WQqFqdR^bQ?o%pr*&t8AnB2gBjd_`*o75>06$TPQvKS_3nAO~7sdu_JUL_2 zLzn_q6oF$j^Fd&Z0Uv~YEPN0Yu!&uqV}#&VAC?i~&T|J}up!Xw5Sj$RQ9;0P?2^;K zCxVD+AfTxRuGKiyFvo2?mf+n-EkqC<+j9&I5FKlhm>TNz2ENXM~1~8HNY712l0vsAL&d zhc9UF+#!sI-3Y4okgN{e4c=7VA_1d~(E%lX{O~-NgG&*yIJC1je3)l%@U}{tt&f$x z0UlfShVU&7=``#O*e4C_4JfU~+zr%6YG0DL6|%D=SR0aj4IWd&Z)Jacz==DvHQ?rO z%lT9cQ^UdSBl9%iluPh5d_^)ZqNG8`DW(SM_XJaee<-u$(3z=01Pyb~A4RJ`E=Y4U z+!2+=N-*6x8lbHf;nJPLQ*bmy1x&%wz}BUhnc)LIfP7{KbuLW7%b;k7{z7;e?$Xiq zX~E&fI9@6Fvyqm8tyESJjY%2qnPu8e%ixP-1>JR0a!a0+0XGpx$`FVWGfvj>v!ODT&t5FqlMAL<J>~U7kucb@JCiVY*4|^FV9*Zm-GrE*TM}F0hUW1v1f@2xFNB7JSQ%NT znV)B2=vYF)#-MYVsT4CZq{>b+GbCFPV$bbqhKA4SQY-Hn){@`O&wvI0tZ=g~#m~T9 zp5SL7OA#qb_gP~y%nX0Pox|g1z>`jMGZ=f|eF^4jVP}BsUvy+=fQg%8XGloWZTt)* z-`_k&hK6y#&+y^o(Q|)A1P!~>ku&Tbf|!B!^hf3hA}Mhia)#Z%G-3ujS*Iap;FHbR z9@je(GeD+oBi9`dF~jauqzs9n>qgA5+mV!E+)m1XyA=7s9tvE;e>5QjTFueh$ieqb z2DBDJhH(VZiarZSs->$uB}3#J(7M_>OS^M0d`=fiHk~sh$-kgtP1ogWtK(nT zJvRP@KbdRr1msI6#cmV#!oi(F0AcrxRp2n(3!m$l7}F0QC07VW-%`8w1Oi!eF0jk& ziE%C%@N18CL3y)rc5Vdf0wMmZxh(D+;ar=;C+uzxzMm$?LmX(wGk~Ow+T;t z?Mk;0turH3`*QSDqzhWGX2i#&pqL z;M6KX?z$biGI58yVQsG`I8efBxle|FLGAxp_l?KCAY$y61qivOcW@EPvVGIWz#z@r z=V&iz++9I%pr{N17k_+I41p^~SKFP8fj^(-)Lm4in23SH*p5iYkd4<&P%%&_Za~FA zmW$f~6~mWK;*_Dh*_DPN?XF1>F?jBnPBaW(F&vmRC3PcWNG8n_F{J#&g@p~9hKRun z@-d_dY>MZS6206;!_Zo0CnAQhl=~)*iUyBwx3HU_jesH1o2o`)eUDr4Af46h3U8#o+CcP+X?ao8!$uU3y69hW$DhoU;+hPp^kka zKB>BMFJ#O0xfeKS)Sr(Y_d?eQh&1~G>ONDlFF5;qXmC6%1H(AQzwmj^l${7!j(Hfe zMxDpb#;}oy*cm?wHim@Q6ml^bUETM0Di#LZZxjoI-Bym5gMpPMxr-IKol_hPQ4ZIe zDErC{l{ae3FfwG>7(|-S-p~l=U5<^xXa#PI zjRY#$niE_M@dXvx@jvpl2F1tln`!tMas#FSkeM6srowph1@_pmc%zMrLEAHLcjjT( zb36=f1!OrGtl7%rLAd?6SQviqI2h1$DaPRaxB;X#^ZiRh!a(!oLo^{f`h^dzhD4KQ zm$x%ZzQEF?zv;FSFXZmA93$}pg-|)-g~+{iMAiixchl*Tx#KuMTZg{0eMGi}WHb2M z_;hV>EDLu?+EWalnyA3Jnu4f~P5+wUHs4eYc@`YUg0Y<0$_&H8wp=5%>hzdyei24t zAt!1h&%$lrA9DK$&ChTxxJDbEgk@^fbcEa2hSwDgz%6WYAoD)#F|6k zmd;%%F!*#|>&{JjKnhA|TcKKC}-ag)IJ&gLERDa1V##%E>w$d4`6 zObS~80QaYgP2myW;fr__IO0vj2`Ff zvP+OO?Su4_Y;4sT(>o5JA9hT^{;;Tt-Zf|TY1@-phv}rC`BhkCDjYB1P&k%e`%-;1 z?eg9?XSeZvbHRCwOv+BCrP21-6llB)Ib}4Lg2Wpzu%yO{vw0a%g)~=Udv7128B>HF zA{Pnsga)Yte}Vx-@Lkab1!Z2BKzFuxZ}}QSf=MC3b_=)^0=GFc@Z3ZXvK7BJqgqlt z3OEAXOH=VESm~IpbsCsi&e%MgLI*B|8-_~(C!~0#4VQw9sleYWqh5HZ9W#@{v8>EL zPIr!Pq(bjDvVl#HgP(TNE0cT zL&Z0@Mp0=rTrlqU$+lp)l*Td=>mhvt`(G%%*!I;QhA`Zup zeZlfIg~5_hrIk(Lb07Q09L1^8_D;;ialM{nQPA@s&!RvkanngtvM99ND0D0e$=zem z7X>zG&A4UOjYGk1vN&PGQNd@5L%|XdCdHwr918z0awsT4Jqq0OSrm*Z2-y>0syt;+ z`0HR#pyMoHPvH9!w;g)|sbCM%2l2WWk13Dxn}^4kkiVFeMji6l5`3(Oae^;_MR;e- zpQ486bK1d1P;0=M5Z2OxGa*ofli*Kj-h_=MwP$I8 From 6030d43ef89fd7b9043a93937f20d3fdc9915309 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 8 Dec 2020 12:11:21 -0600 Subject: [PATCH 45/82] Update axis squaring checking (#699) --- Grbl_Esp32/src/MotionControl.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 5d725ac9..74b53d26 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -246,16 +246,17 @@ static bool mask_is_single_axis(uint8_t axis_mask) { return axis_mask && ((axis_mask & (axis_mask - 1)) == 0); } -// return true if the axis is defined as a squared axis -// Squaring: is used on gantry type axes that have two motors -// Each motor with touch off its own switch to square the axis -static bool mask_has_squared_axis(uint8_t axis_mask) { - return axis_mask & homing_squared_axes->get(); -} - -// return true if axis_mask refers to a single squared axis static bool axis_is_squared(uint8_t axis_mask) { - return mask_is_single_axis(axis_mask) && mask_has_squared_axis(axis_mask); + // Squaring can only be done if it is the only axis in the mask + if (axis_mask & homing_squared_axes->get()) { + if (mask_is_single_axis(axis_mask)) { + return true; + } + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Cannot multi-axis home with squared axes. Homing normally"); + return false; + } + + return false; } #ifdef USE_I2S_STEPS From 76f742682bcb2d99120f130940ec59161db060c0 Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 9 Dec 2020 09:42:40 -0600 Subject: [PATCH 46/82] Reverting some spindle changes... CLIENT_ALL caused queue issues --- Grbl_Esp32/src/Spindles/H2ASpindle.cpp | 2 +- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 82 +++++++++++--------------- Grbl_Esp32/src/Spindles/VFDSpindle.h | 11 ++-- 3 files changed, 38 insertions(+), 57 deletions(-) diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp index 8629965e..b18249e5 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp @@ -98,7 +98,7 @@ namespace Spindles { uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); vfd->_max_rpm = rpm; - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); return true; }; diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index ceffdb42..5ac0f630 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -49,7 +49,6 @@ const int VFD_RS485_POLL_RATE = 200; // in milliseconds between comman namespace Spindles { QueueHandle_t VFD::vfd_cmd_queue = nullptr; TaskHandle_t VFD::vfd_cmdTaskHandle = nullptr; - bool VFD::task_active = true; // The communications task void VFD::vfd_cmd_task(void* pvParameters) { @@ -61,12 +60,6 @@ namespace Spindles { uint8_t rx_message[VFD_RS485_MAX_MSG_SIZE]; while (true) { - if (!task_active) { - uart_driver_delete(VFD_RS485_UART_PORT); - xQueueReset(vfd_cmd_queue); - vTaskDelete(NULL); - } - response_parser parser = nullptr; next_cmd.msg[0] = VFD_RS485_ADDR; // Always default to this @@ -165,7 +158,7 @@ namespace Spindles { // Not succesful! Now what? unresponsive = true; - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); } } else { #ifdef VFD_DEBUG_MODE @@ -174,18 +167,18 @@ namespace Spindles { if (read_length != 0) { if (rx_message[0] != VFD_RS485_ADDR) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 received message from other modbus device"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 received message from other modbus device"); } else if (read_length != next_cmd.rx_length) { - grbl_msg_sendf(CLIENT_ALL, + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 received message of unexpected length; expected %d, got %d", int(next_cmd.rx_length), int(read_length)); } else { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 CRC check failed"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 CRC check failed"); } } else { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 No response"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 No response"); } #endif @@ -194,17 +187,15 @@ namespace Spindles { vTaskDelay(VFD_RS485_POLL_RATE); static UBaseType_t uxHighWaterMark = 0; -#ifdef DEBUG_TASK_STACK reportTaskStackSize(uxHighWaterMark); -#endif } } if (retry_count == MAX_RETRIES) { if (!unresponsive) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); sys_rt_exec_alarm = ExecAlarm::SpindleControl; } unresponsive = true; @@ -227,12 +218,12 @@ namespace Spindles { void VFD::init() { vfd_ok = false; // initialize - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Initializing RS485 VFD spindle"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Initializing RS485 VFD spindle"); // fail if required items are not defined if (!get_pins_and_settings()) { vfd_ok = false; - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD spindle errors"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD spindle errors"); return; } @@ -255,37 +246,38 @@ namespace Spindles { uart_config.rx_flow_ctrl_thresh = 122; if (uart_param_config(VFD_RS485_UART_PORT, &uart_config) != ESP_OK) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart parameters failed"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart parameters failed"); return; } if (uart_set_pin(VFD_RS485_UART_PORT, _txd_pin, _rxd_pin, _rts_pin, UART_PIN_NO_CHANGE) != ESP_OK) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart pin config failed"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart pin config failed"); return; } if (uart_driver_install(VFD_RS485_UART_PORT, VFD_RS485_BUF_SIZE * 2, 0, 0, NULL, 0) != ESP_OK) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart driver install failed"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart driver install failed"); return; } if (uart_set_mode(VFD_RS485_UART_PORT, UART_MODE_RS485_HALF_DUPLEX) != ESP_OK) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "RS485 VFD uart set half duplex failed"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "RS485 VFD uart set half duplex failed"); return; } // Initialization is complete, so now it's okay to run the queue task: - task_active = true; - if (vfd_cmd_queue != nullptr) { + if (!_task_running) { // init can happen many times, we only want to start one task vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); + xTaskCreatePinnedToCore(vfd_cmd_task, // task + "vfd_cmdTaskHandle", // name for task + 2048, // size of task stack + this, // parameters + 1, // priority + &vfd_cmdTaskHandle, + SUPPORT_TASK_CORE // core + ); + _task_running = true; } - xTaskCreatePinnedToCore(vfd_cmd_task, // task - "vfd_cmdTaskHandle", // name for task - 2048, // size of task stack - this, // parameters - 1, // priority - &vfd_cmdTaskHandle, - SUPPORT_TASK_CORE); is_reversable = true; // these VFDs are always reversable use_delays = true; @@ -307,27 +299,26 @@ namespace Spindles { #ifdef VFD_RS485_TXD_PIN _txd_pin = VFD_RS485_TXD_PIN; #else - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_TXD_PIN"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_TXD_PIN"); pins_settings_ok = false; #endif #ifdef VFD_RS485_RXD_PIN _rxd_pin = VFD_RS485_RXD_PIN; #else - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_RXD_PIN"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_RXD_PIN"); pins_settings_ok = false; #endif #ifdef VFD_RS485_RTS_PIN _rts_pin = VFD_RS485_RTS_PIN; #else - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Undefined VFD_RS485_RTS_PIN"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Undefined VFD_RS485_RTS_PIN"); pins_settings_ok = false; #endif - // TODO Test no longer required. if (laser_mode->get()) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD spindle disabled in laser mode. Set $GCode/LaserMode=Off and restart"); pins_settings_ok = false; } @@ -338,7 +329,7 @@ namespace Spindles { } void VFD::config_message() { - grbl_msg_sendf(CLIENT_ALL, + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD RS485 Tx:%s Rx:%s RTS:%s", pinName(_txd_pin).c_str(), @@ -391,7 +382,7 @@ namespace Spindles { if (mode == SpindleState::Disable) { if (!xQueueReset(vfd_cmd_queue)) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD spindle off, queue could not be reset"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD spindle off, queue could not be reset"); } } @@ -399,7 +390,7 @@ namespace Spindles { _current_state = mode; if (xQueueSend(vfd_cmd_queue, &mode_cmd, 0) != pdTRUE) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD Queue Full"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); } return true; @@ -411,7 +402,7 @@ namespace Spindles { } #ifdef VFD_DEBUG_MODE - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); #endif // apply override @@ -442,7 +433,7 @@ namespace Spindles { rpm_cmd.critical = false; if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD Queue Full"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); } return rpm; @@ -474,11 +465,4 @@ namespace Spindles { return crc; } - - void VFD::deinit() { - _current_state = SpindleState::Disable; - _current_rpm = 0; - - task_active = false; - } -} +} \ No newline at end of file diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h index b2b091ec..7e81443d 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.h +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h @@ -22,6 +22,8 @@ #include +// #define VFD_DEBUG_MODE + namespace Spindles { class VFD : public Spindle { @@ -39,18 +41,13 @@ namespace Spindles { uint32_t _current_rpm = 0; bool _task_running = false; bool vfd_ok = true; - - static bool task_active; + static QueueHandle_t vfd_cmd_queue; static TaskHandle_t vfd_cmdTaskHandle; static void vfd_cmd_task(void* pvParameters); static uint16_t ModRTU_CRC(uint8_t* buf, int msg_len); - void deinit() override; - - - protected: struct ModbusCommand { bool critical; // TODO SdB: change into `uint8_t critical : 1;`: We want more flags... @@ -95,4 +92,4 @@ namespace Spindles { virtual ~VFD() {} }; -} +} \ No newline at end of file From 1f372d3d58b37aab39122633747f038947b75fa3 Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 9 Dec 2020 17:05:35 -0600 Subject: [PATCH 47/82] Rate Adjusting Fix --- Grbl_Esp32/src/GCode.cpp | 1 + Grbl_Esp32/src/Stepper.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 6f8daf15..70cce7d3 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -476,6 +476,7 @@ Error gc_execute_line(char* line, uint8_t client) { if (spindle->is_reversable || spindle->inLaserMode()) { gc_block.modal.spindle = SpindleState::Ccw; } else { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "M4 requires laser mode or a reversable spindle"); FAIL(Error::GcodeUnsupportedCommand); } break; diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 83aee938..d4919614 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -551,6 +551,8 @@ void st_prep_buffer() { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } + st_prep_block->is_pwm_rate_adjusted = false; // set default value + // prep.inv_rate is only used if is_pwm_rate_adjusted is true if (spindle->inLaserMode()) { // if (pl_block->spindle == SpindleState::Ccw) { // Pre-compute inverse programmed rate to speed up PWM updating per step segment. From f82c00a178fbefe931fa16c9015cf57fa4e60fde Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 9 Dec 2020 18:58:48 -0600 Subject: [PATCH 48/82] Fix SD card hanging on bad gcode --- Grbl_Esp32/src/Report.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index ce35714f..33c53c11 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -239,6 +239,7 @@ void report_status_message(Error status_code, uint8_t client) { grbl_sendf(client, "error:%d\r\n", status_code); // most senders seem to tolerate this error and keep on going grbl_sendf(CLIENT_ALL, "error:%d in SD file at line %d\r\n", status_code, sd_get_current_line_number()); // don't close file + SD_ready_next = true; // flag so system_execute_line() will send the next line } else { grbl_notifyf("SD print error", "Error:%d during SD file at line: %d", status_code, sd_get_current_line_number()); grbl_sendf(CLIENT_ALL, "error:%d in SD file at line %d\r\n", status_code, sd_get_current_line_number()); From b627f31e3b70de43778ee30e0616aac6f10c8a9a Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 10 Dec 2020 04:07:19 -1000 Subject: [PATCH 49/82] Fix hang on error 20 from SD/Run (#701) From 8e3e23c148cbd347216206dddccef3efbe87c4fe Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 10 Dec 2020 04:34:44 -1000 Subject: [PATCH 50/82] Fixed strange WCO values on first load (#702) When loading Grbl_Esp32 into a fresh ESP32, the WCOs would often have strange, very large, values. The problem was the code that tries to propagate data from the old "Eeprom" storage format into the new NVS scheme. The old format had a broken checksum computation that made the checksum so weak that if succeeds about half the time on random data. The solution is to get rid of all that old code. The downside is that migration from a build that uses the old format will lose the WCO values. The user will have to reestablish them. Subsequent updates between different versions that both use the new NVS format will propagate WCO values correctly. --- Grbl_Esp32/src/Eeprom.cpp | 81 -------------------------- Grbl_Esp32/src/Eeprom.h | 61 ------------------- Grbl_Esp32/src/GCode.cpp | 6 ++ Grbl_Esp32/src/GCode.h | 26 +++++++++ Grbl_Esp32/src/Grbl.h | 1 - Grbl_Esp32/src/ProcessSettings.cpp | 1 - Grbl_Esp32/src/SettingsDefinitions.cpp | 18 ++---- 7 files changed, 36 insertions(+), 158 deletions(-) delete mode 100644 Grbl_Esp32/src/Eeprom.cpp delete mode 100644 Grbl_Esp32/src/Eeprom.h diff --git a/Grbl_Esp32/src/Eeprom.cpp b/Grbl_Esp32/src/Eeprom.cpp deleted file mode 100644 index 083570c1..00000000 --- a/Grbl_Esp32/src/Eeprom.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - Eeprom.cpp - Coordinate data stored in EEPROM - Part of Grbl - Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC - - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P - - Grbl 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. - Grbl 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 Grbl. If not, see . -*/ - -#include "Grbl.h" - -void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size) { - unsigned char checksum = 0; - for (; size > 0; size--) { - unsigned char data = static_cast(*source++); - // Note: This checksum calculation is broken as described below. - checksum = (checksum << 1) || (checksum >> 7); - checksum += data; - EEPROM.write(destination++, *(source++)); - } - EEPROM.write(destination, checksum); - EEPROM.commit(); -} - -int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size) { - unsigned char data, checksum = 0; - for (; size > 0; size--) { - data = EEPROM.read(source++); - // Note: This checksum calculation is broken - the || should be just | - - // thus making the checksum very weak. - // We leave it as-is so we can read old data after a firmware upgrade. - // The new storage format uses the tagged NVS mechanism, avoiding this bug. - checksum = (checksum << 1) || (checksum >> 7); - checksum += data; - *(destination++) = data; - } - return (checksum == EEPROM.read(source)); -} -int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size) { - unsigned char data, checksum = 0; - for (; size > 0; size--) { - data = EEPROM.read(source++); - checksum = (checksum << 1) | (checksum >> 7); - checksum += data; - *(destination++) = data; - } - return (checksum == EEPROM.read(source)); -} - -// Read selected coordinate data from EEPROM. Updates pointed coord_data value. -// This is now a compatibility routine that is used to propagate coordinate data -// in the old EEPROM format to the new tagged NVS format. -bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) { - uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; - if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) && - !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) { - // Reset with default zero vector - clear_vector_float(coord_data); - // The old code used to rewrite the zeroed data but now that is done - // elsewhere, in a different format. - return false; - } - return true; -} - -// Allow iteration over CoordIndex values -CoordIndex& operator ++ (CoordIndex& i) { - i = static_cast(static_cast(i) + 1); - return i; -} diff --git a/Grbl_Esp32/src/Eeprom.h b/Grbl_Esp32/src/Eeprom.h deleted file mode 100644 index 32d66c65..00000000 --- a/Grbl_Esp32/src/Eeprom.h +++ /dev/null @@ -1,61 +0,0 @@ -#pragma once - -/* - Eeprom.h - Header for system level commands and real-time processes - Part of Grbl - Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC - - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P - - Grbl 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. - Grbl 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 Grbl. If not, see . -*/ - -#include "Grbl.h" - -// Define EEPROM memory address location values for saved coordinate data. -const int EEPROM_SIZE = 1024U; -const int EEPROM_ADDR_PARAMETERS = 512U; - -//unsigned char eeprom_get_char(unsigned int addr); -//void eeprom_put_char(unsigned int addr, unsigned char new_value); -void memcpy_to_eeprom_with_checksum(unsigned int destination, const char* source, unsigned int size); -int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size); -int memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size); - -// Reads selected coordinate data from EEPROM -bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data); - -// Various places in the code access saved coordinate system data -// by a small integer index according to the values below. -enum CoordIndex : uint8_t{ - Begin = 0, - G54 = Begin, - G55, - G56, - G57, - G58, - G59, - // To support 9 work coordinate systems it would be necessary to define - // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 - // G59_1, - // G59_2, - // G59_3, - NWCSystems, - G28 = NWCSystems, - G30, - // G92_2, - // G92_3, - End, -}; -// Allow iteration over CoordIndex values -CoordIndex& operator ++ (CoordIndex& i); diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 70cce7d3..b1a7a079 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -24,6 +24,12 @@ #include "Grbl.h" +// Allow iteration over CoordIndex values +CoordIndex& operator++(CoordIndex& i) { + i = static_cast(static_cast(i) + 1); + return i; +} + // NOTE: Max line number is defined by the g-code standard to be 99999. It seems to be an // arbitrary value, and some GUIs may require more. So we increased it based on a max safe // value when converting a float (7.2 digit precision)s to an integer. diff --git a/Grbl_Esp32/src/GCode.h b/Grbl_Esp32/src/GCode.h index 4b070459..1ef3abf6 100644 --- a/Grbl_Esp32/src/GCode.h +++ b/Grbl_Esp32/src/GCode.h @@ -232,6 +232,32 @@ enum GCParserFlags { GCParserLaserIsMotion = bit(7), }; +// Various places in the code access saved coordinate system data +// by a small integer index according to the values below. +enum CoordIndex : uint8_t{ + Begin = 0, + G54 = Begin, + G55, + G56, + G57, + G58, + G59, + // To support 9 work coordinate systems it would be necessary to define + // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3 + // G59_1, + // G59_2, + // G59_3, + NWCSystems, + G28 = NWCSystems, + G30, + // G92_2, + // G92_3, + End, +}; + +// Allow iteration over CoordIndex values +CoordIndex& operator ++ (CoordIndex& i); + // NOTE: When this struct is zeroed, the 0 values in the above types set the system defaults. typedef struct { Motion motion; // {G0,G1,G2,G3,G38.2,G80} diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 5b1bf3db..31413e8a 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -41,7 +41,6 @@ const char* const GRBL_VERSION_BUILD = "20201207"; #include "Defaults.h" #include "Error.h" -#include "Eeprom.h" #include "WebUI/Authentication.h" #include "WebUI/Commands.h" #include "Probe.h" diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index dc19fd65..a6831713 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -79,7 +79,6 @@ namespace WebUI { } void settings_init() { - EEPROM.begin(EEPROM_SIZE); make_settings(); WebUI::make_web_settings(); make_grbl_commands(); diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index c25f35a5..2c0ac6c4 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -196,9 +196,9 @@ static bool checkSpindleChange(char* val) { } grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle turned off with setting change"); } - gc_state.spindle_speed = 0; // Set S value to 0 - spindle->deinit(); // old spindle - Spindles::Spindle::select(); // get new spindle + gc_state.spindle_speed = 0; // Set S value to 0 + spindle->deinit(); // old spindle + Spindles::Spindle::select(); // get new spindle return true; } return true; @@ -219,17 +219,7 @@ void make_coordinate(CoordIndex index, const char* name) { auto coord = new Coordinates(name); coords[index] = coord; if (!coord->load()) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Propagating %s data to NVS format", coord->getName()); - // If coord->load() returns false it means that no entry - // was present in non-volatile storage. In that case we - // first look for an old-format entry in the EEPROM section. - // If an entry is present some number of float values at - // the beginning of coord_data will be overwritten with - // the EEPROM data, and the rest will remain at 0.0. - // If no old-format entry is present, all will remain 0.0 - // Regardless, we create a new entry with that data. - (void)old_settings_read_coord_data(index, coord_data); - coords[index]->set(coord_data); + coords[index]->setDefault(); } } void make_settings() { From 6a0e3a73354cb0e2a3d801150871707c79e23748 Mon Sep 17 00:00:00 2001 From: bdring Date: Sun, 13 Dec 2020 13:29:47 -0600 Subject: [PATCH 51/82] Fixes to homing (#706) * Fixes to homing * Update Grbl.h * Clean up after code review. --- Grbl_Esp32/Custom/CoreXY.cpp | 32 ++++++++++++++++------- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Motors/StandardStepper.cpp | 6 ++++- Grbl_Esp32/src/Motors/StandardStepper.h | 1 + Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 5 ++-- Grbl_Esp32/src/SettingsDefinitions.cpp | 16 ++++++------ 6 files changed, 40 insertions(+), 22 deletions(-) diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index cefab842..238a46df 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -51,9 +51,15 @@ float three_axis_dist(float* point1, float* point2); void machine_init() { // print a startup message to show the kinematics are enable + +#ifdef MIDTBOT + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY (midTbot) Kinematics Init"); +#else grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Kinematics Init"); +#endif } +// Cycle mask is 0 unless the user sends a single axis command like $HZ // This will always return true to prevent the normal Grbl homing cycle bool user_defined_homing(uint8_t cycle_mask) { uint8_t n_cycle; // each home is a multi cycle operation approach, pulloff, approach..... @@ -61,12 +67,6 @@ bool user_defined_homing(uint8_t cycle_mask) { float max_travel; uint8_t axis; - // check for single axis homing - if (cycle_mask != 0) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "CoreXY Single axis homing not allowed. Use $H only"); - return true; - } - // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY bool setting_error = false; for (int cycle = 0; cycle < 3; cycle++) { @@ -90,9 +90,21 @@ bool user_defined_homing(uint8_t cycle_mask) { pl_data->motion.systemMotion = 1; pl_data->motion.noFeedOverride = 1; - for (int cycle = 0; cycle < 3; cycle++) { - AxisMask mask = homing_cycle[cycle]->get(); + uint8_t cycle_count = (cycle_mask == 0) ? 3 : 1; // if we have a cycle_mask, we are only going to do one axis + + AxisMask mask = 0; + for (int cycle = 0; cycle < cycle_count; cycle++) { + // if we have a cycle_mask, do that. Otherwise get the cycle from the settings + mask = cycle_mask ? cycle_mask : homing_cycle[cycle]->get(); + + // If not X or Y do a normal home + if (!(bitnum_istrue(mask, X_AXIS) || bitnum_istrue(mask, Y_AXIS))) { + limits_go_home(mask); // Homing cycle 0 + continue; // continue to next item in for loop + } + mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask + for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) { if (bit(axis) == mask) { // setup for the homing of this axis @@ -190,7 +202,7 @@ bool user_defined_homing(uint8_t cycle_mask) { } while (n_cycle-- > 0); } } - } + } // for // after sussefully setting X & Y axes, we set the current positions @@ -231,7 +243,7 @@ void inverse_kinematics(float* position) { motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; - motors[Z_AXIS] = position[Z_AXIS]; + motors[Z_AXIS] = position[Z_AXIS]; position[0] = motors[0]; position[1] = motors[1]; diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 31413e8a..93c3f1e6 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 = "20201207"; +const char* const GRBL_VERSION_BUILD = "20201212"; //#include #include diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index d2a2e51a..8e782ba4 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -44,10 +44,14 @@ namespace Motors { } void StandardStepper::init() { - init_step_dir_pins(); + read_settings(); config_message(); } + void StandardStepper::read_settings() { + init_step_dir_pins(); + } + void StandardStepper::init_step_dir_pins() { _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index); _invert_dir_pin = bitnum_istrue(dir_invert_mask->get(), _axis_index); diff --git a/Grbl_Esp32/src/Motors/StandardStepper.h b/Grbl_Esp32/src/Motors/StandardStepper.h index efc72f66..269e5b2f 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.h +++ b/Grbl_Esp32/src/Motors/StandardStepper.h @@ -15,6 +15,7 @@ namespace Motors { void set_direction(bool) override; void step() override; void unstep() override; + void read_settings() override; void init_step_dir_pins(); diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 2a7c9f4f..26229dd2 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -67,8 +67,7 @@ namespace Motors { return; } - _has_errors = false; - init_step_dir_pins(); // from StandardStepper + _has_errors = false; digitalWrite(_cs_pin, HIGH); pinMode(_cs_pin, OUTPUT); @@ -211,6 +210,8 @@ namespace Motors { } tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); tmcstepper->rms_current(run_i_ma, hold_i_percent); + + init_step_dir_pins(); } bool TrinamicDriver::set_homing_mode(bool isHoming) { diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 2c0ac6c4..02115c00 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -179,7 +179,7 @@ static bool checkStartupLine(char* value) { return gc_execute_line(value, CLIENT_SERIAL) == Error::Ok; } -static bool postTMC(char* value) { +static bool postMotorSetting(char* value) { if (!value) { motors_read_settings(); } @@ -258,28 +258,28 @@ void make_settings() { for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = - new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, postTMC); + new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->stallguard = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = - new IntSetting(EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postTMC); + new IntSetting(EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->microsteps = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, postTMC); // Amps + EXTENDED, WG, makeGrblName(axis, 150), makename(def->name, "Current/Hold"), def->hold_current, 0.05, 20.0, postMotorSetting); // Amps setting->setAxis(axis); axis_settings[axis]->hold_current = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = new FloatSetting( - EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, postTMC); // Amps + EXTENDED, WG, makeGrblName(axis, 140), makename(def->name, "Current/Run"), def->run_current, 0.0, 20.0, postMotorSetting); // Amps setting->setAxis(axis); axis_settings[axis]->run_current = setting; } @@ -377,12 +377,12 @@ void make_settings() { probe_invert = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN); limit_invert = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS); step_enable_invert = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE); - dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK); - step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK); + dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK, postMotorSetting); + step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting); stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); - stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postTMC); + stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postMotorSetting); homing_cycle[5] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle5", DEFAULT_HOMING_CYCLE_5); homing_cycle[4] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle4", DEFAULT_HOMING_CYCLE_4); From a1b4cbc192e928c0900dbad0b258364b8083dbfc Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 14 Dec 2020 11:44:13 -0600 Subject: [PATCH 52/82] Trinamic uart (#700) * WIP * WIP * Updates * Update Grbl.h * Removing some test machine definitions * TMC5160 Drivers were not in tests --- Grbl_Esp32/src/Machines/fystec_e4.h | 81 ++++ Grbl_Esp32/src/Motors/Motors.cpp | 183 +++++++-- Grbl_Esp32/src/Motors/TrinamicUartDriver.h | 131 +++++++ .../src/Motors/TrinamicUartDriverClass.cpp | 370 ++++++++++++++++++ 4 files changed, 740 insertions(+), 25 deletions(-) create mode 100644 Grbl_Esp32/src/Machines/fystec_e4.h create mode 100644 Grbl_Esp32/src/Motors/TrinamicUartDriver.h create mode 100644 Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp diff --git a/Grbl_Esp32/src/Machines/fystec_e4.h b/Grbl_Esp32/src/Machines/fystec_e4.h new file mode 100644 index 00000000..63b47bab --- /dev/null +++ b/Grbl_Esp32/src/Machines/fystec_e4.h @@ -0,0 +1,81 @@ +#pragma once +// clang-format off + +/* + fystec_e4.h + https://github.com/FYSETC/FYSETC-E4 + + 2020-12-03 B. Dring + + This is a machine definition file to use the FYSTEC E4 3D Printer controller + This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. + There are 5 inputs + The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to + spindle, mist and flood coolant to drive an external relay. + + Grbl_ESP32 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. + + Grbl 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 Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "FYSTEC E4 3D Printer Controller" + +#define N_AXIS 4 + +#define TRINAMIC_RUN_MODE TrinamicMode :: StealthChop +#define TRINAMIC_HOMING_MODE TrinamicMode :: StealthChop + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_27 +#define X_DIRECTION_PIN GPIO_NUM_26 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 1 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_33 +#define Y_DIRECTION_PIN GPIO_NUM_32 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 3 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN GPIO_NUM_14 +#define Z_DIRECTION_PIN GPIO_NUM_12 +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 0 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN GPIO_NUM_16 +#define A_DIRECTION_PIN GPIO_NUM_17 +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 2 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN GPIO_NUM_34 +#define Y_LIMIT_PIN GPIO_NUM_35 +#define Z_LIMIT_PIN GPIO_NUM_15 +#define A_LIMIT_PIN GPIO_NUM_36 // Labeled TB +#define PROBE_PIN GPIO_NUM_39 // Labeled TE + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN GPIO_NUM_25 + +#define SPINDLE_TYPE SpindleType::RELAY +#define SPINDLE_OUTPUT_PIN GPIO_NUM_13 // labeled Fan +#define COOLANT_MIST_PIN GPIO_NUM_2 // Labeled Hotbed +#define COOLANT_FLOOD_PIN GPIO_NUM_4 // Labeled Heater \ No newline at end of file diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 1a08c0c8..1c06a7d7 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -42,6 +42,7 @@ #include "RcServo.h" #include "Dynamixel2.h" #include "TrinamicDriver.h" +#include "TrinamicUartDriver.h" Motors::Motor* myMotor[MAX_AXES][MAX_GANGED]; // number of axes (normal and ganged) void init_motors() { @@ -51,8 +52,19 @@ void init_motors() { if (n_axis >= 1) { #ifdef X_TRINAMIC_DRIVER - myMotor[X_AXIS][0] = - new Motors::TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[X_AXIS][0] = + new Motors::TrinamicDriver(X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE); + } +# elif (X_TRINAMIC_DRIVER == 2208 || X_TRINAMIC_DRIVER == 2209) + { + myMotor[X_AXIS][0] = new Motors::TrinamicUartDriver( + X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_TRINAMIC_DRIVER, X_RSENSE, X_DRIVER_ADDRESS); + } +# else +# error X Axis undefined motor p/n +# endif #elif defined(X_SERVO_PIN) myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN); #elif defined(X_UNIPOLAR) @@ -66,8 +78,19 @@ void init_motors() { #endif #ifdef X2_TRINAMIC_DRIVER - myMotor[X_AXIS][1] = - new Motors::TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[X_AXIS][1] = + new Motors::TrinamicDriver(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE); + } +# elif (X2_TRINAMIC_DRIVER == 2208 || X2_TRINAMIC_DRIVER == 2209) + { + myMotor[X_AXIS][1] = new Motors::TrinamicUartDriver( + X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE, X2_DRIVER_ADDRESS); + } +# else +# error X2 Axis undefined motor p/n +# endif #elif defined(X2_UNIPOLAR) myMotor[X_AXIS][1] = new Motors::UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3); #elif defined(X2_STEP_PIN) @@ -83,8 +106,19 @@ void init_motors() { if (n_axis >= 2) { // this WILL be done better with settings #ifdef Y_TRINAMIC_DRIVER - myMotor[Y_AXIS][0] = - new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[Y_AXIS][0] = + new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); + } +# elif (Y_TRINAMIC_DRIVER == 2208 || Y_TRINAMIC_DRIVER == 2209) + { + myMotor[Y_AXIS][0] = new Motors::TrinamicUartDriver( + Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE, Y_DRIVER_ADDRESS); + } +# else +# error Y Axis undefined motor p/n +# endif #elif defined(Y_SERVO_PIN) myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN); #elif defined(Y_UNIPOLAR) @@ -98,8 +132,19 @@ void init_motors() { #endif #ifdef Y2_TRINAMIC_DRIVER - myMotor[Y_AXIS][1] = - new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[Y_AXIS][1] = + new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); + } +# elif (Y2_TRINAMIC_DRIVER == 2208 || Y2_TRINAMIC_DRIVER == 2209) + { + myMotor[Y_AXIS][1] = new Motors::TrinamicUartDriver( + Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE, Y2_DRIVER_ADDRESS); + } +# else +# error Y2 Axis undefined motor p/n +# endif #elif defined(Y2_UNIPOLAR) myMotor[Y_AXIS][1] = new Motors::UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3); #elif defined(Y2_STEP_PIN) @@ -115,8 +160,19 @@ void init_motors() { if (n_axis >= 3) { // this WILL be done better with settings #ifdef Z_TRINAMIC_DRIVER - myMotor[Z_AXIS][0] = - new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[Z_AXIS][0] = + new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); + } +# elif (Z_TRINAMIC_DRIVER == 2208 || Z_TRINAMIC_DRIVER == 2209) + { + myMotor[Z_AXIS][0] = new Motors::TrinamicUartDriver( + Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE, Z_DRIVER_ADDRESS); + } +# else +# error Z Axis undefined motor p/n +# endif #elif defined(Z_SERVO_PIN) myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN); #elif defined(Z_UNIPOLAR) @@ -130,8 +186,19 @@ void init_motors() { #endif #ifdef Z2_TRINAMIC_DRIVER - myMotor[Z_AXIS][1] = - new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[Z_AXIS][1] = + new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); + } +# elif (Z2_TRINAMIC_DRIVER == 2208 || Z2_TRINAMIC_DRIVER == 2209) + { + myMotor[Z_AXIS][1] = new Motors::TrinamicUartDriver( + Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE, Z2_DRIVER_ADDRESS); + } +# else +# error Z2 Axis undefined motor p/n +# endif #elif defined(Z2_UNIPOLAR) myMotor[Z_AXIS][1] = new Motors::UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3); #elif defined(Z2_STEP_PIN) @@ -147,8 +214,19 @@ void init_motors() { if (n_axis >= 4) { // this WILL be done better with settings #ifdef A_TRINAMIC_DRIVER - myMotor[A_AXIS][0] = - new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[A_AXIS][1] = + new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); + } +# elif (A_TRINAMIC_DRIVER == 2208 || A_TRINAMIC_DRIVER == 2209) + { + myMotor[A_AXIS][0] = new Motors::TrinamicUartDriver( + A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_TRINAMIC_DRIVER, A_RSENSE, A_DRIVER_ADDRESS); + } +# else +# error A Axis undefined motor p/n +# endif #elif defined(A_SERVO_PIN) myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN); #elif defined(A_UNIPOLAR) @@ -162,8 +240,19 @@ void init_motors() { #endif #ifdef A2_TRINAMIC_DRIVER - myMotor[A_AXIS][1] = - new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[A_AXIS][1] = + new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); + } +# elif (A2_TRINAMIC_DRIVER == 2208 || A2_TRINAMIC_DRIVER == 2209) + { + myMotor[A_AXIS][1] = new Motors::TrinamicUartDriver( + A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, A2_DRIVER_ADDRESS); + } +# else +# error A2 Axis undefined motor p/n +# endif #elif defined(A2_UNIPOLAR) myMotor[A_AXIS][1] = new Motors::UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3); #elif defined(A2_STEP_PIN) @@ -179,8 +268,19 @@ void init_motors() { if (n_axis >= 5) { // this WILL be done better with settings #ifdef B_TRINAMIC_DRIVER - myMotor[B_AXIS][0] = - new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[B_AXIS][1] = + new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); + } +# elif (B_TRINAMIC_DRIVER == 2208 || B_TRINAMIC_DRIVER == 2209) + { + myMotor[B_AXIS][0] = new Motors::TrinamicUartDriver( + B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_TRINAMIC_DRIVER, B_RSENSE, B_DRIVER_ADDRESS); + } +# else +# error B Axis undefined motor p/n +# endif #elif defined(B_SERVO_PIN) myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN); #elif defined(B_UNIPOLAR) @@ -194,8 +294,19 @@ void init_motors() { #endif #ifdef B2_TRINAMIC_DRIVER - myMotor[B_AXIS][1] = - new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[B_AXIS][1] = + new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); + } +# elif (B2_TRINAMIC_DRIVER == 2208 || B2_TRINAMIC_DRIVER == 2209) + { + myMotor[B_AXIS][1] = new Motors::TrinamicUartDriver( + B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, B2_DRIVER_ADDRESS); + } +# else +# error B2 Axis undefined motor p/n +# endif #elif defined(B2_UNIPOLAR) myMotor[B_AXIS][1] = new Motors::UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3); #elif defined(B2_STEP_PIN) @@ -211,8 +322,19 @@ void init_motors() { if (n_axis >= 6) { // this WILL be done better with settings #ifdef C_TRINAMIC_DRIVER - myMotor[C_AXIS][0] = - new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[C_AXIS][1] = + new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); + } +# elif (C_TRINAMIC_DRIVER == 2208 || C_TRINAMIC_DRIVER == 2209) + { + myMotor[C_AXIS][0] = new Motors::TrinamicUartDriver( + C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_TRINAMIC_DRIVER, C_RSENSE, C_DRIVER_ADDRESS); + } +# else +# error C Axis undefined motor p/n +# endif #elif defined(C_SERVO_PIN) myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN); #elif defined(C_UNIPOLAR) @@ -226,8 +348,19 @@ void init_motors() { #endif #ifdef C2_TRINAMIC_DRIVER - myMotor[C_AXIS][1] = - new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); +# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) + { + myMotor[C_AXIS][1] = + new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); + } +# elif (C2_TRINAMIC_DRIVER == 2208 || C2_TRINAMIC_DRIVER == 2209) + { + myMotor[C_AXIS][1] = new Motors::TrinamicUartDriver( + C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, C2_DRIVER_ADDRESS); + } +# else +# error C2 Axis undefined motor p/n +# endif #elif defined(C2_UNIPOLAR) myMotor[C_AXIS][1] = new Motors::UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3); #elif defined(C2_STEP_PIN) @@ -360,4 +493,4 @@ void motors_unstep() { myMotor[axis][0]->unstep(); myMotor[axis][1]->unstep(); } -} +} \ No newline at end of file diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h new file mode 100644 index 00000000..3fb6e7cd --- /dev/null +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h @@ -0,0 +1,131 @@ +#pragma once + +/* + TrinamicUartDriver.h + + Part of Grbl_ESP32 + 2020 - The Ant Team + 2020 - Bart Dring + + Grbl 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. + Grbl 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 Grbl. If not, see . +*/ + +#include "Motor.h" +#include "StandardStepper.h" + +#include // https://github.com/teemuatlut/TMCStepper + +const float TMC2208_RSENSE_DEFAULT = 0.11f; +const float TMC2209_RSENSE_DEFAULT = 0.11f; + +const double TRINAMIC_UART_FCLK = 12700000.0; // Internal clock Approx (Hz) used to calculate TSTEP from homing rate + +// ==== defaults OK to define them in your machine definition ==== + +#ifndef TRINAMIC_UART_RUN_MODE +# define TRINAMIC_UART_RUN_MODE TrinamicUartMode ::StealthChop +#endif + +#ifndef TRINAMIC_UART_HOMING_MODE +# define TRINAMIC_UART_HOMING_MODE TRINAMIC_UART_RUN_MODE +#endif + +#ifndef TRINAMIC_UART_TOFF_DISABLE +# define TRINAMIC_UART_TOFF_DISABLE 0 +#endif + +#ifndef TRINAMIC_UART_TOFF_STEALTHCHOP +# define TRINAMIC_UART_TOFF_STEALTHCHOP 5 +#endif + +#ifndef TRINAMIC_UART_TOFF_COOLSTEP +# define TRINAMIC_UART_TOFF_COOLSTEP 3 +#endif + +#ifndef TMC_UART +# define TMC_UART UART_NUM_2 +#endif + +#ifndef TMC_UART_RX +# define TMC_UART_RX UNDEFINED_PIN +#endif + +#ifndef TMC_UART_TX +# define TMC_UART_TX UNDEFINED_PIN +#endif + +extern HardwareSerial tmc_serial; + +namespace Motors { + + enum class TrinamicUartMode : uint8_t { + None = 0, // not for machine defs! + StealthChop = 1, + CoolStep = 2, + StallGuard = 3, + }; + + class TrinamicUartDriver : public StandardStepper { + public: + TrinamicUartDriver(uint8_t axis_index, + uint8_t step_pin, + uint8_t dir_pin, + uint8_t disable_pin, + uint16_t driver_part_number, + float r_senseS, + uint8_t address); + + void config_message(); + void hw_serial_init(); + void init(); + void set_mode(); + void read_settings(); + void debug_message(); + bool set_homing_mode(bool is_homing) override; + void set_disable(bool disable) override; + + uint8_t addr; + + private: + uint32_t calc_tstep(float speed, float percent); //TODO: see if this is useful/used. + + TMC2208Stepper* tmcstepper; // all other driver types are subclasses of this one + TrinamicUartMode _homing_mode; + uint16_t _driver_part_number; // example: use 2209 for TMC2209 + float _r_sense; + bool _has_errors; + bool _disabled; + + TrinamicUartMode _mode = TrinamicUartMode::None; + bool test(); + void set_mode(bool isHoming); + void trinamic_test_response(); + void trinamic_stepper_enable(bool enable); + + bool report_open_load(TMC2208_n ::DRV_STATUS_t status); + bool report_short_to_ground(TMC2208_n ::DRV_STATUS_t status); + bool report_over_temp(TMC2208_n ::DRV_STATUS_t status); + bool report_short_to_ps(TMC2208_n ::DRV_STATUS_t status); + + uint8_t get_next_index(); + + // Linked list of Trinamic driver instances, used by the + // StallGuard reporting task. TODO: verify if this is really used/useful. + static TrinamicUartDriver* List; + TrinamicUartDriver* link; + static void readSgTask(void*); + + protected: + // void config_message() override; + }; + +} \ No newline at end of file diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp new file mode 100644 index 00000000..e930a848 --- /dev/null +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp @@ -0,0 +1,370 @@ +/* + TrinamicUartDriverClass.cpp + + This is used for Trinamic UART controlled stepper motor drivers. + + Part of Grbl_ESP32 + 2020 - The Ant Team + 2020 - Bart Dring + + Grbl 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. + Grbl 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 Grbl. If not, see . + +*/ +#include "TrinamicUartDriver.h" + +#include + +HardwareSerial tmc_serial(TMC_UART); + +namespace Motors { + + /* HW Serial Constructor. */ + TrinamicUartDriver::TrinamicUartDriver( + uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin, uint16_t driver_part_number, float r_sense, uint8_t addr) : + StandardStepper(axis_index, step_pin, dir_pin, disable_pin) { + _driver_part_number = driver_part_number; + _has_errors = false; + _r_sense = r_sense; + this->addr = addr; + + uart_set_pin(TMC_UART, TMC_UART_TX, TMC_UART_RX, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + tmc_serial.begin(115200, SERIAL_8N1, TMC_UART_RX, TMC_UART_TX); + tmc_serial.setRxBufferSize(128); + hw_serial_init(); + } + + void TrinamicUartDriver::hw_serial_init() { + if (_driver_part_number == 2208) + // TMC 2208 does not use address, this field is 0 + tmcstepper = new TMC2208Stepper(&tmc_serial, _r_sense); + else if (_driver_part_number == 2209) + tmcstepper = new TMC2209Stepper(&tmc_serial, _r_sense, addr); + else { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unsupported Trinamic motor p/n:%d", _driver_part_number); + return; + } + } + + void TrinamicUartDriver ::init() { + if (_has_errors) { + return; + } + + init_step_dir_pins(); // from StandardStepper + config_message(); + + tmcstepper->begin(); + + _has_errors = !test(); // Try communicating with motor. Prints an error if there is a problem. + + /* If communication with the driver is working, read the + main settings, apply new driver settings and then read + them back. */ + if (!_has_errors) { //TODO: verify if this is the right way to set the Irun/IHold and microsteps. + read_settings(); + set_mode(false); + } + } + + /* + This is the startup message showing the basic definition. + */ + void TrinamicUartDriver::config_message() { //TODO: The RX/TX pin could be added to the msg. + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s motor Trinamic TMC%d Step:%s Dir:%s Disable:%s UART%d Rx:%s Tx:%s Addr:%d R:%0.3f %s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + _driver_part_number, + pinName(_step_pin).c_str(), + pinName(_dir_pin).c_str(), + pinName(_disable_pin).c_str(), + TMC_UART, + pinName(TMC_UART_RX), + pinName(TMC_UART_TX), + this->addr, + _r_sense, + reportAxisLimitsMsg(_axis_index)); + } + + bool TrinamicUartDriver::test() { + if (_has_errors) { + return false; + } + switch (tmcstepper->test_connection()) { + case 1: + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s driver test failed. Check connection", + reportAxisNameMsg(_axis_index, _dual_axis_index)); + return false; + case 2: + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s driver test failed. Check motor power", + reportAxisNameMsg(_axis_index, _dual_axis_index)); + return false; + default: + // driver responded, so check for other errors from the DRV_STATUS register + + TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. + status.sr = tmcstepper->DRV_STATUS(); + + bool err = false; + + // look for errors + if (report_short_to_ground(status)) { + err = true; + } + + if (report_over_temp(status)) { + err = true; + } + + if (report_short_to_ps(status)) { + err = true; + } + + if (err) { + return false; + } + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s driver test passed", reportAxisNameMsg(_axis_index, _dual_axis_index)); + return true; + } + } + + /* + Read setting and send them to the driver. Called at init() and whenever related settings change + both are stored as float Amps, but TMCStepper library expects... + uint16_t run (mA) + float hold (as a percentage of run) + */ + void TrinamicUartDriver::read_settings() { + if (_has_errors) { + return; + } + + uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0); + float hold_i_percent; + + if (axis_settings[_axis_index]->run_current->get() == 0) + hold_i_percent = 0; + else { + hold_i_percent = axis_settings[_axis_index]->hold_current->get() / axis_settings[_axis_index]->run_current->get(); + if (hold_i_percent > 1.0) + hold_i_percent = 1.0; + } + tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get()); + tmcstepper->rms_current(run_i_ma, hold_i_percent); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "Setting current of driver %s, target: %u, read irun: %d, hold percent: %f, usteps: %d", + // reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, tmcstepper->rms_current(), hold_i_percent, axis_settings[_axis_index]->microsteps->get()); + } + + bool TrinamicUartDriver::set_homing_mode(bool isHoming) { + set_mode(isHoming); + return true; + } + + /* + There are ton of settings. I'll start by grouping then into modes for now. + Many people will want quiet and stallgaurd homing. Stallguard only run in + Coolstep mode, so it will need to switch to Coolstep when homing + */ + void TrinamicUartDriver::set_mode(bool isHoming) { + if (_has_errors) { + return; + } + + TrinamicUartMode newMode = isHoming ? TRINAMIC_UART_HOMING_MODE : TRINAMIC_UART_RUN_MODE; + + if (newMode == _mode) { + return; + } + _mode = newMode; + + switch (_mode) { + case TrinamicUartMode ::StealthChop: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); + // tmcstepper->en_pwm_mode(true); //TODO: check if this is present in TMC2208/09 + tmcstepper->en_spreadCycle(false); + tmcstepper->pwm_autoscale(true); + // if (_driver_part_number == 2209) { + // tmcstepper->diag1_stall(false); //TODO: check the equivalent in TMC2209 + // } + break; + case TrinamicUartMode ::CoolStep: + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); + // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09 + tmcstepper->en_spreadCycle(true); + tmcstepper->pwm_autoscale(false); + if (_driver_part_number == 2209) { + // tmcstepper->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep //TODO: add this solving compilation issue. + // tmcstepper->THIGH(NORMAL_THIGH); //TODO: this does not exist in TMC2208/09. verify and eventually remove. + } + break; + case TrinamicUartMode ::StallGuard: //TODO: check all configurations for stallguard + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); + // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09 + tmcstepper->en_spreadCycle(false); + tmcstepper->pwm_autoscale(false); + // tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); + // tmcstepper->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); + // tmcstepper->sfilt(1); + // tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 + // tmcstepper->sgt(axis_settings[_axis_index]->stallguard->get()); + break; + default: + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Trinamic mode:d", _mode); + } + } + + /* + This is the stallguard tuning info. It is call debug, so it could be generic across all classes. + */ + void TrinamicUartDriver::debug_message() { + if (_has_errors) { + return; + } + uint32_t tstep = tmcstepper->TSTEP(); + + if (tstep == 0xFFFFF || tstep < 1) { // if axis is not moving return + return; + } + float feedrate = st_get_realtime_rate(); //* settings.microsteps[axis_index] / 60.0 ; // convert mm/min to Hz + + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Stallguard %d SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", + reportAxisNameMsg(_axis_index, _dual_axis_index), + 0, // tmcstepper->stallguard(), // TODO: add this again solving the compilation issues + 0, // tmcstepper->sg_result(), + feedrate, + axis_settings[_axis_index]->stallguard->get()); + + TMC2208_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. + status.sr = tmcstepper->DRV_STATUS(); + + // these only report if there is a fault condition + report_open_load(status); + report_short_to_ground(status); + report_over_temp(status); + report_short_to_ps(status); + + // grbl_msg_sendf(CLIENT_SERIAL, + // MsgLevel::Info, + // "%s Status Register %08x GSTAT %02x", + // reportAxisNameMsg(_axis_index, _dual_axis_index), + // status.sr, + // tmcstepper->GSTAT()); + } + + // calculate a tstep from a rate + // tstep = TRINAMIC_UART_FCLK / (time between 1/256 steps) + // This is used to set the stallguard window from the homing speed. + // The percent is the offset on the window + uint32_t TrinamicUartDriver::calc_tstep(float speed, float percent) { + float tstep = + speed / 60.0 * axis_settings[_axis_index]->steps_per_mm->get() * (float)(256 / axis_settings[_axis_index]->microsteps->get()); + tstep = TRINAMIC_UART_FCLK / tstep * percent / 100.0; + + return static_cast(tstep); + } + + // this can use the enable feature over SPI. The dedicated pin must be in the enable mode, + // but that can be hardwired that way. + void TrinamicUartDriver::set_disable(bool disable) { + if (_has_errors) { + return; + } + + if (_disabled == disable) { + return; + } + + _disabled = disable; + + digitalWrite(_disable_pin, _disabled); + +#ifdef USE_TRINAMIC_ENABLE + if (_disabled) { + tmcstepper->toff(TRINAMIC_UART_TOFF_DISABLE); + } else { + if (_mode == TrinamicUartMode::StealthChop) { + tmcstepper->toff(TRINAMIC_UART_TOFF_STEALTHCHOP); + } else { + tmcstepper->toff(TRINAMIC_UART_TOFF_COOLSTEP); + } + } +#endif + // the pin based enable could be added here. + // This would be for individual motors, not the single pin for all motors. + } + + // =========== Reporting functions ======================== + + bool TrinamicUartDriver::report_open_load(TMC2208_n ::DRV_STATUS_t status) { + if (status.ola || status.olb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver open load A:%s B:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.ola ? "Y" : "N", + status.olb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicUartDriver::report_short_to_ground(TMC2208_n ::DRV_STATUS_t status) { + if (status.s2ga || status.s2gb) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver shorted coil A:%s B:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.s2ga ? "Y" : "N", + status.s2gb ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicUartDriver::report_over_temp(TMC2208_n ::DRV_STATUS_t status) { + if (status.ot || status.otpw) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver temp Warning:%s Fault:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + status.otpw ? "Y" : "N", + status.ot ? "Y" : "N"); + return true; + } + return false; // no error + } + + bool TrinamicUartDriver::report_short_to_ps(TMC2208_n ::DRV_STATUS_t status) { + // check for short to power supply + if ((status.sr & bit(12)) || (status.sr & bit(13))) { + grbl_msg_sendf(CLIENT_SERIAL, + MsgLevel::Info, + "%s Driver short vsa:%s vsb:%s", + reportAxisNameMsg(_axis_index, _dual_axis_index), + (status.sr & bit(12)) ? "Y" : "N", + (status.sr & bit(13)) ? "Y" : "N"); + return true; + } + return false; // no error + } + +} \ No newline at end of file From 1a46f444fed512d86b2178f428d5ba411be5ef22 Mon Sep 17 00:00:00 2001 From: Scott Bezek Date: Mon, 14 Dec 2020 10:00:26 -0800 Subject: [PATCH 53/82] Fix a few issues with VFDSpindle critical error handling (#705) If a command is critical and fails to receive a response, it should trigger an Alarm. However, because the critical check was only evaluated if the spindle was not already unresponsive, it meant that a critical command failure would be silently ignored if a non-critical command failed before it (putting the VFDSpindle in unresponsive state). Therefore, I've moved the critical check to occur regardless of whether the spindle was already unresponsive. Second, I believe that setting `sys_rt_exec_alarm` is not sufficient to stop the machine and put it into alarm state. Other alarm conditions (such as hard limits) also run an `mc_reset()` to stop motion first. It appears that without this, motion will not be stopped, and in fact, the alarm appears to get cleared if it occurs during motion! --- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 5ac0f630..512e243a 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -194,12 +194,13 @@ namespace Spindles { if (retry_count == MAX_RETRIES) { if (!unresponsive) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); - if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); - sys_rt_exec_alarm = ExecAlarm::SpindleControl; - } unresponsive = true; } + if (next_cmd.critical) { + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + mc_reset(); + sys_rt_exec_alarm = ExecAlarm::SpindleControl; + } } vTaskDelay(VFD_RS485_POLL_RATE); // TODO: What is the best value here? From 6443c92177cb1b189c66522d6d1f26cfc70ac308 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 14 Dec 2020 12:42:56 -0600 Subject: [PATCH 54/82] Update per P.R. #704 on main --- embedded/package-lock.json | 51 ++++++++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 19 deletions(-) diff --git a/embedded/package-lock.json b/embedded/package-lock.json index 156dfe5a..a2205bd0 100644 --- a/embedded/package-lock.json +++ b/embedded/package-lock.json @@ -1861,7 +1861,8 @@ "ansi-regex": { "version": "2.1.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "aproba": { "version": "1.2.0", @@ -1882,12 +1883,14 @@ "balanced-match": { "version": "1.0.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "brace-expansion": { "version": "1.1.11", "bundled": true, "dev": true, + "optional": true, "requires": { "balanced-match": "^1.0.0", "concat-map": "0.0.1" @@ -1902,17 +1905,20 @@ "code-point-at": { "version": "1.1.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "concat-map": { "version": "0.0.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "console-control-strings": { "version": "1.1.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "core-util-is": { "version": "1.0.2", @@ -2029,11 +2035,6 @@ "inherits": { "version": "2.0.3", "bundled": true, - "dev": true - }, - "ini": { - "version": "1.3.5", - "bundled": true, "dev": true, "optional": true }, @@ -2041,6 +2042,7 @@ "version": "1.0.0", "bundled": true, "dev": true, + "optional": true, "requires": { "number-is-nan": "^1.0.0" } @@ -2055,6 +2057,7 @@ "version": "3.0.4", "bundled": true, "dev": true, + "optional": true, "requires": { "brace-expansion": "^1.1.7" } @@ -2062,12 +2065,14 @@ "minimist": { "version": "0.0.8", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "minipass": { "version": "2.3.5", "bundled": true, "dev": true, + "optional": true, "requires": { "safe-buffer": "^5.1.2", "yallist": "^3.0.0" @@ -2086,6 +2091,7 @@ "version": "0.5.1", "bundled": true, "dev": true, + "optional": true, "requires": { "minimist": "0.0.8" } @@ -2166,7 +2172,8 @@ "number-is-nan": { "version": "1.0.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "object-assign": { "version": "4.1.1", @@ -2178,6 +2185,7 @@ "version": "1.4.0", "bundled": true, "dev": true, + "optional": true, "requires": { "wrappy": "1" } @@ -2263,7 +2271,8 @@ "safe-buffer": { "version": "5.1.2", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "safer-buffer": { "version": "2.1.2", @@ -2299,6 +2308,7 @@ "version": "1.0.2", "bundled": true, "dev": true, + "optional": true, "requires": { "code-point-at": "^1.0.0", "is-fullwidth-code-point": "^1.0.0", @@ -2318,6 +2328,7 @@ "version": "3.0.1", "bundled": true, "dev": true, + "optional": true, "requires": { "ansi-regex": "^2.0.0" } @@ -2361,12 +2372,14 @@ "wrappy": { "version": "1.0.2", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "yallist": { "version": "3.0.3", "bundled": true, - "dev": true + "dev": true, + "optional": true } } }, @@ -3466,9 +3479,9 @@ "dev": true }, "ini": { - "version": "1.3.5", - "resolved": "https://registry.npmjs.org/ini/-/ini-1.3.5.tgz", - "integrity": "sha512-RZY5huIKCMRWDUqZlEi72f/lmXKMvuszcMBduliQ3nnWbx9X/ZBQO7DijMEYS9EhHBb2qacRUMtC7svLwe0lcw==", + "version": "1.3.7", + "resolved": "https://registry.npmjs.org/ini/-/ini-1.3.7.tgz", + "integrity": "sha512-iKpRpXP+CrP2jyrxvg1kMUpXDyRUFDWurxbnVT1vQPx+Wz9uCYsMIqYuSBLV+PAaZG/d7kRLKRFc9oDMsH+mFQ==", "dev": true }, "interpret": { @@ -6260,4 +6273,4 @@ } } } -} +} \ No newline at end of file From 83cced5e4c29109574c79ddaad1fd3ff385b6556 Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 17 Dec 2020 19:42:11 -0600 Subject: [PATCH 55/82] Update Motors.cpp --- Grbl_Esp32/src/Motors/Motors.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 1c06a7d7..fe7a9584 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -106,7 +106,7 @@ void init_motors() { if (n_axis >= 2) { // this WILL be done better with settings #ifdef Y_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (Y_TRINAMIC_DRIVER == 2130 || Y_TRINAMIC_DRIVER == 5160) { myMotor[Y_AXIS][0] = new Motors::TrinamicDriver(Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE); @@ -132,7 +132,7 @@ void init_motors() { #endif #ifdef Y2_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (Y2_TRINAMIC_DRIVER == 2130 || Y2_TRINAMIC_DRIVER == 5160) { myMotor[Y_AXIS][1] = new Motors::TrinamicDriver(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE); @@ -160,7 +160,7 @@ void init_motors() { if (n_axis >= 3) { // this WILL be done better with settings #ifdef Z_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (Z_TRINAMIC_DRIVER == 2130 || Z_TRINAMIC_DRIVER == 5160) { myMotor[Z_AXIS][0] = new Motors::TrinamicDriver(Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE); @@ -186,7 +186,7 @@ void init_motors() { #endif #ifdef Z2_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (Z2_TRINAMIC_DRIVER == 2130 || Z2_TRINAMIC_DRIVER == 5160) { myMotor[Z_AXIS][1] = new Motors::TrinamicDriver(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE); @@ -214,9 +214,9 @@ void init_motors() { if (n_axis >= 4) { // this WILL be done better with settings #ifdef A_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (A_TRINAMIC_DRIVER == 2130 || A_TRINAMIC_DRIVER == 5160) { - myMotor[A_AXIS][1] = + myMotor[A_AXIS][0] = new Motors::TrinamicDriver(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE); } # elif (A_TRINAMIC_DRIVER == 2208 || A_TRINAMIC_DRIVER == 2209) @@ -240,7 +240,7 @@ void init_motors() { #endif #ifdef A2_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (A2_TRINAMIC_DRIVER == 2130 || A2_TRINAMIC_DRIVER == 5160) { myMotor[A_AXIS][1] = new Motors::TrinamicDriver(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE); @@ -268,9 +268,9 @@ void init_motors() { if (n_axis >= 5) { // this WILL be done better with settings #ifdef B_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (B_TRINAMIC_DRIVER == 2130 || B_TRINAMIC_DRIVER == 5160) { - myMotor[B_AXIS][1] = + myMotor[B_AXIS][0] = new Motors::TrinamicDriver(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE); } # elif (B_TRINAMIC_DRIVER == 2208 || B_TRINAMIC_DRIVER == 2209) @@ -294,7 +294,7 @@ void init_motors() { #endif #ifdef B2_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (B2_TRINAMIC_DRIVER == 2130 || B2_TRINAMIC_DRIVER == 5160) { myMotor[B_AXIS][1] = new Motors::TrinamicDriver(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE); @@ -322,9 +322,9 @@ void init_motors() { if (n_axis >= 6) { // this WILL be done better with settings #ifdef C_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (C_TRINAMIC_DRIVER == 2130 || C_TRINAMIC_DRIVER == 5160) { - myMotor[C_AXIS][1] = + myMotor[C_AXIS][0] = new Motors::TrinamicDriver(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE); } # elif (C_TRINAMIC_DRIVER == 2208 || C_TRINAMIC_DRIVER == 2209) @@ -348,7 +348,7 @@ void init_motors() { #endif #ifdef C2_TRINAMIC_DRIVER -# if (X_TRINAMIC_DRIVER == 2130 || X_TRINAMIC_DRIVER == 5160) +# if (C2_TRINAMIC_DRIVER == 2130 || C2_TRINAMIC_DRIVER == 5160) { myMotor[C_AXIS][1] = new Motors::TrinamicDriver(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE); From 25dc57c7a2002a03a328d9d9f57351280e5840bf Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 21 Dec 2020 16:55:01 -0600 Subject: [PATCH 56/82] Fix undefined probe reporting if inverted. --- Grbl_Esp32/src/Probe.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp index 661c2d84..dcb57e64 100644 --- a/Grbl_Esp32/src/Probe.cpp +++ b/Grbl_Esp32/src/Probe.cpp @@ -50,7 +50,7 @@ void set_probe_direction(bool is_away) { // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor. bool probe_get_state() { - return digitalRead(PROBE_PIN) ^ probe_invert->get(); + return (PROBE_PIN == UNDEFINED_PIN) ? false : digitalRead(PROBE_PIN) ^ probe_invert->get(); } // Monitors probe pin state and records the system position when detected. Called by the From 07a148875ac10dc903e4d87c76e5aba7e231a902 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Tue, 22 Dec 2020 08:41:49 -1000 Subject: [PATCH 57/82] Settings filtering via regular expressions (#717) * Settings filtering via regular expressions Implements only the most basic - and the most useful - special characters - ^$.* If the search string does not contain a special character, it is interpreted as before. Otherwise the match is either more strict if anchored by ^ or $, or less strict if it contains a . wildcard or a * repetition. * Commentary * Eliminated . metacharacter --- Grbl_Esp32/src/ProcessSettings.cpp | 9 +---- Grbl_Esp32/src/Regex.cpp | 60 ++++++++++++++++++++++++++++++ Grbl_Esp32/src/Regex.h | 5 +++ 3 files changed, 67 insertions(+), 7 deletions(-) create mode 100644 Grbl_Esp32/src/Regex.cpp create mode 100644 Grbl_Esp32/src/Regex.h diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index a6831713..a04ca4df 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -1,5 +1,6 @@ #include "Grbl.h" #include +#include "Regex.h" // WG Readable and writable as guest // WU Readable and writable as user and admin @@ -565,19 +566,13 @@ Error do_command_or_setting(const char* key, char* value, WebUI::AuthenticationL Error retval = Error::InvalidStatement; if (!value) { auto lcKey = String(key); - // We allow the key string to begin with *, which we remove. - // This lets us look at X axis settings with $*x. - // $x by itself is the disable alarm lock command - if (lcKey.startsWith("*")) { - lcKey.remove(0, 1); - } lcKey.toLowerCase(); bool found = false; for (Setting* s = Setting::List; s; s = s->next()) { auto lcTest = String(s->getName()); lcTest.toLowerCase(); - if (lcTest.indexOf(lcKey) >= 0) { + if (regexMatch(lcKey.c_str(), lcTest.c_str())) { const char* displayValue = auth_failed(s, value, auth_level) ? "" : s->getStringValue(); show_setting(s->getName(), displayValue, NULL, out); found = true; diff --git a/Grbl_Esp32/src/Regex.cpp b/Grbl_Esp32/src/Regex.cpp new file mode 100644 index 00000000..7f942ccc --- /dev/null +++ b/Grbl_Esp32/src/Regex.cpp @@ -0,0 +1,60 @@ +// Simple regular expression matcher from Rob Pike per +// https://www.cs.princeton.edu/courses/archive/spr09/cos333/beautiful.html + +// c matches any literal character c +// ^ matches the beginning of the input string +// $ matches the end of the input string +// * matches zero or more occurrences of any character + +// The code therein has been reformatted into the style used in this +// project while replacing ints used as flags by bools. The regex +// syntax was changed by omitting '.' and making '*' equivalent to +// ".*". This regular expression matcher is for matching setting +// names, where arbitrary repetion of literal characters is +// unlikely. Literal character repetition is most useful for +// skipping whitespace, which does not occur in setting names. The +// "bare * wildcard" is similar to filename wildcarding in many shells +// and CLIs. + +static bool matchHere(const char* regexp, const char* text); + +// matchStar - search for *regexp at beginning of text +static bool matchStar(const char* regexp, const char* text) { + do { + if (matchHere(regexp, text)) { + return true; + } + } while (*text++ != '\0'); + return false; +} + +// matchHere - search for regex at beginning of text +static bool matchHere(const char* regexp, const char* text) { + if (regexp[0] == '\0') { + return true; + } + if (regexp[0] == '*') { + return matchStar(regexp + 1, text); + } + if (regexp[0] == '$' && regexp[1] == '\0') { + return *text == '\0'; + } + if (*text != '\0' && (regexp[0] == *text)) { + return matchHere(++regexp, ++text); + } + return false; +} + +// match - search for regular expression anywhere in text +// Returns true if text contains the regular expression regexp +bool regexMatch(const char* regexp, const char* text) { + if (regexp[0] == '^') { + return matchHere(++regexp, text); + } + do { + if (matchHere(regexp, text)) { + return true; + } + } while (*text++ != '\0'); + return false; +} diff --git a/Grbl_Esp32/src/Regex.h b/Grbl_Esp32/src/Regex.h new file mode 100644 index 00000000..9c0c5d13 --- /dev/null +++ b/Grbl_Esp32/src/Regex.h @@ -0,0 +1,5 @@ +// Simple regular expression matcher. +// See Regex.cpp for attribution, description and discussion + +// Returns true if text contains the regular expression regexp +bool regexMatch(const char* regexp, const char* text); From c3318fe3dd38d26d24fbd9a6eac0430d505af8a6 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Wed, 30 Dec 2020 10:08:49 -1000 Subject: [PATCH 58/82] Fix SD/List repetition error (#727) * Fix SD/List repetition error The one line change that actually fixes it is Serial.cpp line 162, where the SD state is compared to "not busy" instead of "is idle", thus also handling the "not present" case. In the process, I converted the "const int SDCARD_ ..." to an SDState enum class, thus proving type safety and eliminating yet another untyped uint8_t . * Updates after testing Co-authored-by: bdring --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Machines/template.h | 233 --------------------------- Grbl_Esp32/src/MotionControl.cpp | 2 +- Grbl_Esp32/src/Report.cpp | 6 +- Grbl_Esp32/src/SDCard.cpp | 20 +-- Grbl_Esp32/src/SDCard.h | 17 +- Grbl_Esp32/src/Serial.cpp | 2 +- Grbl_Esp32/src/WebUI/WebServer.cpp | 23 ++- Grbl_Esp32/src/WebUI/WebSettings.cpp | 24 +-- 9 files changed, 49 insertions(+), 280 deletions(-) delete mode 100644 Grbl_Esp32/src/Machines/template.h diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 93c3f1e6..4cccc59a 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 = "20201212"; +const char* const GRBL_VERSION_BUILD = "20201230"; //#include #include diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h deleted file mode 100644 index 7672d31f..00000000 --- a/Grbl_Esp32/src/Machines/template.h +++ /dev/null @@ -1,233 +0,0 @@ -#pragma once -// clang-format off - -/* - template.h - Part of Grbl_ESP32 - - Template for a machine configuration file. - - 2020 - Mitch Bradley - - Grbl_ESP32 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. - - Grbl 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 Grbl_ESP32. If not, see . -*/ - -// This contains a long list of things that might possibly be -// configured. Most machines - especially simple cartesian machines -// that use stepper motors - will only need to define a few of the -// options herein, often just the pin assignments. - -// Pin assignments depend on how the ESP32 is connected to -// the external machine. Typically the ESP32 module plugs into -// an adapter board that wires specific ESP32 GPIO pins to -// other connectors on the board, such as Pololu sockets for -// stepper drivers or connectors for external drivers, limit -// pins, spindle control, etc. This file describes how those -// GPIO pins are wired to those other connectors. - -// Some machines might choose to use an adapter board in a -// non-standard way, for example a 3-axis board might have axes -// labeled XYZ, but the machine might have only 2 axes one of which is -// driven by two ganged motors. In that case, you would need -// a custom version of this file that assigns the pins differently -// from the adapter board labels. - -// In addition to pin assignments, many other aspects of Grbl -// can be configured, such as spindle speeds, special motor -// types like servos and unipolars, homing order, default values -// for $$ settings, etc. A detailed list of such options is -// given below. - -// Furthermore, it is possible to implement special complex -// behavior in custom C++ code, for non-Cartesian machines, -// unusual homing cycles, etc. See the Special Features section -// below for additional instructions. - -// === Machine Name -// Change TEMPLATE to some name of your own choosing. That name -// will be shown in a Grbl startup message to identify your -// configuration. - -#define MACHINE_NAME "TEMPLATE" - -// If your machine requires custom code as described below in -// Special Features, you must copy Custom/custom_code_template.cpp -// to a new name like Custom/my_custom_code.cpp, implement the -// functions therein, and enable its use by defining: -// #define CUSTOM_CODE_FILENAME "Custom/my_custom_code.cpp" - -// === Number of axes - -// You can set the number of axes that the machine supports -// by defining N_AXIS. If you do not define it, 3 will be -// used. The value must be at least 3, even if your machine -// has fewer axes. -// #define N_AXIS 4 - - -// == Pin Assignments - -// Step and direction pins; these must be output-capable pins, -// specifically ESP32 GPIO numbers 0..31 -// #define X_STEP_PIN GPIO_NUM_12 -// #define X_DIRECTION_PIN GPIO_NUM_14 -// #define Y_STEP_PIN GPIO_NUM_26 -// #define Y_DIRECTION_PIN GPIO_NUM_15 -// #define Z_STEP_PIN GPIO_NUM_27 -// #define Z_DIRECTION_PIN GPIO_NUM_33 - -// #define X_LIMIT_PIN GPIO_NUM_17 -// #define Y_LIMIT_PIN GPIO_NUM_4 -// #define Z_LIMIT_PIN GPIO_NUM_16 - -// Common enable for all steppers. If it is okay to leave -// your drivers enabled at all times, you can leave -// STEPPERS_DISABLE_PIN undefined and use the pin for something else. -// #define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -// Pins for controlling various aspects of the machine. If your -// machine does not support one of these features, you can leave -// the corresponding pin undefined. - -// #define SPINDLE_OUTPUT_PIN GPIO_NUM_2 // labeled SpinPWM -// #define SPINDLE_ENABLE_PIN GPIO_NUM_22 // labeled SpinEnbl -// #define COOLANT_MIST_PIN GPIO_NUM_21 // labeled Mist -// #define COOLANT_FLOOD_PIN GPIO_NUM_25 // labeled Flood -// #define PROBE_PIN GPIO_NUM_32 // labeled Probe - -// Input pins for various functions. If the corresponding pin is not defined, -// the function will not be available. - -// CONTROL_SAFETY_DOOR_PIN shuts off the machine when a door is opened -// or some other unsafe condition exists. -// #define CONTROL_SAFETY_DOOR_PIN GPIO_NUM_35 // labeled Door, needs external pullup - -// RESET, FEED_HOLD, and CYCLE_START can control GCode execution at -// the push of a button. - -// #define CONTROL_RESET_PIN GPIO_NUM_34 // labeled Reset, needs external pullup -// #define CONTROL_FEED_HOLD_PIN GPIO_NUM_36 // labeled Hold, needs external pullup -// #define CONTROL_CYCLE_START_PIN GPIO_NUM_39 // labeled Start, needs external pullup - -// === Ganging -// If you need to use two motors on one axis, you can "gang" the motors by -// defining a second pin to control the other motor on the axis. For example: - -// #define Y2_STEP_PIN GPIO_NUM_27 /* labeled Z */ -// #define Y2_DIRECTION_PIN GPIO_NUM_33 /* labeled Z */ - -// === Servos -// To use a servo motor on an axis, do not define step and direction -// pins for that axis, but instead include a block like this: - -// #define SERVO_Z_PIN GPIO_NUM_22 - -// === Homing cycles -// Set them using $Homing/Cycle0= optionally up to $Homing/Cycle5= - -// === Default settings -// Grbl has many run-time settings that the user can changed by -// commands like $110=2000 . Their values are stored in non-volatile -// storage so they persist after the controller has been powered down. -// Those settings have default values that are used if the user -// has not altered them, or if the settings are explicitly reset -// to the default values wth $RST=$. -// -// The default values are established in defaults.h, but you -// can override any one of them by definining it here, for example: - -//#define DEFAULT_INVERT_LIMIT_PINS 1 -//#define DEFAULT_REPORT_INCHES 1 - -// === Control Pins - -// If some of the control pin switches are normally closed -// (the default is normally open), you can invert some of them -// with INVERT_CONTROL_PIN_MASK. The bits in the mask are -// Cycle Start, Feed Hold, Reset, Safety Door. To use a -// normally open switch on Reset, you would say -// #define INVERT_CONTROL_PIN_MASK B1101 - -// If your control pins do not have adequate hardware signal -// conditioning, you can define these to use software to -// reduce false triggering. -// #define ENABLE_CONTROL_SW_DEBOUNCE // Default disabled. Uncomment to enable. -// #define CONTROL_SW_DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds - - -// Grbl_ESP32 use the ESP32's special RMT (IR remote control) hardware -// engine to achieve more precise high step rates than can be done -// in software. That feature is enabled by default, but there are -// some machines that might not want to use it, such as machines that -// do not use ordinary stepper motors. To turn it off, do this: -// #undef USE_RMT_STEPS - -// === Special Features -// Grbl_ESP32 can support non-Cartesian machines and some other -// scenarios that cannot be handled by choosing from a set of -// predefined selections. Instead they require machine-specific -// C++ code functions. There are callouts in the core code for -// such code, guarded by ifdefs that enable calling the individual -// functions. custom_code_template.cpp describes the functions -// that you can implement. The ifdef guards are described below: -// -// USE_CUSTOM_HOMING enables the user_defined_homing(uint8_t cycle_mask) function -// that can implement an arbitrary homing sequence. -// #define USE_CUSTOM_HOMING - -// USE_KINEMATICS enables the functions inverse_kinematics(), -// kinematics_pre_homing(), and kinematics_post_homing(), -// so non-Cartesian machines can be implemented. -// #define USE_KINEMATICS - -// USE_FWD_KINEMATICS enables the forward_kinematics() function -// that converts motor positions in non-Cartesian coordinate -// systems back to Cartesian form, for status reports. -//#define USE_FWD_KINEMATICS - -// USE_TOOL_CHANGE enables the user_tool_change() function -// that implements custom tool change procedures. -// #define USE_TOOL_CHANGE - -// Any one of MACRO_BUTTON_0_PIN, MACRO_BUTTON_1_PIN, and MACRO_BUTTON_2_PIN -// enables the user_defined_macro(number) function which -// implements custom behavior at the press of a button -// #define MACRO_BUTTON_0_PIN - -// USE_M30 enables the user_m30() function which implements -// custom behavior when a GCode programs stops at the end -// #define USE_M30 - -// USE_TRIAMINIC enables a suite of functions that are defined -// in grbl_triaminic.cpp, allowing the use of Triaminic stepper -// drivers that require software configuration at startup. -// There are several options that control the details of such -// drivers; inspect the code in grbl_triaminic.cpp to see them. -// #define USE_TRIAMINIC -// #define X_TRIAMINIC -// #define X_DRIVER_TMC2209 -// #define TRIAMINIC_DAISY_CHAIN - -// USE_MACHINE_TRINAMIC_INIT enables the machine_triaminic_setup() -// function that replaces the normal setup of Triaminic drivers. -// It could, for, example, setup StallGuard or other special modes. -// #define USE_MACHINE_TRINAMIC_INIT - - -// === Grbl behavior options -// There are quite a few options that control aspects of Grbl that -// are not specific to particular machines. They are listed and -// described in config.h after it includes the file machine.h. -// Normally you would not need to change them, but if you do, -// it will be necessary to make the change in config.h diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 74b53d26..56be3e83 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -509,7 +509,7 @@ void mc_reset() { 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) { + if (get_sd_state(false) == SDState::BusyPrinting) { //Report print stopped report_feedback_message(Message::SdFileQuit); closeFile(); diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 33c53c11..67d4fddd 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -222,7 +222,7 @@ void report_status_message(Error status_code, uint8_t client) { switch (status_code) { case Error::Ok: // Error::Ok #ifdef ENABLE_SD_CARD - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { SD_ready_next = true; // flag so system_execute_line() will send the next line } else { grbl_send(client, "ok\r\n"); @@ -234,7 +234,7 @@ void report_status_message(Error status_code, uint8_t client) { default: #ifdef ENABLE_SD_CARD // do we need to stop a running SD job? - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { if (status_code == Error::GcodeUnsupportedCommand) { grbl_sendf(client, "error:%d\r\n", status_code); // most senders seem to tolerate this error and keep on going grbl_sendf(CLIENT_ALL, "error:%d in SD file at line %d\r\n", status_code, sd_get_current_line_number()); @@ -837,7 +837,7 @@ void report_realtime_status(uint8_t client) { } #endif #ifdef ENABLE_SD_CARD - if (get_sd_state(false) == SDCARD_BUSY_PRINTING) { + if (get_sd_state(false) == SDState::BusyPrinting) { sprintf(temp, "|SD:%4.2f,", sd_report_perc_complete()); strcat(status, temp); sd_get_current_filename(temp); diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index 2a6fab21..32dd95ef 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -67,7 +67,7 @@ boolean openFile(fs::FS& fs, const char* path) { //report_status_message(Error::SdFailedRead, CLIENT_SERIAL); return false; } - set_sd_state(SDCARD_BUSY_PRINTING); + set_sd_state(SDState::BusyPrinting); SD_ready_next = false; // this will get set to true when Grbl issues "ok" message sd_current_line_number = 0; return true; @@ -77,7 +77,7 @@ boolean closeFile() { if (!myFile) { return false; } - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); SD_ready_next = false; sd_current_line_number = 0; myFile.close(); @@ -125,19 +125,19 @@ uint32_t sd_get_current_line_number() { return sd_current_line_number; } -uint8_t sd_state = SDCARD_IDLE; +SDState sd_state = SDState::Idle; -uint8_t get_sd_state(bool refresh) { +SDState get_sd_state(bool refresh) { if (SDCARD_DET_PIN != UNDEFINED_PIN) { if (digitalRead(SDCARD_DET_PIN) != SDCARD_DET_VAL) { - sd_state = SDCARD_NOT_PRESENT; + sd_state = SDState::NotPresent; return sd_state; //no need to go further if SD detect is not correct } } //if busy doing something return state - if (!((sd_state == SDCARD_NOT_PRESENT) || (sd_state == SDCARD_IDLE))) { + if (!((sd_state == SDState::NotPresent) || (sd_state == SDState::Idle))) { return sd_state; } if (!refresh) { @@ -146,19 +146,19 @@ uint8_t get_sd_state(bool refresh) { //SD is idle or not detected, let see if still the case SD.end(); - sd_state = SDCARD_NOT_PRESENT; + sd_state = SDState::NotPresent; //using default value for speed ? should be parameter //refresh content if card was removed if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ, "/sd", 2)) { if (SD.cardSize() > 0) { - sd_state = SDCARD_IDLE; + sd_state = SDState::Idle; } } return sd_state; } -uint8_t set_sd_state(uint8_t flag) { - sd_state = flag; +SDState set_sd_state(SDState state) { + sd_state = state; return sd_state; } diff --git a/Grbl_Esp32/src/SDCard.h b/Grbl_Esp32/src/SDCard.h index 298fbdec..a680e0a4 100644 --- a/Grbl_Esp32/src/SDCard.h +++ b/Grbl_Esp32/src/SDCard.h @@ -23,19 +23,22 @@ //#define SDCARD_DET_PIN -1 const int SDCARD_DET_VAL = 0; // for now, CD is close to ground -const int SDCARD_IDLE = 0; -const int SDCARD_NOT_PRESENT = 1; -const int SDCARD_BUSY_PRINTING = 2; -const int SDCARD_BUSY_UPLOADING = 4; -const int SDCARD_BUSY_PARSING = 8; +enum class SDState : uint8_t { + Idle = 0, + NotPresent = 1, + Busy = 2, + BusyPrinting = 2, + BusyUploading = 3, + BusyParsing = 4, +}; extern bool SD_ready_next; // Grbl has processed a line and is waiting for another extern uint8_t SD_client; extern WebUI::AuthenticationLevel SD_auth_level; //bool sd_mount(); -uint8_t get_sd_state(bool refresh); -uint8_t set_sd_state(uint8_t flag); +SDState get_sd_state(bool refresh); +SDState set_sd_state(SDState state); void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client); boolean openFile(fs::FS& fs, const char* path); boolean closeFile(); diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index 5e5e2549..9a4b9e4e 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -159,7 +159,7 @@ void serialCheckTask(void* pvParameters) { if (is_realtime_command(data)) { execute_realtime_command(static_cast(data), client); } else { - if (get_sd_state(false) == SDCARD_IDLE) { + if (get_sd_state(false) < SDState::Busy) { vTaskEnterCritical(&myMutex); client_buffer[client].write(data); vTaskExitCritical(&myMutex); diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index d58a8b7c..97b2ca34 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -1221,16 +1221,15 @@ namespace WebUI { bool list_files = true; uint64_t totalspace = 0; uint64_t usedspace = 0; - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { + SDState state = get_sd_state(true); + if (state != SDState::Idle) { String status = "{\"status\":\""; - if(state == SDCARD_NOT_PRESENT)status+="No SD Card\"}"; - else status+="Busy\"}"; + status += state == SDState::NotPresent ? "No SD Card\"}" : "Busy\"}"; _webserver->sendHeader("Cache-Control", "no-cache"); _webserver->send(200, "application/json", status); return; } - set_sd_state(SDCARD_BUSY_PARSING); + set_sd_state(SDState::BusyParsing); //get current path if (_webserver->hasArg("path")) { @@ -1400,7 +1399,7 @@ namespace WebUI { _webserver->sendHeader("Cache-Control", "no-cache"); _webserver->send(200, "application/json", jsonfile); _upload_status = UploadStatusType::NONE; - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); SD.end(); } @@ -1427,13 +1426,13 @@ namespace WebUI { filename = "/" + upload.filename; } //check if SD Card is available - if (get_sd_state(true) != SDCARD_IDLE) { + if (get_sd_state(true) != SDState::Idle) { _upload_status = UploadStatusType::FAILED; grbl_send(CLIENT_ALL, "[MSG:Upload cancelled]\r\n"); pushError(ESP_ERROR_UPLOAD_CANCELLED, "Upload cancelled"); } else { - set_sd_state(SDCARD_BUSY_UPLOADING); + set_sd_state(SDState::BusyUploading); //delete file on SD Card if already present if (SD.exists(filename)) { SD.remove(filename); @@ -1468,7 +1467,7 @@ namespace WebUI { //************** } else if (upload.status == UPLOAD_FILE_WRITE) { vTaskDelay(1 / portTICK_RATE_MS); - if (sdUploadFile && (_upload_status == UploadStatusType::ONGOING) && (get_sd_state(false) == SDCARD_BUSY_UPLOADING)) { + if (sdUploadFile && (_upload_status == UploadStatusType::ONGOING) && (get_sd_state(false) == SDState::BusyUploading)) { //no error write post data if (upload.currentSize != sdUploadFile.write(upload.buf, upload.currentSize)) { _upload_status = UploadStatusType::FAILED; @@ -1506,7 +1505,7 @@ namespace WebUI { } if (_upload_status == UploadStatusType::ONGOING) { _upload_status = UploadStatusType::SUCCESSFUL; - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); } else { _upload_status = UploadStatusType::FAILED; pushError(ESP_ERROR_UPLOAD, "Upload error"); @@ -1514,7 +1513,7 @@ namespace WebUI { } else { //Upload cancelled _upload_status = UploadStatusType::FAILED; - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); grbl_send(CLIENT_ALL, "[MSG:Upload failed]\r\n"); if (sdUploadFile) { sdUploadFile.close(); @@ -1532,7 +1531,7 @@ namespace WebUI { if (SD.exists(filename)) { SD.remove(filename); } - set_sd_state(SDCARD_IDLE); + set_sd_state(SDState::Idle); } COMMANDS::wait(0); } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index cec830b9..7058b31d 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -682,9 +682,9 @@ namespace WebUI { if (path[0] != '/') { path = "/" + path; } - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - if (state == SDCARD_NOT_PRESENT) { + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + if (state == SDState::NotPresent) { webPrintln("No SD Card"); return Error::SdFailedMount; } else { @@ -752,9 +752,9 @@ namespace WebUI { webPrintln("Missing file name!"); return Error::InvalidValue; } - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - webPrintln((state == SDCARD_NOT_PRESENT) ? "No SD card" : "Busy"); + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + webPrintln((state == SDState::NotPresent) ? "No SD card" : "Busy"); return Error::Ok; } String path = parameter; @@ -784,9 +784,9 @@ namespace WebUI { } static Error listSDFiles(char* parameter, AuthenticationLevel auth_level) { // ESP210 - int8_t state = get_sd_state(true); - if (state != SDCARD_IDLE) { - if (state == SDCARD_NOT_PRESENT) { + SDState state = get_sd_state(true); + if (state != SDState::Idle) { + if (state == SDState::NotPresent) { webPrintln("No SD Card"); return Error::SdFailedMount; } else { @@ -857,10 +857,10 @@ namespace WebUI { const char* resp = "No SD card"; #ifdef ENABLE_SD_CARD switch (get_sd_state(true)) { - case SDCARD_IDLE: + case SDState::Idle: resp = "SD card detected"; break; - case SDCARD_NOT_PRESENT: + case SDState::NotPresent: resp = "No SD card"; break; default: @@ -868,7 +868,7 @@ namespace WebUI { } #else resp = "SD card not enabled"; -#endif +#endif webPrintln(resp); return Error::Ok; } From a6485da4caecaa14cf6c4ab9d7721f42ea784456 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 1 Jan 2021 09:01:43 -0600 Subject: [PATCH 59/82] Fixed RcServo Cals --- Grbl_Esp32/src/Motors/RcServo.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index 4d9a6dd7..2eefbfc2 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -42,6 +42,9 @@ namespace Motors { sprintf(setting_cal_max, "%c/RcServo/Cal/Max", report_get_axis_letter(_axis_index)); // rc_servo_cal_max = new FloatSetting(EXTENDED, WG, NULL, setting_cal_max, 1.0, 0.5, 2.0); + rc_servo_cal_min->load(); + rc_servo_cal_max->load(); + read_settings(); _channel_num = sys_get_next_PWM_chan_num(); ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); From e496592c15bab710986a9edccfb6d423aa8121a7 Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Fri, 1 Jan 2021 09:54:05 -1000 Subject: [PATCH 60/82] PWM fix and simplification (#722) * PWM fix and simplification This is an alternative solution to the PWM/ISR/float problem. 1. The set_level() argument is the exact value that is passed to the LEDC Write function. It can be interpreted as the numerator of a rational fraction whose denominator is the max PWM value, i.e. the precision, == 1 << _resolution_bits 2. There is a new denominator() method that returns the precision. 3. The sys_pwm_control(mask, duty, synchronize) function is replaced by two functions sys_analog_all_off() and sys_set_analog(io_num, duty). This closely matches the actual usage. The old routine was called from two places, one where the mask was alway 0xFF, the duty was always 0, and synchronize was always false. That is the one that was troublesome from ISR context. The other call always affected a single pin, so the mask formulation with its loop was useless extra baggage. By splitting into two functions, each one becomes much simpler, thus faster and easier to understand. The "synchronize" argument and associated logic moved out to the caller (GCode.cpp), which more closely reflects the behavioral logic. 4. For symmetry, sys_io_control() was similarly split. 5. sys_calc_pwm_precision() was moved into UserOutput.cpp since is it driver specific, not a general system routine. * Update Grbl.h * Delete template.h Co-authored-by: bdring --- Grbl_Esp32/src/GCode.cpp | 15 ++++++++---- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 4 ++-- Grbl_Esp32/src/System.cpp | 39 +++++++++++++------------------- Grbl_Esp32/src/System.h | 9 ++++---- Grbl_Esp32/src/UserOutput.cpp | 28 ++++++++++++++--------- Grbl_Esp32/src/UserOutput.h | 17 +++++++------- 7 files changed, 60 insertions(+), 54 deletions(-) diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index b1a7a079..94ea3237 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1401,10 +1401,11 @@ Error gc_execute_line(char* line, uint8_t client) { if ((gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOffSync) || (gc_block.modal.io_control == IoControl::DigitalOnImmediate) || (gc_block.modal.io_control == IoControl::DigitalOffImmediate)) { if (gc_block.values.p < MaxUserDigitalPin) { - if (!sys_io_control( - bit((int)gc_block.values.p), - (gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOnImmediate), - (gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOffSync))) { + if ((gc_block.modal.io_control == IoControl::DigitalOnSync) || (gc_block.modal.io_control == IoControl::DigitalOffSync)) { + protocol_buffer_synchronize(); + } + bool turnOn = gc_block.modal.io_control == IoControl::DigitalOnSync || gc_block.modal.io_control == IoControl::DigitalOnImmediate; + if (!sys_set_digital((int)gc_block.values.p, turnOn)) { FAIL(Error::PParamMaxExceeded); } } else { @@ -1414,8 +1415,12 @@ Error gc_execute_line(char* line, uint8_t client) { if ((gc_block.modal.io_control == IoControl::SetAnalogSync) || (gc_block.modal.io_control == IoControl::SetAnalogImmediate)) { if (gc_block.values.e < MaxUserDigitalPin) { gc_block.values.q = constrain(gc_block.values.q, 0.0, 100.0); // force into valid range - if (!sys_pwm_control(bit((int)gc_block.values.e), gc_block.values.q, (gc_block.modal.io_control == IoControl::SetAnalogSync))) + if (gc_block.modal.io_control == IoControl::SetAnalogSync) { + protocol_buffer_synchronize(); + } + if (!sys_set_analog((int)gc_block.values.e, gc_block.values.q)) { FAIL(Error::PParamMaxExceeded); + } } else { FAIL(Error::PParamMaxExceeded); } diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 4cccc59a..44418bed 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 = "20201230"; +const char* const GRBL_VERSION_BUILD = "20210101"; //#include #include diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 56be3e83..954009a1 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -505,8 +505,8 @@ void mc_reset() { coolant_stop(); // turn off all User I/O immediately - sys_io_control(0xFF, LOW, false); - sys_pwm_control(0xFF, 0, false); + sys_digital_all_off(); + sys_analog_all_off(); #ifdef ENABLE_SD_CARD // do we need to stop a running SD job? if (get_sd_state(false) == SDState::BusyPrinting) { diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 6b21702c..857ab4e3 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -267,36 +267,29 @@ void system_exec_control_pin(ControlPins pins) { } } -// io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx -// It uses a mask so all can be turned of in ms_reset -bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized) { - bool cmd_ok = true; - if (synchronized) - protocol_buffer_synchronize(); - +void sys_digital_all_off() { for (uint8_t io_num = 0; io_num < MaxUserDigitalPin; io_num++) { - if (io_num_mask & bit(io_num)) { - if (!myDigitalOutputs[io_num]->set_level(turnOn)) - cmd_ok = false; - } + myDigitalOutputs[io_num]->set_level(LOW); } - return cmd_ok; } -// io_num is the virtual pin# and has nothing to do with the actual esp32 GPIO_NUM_xx -// It uses a mask so all can be turned of in ms_reset -bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized) { - bool cmd_ok = true; - if (synchronized) - protocol_buffer_synchronize(); +// io_num is the virtual digital pin# +bool sys_set_digital(uint8_t io_num, bool turnOn) { + return myDigitalOutputs[io_num]->set_level(turnOn); +} +// Turn off all analog outputs +void sys_analog_all_off() { for (uint8_t io_num = 0; io_num < MaxUserDigitalPin; io_num++) { - if (io_num_mask & bit(io_num)) { - if (!myAnalogOutputs[io_num]->set_level(duty)) - cmd_ok = false; - } + myAnalogOutputs[io_num]->set_level(0); } - return cmd_ok; +} + +// io_num is the virtual analog pin# +bool sys_set_analog(uint8_t io_num, float percent) { + auto analog = myAnalogOutputs[io_num]; + uint32_t numerator = percent / 100.0 * analog->denominator(); + return analog->set_level(numerator); } /* diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h index 6803f7e2..d955f01e 100644 --- a/Grbl_Esp32/src/System.h +++ b/Grbl_Esp32/src/System.h @@ -170,8 +170,9 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps); void controlCheckTask(void* pvParameters); void system_exec_control_pin(ControlPins pins); -bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized); -bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized); +bool sys_set_digital(uint8_t io_num, bool turnOn); +void sys_digital_all_off(); +bool sys_set_analog(uint8_t io_num, float percent); +void sys_analog_all_off(); -int8_t sys_get_next_PWM_chan_num(); -uint8_t sys_calc_pwm_precision(uint32_t freq); +int8_t sys_get_next_PWM_chan_num(); diff --git a/Grbl_Esp32/src/UserOutput.cpp b/Grbl_Esp32/src/UserOutput.cpp index 6a1f9ce0..05b856dd 100644 --- a/Grbl_Esp32/src/UserOutput.cpp +++ b/Grbl_Esp32/src/UserOutput.cpp @@ -64,8 +64,17 @@ namespace UserOutput { if (pin == UNDEFINED_PIN) return; - // determine the highest bit precision allowed by frequency - _resolution_bits = sys_calc_pwm_precision(_pwm_frequency); + // determine the highest resolution (number of precision bits) allowed by frequency + + uint32_t apb_frequency = getApbFrequency(); + + // increase the precision (bits) until it exceeds allow by frequency the max or is 16 + _resolution_bits = 0; + while ((1 << _resolution_bits) < (apb_frequency / _pwm_frequency) && _resolution_bits <= 16) { + ++_resolution_bits; + } + // _resolution_bits is now just barely too high, so drop it down one + --_resolution_bits; init(); } @@ -89,9 +98,7 @@ namespace UserOutput { } // returns true if able to set value - bool AnalogOutput::set_level(float percent) { - float duty; - + bool AnalogOutput::set_level(uint32_t numerator) { // look for errors, but ignore if turning off to prevent mask turn off from generating errors if (_pin == UNDEFINED_PIN) { return false; @@ -102,15 +109,14 @@ namespace UserOutput { return false; } - if (_current_value == percent) + if (_current_value == numerator) { return true; + } - _current_value = percent; + _current_value = numerator; - duty = (percent / 100.0) * (1 << _resolution_bits); - - ledcWrite(_pwm_channel, duty); + ledcWrite(_pwm_channel, numerator); return true; } -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/UserOutput.h b/Grbl_Esp32/src/UserOutput.h index 10a7ea30..ca0d6e2c 100644 --- a/Grbl_Esp32/src/UserOutput.h +++ b/Grbl_Esp32/src/UserOutput.h @@ -41,17 +41,18 @@ namespace UserOutput { public: AnalogOutput(); AnalogOutput(uint8_t number, uint8_t pin, float pwm_frequency); - bool set_level(float percent); + bool set_level(uint32_t numerator); + uint32_t denominator() { return 1UL << _resolution_bits; }; protected: void init(); void config_message(); - uint8_t _number = UNDEFINED_PIN; - uint8_t _pin = UNDEFINED_PIN; - uint8_t _pwm_channel = -1; // -1 means invalid or not setup - float _pwm_frequency; - uint8_t _resolution_bits; - float _current_value; + uint8_t _number = UNDEFINED_PIN; + uint8_t _pin = UNDEFINED_PIN; + uint8_t _pwm_channel = -1; // -1 means invalid or not setup + float _pwm_frequency; + uint8_t _resolution_bits; + uint32_t _current_value; }; -} \ No newline at end of file +} From 6c1cc3c4d1fb43024267298551e0efb075751b89 Mon Sep 17 00:00:00 2001 From: bdring Date: Thu, 21 Jan 2021 19:32:22 -0600 Subject: [PATCH 61/82] TMC2209 Stallguard (#748) * TMC2209 Stallguard - Added StallGuard homing support to TMC2209 (UART) - Killed off TMC2208 for now. Too many conflicts with TMC2209. Will return with Diamond motor class hierarchy - Increase StallGuard setting range for TMC2209. Constrianed in each class to actual limits - Added a machine def to test TMC2209 * Update build date --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Machines/TMC2209_4x.h | 89 +++++++++++++++++++ Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 4 +- Grbl_Esp32/src/Motors/TrinamicUartDriver.h | 2 +- .../src/Motors/TrinamicUartDriverClass.cpp | 89 ++++++++++++------- Grbl_Esp32/src/SettingsDefinitions.cpp | 2 +- 6 files changed, 152 insertions(+), 36 deletions(-) create mode 100644 Grbl_Esp32/src/Machines/TMC2209_4x.h diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 44418bed..3f921034 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 = "20210101"; +const char* const GRBL_VERSION_BUILD = "20210121"; //#include #include diff --git a/Grbl_Esp32/src/Machines/TMC2209_4x.h b/Grbl_Esp32/src/Machines/TMC2209_4x.h new file mode 100644 index 00000000..004b351b --- /dev/null +++ b/Grbl_Esp32/src/Machines/TMC2209_4x.h @@ -0,0 +1,89 @@ +#pragma once +// clang-format off + +/* + TMC2209_4x.h + https://github.com/FYSETC/FYSETC-E4 + + 2020-12-29 B. Dring + + This is a machine definition file to use the FYSTEC E4 3D Printer controller + This is a 4 motor controller. This is setup for XYZA, but XYYZ, could also be used. + There are 5 inputs + The controller has outputs for a Fan, Hotbed and Extruder. There are mapped to + spindle, mist and flood coolant to drive an external relay. + + Grbl_ESP32 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. + + Grbl 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 Grbl_ESP32. If not, see . +*/ + +#define MACHINE_NAME "TMC2209 4x Controller" + +#define N_AXIS 4 + +#define TRINAMIC_UART_RUN_MODE TrinamicUartMode :: StealthChop +#define TRINAMIC_UART_HOMING_MODE TrinamicUartMode :: StallGuard + +#define TMC_UART UART_NUM_1 +#define TMC_UART_RX GPIO_NUM_21 +#define TMC_UART_TX GPIO_NUM_22 + +#define X_TRINAMIC_DRIVER 2209 +#define X_STEP_PIN GPIO_NUM_26 +#define X_DIRECTION_PIN GPIO_NUM_27 +#define X_RSENSE TMC2209_RSENSE_DEFAULT +#define X_DRIVER_ADDRESS 0 +#define DEFAULT_X_MICROSTEPS 16 + +#define Y_TRINAMIC_DRIVER 2209 +#define Y_STEP_PIN GPIO_NUM_33 +#define Y_DIRECTION_PIN GPIO_NUM_32 +#define Y_RSENSE TMC2209_RSENSE_DEFAULT +#define Y_DRIVER_ADDRESS 1 +#define DEFAULT_Y_MICROSTEPS 16 + +#define Z_TRINAMIC_DRIVER 2209 +#define Z_STEP_PIN GPIO_NUM_2 +#define Z_DIRECTION_PIN GPIO_NUM_4 +#define Z_RSENSE TMC2209_RSENSE_DEFAULT +#define Z_DRIVER_ADDRESS 2 +#define DEFAULT_Z_MICROSTEPS 16 + +#define A_TRINAMIC_DRIVER 2209 +#define A_STEP_PIN GPIO_NUM_16 +#define A_DIRECTION_PIN GPIO_NUM_17 +#define A_RSENSE TMC2209_RSENSE_DEFAULT +#define A_DRIVER_ADDRESS 3 +#define DEFAULT_A_MICROSTEPS 16 + +#define X_LIMIT_PIN GPIO_NUM_36 +#define Y_LIMIT_PIN GPIO_NUM_39 +#define Z_LIMIT_PIN GPIO_NUM_34 +#define PROBE_PIN GPIO_NUM_35 + +// OK to comment out to use pin for other features +#define STEPPERS_DISABLE_PIN GPIO_NUM_25 + + +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module +// https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module +#define USER_DIGITAL_PIN_0 GPIO_NUM_14 // M62 M63 +#define USER_DIGITAL_PIN_1 GPIO_NUM_13 // M62 M63 +#define USER_DIGITAL_PIN_2 GPIO_NUM_15 // M62 M63 +#define USER_DIGITAL_PIN_3 GPIO_NUM_12 // M62 M63 + + +// ===================== defaults ====================== +// https://github.com/bdring/Grbl_Esp32/wiki/Setting-Defaults + +#define DEFAULT_INVERT_PROBE_PIN 1 diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 26229dd2..16e658b8 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -258,7 +258,7 @@ namespace Motors { tmcstepper->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); tmcstepper->sfilt(1); tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 - tmcstepper->sgt(axis_settings[_axis_index]->stallguard->get()); + tmcstepper->sgt(constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); break; default: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TRINAMIC_MODE_UNDEFINED"); @@ -286,7 +286,7 @@ namespace Motors { tmcstepper->stallguard(), tmcstepper->sg_result(), feedrate, - axis_settings[_axis_index]->stallguard->get()); + constrain(axis_settings[_axis_index]->stallguard->get(), -64, 63)); TMC2130_n ::DRV_STATUS_t status { 0 }; // a useful struct to access the bits. status.sr = tmcstepper->DRV_STATUS(); diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h index 3fb6e7cd..ef489ae1 100644 --- a/Grbl_Esp32/src/Motors/TrinamicUartDriver.h +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriver.h @@ -98,7 +98,7 @@ namespace Motors { private: uint32_t calc_tstep(float speed, float percent); //TODO: see if this is useful/used. - TMC2208Stepper* tmcstepper; // all other driver types are subclasses of this one + TMC2209Stepper* tmcstepper; // all other driver types are subclasses of this one TrinamicUartMode _homing_mode; uint16_t _driver_part_number; // example: use 2209 for TMC2209 float _r_sense; diff --git a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp index e930a848..f2498505 100644 --- a/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicUartDriverClass.cpp @@ -7,6 +7,9 @@ 2020 - The Ant Team 2020 - Bart Dring + TMC2209 Datasheet + https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC2209_Datasheet_V103.pdf + Grbl 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 @@ -27,6 +30,8 @@ HardwareSerial tmc_serial(TMC_UART); namespace Motors { + TrinamicUartDriver* TrinamicUartDriver::List = NULL; // a static ist of all drivers for stallguard reporting + /* HW Serial Constructor. */ TrinamicUartDriver::TrinamicUartDriver( uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin, uint16_t driver_part_number, float r_sense, uint8_t addr) : @@ -40,13 +45,13 @@ namespace Motors { tmc_serial.begin(115200, SERIAL_8N1, TMC_UART_RX, TMC_UART_TX); tmc_serial.setRxBufferSize(128); hw_serial_init(); + + link = List; + List = this; } void TrinamicUartDriver::hw_serial_init() { - if (_driver_part_number == 2208) - // TMC 2208 does not use address, this field is 0 - tmcstepper = new TMC2208Stepper(&tmc_serial, _r_sense); - else if (_driver_part_number == 2209) + if (_driver_part_number == 2209) tmcstepper = new TMC2209Stepper(&tmc_serial, _r_sense, addr); else { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unsupported Trinamic motor p/n:%d", _driver_part_number); @@ -73,6 +78,20 @@ namespace Motors { read_settings(); set_mode(false); } + + // After initializing all of the TMC drivers, create a task to + // display StallGuard data. List == this for the final instance. + if (List == this) { + xTaskCreatePinnedToCore(readSgTask, // task + "readSgTask", // name for task + 4096, // size of task stack + NULL, // parameters + 1, // priority + NULL, + SUPPORT_TASK_CORE // must run the task on same core + // core + ); + } } /* @@ -192,38 +211,27 @@ namespace Motors { if (newMode == _mode) { return; } + _mode = newMode; switch (_mode) { case TrinamicUartMode ::StealthChop: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); - // tmcstepper->en_pwm_mode(true); //TODO: check if this is present in TMC2208/09 + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "StealthChop"); tmcstepper->en_spreadCycle(false); tmcstepper->pwm_autoscale(true); - // if (_driver_part_number == 2209) { - // tmcstepper->diag1_stall(false); //TODO: check the equivalent in TMC2209 - // } break; case TrinamicUartMode ::CoolStep: - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep"); // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09 tmcstepper->en_spreadCycle(true); tmcstepper->pwm_autoscale(false); - if (_driver_part_number == 2209) { - // tmcstepper->TCOOLTHRS(NORMAL_TCOOLTHRS); // when to turn on coolstep //TODO: add this solving compilation issue. - // tmcstepper->THIGH(NORMAL_THIGH); //TODO: this does not exist in TMC2208/09. verify and eventually remove. - } break; case TrinamicUartMode ::StallGuard: //TODO: check all configurations for stallguard - //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); - // tmcstepper->en_pwm_mode(false); //TODO: check if this is present in TMC2208/09 + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard"); tmcstepper->en_spreadCycle(false); tmcstepper->pwm_autoscale(false); - // tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); - // tmcstepper->THIGH(calc_tstep(homing_feed_rate->get(), 60.0)); - // tmcstepper->sfilt(1); - // tmcstepper->diag1_stall(true); // stallguard i/o is on diag1 - // tmcstepper->sgt(axis_settings[_axis_index]->stallguard->get()); + tmcstepper->TCOOLTHRS(calc_tstep(homing_feed_rate->get(), 150.0)); + tmcstepper->SGTHRS(constrain(axis_settings[_axis_index]->stallguard->get(),0,255)); break; default: grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Trinamic mode:d", _mode); @@ -246,10 +254,9 @@ namespace Motors { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, - "%s Stallguard %d SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", + "%s SG_Val: %04d Rate: %05.0f mm/min SG_Setting:%d", reportAxisNameMsg(_axis_index, _dual_axis_index), - 0, // tmcstepper->stallguard(), // TODO: add this again solving the compilation issues - 0, // tmcstepper->sg_result(), + tmcstepper->SG_RESULT(), // tmcstepper->sg_result(), feedrate, axis_settings[_axis_index]->stallguard->get()); @@ -261,13 +268,6 @@ namespace Motors { report_short_to_ground(status); report_over_temp(status); report_short_to_ps(status); - - // grbl_msg_sendf(CLIENT_SERIAL, - // MsgLevel::Info, - // "%s Status Register %08x GSTAT %02x", - // reportAxisNameMsg(_axis_index, _dual_axis_index), - // status.sr, - // tmcstepper->GSTAT()); } // calculate a tstep from a rate @@ -367,4 +367,31 @@ namespace Motors { return false; // no error } + // Prints StallGuard data that is useful for tuning. + void TrinamicUartDriver::readSgTask(void* pvParameters) { + TickType_t xLastWakeTime; + const TickType_t xreadSg = 200; // in ticks (typically ms) + auto n_axis = number_axis->get(); + + xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. + while (true) { // don't ever return from this or the task dies + if (stallguard_debug_mask->get() != 0) { + if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) { + for (TrinamicUartDriver* p = List; p; p = p->link) { + if (bitnum_istrue(stallguard_debug_mask->get(), p->_axis_index)) { + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get()); + p->debug_message(); + } + } + } // sys.state + } // if mask + + vTaskDelayUntil(&xLastWakeTime, xreadSg); + + static UBaseType_t uxHighWaterMark = 0; +#ifdef DEBUG_TASK_STACK + reportTaskStackSize(uxHighWaterMark); +#endif + } + } } \ No newline at end of file diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 02115c00..15072d3b 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -258,7 +258,7 @@ void make_settings() { for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { def = &axis_defaults[axis]; auto setting = - new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 63, postMotorSetting); + new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 255, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->stallguard = setting; } From becf38a4f20e6bec51f85762dcf2e5afeddf7999 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 29 Jan 2021 17:40:25 -0600 Subject: [PATCH 62/82] Web cmd modes (#754) * Update System.cpp * WebCommand with configurable modes * Added a few more ESP commands to work in anu state * Update Grbl.h Co-authored-by: Mitch Bradley --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/ProcessSettings.cpp | 13 ------------- Grbl_Esp32/src/Settings.cpp | 13 +++++++++++++ Grbl_Esp32/src/Settings.h | 23 +++++++++++++++++------ Grbl_Esp32/src/System.cpp | 4 ++++ Grbl_Esp32/src/WebUI/WebSettings.cpp | 10 +++++----- 6 files changed, 40 insertions(+), 25 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 3f921034..26b446e9 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 = "20210121"; +const char* const GRBL_VERSION_BUILD = "20210129"; //#include #include diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp index a04ca4df..05b765ec 100644 --- a/Grbl_Esp32/src/ProcessSettings.cpp +++ b/Grbl_Esp32/src/ProcessSettings.cpp @@ -421,19 +421,6 @@ Error motor_disable(const char* value, WebUI::AuthenticationLevel auth_level, We return Error::Ok; } -static bool anyState() { - return false; -} -static bool idleOrJog() { - return sys.state != State::Idle && sys.state != State::Jog; -} -bool idleOrAlarm() { - return sys.state != State::Idle && sys.state != State::Alarm; -} -static bool notCycleOrHold() { - return sys.state == State::Cycle && sys.state == State::Hold; -} - // Commands use the same syntax as Settings, but instead of setting or // displaying a persistent value, a command causes some action to occur. // That action could be anything, from displaying a run-time parameter diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp index 678a43c0..686b409d 100644 --- a/Grbl_Esp32/src/Settings.cpp +++ b/Grbl_Esp32/src/Settings.cpp @@ -3,6 +3,19 @@ #include #include +bool anyState() { + return false; +} +bool idleOrJog() { + return sys.state != State::Idle && sys.state != State::Jog; +} +bool idleOrAlarm() { + return sys.state != State::Idle && sys.state != State::Alarm; +} +bool notCycleOrHold() { + return sys.state == State::Cycle && sys.state == State::Hold; +} + Word::Word(type_t type, permissions_t permissions, const char* description, const char* grblName, const char* fullName) : _description(description), _grblName(grblName), _fullName(fullName), _type(type), _permissions(permissions) {} diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h index e206b307..26d99ad1 100644 --- a/Grbl_Esp32/src/Settings.h +++ b/Grbl_Esp32/src/Settings.h @@ -76,12 +76,13 @@ class Command : public Word { protected: Command* link; // linked list of setting objects bool (*_cmdChecker)(); + public: static Command* List; Command* next() { return link; } ~Command() {} - Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*cmcChecker)()); + Command(const char* description, type_t type, permissions_t permissions, const char* grblName, const char* fullName, bool (*cmdChecker)()); // The default implementation of addWebui() does nothing. // Derived classes may override it to do something. @@ -444,7 +445,10 @@ public: AxisSettings(const char* axisName); }; +extern bool idleOrJog(); extern bool idleOrAlarm(); +extern bool anyState(); +extern bool notCycleOrHold(); class WebCommand : public Command { private: @@ -457,12 +461,19 @@ public: permissions_t permissions, const char* grblName, const char* name, - Error (*action)(char*, WebUI::AuthenticationLevel)) : - // At some point we might want to be more subtle, but for now we block - // all web commands in Cycle and Hold states, to avoid crashing a - // running job. - Command(description, type, permissions, grblName, name, idleOrAlarm), + Error (*action)(char*, WebUI::AuthenticationLevel), + bool (*cmdChecker)()) : + Command(description, type, permissions, grblName, name, cmdChecker), _action(action) {} + + WebCommand(const char* description, + type_t type, + permissions_t permissions, + const char* grblName, + const char* name, + Error (*action)(char*, WebUI::AuthenticationLevel)) : + WebCommand(description, type, permissions, grblName, name, action, idleOrAlarm) {} + Error action(char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* response); }; diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp index 857ab4e3..60a399b2 100644 --- a/Grbl_Esp32/src/System.cpp +++ b/Grbl_Esp32/src/System.cpp @@ -47,18 +47,22 @@ void system_ini() { // Renamed from system_init() due to conflict with esp32 fi // setup control inputs #ifdef CONTROL_SAFETY_DOOR_PIN + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Door switch on pin %s", pinName(CONTROL_SAFETY_DOOR_PIN).c_str()); pinMode(CONTROL_SAFETY_DOOR_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(CONTROL_SAFETY_DOOR_PIN), isr_control_inputs, CHANGE); #endif #ifdef CONTROL_RESET_PIN + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset switch on pin %s", pinName(CONTROL_RESET_PIN).c_str()); pinMode(CONTROL_RESET_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(CONTROL_RESET_PIN), isr_control_inputs, CHANGE); #endif #ifdef CONTROL_FEED_HOLD_PIN + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Hold switch on pin %s", pinName(CONTROL_FEED_HOLD_PIN).c_str()); pinMode(CONTROL_FEED_HOLD_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(CONTROL_FEED_HOLD_PIN), isr_control_inputs, CHANGE); #endif #ifdef CONTROL_CYCLE_START_PIN + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Start switch on pin %s", pinName(CONTROL_CYCLE_START_PIN).c_str()); pinMode(CONTROL_CYCLE_START_PIN, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(CONTROL_CYCLE_START_PIN), isr_control_inputs, CHANGE); #endif diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 7058b31d..2a47176d 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -1025,7 +1025,7 @@ namespace WebUI { // WU - need user or admin password to set // WA - need admin password to set #ifdef WEB_COMMON - new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo); + new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo, anyState); new WebCommand(NULL, WEBCMD, WU, "ESP720", "LocalFS/Size", SPIFFSSize); new WebCommand("FORMAT", WEBCMD, WA, "ESP710", "LocalFS/Format", formatSpiffs); new WebCommand("path", WEBCMD, WU, "ESP701", "LocalFS/Show", showLocalFile); @@ -1043,14 +1043,14 @@ namespace WebUI { #endif #ifdef WEB_COMMON new WebCommand("RESTART", WEBCMD, WA, "ESP444", "System/Control", setSystemMode); - new WebCommand(NULL, WEBCMD, WU, "ESP420", "System/Stats", showSysStats); + new WebCommand(NULL, WEBCMD, WU, "ESP420", "System/Stats", showSysStats, anyState); #endif #ifdef ENABLE_WIFI new WebCommand(NULL, WEBCMD, WU, "ESP410", "WiFi/ListAPs", listAPs); #endif #ifdef WEB_COMMON new WebCommand("P=position T=type V=value", WEBCMD, WA, "ESP401", "WebUI/Set", setWebSetting); - new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings); + new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings, anyState); #endif #ifdef ENABLE_SD_CARD new WebCommand("path", WEBCMD, WU, "ESP221", "SD/Show", showSDFile); @@ -1067,8 +1067,8 @@ namespace WebUI { new WebCommand("IP=ipaddress MSK=netmask GW=gateway", WEBCMD, WA, "ESP103", "Sta/Setup", showSetStaParams); #endif #ifdef WEB_COMMON - new WebCommand(NULL, WEBCMD, WG, "ESP0", "WebUI/Help", showWebHelp); - new WebCommand(NULL, WEBCMD, WG, "ESP", "WebUI/Help", showWebHelp); + new WebCommand(NULL, WEBCMD, WG, "ESP0", "WebUI/Help", showWebHelp, anyState); + new WebCommand(NULL, WEBCMD, WG, "ESP", "WebUI/Help", showWebHelp, anyState); #endif // WebUI Settings // Standard WEBUI authentication is user+ to get, admin to set unless otherwise specified From 8ddec618cd6549295214b220d045f919942705e3 Mon Sep 17 00:00:00 2001 From: bdring Date: Wed, 3 Feb 2021 14:52:28 -0600 Subject: [PATCH 63/82] Updates from PWM_ISR_Fix branch (#755) - $Message/Level - ISR safe ledcWrite --- Grbl_Esp32/src/Config.h | 4 +--- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Report.cpp | 8 ++++++-- Grbl_Esp32/src/Report.h | 6 +++--- Grbl_Esp32/src/SettingsDefinitions.cpp | 27 ++++++++++++++++++++------ Grbl_Esp32/src/SettingsDefinitions.h | 2 ++ Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 11 ++++++++++- 7 files changed, 44 insertions(+), 16 deletions(-) diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index f22e88aa..2feece69 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -47,7 +47,7 @@ Some features should not be changed. See notes below. // that the machine file might choose to undefine. // Note: HOMING_CYCLES are now settings -#define SUPPORT_TASK_CORE 1 // Reference: CONFIG_ARDUINO_RUNNING_CORE = 1 +#define SUPPORT_TASK_CORE 1 // Reference: CONFIG_ARDUINO_RUNNING_CORE = 1 // Inverts pin logic of the control command pins based on a mask. This essentially means you can use // normally-closed switches on the specified pins, rather than the default normally-open switches. @@ -87,8 +87,6 @@ const int MAX_N_AXIS = 6; # define LIMIT_MASK B0 #endif -#define GRBL_MSG_LEVEL MsgLevel::Info // what level of [MSG:....] do you want to see 0=all off - // Serial baud rate // OK to change, but the ESP32 boot text is 115200, so you will not see that is your // serial monitor, sender, etc uses a different value than 115200 diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 26b446e9..f42b4b85 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 = "20210129"; +const char* const GRBL_VERSION_BUILD = "20210203"; //#include #include diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 67d4fddd..da8b5316 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -111,9 +111,13 @@ void grbl_msg_sendf(uint8_t client, MsgLevel level, const char* format, ...) { if (client == CLIENT_INPUT) { return; } - if (level > GRBL_MSG_LEVEL) { - return; + + if (message_level != NULL) { // might be null before messages are setup + if (level > static_cast(message_level->get())) { + return; + } } + char loc_buf[100]; char* temp = loc_buf; va_list arg; diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h index 5b128b5d..2ffcecee 100644 --- a/Grbl_Esp32/src/Report.h +++ b/Grbl_Esp32/src/Report.h @@ -52,8 +52,8 @@ enum class Message : uint8_t { #define CLIENT_ALL 0xFF #define CLIENT_COUNT 5 // total number of client types regardless if they are used -enum class MsgLevel : uint8_t { - None = 0, // set GRBL_MSG_LEVEL in config.h to the level you want to see +enum class MsgLevel : int8_t { // Use $Message/Level + None = 0, Error = 1, Warning = 2, Info = 3, @@ -128,4 +128,4 @@ char* reportAxisLimitsMsg(uint8_t axis); char* reportAxisNameMsg(uint8_t axis); char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis); -void reportTaskStackSize(UBaseType_t& saved); +void reportTaskStackSize(UBaseType_t& saved); diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 15072d3b..bde5e97f 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -57,6 +57,8 @@ IntSetting* spindle_pwm_bit_precision; EnumSetting* spindle_type; +EnumSetting* message_level; + enum_opt_t spindleTypes = { // clang-format off { "NONE", int8_t(SpindleType::NONE) }, @@ -71,6 +73,17 @@ enum_opt_t spindleTypes = { // clang-format on }; +enum_opt_t messageLevels = { + // clang-format off + { "None", int8_t(MsgLevel::None) }, + { "Error", int8_t(MsgLevel::Error) }, + { "Warning", int8_t(MsgLevel::Warning) }, + { "Info", int8_t(MsgLevel::Info) }, + { "Debug", int8_t(MsgLevel::Debug) }, + { "Verbose", int8_t(MsgLevel::Verbose) }, + // clang-format on +}; + AxisSettings* x_axis_settings; AxisSettings* y_axis_settings; AxisSettings* z_axis_settings; @@ -256,16 +269,16 @@ void make_settings() { b_axis_settings = axis_settings[B_AXIS]; c_axis_settings = axis_settings[C_AXIS]; for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { - def = &axis_defaults[axis]; - auto setting = - new IntSetting(EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 255, postMotorSetting); + def = &axis_defaults[axis]; + auto setting = new IntSetting( + EXTENDED, WG, makeGrblName(axis, 170), makename(def->name, "StallGuard"), def->stallguard, -64, 255, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->stallguard = setting; } for (axis = MAX_N_AXIS - 1; axis >= 0; axis--) { - def = &axis_defaults[axis]; - auto setting = - new IntSetting(EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postMotorSetting); + def = &axis_defaults[axis]; + auto setting = new IntSetting( + EXTENDED, WG, makeGrblName(axis, 160), makename(def->name, "Microsteps"), def->microsteps, 0, 256, postMotorSetting); setting->setAxis(axis); axis_settings[axis]->microsteps = setting; } @@ -395,4 +408,6 @@ void make_settings() { user_macro2 = new StringSetting(EXTENDED, WG, NULL, "User/Macro2", DEFAULT_USER_MACRO2); user_macro1 = new StringSetting(EXTENDED, WG, NULL, "User/Macro1", DEFAULT_USER_MACRO1); user_macro0 = new StringSetting(EXTENDED, WG, NULL, "User/Macro0", DEFAULT_USER_MACRO0); + + message_level = +new EnumSetting(NULL, EXTENDED, WG, NULL, "Message/Level", static_cast(MsgLevel::Info), &messageLevels, NULL); } diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 7315637a..95c229e9 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -65,3 +65,5 @@ extern StringSetting* user_macro0; extern StringSetting* user_macro1; extern StringSetting* user_macro2; extern StringSetting* user_macro3; + +extern EnumSetting* message_level; diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index 23477494..ba364768 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -20,6 +20,7 @@ */ #include "PWMSpindle.h" +#include "soc/ledc_struct.h" // ======================= PWM ============================== /* @@ -222,7 +223,15 @@ namespace Spindles { duty = (1 << _pwm_precision) - duty; } - ledcWrite(_pwm_chan_num, duty); + //ledcWrite(_pwm_chan_num, duty); + + // This was ledcWrite, but this is called from an ISR + // and ledcWrite uses RTOS features not compatible with ISRs + LEDC.channel_group[0].channel[0].duty.duty = duty << 4; + bool on = !!duty; + LEDC.channel_group[0].channel[0].conf0.sig_out_en = on; + LEDC.channel_group[0].channel[0].conf1.duty_start = on; + LEDC.channel_group[0].channel[0].conf0.clk_en = on; } void PWM::set_enable_pin(bool enable) { From 447d1739f3da9cd18004b7a92c9fc3441fba2147 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 5 Feb 2021 14:53:02 -0600 Subject: [PATCH 64/82] Core XY fixes (#756) * Updates for CoreXY * Delete fystec_ant.h --- Grbl_Esp32/Custom/CoreXY.cpp | 71 +++++++++++++++++++++++--------- Grbl_Esp32/src/Grbl.cpp | 7 ++-- Grbl_Esp32/src/Grbl.h | 7 ++-- Grbl_Esp32/src/Limits.cpp | 4 ++ Grbl_Esp32/src/MotionControl.cpp | 4 +- Grbl_Esp32/src/Report.cpp | 6 +-- 6 files changed, 67 insertions(+), 32 deletions(-) diff --git a/Grbl_Esp32/Custom/CoreXY.cpp b/Grbl_Esp32/Custom/CoreXY.cpp index 238a46df..b1c6fb87 100644 --- a/Grbl_Esp32/Custom/CoreXY.cpp +++ b/Grbl_Esp32/Custom/CoreXY.cpp @@ -69,7 +69,8 @@ bool user_defined_homing(uint8_t cycle_mask) { // check for multi axis homing per cycle ($Homing/Cycle0=XY type)...not allowed in CoreXY bool setting_error = false; - for (int cycle = 0; cycle < 3; cycle++) { + auto n_axis = number_axis->get(); + for (int cycle = 0; cycle < n_axis; cycle++) { if (numberOfSetBits(homing_cycle[cycle]->get()) > 1) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, @@ -90,7 +91,7 @@ bool user_defined_homing(uint8_t cycle_mask) { pl_data->motion.systemMotion = 1; pl_data->motion.noFeedOverride = 1; - uint8_t cycle_count = (cycle_mask == 0) ? 3 : 1; // if we have a cycle_mask, we are only going to do one axis + uint8_t cycle_count = (cycle_mask == 0) ? n_axis : 1; // if we have a cycle_mask, we are only going to do one axis AxisMask mask = 0; for (int cycle = 0; cycle < cycle_count; cycle++) { @@ -105,7 +106,7 @@ bool user_defined_homing(uint8_t cycle_mask) { mask = motors_set_homing_mode(mask, true); // non standard homing motors will do their own thing and get removed from the mask - for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) { + for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { if (bit(axis) == mask) { // setup for the homing of this axis bool approach = true; @@ -129,7 +130,9 @@ bool user_defined_homing(uint8_t cycle_mask) { approach ? target[axis] = max_travel : target[axis] = -max_travel; } - target[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + for (int axis = Z_AXIS; axis < n_axis; axis++) { + target[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + } // convert back to motor steps inverse_kinematics(target); @@ -217,7 +220,10 @@ bool user_defined_homing(uint8_t cycle_mask) { last_cartesian[X_AXIS] = target[X_AXIS]; last_cartesian[Y_AXIS] = target[Y_AXIS]; - last_cartesian[Z_AXIS] = system_convert_axis_steps_to_mpos(sys_position, Z_AXIS); + + for (int axis = Z_AXIS; axis < n_axis; axis++) { + last_cartesian[axis] = system_convert_axis_steps_to_mpos(sys_position, axis); + } // convert to motors inverse_kinematics(target); @@ -239,15 +245,15 @@ bool user_defined_homing(uint8_t cycle_mask) { // This function is used by Grbl convert Cartesian to motors // this does not do any motion control void inverse_kinematics(float* position) { - float motors[3]; + float motors[MAX_N_AXIS]; motors[X_AXIS] = geometry_factor * position[X_AXIS] + position[Y_AXIS]; motors[Y_AXIS] = geometry_factor * position[X_AXIS] - position[Y_AXIS]; - motors[Z_AXIS] = position[Z_AXIS]; - position[0] = motors[0]; - position[1] = motors[1]; - position[2] = motors[2]; + position[X_AXIS] = motors[X_AXIS]; + position[Y_AXIS] = motors[Y_AXIS]; + + // Z and higher just pass through unchanged } // Inverse Kinematics calculates motor positions from real world cartesian positions @@ -268,7 +274,11 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio motors[X_AXIS] = geometry_factor * target[X_AXIS] + target[Y_AXIS]; motors[Y_AXIS] = geometry_factor * target[X_AXIS] - target[Y_AXIS]; - motors[Z_AXIS] = target[Z_AXIS]; + + auto n_axis = number_axis->get(); + for (uint8_t axis = Z_AXIS; axis <= n_axis; axis++) { + motors[axis] = target[axis]; + } float motor_distance = three_axis_dist(motors, last_motors); @@ -283,14 +293,36 @@ void inverse_kinematics(float* target, plan_line_data_t* pl_data, float* positio // motors -> cartesian void forward_kinematics(float* position) { - float calc_fwd[MAX_N_AXIS]; + float calc_fwd[MAX_N_AXIS]; + float wco[MAX_N_AXIS]; + float print_position[N_AXIS]; + int32_t current_position[N_AXIS]; // Copy current state of the system position variable + memcpy(current_position, sys_position, sizeof(sys_position)); + system_convert_array_steps_to_mpos(print_position, current_position); + + // determine the Work Coordinate Offsets for each axis + auto n_axis = number_axis->get(); + for (int axis = 0; axis < n_axis; axis++) { + // Apply work coordinate offsets and tool length offset to current position. + wco[axis] = gc_state.coord_system[axis] + gc_state.coord_offset[axis]; + if (axis == TOOL_LENGTH_OFFSET_AXIS) { + wco[axis] += gc_state.tool_length_offset; + } + } + + // apply the forward kinemetics to the machine coordinates // https://corexy.com/theory.html - calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); - calc_fwd[Y_AXIS] = 0.5 * (position[X_AXIS] - position[Y_AXIS]); + //calc_fwd[X_AXIS] = 0.5 / geometry_factor * (position[X_AXIS] + position[Y_AXIS]); + calc_fwd[X_AXIS] = ((0.5 * (print_position[X_AXIS] + print_position[Y_AXIS]) * geometry_factor) - wco[X_AXIS]); + calc_fwd[Y_AXIS] = ((0.5 * (print_position[X_AXIS] - print_position[Y_AXIS])) - wco[Y_AXIS]); - position[X_AXIS] = calc_fwd[X_AXIS]; - position[Y_AXIS] = calc_fwd[Y_AXIS]; + for (int axis = 0; axis < n_axis; axis++) { + if (axis > Y_AXIS) { // for axes above Y there is no kinematics + calc_fwd[axis] = print_position[axis] - wco[axis]; + } + position[axis] = calc_fwd[axis]; // this is the value returned to reporting + } } bool kinematics_pre_homing(uint8_t cycle_mask) { @@ -298,9 +330,10 @@ bool kinematics_pre_homing(uint8_t cycle_mask) { } void kinematics_post_homing() { - gc_state.position[X_AXIS] = last_cartesian[X_AXIS]; - gc_state.position[Y_AXIS] = last_cartesian[Y_AXIS]; - gc_state.position[Z_AXIS] = last_cartesian[Z_AXIS]; + auto n_axis = number_axis->get(); + for (uint8_t axis = X_AXIS; axis <= n_axis; axis++) { + gc_state.position[axis] = last_cartesian[axis]; + } } // this is used used by Grbl soft limits to see if the range of the machine is exceeded. diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index 7a29bc4f..a338766f 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -42,10 +42,7 @@ void grbl_init() { init_motors(); system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) memset(sys_position, 0, sizeof(sys_position)); // Clear machine position. - -#ifdef USE_MACHINE_INIT - machine_init(); // user supplied function for special initialization -#endif + machine_init(); // weak definition in Grbl.cpp does nothing // Initialize system state. #ifdef FORCE_INITIALIZATION_ALARM // Force Grbl into an ALARM state upon a power-cycle or hard reset. @@ -117,6 +114,8 @@ void run_once() { protocol_main_loop(); } +void __attribute__((weak)) machine_init() {} + /* setup() and loop() in the Arduino .ino implements this control flow: diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index f42b4b85..0d35d07c 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 = "20210203"; +const char* const GRBL_VERSION_BUILD = "20210204"; //#include #include @@ -94,8 +94,7 @@ void run_once(); // Called if USE_MACHINE_INIT is defined void machine_init(); -// Called if USE_CUSTOM_HOMING is defined -bool user_defined_homing(uint8_t cycle_mask); +bool user_defined_homing(uint8_t cycle_mask); // weak definition in Limits.cpp // Called if USE_KINEMATICS is defined @@ -106,7 +105,7 @@ uint8_t kinematic_limits_check(float* target); // Called if USE_FWD_KINEMATICS is defined void inverse_kinematics(float* position); // used to return a converted value -void forward_kinematics(float* position); +void forward_kinematics(float* position); // weak definition in Report.cpp // Called if MACRO_BUTTON_0_PIN or MACRO_BUTTON_1_PIN or MACRO_BUTTON_2_PIN is defined void user_defined_macro(uint8_t index); diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp index 029e3477..41596e9d 100644 --- a/Grbl_Esp32/src/Limits.cpp +++ b/Grbl_Esp32/src/Limits.cpp @@ -404,3 +404,7 @@ bool limitsCheckTravel(float* target) { } return false; } + +bool __attribute__((weak)) user_defined_homing(uint8_t cycle_mask) { + return false; +} diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 954009a1..3387ef23 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -283,11 +283,11 @@ static bool axis_is_squared(uint8_t axis_mask) { // executing the homing cycle. This prevents incorrect buffered plans after homing. void mc_homing_cycle(uint8_t cycle_mask) { bool no_cycles_defined = true; -#ifdef USE_CUSTOM_HOMING + if (user_defined_homing(cycle_mask)) { return; } -#endif + // This give kinematics a chance to do something before normal homing // if it returns true, the homing is canceled. #ifdef USE_KINEMATICS diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index da8b5316..892230e8 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -659,13 +659,11 @@ void report_realtime_status(uint8_t client) { } } } + forward_kinematics(print_position); // a weak definition does nothing. Users can provide strong version // Report machine position if (bit_istrue(status_mask->get(), RtStatus::Position)) { strcat(status, "|MPos:"); } else { -#ifdef USE_FWD_KINEMATICS - forward_kinematics(print_position); -#endif strcat(status, "|WPos:"); } report_util_axis_values(print_position, temp); @@ -957,3 +955,5 @@ void reportTaskStackSize(UBaseType_t& saved) { } #endif } + +void __attribute__((weak)) forward_kinematics(float* position) {} // This version does nothing. Make your own to do something with it From 04304141c586244ce47bf3be26c7151169d25fba Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 13 Feb 2021 18:22:09 -0600 Subject: [PATCH 65/82] Parking delay fix (#770) * Changed delay type - mc_dwell was causing a recursive loop the overflowed the stack - See https://discord.com/channels/780079161460916227/786061602754396160/809288050387189782 * Changed spindle delays from floats to ints in spindle classes - Used local copies, because I did not want to change/break the existing setting. * Cleaning up parking - Added a coolant delay setting - Made an enum class for the dwell types - Got rid of the safety door sepcific delays * Update Grbl.h --- Grbl_Esp32/src/Config.h | 9 ++------- Grbl_Esp32/src/Defaults.h | 4 ++++ Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/MotionControl.cpp | 4 ++-- Grbl_Esp32/src/NutsBolts.cpp | 6 +++--- Grbl_Esp32/src/NutsBolts.h | 9 ++++++--- Grbl_Esp32/src/Protocol.cpp | 7 ++++--- Grbl_Esp32/src/SettingsDefinitions.cpp | 9 +++++++-- Grbl_Esp32/src/SettingsDefinitions.h | 1 + Grbl_Esp32/src/Spindles/PWMSpindle.cpp | 7 +++++-- Grbl_Esp32/src/Spindles/Spindle.h | 2 ++ Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 17 +++++++++++------ 12 files changed, 48 insertions(+), 29 deletions(-) diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index 2feece69..f53ca288 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -258,11 +258,6 @@ static const uint8_t NHomingLocateCycle = 1; // Integer (1-128) // previous tool path, as if nothing happened. #define ENABLE_SAFETY_DOOR_INPUT_PIN // ESP32 Leave this enabled for now .. code for undefined not ready -// After the safety door switch has been toggled and restored, this setting sets the power-up delay -// between restoring the spindle and coolant and resuming the cycle. -const double SAFETY_DOOR_SPINDLE_DELAY = 4.0; // Float (seconds) -const double SAFETY_DOOR_COOLANT_DELAY = 1.0; // Float (seconds) - // Inverts select limit pin states based on the following mask. This effects all limit pin functions, // such as hard limits and homing. However, this is different from overall invert limits setting. // This build option will invert only the limit pins defined here, and then the invert limits setting @@ -582,8 +577,8 @@ const int DEBOUNCE_PERIOD = 32; // in milliseconds default 32 microseconds // Configure options for the parking motion, if enabled. #define PARKING_AXIS Z_AXIS // Define which axis that performs the parking motion const double PARKING_TARGET = -5.0; // Parking axis target. In mm, as machine coordinate. -const double PARKING_RATE = 500.0; // Parking fast rate after pull-out in mm/min. -const double PARKING_PULLOUT_RATE = 100.0; // Pull-out/plunge slow feed rate in mm/min. +const double PARKING_RATE = 800.0; // Parking fast rate after pull-out in mm/min. +const double PARKING_PULLOUT_RATE = 250.0; // Pull-out/plunge slow feed rate in mm/min. const double PARKING_PULLOUT_INCREMENT = 5.0; // Spindle pull-out and plunge distance in mm. Incremental distance. // Must be positive value or equal to zero. diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 41776f56..b31197de 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -187,6 +187,10 @@ # define DEFAULT_SPINDLE_DELAY_SPINUP 0 #endif +#ifndef DEFAULT_COOLANT_DELAY_TURNON +# define DEFAULT_COOLANT_DELAY_TURNON 1.0 +#endif + #ifndef DEFAULT_SPINDLE_DELAY_SPINDOWN # define DEFAULT_SPINDLE_DELAY_SPINDOWN 0 #endif diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 0d35d07c..82a85a84 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 = "20210204"; +const char* const GRBL_VERSION_BUILD = "20210213"; //#include #include diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 3387ef23..aa02fec1 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -225,11 +225,11 @@ void mc_arc(float* target, // Execute dwell in seconds. void mc_dwell(float seconds) { - if (sys.state == State::CheckMode) { + if (seconds == 0 || sys.state == State::CheckMode) { return; } protocol_buffer_synchronize(); - delay_sec(seconds, DELAY_MODE_DWELL); + delay_sec(seconds, DwellMode::Dwell); } // return true if the mask has exactly one bit set, diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 036be861..51245465 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -112,15 +112,15 @@ void delay_ms(uint16_t ms) { } // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, uint8_t mode) { +void delay_sec(float seconds, DwellMode mode) { uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds); while (i-- > 0) { if (sys.abort) { return; } - if (mode == DELAY_MODE_DWELL) { + if (mode == DwellMode::Dwell) { protocol_execute_realtime(); - } else { // DELAY_MODE_SYS_SUSPEND + } else { // DwellMode::SysSuspend // Execute rt_system() only to avoid nesting suspend loops. protocol_exec_rt_system(); if (sys.suspend.bit.restartRetract) { diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index d133638f..15705065 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -25,6 +25,11 @@ // #define false 0 // #define true 1 +enum class DwellMode : uint8_t { + Dwell = 0, // (Default: Must be zero) + SysSuspend = 1, //G92.1 (Do not alter value) +}; + const double SOME_LARGE_VALUE = 1.0E+38; // Axis array index values. Must start with 0 and be continuous. @@ -57,8 +62,6 @@ static inline int toMotor2(int axis) { const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); -const int DELAY_MODE_DWELL = 0; -const int DELAY_MODE_SYS_SUSPEND = 1; // Useful macros #define clear_vector(a) memset(a, 0, sizeof(a)) @@ -87,7 +90,7 @@ const int DELAY_MODE_SYS_SUSPEND = 1; uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr); // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, uint8_t mode); +void delay_sec(float seconds, DwellMode mode); // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms(). void delay_ms(uint16_t ms); diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index bfda25f5..126015e8 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -664,9 +664,10 @@ static void protocol_exec_rt_suspend() { if (spindle->inLaserMode()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. sys.step_control.updateSpindleRpm = true; - } else { + } else { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); - delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND); + // restore delay is done in the spindle class + //delay_sec(spindle_delay_spinup->get(), DwellMode::SysSuspend); } } } @@ -675,7 +676,7 @@ static void protocol_exec_rt_suspend() { if (!sys.suspend.bit.restartRetract) { // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. coolant_set_state(restore_coolant); - delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); + delay_sec(coolant_start_delay->get(), DwellMode::SysSuspend); } } #ifdef PARKING_ENABLE diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index bde5e97f..1808e94f 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -46,6 +46,7 @@ FloatSetting* rpm_max; FloatSetting* rpm_min; FloatSetting* spindle_delay_spinup; FloatSetting* spindle_delay_spindown; +FloatSetting* coolant_start_delay; FlagSetting* spindle_enbl_off_with_zero_speed; FlagSetting* spindle_enable_invert; FlagSetting* spindle_output_invert; @@ -345,8 +346,12 @@ void make_settings() { spindle_pwm_freq = new FloatSetting(EXTENDED, WG, "33", "Spindle/PWM/Frequency", DEFAULT_SPINDLE_FREQ, 0, 100000, checkSpindleChange); spindle_output_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/PWM/Invert", DEFAULT_INVERT_SPINDLE_OUTPUT_PIN, checkSpindleChange); - spindle_delay_spinup = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); - spindle_delay_spindown = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30); + spindle_delay_spinup = + new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange); + spindle_delay_spindown = + new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange); + coolant_start_delay = + new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30); spindle_enbl_off_with_zero_speed = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 95c229e9..0fe91de1 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -48,6 +48,7 @@ extern FloatSetting* rpm_max; extern FloatSetting* rpm_min; extern FloatSetting* spindle_delay_spinup; extern FloatSetting* spindle_delay_spindown; +extern FloatSetting* coolant_start_delay; extern FlagSetting* spindle_enbl_off_with_zero_speed; extern FlagSetting* spindle_enable_invert; extern FlagSetting* spindle_output_invert; diff --git a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp index ba364768..402af6e4 100644 --- a/Grbl_Esp32/src/Spindles/PWMSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/PWMSpindle.cpp @@ -114,6 +114,9 @@ namespace Spindles { // _pwm_gradient = (_pwm_max_value - _pwm_min_value) / (_max_rpm - _min_rpm); _pwm_chan_num = 0; // Channel 0 is reserved for spindle use + + _spinup_delay = spindle_delay_spinup->get() * 1000.0; + _spindown_delay = spindle_delay_spindown->get() * 1000.0; } uint32_t PWM::set_rpm(uint32_t rpm) { @@ -163,14 +166,14 @@ namespace Spindles { sys.spindle_speed = 0; stop(); if (use_delays && (_current_state != state)) { - mc_dwell(spindle_delay_spindown->get()); + delay(_spinup_delay); } } else { set_dir_pin(state == SpindleState::Cw); set_rpm(rpm); set_enable_pin(state != SpindleState::Disable); // must be done after setting rpm for enable features to work if (use_delays && (_current_state != state)) { - mc_dwell(spindle_delay_spinup->get()); + delay(_spindown_delay); } } diff --git a/Grbl_Esp32/src/Spindles/Spindle.h b/Grbl_Esp32/src/Spindles/Spindle.h index 6851544a..2f507767 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.h +++ b/Grbl_Esp32/src/Spindles/Spindle.h @@ -73,6 +73,8 @@ namespace Spindles { bool is_reversable; bool use_delays; // will SpinUp and SpinDown delays be used. volatile SpindleState _current_state = SpindleState::Disable; + uint32_t _spinup_delay; + uint32_t _spindown_delay; static void select(); }; diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 512e243a..76e30c80 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -326,6 +326,9 @@ namespace Spindles { _min_rpm = rpm_min->get(); _max_rpm = rpm_max->get(); + _spinup_delay = spindle_delay_spinup->get() * 1000.0; + _spindown_delay = spindle_delay_spindown->get() * 1000.0; + return pins_settings_ok; } @@ -348,16 +351,18 @@ namespace Spindles { if (_current_state != state) { // already at the desired state. This function gets called a lot. set_mode(state, critical); // critical if we are in a job set_rpm(rpm); + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin1"); + if (state == SpindleState::Disable) { sys.spindle_speed = 0; - if (_current_state != state) { - mc_dwell(spindle_delay_spindown->get()); - } + delay(_spindown_delay); + } else { - if (_current_state != state) { - mc_dwell(spindle_delay_spinup->get()); - } + delay(_spinup_delay); } + + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin2"); } else { if (_current_rpm != rpm) { set_rpm(rpm); From 6d5cbde67a8596e115b2c38b3da8b606746f69e8 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 15 Feb 2021 07:43:05 -0600 Subject: [PATCH 66/82] Enable per motor fix (#771) * - moved invert option in front of per motor enables. * Added code to prevent motors_set_disable() from setting values that already exist. * Added the enable delay from PR 720 * Adding a defined default for step enable delay * Fixing feed rates with kinematics and arcs. - Kinematics changes the feed rate and the loop that creates arc was re-using the altered rate. This caused a runaway situation. --- Grbl_Esp32/src/Defaults.h | 4 ++++ Grbl_Esp32/src/MotionControl.cpp | 6 ++++-- Grbl_Esp32/src/Motors/Motors.cpp | 30 +++++++++++++++++++++----- Grbl_Esp32/src/SettingsDefinitions.cpp | 2 ++ Grbl_Esp32/src/SettingsDefinitions.h | 1 + 5 files changed, 36 insertions(+), 7 deletions(-) diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index b31197de..91c9e396 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -42,6 +42,10 @@ # define DEFAULT_STEP_PULSE_MICROSECONDS 3 // $0 #endif +#ifndef DEFAULT_STEP_ENABLE_DELAY +# define DEFAULT_STEP_ENABLE_DELAY 0 +#endif + #ifndef DEFAULT_STEPPER_IDLE_LOCK_TIME # define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // $1 msec (0-254, 255 keeps steppers enabled) #endif diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index aa02fec1..5ea64cb2 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -184,8 +184,9 @@ void mc_arc(float* target, float cos_Ti; float r_axisi; uint16_t i; - uint8_t count = 0; - for (i = 1; i < segments; i++) { // Increment (segments-1). + uint8_t count = 0; + float original_feedrate = pl_data->feed_rate; // Kinematics may alter the feedrate, so save an original copy + for (i = 1; i < segments; i++) { // Increment (segments-1). if (count < N_ARC_CORRECTION) { // Apply vector rotation matrix. ~40 usec r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; @@ -206,6 +207,7 @@ void mc_arc(float* target, position[axis_1] = center_axis1 + r_axis1; position[axis_linear] += linear_per_segment; #ifdef USE_KINEMATICS + pl_data->feed_rate = original_feedrate; // This restores the feedrate kinematics may have altered mc_line_kins(position, pl_data, previous_position); previous_position[axis_0] = position[axis_0]; previous_position[axis_1] = position[axis_1]; diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index fe7a9584..bedfcd0a 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -413,7 +413,19 @@ void init_motors() { } void motors_set_disable(bool disable, uint8_t mask) { - static bool previous_state = true; + static bool prev_disable = true; + static uint8_t prev_mask = 0; + + if ((disable == prev_disable) && (mask == prev_mask)) { + return; + } + + prev_disable = disable; + prev_mask = mask; + + if (step_enable_invert->get()) { + disable = !disable; // Apply pin invert. + } // now loop through all the motors to see if they can individually disable auto n_axis = number_axis->get(); @@ -425,11 +437,19 @@ void motors_set_disable(bool disable, uint8_t mask) { } } - // invert only inverts the global stepper disable pin. - if (step_enable_invert->get()) { - disable = !disable; // Apply pin invert. - } + // global disable. digitalWrite(STEPPERS_DISABLE_PIN, disable); + + // Add an optional delay for stepper drivers. that need time + // Some need time after the enable before they can step. + auto wait_disable_change = enable_delay_microseconds->get(); + if (wait_disable_change != 0) { + auto disable_start_time = esp_timer_get_time() + wait_disable_change; + + while ((esp_timer_get_time() - disable_start_time) < 0) { + NOP(); + } + } } void motors_read_settings() { diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 1808e94f..8d0aecc2 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -10,6 +10,7 @@ StringSetting* build_info; IntSetting* pulse_microseconds; IntSetting* stepper_idle_lock_time; +IntSetting* enable_delay_microseconds; AxisMaskSetting* step_invert_mask; AxisMaskSetting* dir_invert_mask; @@ -399,6 +400,7 @@ void make_settings() { step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting); stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); + enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postMotorSetting); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 0fe91de1..84c62367 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -18,6 +18,7 @@ extern StringSetting* build_info; extern IntSetting* pulse_microseconds; extern IntSetting* stepper_idle_lock_time; +extern IntSetting* enable_delay_microseconds; extern AxisMaskSetting* step_invert_mask; extern AxisMaskSetting* dir_invert_mask; From 67f8e7aab42913401ba114442ef5ceb396af7ebf Mon Sep 17 00:00:00 2001 From: Mitch Bradley Date: Thu, 18 Feb 2021 15:42:44 -1000 Subject: [PATCH 67/82] SD Upload fix by luc (#779) --- Grbl_Esp32/src/Grbl.h | 4 ++-- Grbl_Esp32/src/WebUI/WebServer.cpp | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 46614405..c32b5f50 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -21,8 +21,8 @@ */ // Grbl versioning system - -const char* const GRBL_VERSION_BUILD = "20210213"; +const char* const GRBL_VERSION = "1.3a"; +const char* const GRBL_VERSION_BUILD = "20210218"; //#include #include diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 97b2ca34..311c980a 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -280,7 +280,14 @@ namespace WebUI { if ((path.substring(0, 4) == "/SD/")) { //remove /SD path = path.substring(3); + if (SDState::Idle != get_sd_state(true)) { + String content = "cannot open: "; + content += path + ", SD is not available."; + + _webserver->send(500, "text/plain", content); + } if (SD.exists(pathWithGz) || SD.exists(path)) { + set_sd_state(SDState::BusyUploading); if (SD.exists(pathWithGz)) { path = pathWithGz; } @@ -311,8 +318,10 @@ namespace WebUI { if (i != totalFileSize) { //error: TBD } + set_sd_state(SDState::Idle); return; } + set_sd_state(SDState::Idle); } String content = "cannot find "; content += path; From 5bc3cf819b27b892c46a1394ebbb1798b2feee8e Mon Sep 17 00:00:00 2001 From: Florian Ragwitz Date: Fri, 19 Feb 2021 09:12:21 -0800 Subject: [PATCH 68/82] Configure motors after I/O pins (#742) So machine definitions can change the SPI pins before we talk to any Trinamic drivers. --- Grbl_Esp32/src/Grbl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index a338766f..be3d311d 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -39,8 +39,8 @@ void grbl_init() { #endif settings_init(); // Load Grbl settings from non-volatile storage stepper_init(); // Configure stepper pins and interrupt timers - init_motors(); system_ini(); // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files) + init_motors(); memset(sys_position, 0, sizeof(sys_position)); // Clear machine position. machine_init(); // weak definition in Grbl.cpp does nothing // Initialize system state. From 32926abbcc0fd4e4b89dd69f400a43103bdb29ca Mon Sep 17 00:00:00 2001 From: Luc <8822552+luc-github@users.noreply.github.com> Date: Fri, 26 Feb 2021 15:30:14 +0100 Subject: [PATCH 69/82] 1.0.5 compilation fixes (#782) * Fix compilations error due to new enum in esp32 core release 1.0.5 * Update Grbl.h Co-authored-by: Luc --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/WebUI/WebSettings.cpp | 4 ++-- Grbl_Esp32/src/WebUI/WifiConfig.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index c32b5f50..23ae52b5 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210218"; +const char* const GRBL_VERSION_BUILD = "20210226"; //#include #include diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 2a47176d..84e2ec18 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -466,7 +466,7 @@ namespace WebUI { webPrintln("Signal: ", String(wifi_config.getSignal(WiFi.RSSI())) + "%"); uint8_t PhyMode; - esp_wifi_get_protocol(ESP_IF_WIFI_STA, &PhyMode); + esp_wifi_get_protocol(WIFI_IF_STA, &PhyMode); const char* modeName; switch (PhyMode) { case WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N: @@ -501,7 +501,7 @@ namespace WebUI { print_mac("AP", WiFi.softAPmacAddress()); wifi_config_t conf; - esp_wifi_get_config(ESP_IF_WIFI_AP, &conf); + esp_wifi_get_config(WIFI_IF_AP, &conf); webPrintln("SSID: ", (const char*)conf.ap.ssid); webPrintln("Visible: ", (conf.ap.ssid_hidden == 0) ? "Yes" : "No"); diff --git a/Grbl_Esp32/src/WebUI/WifiConfig.cpp b/Grbl_Esp32/src/WebUI/WifiConfig.cpp index e42c0c05..f3ca256c 100644 --- a/Grbl_Esp32/src/WebUI/WifiConfig.cpp +++ b/Grbl_Esp32/src/WebUI/WifiConfig.cpp @@ -71,7 +71,7 @@ namespace WebUI { } result += "Mode=AP:SSDI="; wifi_config_t conf; - esp_wifi_get_config(ESP_IF_WIFI_AP, &conf); + esp_wifi_get_config(WIFI_IF_AP, &conf); result += (const char*)conf.ap.ssid; result += ":IP="; result += WiFi.softAPIP().toString(); From 62193c39880ee1e2ff5c168c82fe4d55f15e48f2 Mon Sep 17 00:00:00 2001 From: Stefan de Bruijn Date: Sun, 28 Feb 2021 21:17:02 +0100 Subject: [PATCH 70/82] Introduced delays for enable and direction pins (#720) * Added some timing changes to steppers. Basically this should introduce a small (configurable) delay for enable and direction pins. * Updates after testing * Fixed small casting bug * Fixed subtractions as per @Mitch's comments * Added STEP_PULSE_DELAY handling for compatibility with RMT_STEPS. Functionality should be unchanged. * Updates to help merge Co-authored-by: Stefan de Bruijn Co-authored-by: bdring --- .gitignore | 1 + Grbl_Esp32/src/Config.h | 12 +----- Grbl_Esp32/src/Defaults.h | 6 ++- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Motors/Motors.cpp | 14 ++++++- Grbl_Esp32/src/Motors/Motors.h | 3 +- Grbl_Esp32/src/Motors/StandardStepper.cpp | 19 ++++----- Grbl_Esp32/src/Protocol.cpp | 2 +- Grbl_Esp32/src/SettingsDefinitions.cpp | 4 ++ Grbl_Esp32/src/SettingsDefinitions.h | 1 + Grbl_Esp32/src/Spindles/Spindle.cpp | 2 +- Grbl_Esp32/src/Stepper.cpp | 50 +++++++++++++++++++---- 12 files changed, 78 insertions(+), 38 deletions(-) diff --git a/.gitignore b/.gitignore index dec931d0..44cc0076 100644 --- a/.gitignore +++ b/.gitignore @@ -20,3 +20,4 @@ __vm/ *.vcxproj.filters *.suo Grbl_Esp32.ino.cpp +/packages diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h index f53ca288..ffba4148 100644 --- a/Grbl_Esp32/src/Config.h +++ b/Grbl_Esp32/src/Config.h @@ -446,17 +446,7 @@ const int DWELL_TIME_STEP = 50; // Integer (1-255) (milliseconds) // While this is experimental, it is intended to be the future default method after testing //#define USE_RMT_STEPS -// Creates a delay between the direction pin setting and corresponding step pulse by creating -// another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare) -// sets the direction pins, and does not immediately set the stepper pins, as it would in -// normal operation. The Timer2 compare fires next to set the stepper pins after the step -// pulse delay time, and Timer2 overflow will complete the step pulse, except now delayed -// by the step pulse time plus the step pulse delay. (Thanks langwadt for the idea!) -// NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the -// user-supplied step pulse time, the total time must not exceed 127us. Reported successful -// values for certain setups have ranged from 5 to 20us. -// must use #define USE_RMT_STEPS for this to work -//#define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. +// STEP_PULSE_DELAY is now a setting...$Stepper/Direction/Delay // The number of linear motions in the planner buffer to be planned at any give time. The vast // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra diff --git a/Grbl_Esp32/src/Defaults.h b/Grbl_Esp32/src/Defaults.h index 91c9e396..9c5ac586 100644 --- a/Grbl_Esp32/src/Defaults.h +++ b/Grbl_Esp32/src/Defaults.h @@ -46,6 +46,10 @@ # define DEFAULT_STEP_ENABLE_DELAY 0 #endif +#ifndef STEP_PULSE_DELAY +# define STEP_PULSE_DELAY 0 +#endif + #ifndef DEFAULT_STEPPER_IDLE_LOCK_TIME # define DEFAULT_STEPPER_IDLE_LOCK_TIME 250 // $1 msec (0-254, 255 keeps steppers enabled) #endif @@ -674,4 +678,4 @@ #ifndef DEFAULT_USER_MACRO3 # define DEFAULT_USER_MACRO3 "" -#endif \ No newline at end of file +#endif diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 23ae52b5..b6d0c438 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210226"; +const char* const GRBL_VERSION_BUILD = "20210228"; //#include #include diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index bedfcd0a..4cb5289a 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -478,7 +478,7 @@ uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) { return can_home; } -void motors_step(uint8_t step_mask, uint8_t dir_mask) { +bool motors_direction(uint8_t dir_mask) { auto n_axis = number_axis->get(); //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask); @@ -493,7 +493,17 @@ void motors_step(uint8_t step_mask, uint8_t dir_mask) { myMotor[axis][0]->set_direction(thisDir); myMotor[axis][1]->set_direction(thisDir); } + + return true; + } else { + return false; } +} + +void motors_step(uint8_t step_mask) { + auto n_axis = number_axis->get(); + //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask); + // Turn on step pulses for motors that are supposed to step now for (uint8_t axis = X_AXIS; axis < n_axis; axis++) { if (bitnum_istrue(step_mask, axis)) { @@ -513,4 +523,4 @@ void motors_unstep() { myMotor[axis][0]->unstep(); myMotor[axis][1]->unstep(); } -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h index 14550f68..d29fa3c4 100644 --- a/Grbl_Esp32/src/Motors/Motors.h +++ b/Grbl_Esp32/src/Motors/Motors.h @@ -40,7 +40,8 @@ void motors_read_settings(); // The return value is a bitmask of axes that can home uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming); void motors_set_disable(bool disable, uint8_t mask = B11111111); // default is all axes -void motors_step(uint8_t step_mask, uint8_t dir_mask); +bool motors_direction(uint8_t dir_mask); +void motors_step(uint8_t step_mask); void motors_unstep(); void servoUpdateTask(void* pvParameters); diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index 8e782ba4..68803a46 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -40,17 +40,14 @@ namespace Motors { } StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) : - Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) { - } + Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) {} void StandardStepper::init() { read_settings(); config_message(); } - void StandardStepper::read_settings() { - init_step_dir_pins(); - } + void StandardStepper::read_settings() { init_step_dir_pins(); } void StandardStepper::init_step_dir_pins() { _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index); @@ -68,11 +65,8 @@ namespace Motors { rmtConfig.tx_config.carrier_level = RMT_CARRIER_LEVEL_LOW; rmtConfig.tx_config.idle_output_en = true; -# ifdef STEP_PULSE_DELAY - rmtItem[0].duration0 = STEP_PULSE_DELAY * 4; -# else - rmtItem[0].duration0 = 1; -# endif + auto stepPulseDelay = direction_delay_microseconds->get(); + rmtItem[0].duration0 = stepPulseDelay < 1 ? 1 : stepPulseDelay * 4; rmtItem[0].duration1 = 4 * pulse_microseconds->get(); rmtItem[1].duration0 = 0; @@ -126,5 +120,8 @@ namespace Motors { void StandardStepper::set_direction(bool dir) { digitalWrite(_dir_pin, dir ^ _invert_dir_pin); } - void StandardStepper::set_disable(bool disable) { digitalWrite(_disable_pin, disable); } + void StandardStepper::set_disable(bool disable) { + disable ^= step_enable_invert->get(); + digitalWrite(_disable_pin, disable); + } } diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 126015e8..111e6229 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -193,7 +193,7 @@ void protocol_main_loop() { return; // Bail to main() program loop to reset system. } // check to see if we should disable the stepper drivers ... esp32 work around for disable in main loop. - if (stepper_idle) { + if (stepper_idle && stepper_idle_lock_time->get() != 0xff) { if (esp_timer_get_time() > stepper_idle_counter) { motors_set_disable(true); } diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 8d0aecc2..24e7b1c2 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -10,6 +10,7 @@ StringSetting* build_info; IntSetting* pulse_microseconds; IntSetting* stepper_idle_lock_time; +IntSetting* direction_delay_microseconds; IntSetting* enable_delay_microseconds; AxisMaskSetting* step_invert_mask; @@ -402,6 +403,9 @@ void make_settings() { pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds + direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", STEP_PULSE_DELAY, 0, 1000); + enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds + stallguard_debug_mask = new AxisMaskSetting(EXTENDED, WG, NULL, "Report/StallGuard", 0, postMotorSetting); homing_cycle[5] = new AxisMaskSetting(EXTENDED, WG, NULL, "Homing/Cycle5", DEFAULT_HOMING_CYCLE_5); diff --git a/Grbl_Esp32/src/SettingsDefinitions.h b/Grbl_Esp32/src/SettingsDefinitions.h index 84c62367..b079786e 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.h +++ b/Grbl_Esp32/src/SettingsDefinitions.h @@ -18,6 +18,7 @@ extern StringSetting* build_info; extern IntSetting* pulse_microseconds; extern IntSetting* stepper_idle_lock_time; +extern IntSetting* direction_delay_microseconds; extern IntSetting* enable_delay_microseconds; extern AxisMaskSetting* step_invert_mask; diff --git a/Grbl_Esp32/src/Spindles/Spindle.cpp b/Grbl_Esp32/src/Spindles/Spindle.cpp index f705aea7..b9cfed56 100644 --- a/Grbl_Esp32/src/Spindles/Spindle.cpp +++ b/Grbl_Esp32/src/Spindles/Spindle.cpp @@ -104,4 +104,4 @@ namespace Spindles { void Spindle::deinit() { stop(); } } -Spindles::Spindle* spindle; + Spindles::Spindle* spindle; diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index d4919614..1fa4adf1 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -58,10 +58,7 @@ typedef struct { uint32_t counter[MAX_N_AXIS]; // Counter variables for the bresenham line tracer -#ifdef STEP_PULSE_DELAY - uint8_t step_bits; // Stores out_bits output to complete the step pulse delay -#endif - + uint8_t step_bits; // Stores out_bits output to complete the step pulse delay uint8_t execute_step; // Flags step execution for each interrupt. uint8_t step_pulse_time; // Step pulse reset time after step rise uint8_t step_outbits; // The next stepping-bits to be output @@ -182,7 +179,7 @@ stepper_id_t current_stepper = DEFAULT_STEPPER; The complete step timing should look this... Direction pin is set - An optional (via STEP_PULSE_DELAY in config.h) is put after this + An optional delay (direction_delay_microseconds) is put after this The step pin is started A pulse length is determine (via option $0 ... pulse_microseconds) The pulse is ended @@ -196,8 +193,8 @@ static void stepper_pulse_func(); // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller // int8 variables and update position counters only when a segment completes. This can get complicated // with probing and homing cycles that require true real-time positions. -void IRAM_ATTR onStepperDriverTimer( - void* para) { // ISR It is time to take a step ======================================================================================= +void IRAM_ATTR onStepperDriverTimer(void* para) { + // ISR It is time to take a step ======================================================================================= //const int timer_idx = (int)para; // get the timer index TIMERG0.int_clr_timers.t0 = 1; if (busy) { @@ -221,14 +218,42 @@ void IRAM_ATTR onStepperDriverTimer( static void stepper_pulse_func() { auto n_axis = number_axis->get(); - motors_step(st.step_outbits, st.dir_outbits); + if (motors_direction(st.dir_outbits)) { + auto wait_direction = direction_delay_microseconds->get(); + if (wait_direction > 0) { + // Stepper drivers need some time between changing direction and doing a pulse. + switch (current_stepper) { + case ST_I2S_STREAM: + i2s_out_push_sample(wait_direction); + break; + case ST_I2S_STATIC: + case ST_TIMED: { + // wait for step pulse time to complete...some time expired during code above + // + // If we are using GPIO stepping as opposed to RMT, record the + // time that we turned on the direction pins so we can delay a bit. + // If we are using RMT, we can't delay here. + auto direction_pulse_start_time = esp_timer_get_time() + wait_direction; + while ((esp_timer_get_time() - direction_pulse_start_time) < 0) { + NOP(); // spin here until time to turn off step + } + break; + } + case ST_RMT: + break; + } + } + } // If we are using GPIO stepping as opposed to RMT, record the // time that we turned on the step pins so we can turn them off // at the end of this routine without incurring another interrupt. // This is unnecessary with RMT and I2S stepping since both of // those methods time the turn off automatically. + // + // NOTE: We could use direction_pulse_start_time + wait_direction, but let's play it safe uint64_t step_pulse_start_time = esp_timer_get_time(); + motors_step(st.step_outbits); // If there is no step segment, attempt to pop one from the stepper buffer if (st.exec_segment == NULL) { @@ -359,13 +384,20 @@ void st_wake_up() { // Enable stepper drivers. motors_set_disable(false); stepper_idle = false; + // Initialize step pulse timing from settings. Here to ensure updating after re-writing. -#ifdef STEP_PULSE_DELAY +#ifdef USE_RMT_STEPS // Step pulse delay handling is not require with ESP32...the RMT function does it. + if (direction_delay_microseconds->get() < 1) + { + // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. + st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3); + } #else // Normal operation // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3); #endif + // Enable Stepper Driver Interrupt Stepper_Timer_Start(); } From 2b2abb792205be0aeac57f6e20aacb03fadd1b44 Mon Sep 17 00:00:00 2001 From: Stefan de Bruijn Date: Thu, 4 Mar 2021 18:53:51 +0100 Subject: [PATCH 71/82] Vfd and mc_delay issues. Includes VFD sync speed. (#765) * Added some timing changes to steppers. Basically this should introduce a small (configurable) delay for enable and direction pins. * Updates after testing * Fixed small casting bug * Fixed subtractions as per @Mitch's comments * Fixed busy flag in Stepper.cpp * - Changed mc_dwell to double's, because float's have issues with context switches - Changed spinup/spindown of VFD's to something more sensible - Implemented advanced wait for spinup/spindown of VFD's * Changed machine defs * Implemented spindle speed sync. Made spindle safety polling optional; all the chatter appears to give issues somehow, not sure why. * Made safety-polling optional. * Added 'todo' for actual RPM code. Not sure how this should work though. * Added safety check for unchanged RPM. Changed error output channels. * Enabled the RPM code. * Commented out again. * Changed initialization sequence. Implemented Huanyang initialization sequence. * Made some fixes according to our latest insights in the manual * Fixed docs. * Fixed another bug in the comments * Fixed set_speed RPM. * Fixed huanyang message parsing. Fixed comments. * Forgot a fall-through return. * Updated status message after VFD initializes * Changed output RPM test to output frequency test for sync. * Changed mc_dwell to milliseconds and made it an integer. Fixed bug with negative values. Fixed a dwelling bug in the VFD code, which would dwell indefinitely in cases such as safety door. * Fixed a few small issues during the devt merge. Nothing serious. * Not having critical initialization makes sense for the collet wrench lock thing (or just VFD's that are powered up later) in the sense that initialization should eventually happen to get the right values. Moreover, when the VFD enters an unresponsive state, re-initialization happens eventually. * Changed 50 to 5000 for huanyang frequency calc. * Updates after testing Huanyang * Ran clang-format. Cleaned up the code a bit. Co-authored-by: Stefan de Bruijn Co-authored-by: bdring --- Grbl_Esp32/src/GCode.cpp | 2 +- Grbl_Esp32/src/Machines/3axis_rs485.h | 59 ----- Grbl_Esp32/src/MotionControl.cpp | 12 +- Grbl_Esp32/src/MotionControl.h | 2 +- Grbl_Esp32/src/NutsBolts.cpp | 13 +- Grbl_Esp32/src/NutsBolts.h | 5 +- Grbl_Esp32/src/Protocol.cpp | 12 +- Grbl_Esp32/src/SettingsDefinitions.cpp | 20 +- Grbl_Esp32/src/Spindles/H2ASpindle.cpp | 50 ++-- Grbl_Esp32/src/Spindles/H2ASpindle.h | 5 +- Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp | 247 ++++++++++++++++-- Grbl_Esp32/src/Spindles/HuanyangSpindle.h | 13 +- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 275 ++++++++++++++------ Grbl_Esp32/src/Spindles/VFDSpindle.h | 14 +- Grbl_Esp32/src/Stepper.cpp | 30 ++- 15 files changed, 532 insertions(+), 227 deletions(-) delete mode 100644 Grbl_Esp32/src/Machines/3axis_rs485.h diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp index 94ea3237..bd1bf2b9 100644 --- a/Grbl_Esp32/src/GCode.cpp +++ b/Grbl_Esp32/src/GCode.cpp @@ -1435,7 +1435,7 @@ Error gc_execute_line(char* line, uint8_t client) { #endif // [10. Dwell ]: if (gc_block.non_modal_command == NonModal::Dwell) { - mc_dwell(gc_block.values.p); + mc_dwell(int32_t(gc_block.values.p * 1000.0f)); } // [11. Set active plane ]: gc_state.modal.plane_select = gc_block.modal.plane_select; diff --git a/Grbl_Esp32/src/Machines/3axis_rs485.h b/Grbl_Esp32/src/Machines/3axis_rs485.h deleted file mode 100644 index 39a565cb..00000000 --- a/Grbl_Esp32/src/Machines/3axis_rs485.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once -// clang-format off - -/* - 3axis_xyx.h - Part of Grbl_ESP32 - - This is a general XYZ-axis RS-485 CNC machine. The schematic is quite - easy, you basically need a MAX485 wired through a logic level converter - for the VFD, and a few pins wired through an ULN2803A to the external - stepper drivers. It's common to have a dual gantry for the Y axis. - - Optional limit pins are slightly more difficult, as these require a - Schmitt trigger and optocouplers. - - 2020 - Stefan de Bruijn - - Grbl_ESP32 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. - - Grbl 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 Grbl_ESP32. If not, see . -*/ - -#define MACHINE_NAME "ESP32_XYZ_RS485" -#define X_STEP_PIN GPIO_NUM_4 // labeled X -#define X_DIRECTION_PIN GPIO_NUM_16 // labeled X -#define Y_STEP_PIN GPIO_NUM_17 // labeled Y -#define Y_DIRECTION_PIN GPIO_NUM_18 // labeled Y -#define Y2_STEP_PIN GPIO_NUM_19 // labeled Y2 -#define Y2_DIRECTION_PIN GPIO_NUM_21 // labeled Y2 -#define Z_STEP_PIN GPIO_NUM_22 // labeled Z -#define Z_DIRECTION_PIN GPIO_NUM_23 // labeled Z - -#define SPINDLE_TYPE SpindleType::H2A -#define VFD_RS485_TXD_PIN GPIO_NUM_13 // RS485 TX -#define VFD_RS485_RTS_PIN GPIO_NUM_15 // RS485 RTS -#define VFD_RS485_RXD_PIN GPIO_NUM_2 // RS485 RX - -#define X_LIMIT_PIN GPIO_NUM_33 -#define Y_LIMIT_PIN GPIO_NUM_32 -#define Y2_LIMIT_PIN GPIO_NUM_35 -#define Z_LIMIT_PIN GPIO_NUM_34 - -// Set $Homing/Cycle0=X and $Homing/Cycle=XY - -#define PROBE_PIN GPIO_NUM_14 // labeled Probe -#define CONTROL_RESET_PIN GPIO_NUM_27 // labeled Reset -#define CONTROL_FEED_HOLD_PIN GPIO_NUM_26 // labeled Hold -#define CONTROL_CYCLE_START_PIN GPIO_NUM_25 // labeled Start - -// #define VFD_DEBUG_MODE diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp index 5ea64cb2..1b8cda8f 100644 --- a/Grbl_Esp32/src/MotionControl.cpp +++ b/Grbl_Esp32/src/MotionControl.cpp @@ -5,8 +5,8 @@ Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P + 2018 - Bart Dring This file was modifed for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -226,12 +226,12 @@ void mc_arc(float* target, } // Execute dwell in seconds. -void mc_dwell(float seconds) { - if (seconds == 0 || sys.state == State::CheckMode) { - return; +bool mc_dwell(int32_t milliseconds) { + if (milliseconds <= 0 || sys.state == State::CheckMode) { + return false; } protocol_buffer_synchronize(); - delay_sec(seconds, DwellMode::Dwell); + return delay_msec(milliseconds, DwellMode::Dwell); } // return true if the mask has exactly one bit set, diff --git a/Grbl_Esp32/src/MotionControl.h b/Grbl_Esp32/src/MotionControl.h index c9177f0b..c7adebc5 100644 --- a/Grbl_Esp32/src/MotionControl.h +++ b/Grbl_Esp32/src/MotionControl.h @@ -53,7 +53,7 @@ void mc_arc(float* target, uint8_t is_clockwise_arc); // Dwell for a specific number of seconds -void mc_dwell(float seconds); +bool mc_dwell(int32_t milliseconds); // Perform homing cycle to locate machine zero. Requires limit switches. void mc_homing_cycle(uint8_t cycle_mask); diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp index 51245465..8039bdd6 100644 --- a/Grbl_Esp32/src/NutsBolts.cpp +++ b/Grbl_Esp32/src/NutsBolts.cpp @@ -112,11 +112,14 @@ void delay_ms(uint16_t ms) { } // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, DwellMode mode) { - uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds); +bool delay_msec(int32_t milliseconds, DwellMode mode) { + // Note: i must be signed, because of the 'i-- > 0' check below. + int32_t i = milliseconds / DWELL_TIME_STEP; + int32_t remainder = i < 0 ? 0 : (milliseconds - DWELL_TIME_STEP * i); + while (i-- > 0) { if (sys.abort) { - return; + return false; } if (mode == DwellMode::Dwell) { protocol_execute_realtime(); @@ -124,11 +127,13 @@ void delay_sec(float seconds, DwellMode mode) { // Execute rt_system() only to avoid nesting suspend loops. protocol_exec_rt_system(); if (sys.suspend.bit.restartRetract) { - return; // Bail, if safety door reopens. + return false; // Bail, if safety door reopens. } } delay(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment } + delay(remainder); + return true; } // Simple hypotenuse computation function. diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h index 15705065..77dd45f0 100644 --- a/Grbl_Esp32/src/NutsBolts.h +++ b/Grbl_Esp32/src/NutsBolts.h @@ -26,7 +26,7 @@ // #define true 1 enum class DwellMode : uint8_t { - Dwell = 0, // (Default: Must be zero) + Dwell = 0, // (Default: Must be zero) SysSuspend = 1, //G92.1 (Do not alter value) }; @@ -62,7 +62,6 @@ static inline int toMotor2(int axis) { const double MM_PER_INCH = (25.40); const double INCH_PER_MM = (0.0393701); - // Useful macros #define clear_vector(a) memset(a, 0, sizeof(a)) #define clear_vector_float(a) memset(a, 0.0, sizeof(float) * MAX_N_AXIS) @@ -90,7 +89,7 @@ const double INCH_PER_MM = (0.0393701); uint8_t read_float(const char* line, uint8_t* char_counter, float* float_ptr); // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, DwellMode mode); +bool delay_msec(int32_t milliseconds, DwellMode mode); // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms(). void delay_ms(uint16_t ms); diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp index 111e6229..0a331983 100644 --- a/Grbl_Esp32/src/Protocol.cpp +++ b/Grbl_Esp32/src/Protocol.cpp @@ -5,8 +5,8 @@ Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC Copyright (c) 2009-2011 Simen Svale Skogsrud - 2018 - Bart Dring This file was modifed for use on the ESP32 - CPU. Do not use this with Grbl for atMega328P + 2018 - Bart Dring This file was modifed for use on the ESP32 + CPU. Do not use this with Grbl for atMega328P Grbl is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -272,7 +272,7 @@ void protocol_exec_rt_system() { } ExecState rt_exec_state; rt_exec_state.value = sys_rt_exec_state.value; // Copy volatile sys_rt_exec_state. - if (rt_exec_state.value != 0 || cycle_stop) { // Test if any bits are on + if (rt_exec_state.value != 0 || cycle_stop) { // Test if any bits are on // Execute system abort. if (rt_exec_state.bit.reset) { sys.abort = true; // Only place this is set true. @@ -664,10 +664,10 @@ static void protocol_exec_rt_suspend() { if (spindle->inLaserMode()) { // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. sys.step_control.updateSpindleRpm = true; - } else { + } else { spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed); // restore delay is done in the spindle class - //delay_sec(spindle_delay_spinup->get(), DwellMode::SysSuspend); + //delay_sec(int32_t(1000.0 * spindle_delay_spinup->get()), DwellMode::SysSuspend); } } } @@ -676,7 +676,7 @@ static void protocol_exec_rt_suspend() { if (!sys.suspend.bit.restartRetract) { // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. coolant_set_state(restore_coolant); - delay_sec(coolant_start_delay->get(), DwellMode::SysSuspend); + delay_msec(int32_t(1000.0 * coolant_start_delay->get()), DwellMode::SysSuspend); } } #ifdef PARKING_ENABLE diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 24e7b1c2..9d7ad8f6 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -352,8 +352,7 @@ void make_settings() { new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinUp", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange); spindle_delay_spindown = new FloatSetting(EXTENDED, WG, NULL, "Spindle/Delay/SpinDown", DEFAULT_SPINDLE_DELAY_SPINUP, 0, 30, checkSpindleChange); - coolant_start_delay = - new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30); + coolant_start_delay = new FloatSetting(EXTENDED, WG, NULL, "Coolant/Delay/TurnOn", DEFAULT_COOLANT_DELAY_TURNON, 0, 30); spindle_enbl_off_with_zero_speed = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/OffWithSpeed", DEFAULT_SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED, checkSpindleChange); @@ -394,14 +393,15 @@ void make_settings() { junction_deviation = new FloatSetting(GRBL, WG, "11", "GCode/JunctionDeviation", DEFAULT_JUNCTION_DEVIATION, 0, 10); status_mask = new IntSetting(GRBL, WG, "10", "Report/Status", DEFAULT_STATUS_REPORT_MASK, 0, 3); - probe_invert = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN); - limit_invert = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS); - step_enable_invert = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE); - dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK, postMotorSetting); - step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting); - stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); - pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); - enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds + probe_invert = new FlagSetting(GRBL, WG, "6", "Probe/Invert", DEFAULT_INVERT_PROBE_PIN); + limit_invert = new FlagSetting(GRBL, WG, "5", "Limits/Invert", DEFAULT_INVERT_LIMIT_PINS); + step_enable_invert = new FlagSetting(GRBL, WG, "4", "Stepper/EnableInvert", DEFAULT_INVERT_ST_ENABLE); + dir_invert_mask = new AxisMaskSetting(GRBL, WG, "3", "Stepper/DirInvert", DEFAULT_DIRECTION_INVERT_MASK, postMotorSetting); + step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting); + stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); + pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); + direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", 0, 0, 1000); + enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", 0, 0, 1000); // microseconds direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", STEP_PULSE_DELAY, 0, 1000); enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp index b18249e5..a62112fd 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.cpp +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.cpp @@ -24,7 +24,7 @@ Remove power before changing bits. The documentation is okay once you get how it works, but unfortunately - incomplete... See H2ASpindle.md for the remainder of the docs that I + incomplete... See H2ASpindle.md for the remainder of the docs that I managed to piece together. */ @@ -80,31 +80,35 @@ namespace Spindles { data.msg[5] = uint8_t(speed & 0xFF); } - H2A::response_parser H2A::get_max_rpm(ModbusCommand& data) { - // NOTE: data length is excluding the CRC16 checksum. - data.tx_length = 6; - data.rx_length = 8; + VFD::response_parser H2A::initialization_sequence(int index, ModbusCommand& data) { + if (index == -1) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 8; - // Send: 01 03 B005 0002 - data.msg[1] = 0x03; // READ - data.msg[2] = 0xB0; // B0.05 = Get RPM - data.msg[3] = 0x05; - data.msg[4] = 0x00; // Read 2 values - data.msg[5] = 0x02; + // Send: 01 03 B005 0002 + data.msg[1] = 0x03; // READ + data.msg[2] = 0xB0; // B0.05 = Get RPM + data.msg[3] = 0x05; + data.msg[4] = 0x00; // Read 2 values + data.msg[5] = 0x02; - // Recv: 01 03 00 04 5D C0 03 F6 - // -- -- = 24000 (val #1) - return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { - uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); - vfd->_max_rpm = rpm; + // Recv: 01 03 00 04 5D C0 03 F6 + // -- -- = 24000 (val #1) + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); + vfd->_max_rpm = rpm; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "H2A spindle is initialized at %d RPM", int(rpm)); - return true; - }; + return true; + }; + } else { + return nullptr; + } } - H2A::response_parser H2A::get_current_rpm(ModbusCommand& data) { + VFD::response_parser H2A::get_current_rpm(ModbusCommand& data) { // NOTE: data length is excluding the CRC16 checksum. data.tx_length = 6; data.rx_length = 8; @@ -118,16 +122,16 @@ namespace Spindles { // Recv: 01 03 0004 095D 0000 // ---- = 2397 (val #1) - - // TODO: What are we going to do with this? Update sys.spindle_speed? Update vfd state? return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { uint16_t rpm = (uint16_t(response[4]) << 8) | uint16_t(response[5]); + // Set current RPM value? Somewhere? + vfd->_sync_rpm = rpm; return true; }; } - H2A::response_parser H2A::get_current_direction(ModbusCommand& data) { + VFD::response_parser H2A::get_current_direction(ModbusCommand& data) { // NOTE: data length is excluding the CRC16 checksum. data.tx_length = 6; data.rx_length = 6; diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.h b/Grbl_Esp32/src/Spindles/H2ASpindle.h index d206603b..8243867a 100644 --- a/Grbl_Esp32/src/Spindles/H2ASpindle.h +++ b/Grbl_Esp32/src/Spindles/H2ASpindle.h @@ -29,9 +29,12 @@ namespace Spindles { void direction_command(SpindleState mode, ModbusCommand& data) override; void set_speed_command(uint32_t rpm, ModbusCommand& data) override; - response_parser get_max_rpm(ModbusCommand& data) override; + response_parser initialization_sequence(int index, ModbusCommand& data) override; response_parser get_current_rpm(ModbusCommand& data) override; response_parser get_current_direction(ModbusCommand& data) override; response_parser get_status_ok(ModbusCommand& data) override { return nullptr; } + + bool supports_actual_rpm() const override { return true; } + bool safety_polling() const override { return false; } }; } diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp index e0172de3..22fddb9d 100644 --- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.cpp @@ -41,24 +41,31 @@ Protocol Details - VFD frequencies are in Hz. Multiply by 60 for RPM + A lot of good information about the details of all these parameters and how they should + be setup can be found on this page: + https://community.carbide3d.com/t/vfd-parameters-huanyang-model/15459/7 . - before using spindle, VFD must be setup for RS485 and match your spindle - PD001 2 RS485 Control of run commands - PD002 2 RS485 Control of operating frequency - PD005 400 Maximum frequency Hz (Typical for spindles) - PD011 120 Min Speed (Recommend Aircooled=120 Water=100) - PD014 10 Acceleration time (Test to optimize) - PD015 10 Deceleration time (Test to optimize) - PD023 1 Reverse run enabled - PD142 3.7 Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??) - PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) - PD163 1 RS485 Address: 1 (Typical. OK to change...see below) - PD164 1 RS485 Baud rate: 9600 (Typical. OK to change...see below) - PD165 3 RS485 Mode: RTU, 8N1 + Before using spindle, VFD must be setup for RS485 and match your spindle: + + PD004 400 Base frequency as rated on my spindle (default was 50) + PD005 400 Maximum frequency Hz (Typical for spindles) + PD011 120 Min Speed (Recommend Aircooled=120 Water=100) + PD014 10 Acceleration time (Test to optimize) + PD015 10 Deceleration time (Test to optimize) + PD023 1 Reverse run enabled + PD141 220 Spindle max rated voltage + PD142 3.7 Max current Amps (0.8kw=3.7 1.5kw=7.0, 2.2kw=??) + PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) + PD144 3000 Max rated motor revolution at 50 Hz => 24000@400Hz = 3000@50HZ + + PD001 2 RS485 Control of run commands + PD002 2 RS485 Control of operating frequency + PD163 1 RS485 Address: 1 (Typical. OK to change...see below) + PD164 1 RS485 Baud rate: 9600 (Typical. OK to change...see below) + PD165 3 RS485 Mode: RTU, 8N1 The official documentation of the RS485 is horrible. I had to piece it together from - a lot of different sources + a lot of different sources: Manuals: https://github.com/RobertOlechowski/Huanyang_VFD/tree/master/Documentations/pdf Reference: https://github.com/Smoothieware/Smoothieware/blob/edge/src/modules/tools/spindle/HuanyangSpindleControl.cpp @@ -95,6 +102,17 @@ ========================================================================== + Setting registers + Addr Read Len Reg DataH DataL CRC CRC + 0x01 0x01 0x03 5 0x00 0x00 CRC CRC // PD005 + 0x01 0x01 0x03 11 0x00 0x00 CRC CRC // PD011 + 0x01 0x01 0x03 143 0x00 0x00 CRC CRC // PD143 + 0x01 0x01 0x03 144 0x00 0x00 CRC CRC // PD144 + + Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above) + + ========================================================================== + Status registers Addr Read Len Reg DataH DataL CRC CRC 0x01 0x04 0x03 0x00 0x00 0x00 CRC CRC // Set Frequency * 100 (25Hz = 2500) @@ -105,8 +123,30 @@ 0x01 0x04 0x03 0x05 0x00 0x00 CRC CRC // AC voltage 0x01 0x04 0x03 0x06 0x00 0x00 CRC CRC // Cont 0x01 0x04 0x03 0x07 0x00 0x00 CRC CRC // VFD Temp + Message is returned with requested value = (DataH * 16) + DataL (see decimal offset above) + ========================================================================== + + The math: + + PD005 400 Maximum frequency Hz (Typical for spindles) + PD011 120 Min Speed (Recommend Aircooled=120 Water=100) + PD143 2 Poles most are 2 (I think this is only used for RPM calc from Hz) + PD144 3000 Max rated motor revolution at 50 Hz => 24000@400Hz = 3000@50HZ + + During initialization these 4 are pulled from the VFD registers. It then sets min and max RPM + of the spindle. So: + + MinRPM = PD011 * PD144 / 50 = 120 * 3000 / 50 = 7200 RPM min + MaxRPM = PD005 * PD144 / 50 = 400 * 3000 / 50 = 24000 RPM max + + If you then set 12000 RPM, it calculates the frequency: + + int targetFrequency = targetRPM * PD005 / MaxRPM = targetRPM * PD005 / (PD005 * PD144 / 50) = + targetRPM * 50 / PD144 = 12000 * 50 / 3000 = 200 + + If the frequency is -say- 25 Hz, Huanyang wants us to send 2500 (eg. 25.00 Hz). */ #include @@ -151,13 +191,148 @@ namespace Spindles { data.msg[1] = 0x05; data.msg[2] = 0x02; - uint16_t value = (uint16_t)(rpm * 100 / 60); // send Hz * 10 (Ex:1500 RPM = 25Hz .... Send 2500) + // Frequency comes from a conversion of revolutions per second to revolutions per minute + // (factor of 60) and a factor of 2 from counting the number of poles. E.g. rpm * 120 / 100. + + // int targetFrequency = targetRPM * PD005 / MaxRPM = targetRPM * PD005 / (PD005 * PD144 / 50) = + // targetRPM * 50 / PD144 + // + // Huanyang wants a factor 100 bigger numbers. So, 1500 rpm -> 25 HZ. Send 2500. + + uint16_t value = rpm * 5000 / _maxRpmAt50Hz; data.msg[3] = (value >> 8) & 0xFF; data.msg[4] = (value & 0xFF); } - Huanyang::response_parser Huanyang::get_status_ok(ModbusCommand& data) { + VFD::response_parser Huanyang::initialization_sequence(int index, ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + // data.msg[0] is omitted (modbus address is filled in later) + data.msg[1] = 0x01; // Read setting + data.msg[2] = 0x03; // Len + // [3] = set below... + data.msg[4] = 0x00; + data.msg[5] = 0x00; + + if (index == -1) { + // Max frequency + data.msg[3] = 5; // PD005: max frequency the VFD will allow. Normally 400. + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t value = (response[4] << 8) | response[5]; +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Max frequency = %d", value); +#endif + + // Set current RPM value? Somewhere? + auto huanyang = static_cast(vfd); + huanyang->_maxFrequency = value; + return true; + }; + + } else if (index == -2) { + // Min Frequency + data.msg[3] = 11; // PD011: frequency lower limit. Normally 0. + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t value = (response[4] << 8) | response[5]; + +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Min frequency set to %d", value); +#endif + + // Set current RPM value? Somewhere? + auto huanyang = static_cast(vfd); + huanyang->_minFrequency = value; + return true; + }; + } else if (index == -3) { + // Max rated revolutions @ 50Hz + + data.msg[3] = 144; // PD144: max rated motor revolution at 50Hz => 24000@400Hz = 3000@50HZ + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t value = (response[4] << 8) | response[5]; +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Max rated revolutions @ 50Hz = %d", value); +#endif + // Set current RPM value? Somewhere? + auto huanyang = static_cast(vfd); + huanyang->_maxRpmAt50Hz = value; + + // Regarding PD144, the 2 versions of the manuals both say "This is set according to the + // actual revolution of the motor. The displayed value is the same as this set value. It + // can be used as a monitoring parameter, which is convenient to the user. This set value + // corresponds to the revolution at 50Hz". + + // Calculate the VFD settings: + huanyang->updateRPM(); + + return true; + }; + } + /* + The number of poles seems to be over constrained information with PD144. If we're wrong, here's how + to get this information. Note that you probably have to call 'updateRPM' here then: + -- + + else if (index == -4) { + // Number Poles + + data.msg[3] = 143; // PD143: 4 or 2 poles in motor. Default is 4. A spindle being 24000RPM@400Hz implies 2 poles + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint8_t value = response[4]; // Single byte response. + + // Sanity check. We expect something like 2 or 4 poles. + if (value <= 4 && value >= 2) { +#ifdef VFD_DEBUG_MODE + // Set current RPM value? Somewhere? + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Number of poles set to %d", value); +#endif + + auto huanyang = static_cast(vfd); + huanyang->_numberPoles = value; + return true; + } + else { + grbl_msg_sendf(CLIENT_ALL, + MsgLevel::Error, + "Initialization of Huanyang spindle failed. Number of poles (PD143, expected 2-4, got %d) is not sane.", + value); + return false; + } + }; + + } + */ + + // Done. + return nullptr; + } + + void Huanyang::updateRPM() { + /* + PD005 = 400 ; max frequency the VFD will allow + MaxRPM = PD005 * 50 / PD176 + + Frequencies are a factor 100 of what they should be. + */ + + if (_minFrequency > _maxFrequency) { + _minFrequency = _maxFrequency; + } + + this->_min_rpm = uint32_t(_minFrequency) * uint32_t(_maxRpmAt50Hz) / 5000; // 0 * 3000 / 50 = 0 RPM. + this->_max_rpm = uint32_t(_maxFrequency) * uint32_t(_maxRpmAt50Hz) / 5000; // 400 * 3000 / 50 = 24k RPM. + + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: VFD settings read: RPM Range(%d, %d)]", _min_rpm, _max_rpm); + } + + VFD::response_parser Huanyang::get_status_ok(ModbusCommand& data) { // NOTE: data length is excluding the CRC16 checksum. data.tx_length = 6; data.rx_length = 6; @@ -176,4 +351,42 @@ namespace Spindles { } return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { return true; }; } + + VFD::response_parser Huanyang::get_current_rpm(ModbusCommand& data) { + // NOTE: data length is excluding the CRC16 checksum. + data.tx_length = 6; + data.rx_length = 6; + + // data.msg[0] is omitted (modbus address is filled in later) + data.msg[1] = 0x04; + data.msg[2] = 0x03; + data.msg[3] = 0x01; // Output frequency + data.msg[4] = 0x00; + data.msg[5] = 0x00; + + return [](const uint8_t* response, Spindles::VFD* vfd) -> bool { + uint16_t frequency = (response[4] << 8) | response[5]; + + auto huanyang = static_cast(vfd); + + // Since we set a frequency, it's only fair to check if that's output in the spindle sync. + // The reason we check frequency instead of RPM is because RPM assumes a linear relation + // between RPM and frequency - which isn't necessarily true. + // + // We calculate the frequency back to RPM the same way as we calculated the frequency in the + // first place. In other words, this code must match the set_speed_command method. Note that + // we test sync_rpm instead of frequency, because that matches whatever a vfd can throw at us. + // + // value = rpm * 5000 / maxRpmAt50Hz + // + // It follows that: + // frequency_value_x100 * maxRpmAt50Hz / 5000 = rpm + + auto rpm = uint32_t(frequency) * uint32_t(huanyang->_maxRpmAt50Hz) / 5000; + + // Store RPM for synchronization + vfd->_sync_rpm = rpm; + return true; + }; + } } diff --git a/Grbl_Esp32/src/Spindles/HuanyangSpindle.h b/Grbl_Esp32/src/Spindles/HuanyangSpindle.h index 48fecfdf..9c6ee875 100644 --- a/Grbl_Esp32/src/Spindles/HuanyangSpindle.h +++ b/Grbl_Esp32/src/Spindles/HuanyangSpindle.h @@ -6,7 +6,7 @@ HuanyangSpindle.h Part of Grbl_ESP32 - 2020 - Bart Dring + 2020 - Bart Dring 2020 - Stefan de Bruijn Grbl is free software: you can redistribute it and/or modify @@ -28,11 +28,22 @@ namespace Spindles { int reg; protected: + uint16_t _minFrequency = 0; // PD011: frequency lower limit. Normally 0. + uint16_t _maxFrequency = 400; // PD005: max frequency the VFD will allow. Normally 400. + uint16_t _maxRpmAt50Hz = 100; // PD144: rated motor revolution at 50Hz => 24000@400Hz = 3000@50HZ + // uint16_t _numberPoles = 2; // PD143: 4 or 2 poles in motor. Default is 4. A spindle being 24000RPM@400Hz implies 2 poles + + void updateRPM(); + void default_modbus_settings(uart_config_t& uart) override; void direction_command(SpindleState mode, ModbusCommand& data) override; void set_speed_command(uint32_t rpm, ModbusCommand& data) override; + response_parser initialization_sequence(int index, ModbusCommand& data) override; response_parser get_status_ok(ModbusCommand& data) override; + response_parser get_current_rpm(ModbusCommand& data) override; + + bool supports_actual_rpm() const override { return true; } }; } diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index 76e30c80..a332d0fd 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -1,9 +1,9 @@ /* VFDSpindle.cpp - This is for a VFD based spindles via RS485 Modbus. The details of the - VFD protocol heavily depend on the VFD in question here. We have some - implementations, but if yours is not here, the place to start is the + This is for a VFD based spindles via RS485 Modbus. The details of the + VFD protocol heavily depend on the VFD in question here. We have some + implementations, but if yours is not here, the place to start is the manual. This VFD class implements the modbus functionality. Part of Grbl_ESP32 @@ -26,7 +26,7 @@ Remove power before changing bits. TODO: - - We can report spindle_state and rpm better with VFD's that support + - We can report spindle_state and rpm better with VFD's that support either mode, register RPM or actual RPM. - Destructor should break down the task. - Move min/max RPM to protected members. @@ -34,11 +34,19 @@ */ #include "VFDSpindle.h" +#include + +// Timing and modbus... The manual states that between communications, we should respect a +// silent interval of 3,5 characters. If we received communications between these times, we +// have to assume that the message is broken. We use a poll rate of 250 ms here, which should +// be plenty: assuming 9600 8N1, that's roughly 250 chars. A message of 2x16 chars with 4x4 +// chars buffering is just 40 chars. + const uart_port_t VFD_RS485_UART_PORT = UART_NUM_2; // hard coded for this port right now const int VFD_RS485_BUF_SIZE = 127; -const int VFD_RS485_QUEUE_SIZE = 10; // numv\ber of commands that can be queued up. -const int RESPONSE_WAIT_TICKS = 50; // how long to wait for a response -const int VFD_RS485_POLL_RATE = 200; // in milliseconds between commands +const int VFD_RS485_QUEUE_SIZE = 10; // numv\ber of commands that can be queued up. +const int RESPONSE_WAIT_MILLIS = 1000; // how long to wait for a response in milliseconds +const int VFD_RS485_POLL_RATE = 250; // in milliseconds between commands // OK to change these // #define them in your machine definition file if you want different values @@ -53,11 +61,12 @@ namespace Spindles { // The communications task void VFD::vfd_cmd_task(void* pvParameters) { static bool unresponsive = false; // to pop off a message once each time it becomes unresponsive - static int pollidx = 0; + static int pollidx = -1; // -1 starts the VFD initialization sequence VFD* instance = static_cast(pvParameters); ModbusCommand next_cmd; uint8_t rx_message[VFD_RS485_MAX_MSG_SIZE]; + bool safetyPollingEnabled = instance->safety_polling(); while (true) { response_parser parser = nullptr; @@ -66,10 +75,10 @@ namespace Spindles { // First check if we should ask the VFD for the max RPM value as part of the initialization. We // should also query this is max_rpm is 0, because that means a previous initialization failed: - if (pollidx == 0 || (instance->_max_rpm == 0 && (parser = instance->get_max_rpm(next_cmd)) != nullptr)) { - pollidx = 1; - next_cmd.critical = true; + if ((pollidx < 0 || instance->_max_rpm == 0) && (parser = instance->initialization_sequence(pollidx, next_cmd)) != nullptr) { + next_cmd.critical = false; } else { + pollidx = 1; // Done with initialization. Main sequence. next_cmd.critical = false; } @@ -77,34 +86,39 @@ namespace Spindles { if (parser == nullptr && xQueueReceive(vfd_cmd_queue, &next_cmd, 0) != pdTRUE) { // We poll in a cycle. Note that the switch will fall through unless we encounter a hit. // The weakest form here is 'get_status_ok' which should be implemented if the rest fails. - switch (pollidx) { - case 1: - parser = instance->get_current_rpm(next_cmd); - if (parser) { - pollidx = 2; - break; - } - // fall through intentionally: - case 2: - parser = instance->get_current_direction(next_cmd); - if (parser) { - pollidx = 3; - break; - } - // fall through intentionally: - case 3: - parser = instance->get_status_ok(next_cmd); - pollidx = 1; + if (instance->_syncing) { + parser = instance->get_current_rpm(next_cmd); + } else if (safetyPollingEnabled) { + switch (pollidx) { + case 1: + parser = instance->get_current_rpm(next_cmd); + if (parser) { + pollidx = 2; + break; + } + // fall through intentionally: + case 2: + parser = instance->get_current_direction(next_cmd); + if (parser) { + pollidx = 3; + break; + } + // fall through intentionally: + case 3: + default: + parser = instance->get_status_ok(next_cmd); + pollidx = 1; - // we could complete this in case parser == nullptr with some ifs, but let's - // just keep it easy and wait an iteration. - break; + // we could complete this in case parser == nullptr with some ifs, but let's + // just keep it easy and wait an iteration. + break; + } } // If we have no parser, that means get_status_ok is not implemented (and we have // nothing resting in our queue). Let's fall back on a simple continue. if (parser == nullptr) { - vTaskDelay(VFD_RS485_POLL_RATE); + vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS); continue; // main while loop } } @@ -120,7 +134,7 @@ namespace Spindles { next_cmd.msg[next_cmd.tx_length - 1] = (crc16 & 0xFF00) >> 8; next_cmd.msg[next_cmd.tx_length - 2] = (crc16 & 0xFF); -#ifdef VFD_DEBUG_MODE +#ifdef VFD_DEBUG_MODE2 if (parser == nullptr) { report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length); } @@ -130,12 +144,36 @@ namespace Spindles { // Assume for the worst, and retry... int retry_count = 0; for (; retry_count < MAX_RETRIES; ++retry_count) { - // Flush the UART and write the data: + // Flush the UART: uart_flush(VFD_RS485_UART_PORT); + + // Write the data: uart_write_bytes(VFD_RS485_UART_PORT, reinterpret_cast(next_cmd.msg), next_cmd.tx_length); + uart_wait_tx_done(VFD_RS485_UART_PORT, RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS); // Read the response - uint16_t read_length = uart_read_bytes(VFD_RS485_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_TICKS); + uint16_t read_length = 0; + uint16_t current_read = + uart_read_bytes(VFD_RS485_UART_PORT, rx_message, next_cmd.rx_length, RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS); + read_length += current_read; + + // Apparently some Huanyang report modbus errors in the correct way, and the rest not. Sigh. + // Let's just check for the condition, and truncate the first byte. + if (read_length > 0 && VFD_RS485_ADDR != 0 && rx_message[0] == 0) { + memmove(rx_message + 1, rx_message, read_length - 1); + } + + while (read_length < next_cmd.rx_length && current_read > 0) { + // Try to read more; we're not there yet... + current_read = uart_read_bytes(VFD_RS485_UART_PORT, + rx_message + read_length, + next_cmd.rx_length - read_length, + RESPONSE_WAIT_MILLIS / portTICK_PERIOD_MS); + read_length += current_read; + } + if (current_read < 0) { + read_length = 0; + } // Generate crc16 for the response: auto crc16response = ModRTU_CRC(rx_message, next_cmd.rx_length - 2); @@ -145,20 +183,30 @@ namespace Spindles { rx_message[read_length - 1] == (crc16response & 0xFF00) >> 8 && // check CRC byte 1 rx_message[read_length - 2] == (crc16response & 0xFF)) { // check CRC byte 1 - // success + // Success unresponsive = false; retry_count = MAX_RETRIES + 1; // stop retry'ing // Should we parse this? - if (parser != nullptr && !parser(rx_message, instance)) { + if (parser != nullptr) { + if (parser(rx_message, instance)) { + // If we're initializing, move to the next initialization command: + if (pollidx < 0) { + --pollidx; + } + } else { + // If we were initializing, move back to where we started. #ifdef VFD_DEBUG_MODE - report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length); - report_hex_msg(rx_message, "RS485 Rx: ", read_length); + // Parsing failed + report_hex_msg(next_cmd.msg, "RS485 Tx: ", next_cmd.tx_length); + report_hex_msg(rx_message, "RS485 Rx: ", read_length); #endif - // Not succesful! Now what? - unresponsive = true; - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + // Not succesful! Now what? + unresponsive = true; + pollidx = -1; // Re-initializing the VFD seems like a plan + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 did not give a satisfying response"); + } } } else { #ifdef VFD_DEBUG_MODE @@ -184,7 +232,7 @@ namespace Spindles { // Wait a bit before we retry. Set the delay to poll-rate. Not sure // if we should use a different value... - vTaskDelay(VFD_RS485_POLL_RATE); + vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS); static UBaseType_t uxHighWaterMark = 0; reportTaskStackSize(uxHighWaterMark); @@ -193,17 +241,18 @@ namespace Spindles { if (retry_count == MAX_RETRIES) { if (!unresponsive) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length); unresponsive = true; + pollidx = -1; } if (next_cmd.critical) { - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive"); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Error, "Critical Spindle RS485 Unresponsive"); mc_reset(); sys_rt_exec_alarm = ExecAlarm::SpindleControl; } } - vTaskDelay(VFD_RS485_POLL_RATE); // TODO: What is the best value here? + vTaskDelay(VFD_RS485_POLL_RATE / portTICK_PERIOD_MS); } } @@ -217,7 +266,9 @@ namespace Spindles { } void VFD::init() { - vfd_ok = false; // initialize + vfd_ok = false; // initialize + _sync_rpm = 0; + _syncing = false; grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Initializing RS485 VFD spindle"); @@ -266,6 +317,15 @@ namespace Spindles { return; } + // We have to initialize the constants before starting the task: + is_reversable = true; // these VFDs are always reversable + use_delays = true; + vfd_ok = true; + + // Initially we initialize this to 0; over time, we might poll better information from the VFD. + _current_rpm = 0; + _current_state = SpindleState::Disable; + // Initialization is complete, so now it's okay to run the queue task: if (!_task_running) { // init can happen many times, we only want to start one task vfd_cmd_queue = xQueueCreate(VFD_RS485_QUEUE_SIZE, sizeof(ModbusCommand)); @@ -280,14 +340,6 @@ namespace Spindles { _task_running = true; } - is_reversable = true; // these VFDs are always reversable - use_delays = true; - vfd_ok = true; - - // Initially we initialize this to 0; over time, we might poll better information from the VFD. - _current_rpm = 0; - _current_state = SpindleState::Disable; - config_message(); } @@ -323,6 +375,7 @@ namespace Spindles { pins_settings_ok = false; } + // Use config for initial RPM values: _min_rpm = rpm_min->get(); _max_rpm = rpm_max->get(); @@ -346,32 +399,93 @@ namespace Spindles { return; // Block during abort. } - bool critical = (sys.state == State::Cycle || state != SpindleState::Disable); + bool shouldWait = state != _current_state || state != SpindleState::Disable; + bool critical = (sys.state == State::Cycle || state != SpindleState::Disable); + + int32_t delayMillis = 1000; if (_current_state != state) { // already at the desired state. This function gets called a lot. set_mode(state, critical); // critical if we are in a job set_rpm(rpm); - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin1"); - if (state == SpindleState::Disable) { sys.spindle_speed = 0; - delay(_spindown_delay); + delayMillis = _spindown_delay; + rpm = 0; } else { - delay(_spinup_delay); + delayMillis = _spinup_delay; + } + + if (_current_state != state && !supports_actual_rpm()) { + delay(delayMillis); } - - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spin2"); } else { if (_current_rpm != rpm) { set_rpm(rpm); + + if (rpm > _current_rpm) { + delayMillis = _spinup_delay; + } else { + delayMillis = _spindown_delay; + } } } - _current_state = state; // store locally for faster get_state() + if (shouldWait) { + if (supports_actual_rpm()) { + _syncing = true; - sys.report_ovr_counter = 0; // Set to report change immediately + // Allow 2.5% difference from what we asked for. Should be fine. + uint32_t drpm = (_max_rpm - _min_rpm) / 40; + if (drpm < 100) { + drpm = 100; + } // Just a sanity check + + auto minRpmAllowed = _current_rpm > drpm ? (_current_rpm - drpm) : 0; + auto maxRpmAllowed = _current_rpm + drpm; + + int unchanged = 0; + const int limit = 20; // 20 * 0.5s = 10 sec + auto last = _sync_rpm; + + while ((_sync_rpm < minRpmAllowed || _sync_rpm > maxRpmAllowed) && unchanged < limit) { +#ifdef VFD_DEBUG_MODE + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Syncing RPM. Requested %d, current %d", int(rpm), int(_sync_rpm)); +#endif + if (!mc_dwell(500)) { + // Something happened while we were dwelling, like a safety door. + unchanged = limit; + last = _sync_rpm; + break; + } + + if (_sync_rpm == last) { + ++unchanged; + } else { + unchanged = 0; + } + last = _sync_rpm; + } + + if (unchanged == limit) { + grbl_msg_sendf(CLIENT_ALL, + MsgLevel::Error, + "Critical Spindle RS485 did not reach speed %d. Reported speed is %d rpm.", + rpm, + _sync_rpm); + mc_reset(); + sys_rt_exec_alarm = ExecAlarm::SpindleControl; + } + + _syncing = false; + } else { + delay(delayMillis); + } + } + + _current_state = state; // store locally for faster get_state() + sys.report_ovr_counter = 0; // Set to report change immediately return; } @@ -407,26 +521,32 @@ namespace Spindles { return 0; } -#ifdef VFD_DEBUG_MODE - grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); -#endif - // apply override rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) - // apply limits - if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) { - rpm = _max_rpm; - } else if (rpm != 0 && rpm <= _min_rpm) { - rpm = _min_rpm; + if (rpm < _min_rpm || rpm > _max_rpm) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed %d outside range:(%d,%d)", rpm, _min_rpm, _max_rpm); + rpm = constrain(rpm, _min_rpm, _max_rpm); + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed changed to %d", rpm); } + // apply limits + // if ((_min_rpm >= _max_rpm) || (rpm >= _max_rpm)) { + // rpm = _max_rpm; + // } else if (rpm != 0 && rpm <= _min_rpm) { + // rpm = _min_rpm; + // } + sys.spindle_speed = rpm; if (rpm == _current_rpm) { // prevent setting same RPM twice return rpm; } +#ifdef VFD_DEBUG_MODE2 + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Setting spindle speed to %d rpm (%d, %d)", int(rpm), int(_min_rpm), int(_max_rpm)); +#endif + _current_rpm = rpm; // TODO add the speed modifiers override, linearization, etc. @@ -434,9 +554,10 @@ namespace Spindles { ModbusCommand rpm_cmd; rpm_cmd.msg[0] = VFD_RS485_ADDR; + set_speed_command(rpm, rpm_cmd); - rpm_cmd.critical = false; + rpm_cmd.critical = (rpm == 0); if (xQueueSend(vfd_cmd_queue, &rpm_cmd, 0) != pdTRUE) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "VFD Queue Full"); @@ -445,7 +566,7 @@ namespace Spindles { return rpm; } - void VFD::stop() { set_mode(SpindleState::Disable, false); } + void VFD::stop() { set_mode(SpindleState::Disable, true); } // state is cached rather than read right now to prevent delays SpindleState VFD::get_state() { return _current_state; } @@ -471,4 +592,4 @@ namespace Spindles { return crc; } -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.h b/Grbl_Esp32/src/Spindles/VFDSpindle.h index 7e81443d..2661d246 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.h +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.h @@ -2,11 +2,11 @@ /* VFDSpindle.h - + Part of Grbl_ESP32 2020 - Bart Dring 2020 - Stefan de Bruijn - + Grbl 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 @@ -29,7 +29,7 @@ namespace Spindles { class VFD : public Spindle { private: static const int VFD_RS485_MAX_MSG_SIZE = 16; // more than enough for a modbus message - static const int MAX_RETRIES = 3; // otherwise the spindle is marked 'unresponsive' + static const int MAX_RETRIES = 5; // otherwise the spindle is marked 'unresponsive' bool set_mode(SpindleState mode, bool critical); bool get_pins_and_settings(); @@ -66,10 +66,12 @@ namespace Spindles { // Commands that return the status. Returns nullptr if unavailable by this VFD (default): using response_parser = bool (*)(const uint8_t* response, VFD* spindle); - virtual response_parser get_max_rpm(ModbusCommand& data) { return nullptr; } + virtual response_parser initialization_sequence(int index, ModbusCommand& data) { return nullptr; } virtual response_parser get_current_rpm(ModbusCommand& data) { return nullptr; } virtual response_parser get_current_direction(ModbusCommand& data) { return nullptr; } virtual response_parser get_status_ok(ModbusCommand& data) = 0; + virtual bool supports_actual_rpm() const { return false; } + virtual bool safety_polling() const { return true; } public: VFD() = default; @@ -82,6 +84,8 @@ namespace Spindles { // Should hide them and use a member function. volatile uint32_t _min_rpm; volatile uint32_t _max_rpm; + volatile uint32_t _sync_rpm; + volatile bool _syncing; void init(); void config_message(); @@ -92,4 +96,4 @@ namespace Spindles { virtual ~VFD() {} }; -} \ No newline at end of file +} diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index 1fa4adf1..c0d81a18 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -25,6 +25,8 @@ #include "Grbl.h" +#include + // Stores the planner block Bresenham algorithm execution data for the segments in the segment // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will // never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1). @@ -78,7 +80,7 @@ static uint8_t segment_buffer_head; static uint8_t segment_next_head; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. -static volatile uint8_t busy; +static std::atomic busy; // Pointers for the step segment being prepped from the planner buffer. Accessed only by the // main program. Pointers may be planning segments or planner blocks ahead of what being executed. @@ -194,18 +196,20 @@ static void stepper_pulse_func(); // int8 variables and update position counters only when a segment completes. This can get complicated // with probing and homing cycles that require true real-time positions. void IRAM_ATTR onStepperDriverTimer(void* para) { - // ISR It is time to take a step ======================================================================================= - //const int timer_idx = (int)para; // get the timer index + // Timer ISR, normally takes a step. + // + // When handling an interrupt within an interrupt serivce routine (ISR), the interrupt status bit + // needs to be explicitly cleared. TIMERG0.int_clr_timers.t0 = 1; - if (busy) { - return; // The busy-flag is used to avoid reentering this interrupt + + bool expected = false; + if (busy.compare_exchange_strong(expected, true)) { + stepper_pulse_func(); + + TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; + + busy.store(false); } - busy = true; - - stepper_pulse_func(); - - TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; - busy = false; } /** @@ -348,6 +352,8 @@ static void stepper_pulse_func() { } void stepper_init() { + busy.store(false); + grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Axis count %d", number_axis->get()); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s", stepper_names[current_stepper]); @@ -422,7 +428,6 @@ void st_reset() { segment_buffer_tail = 0; segment_buffer_head = 0; // empty = tail segment_next_head = 1; - busy = false; st.step_outbits = 0; st.dir_outbits = 0; // Initialize direction bits to default. // TODO do we need to turn step pins off? @@ -432,7 +437,6 @@ void st_reset() { void st_go_idle() { // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active. Stepper_Timer_Stop(); - busy = false; // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances. if (((stepper_idle_lock_time->get() != 0xff) || sys_rt_exec_alarm != ExecAlarm::None || sys.state == State::Sleep) && From d24df1fda47a22827ae5987344f8899e5c9e3786 Mon Sep 17 00:00:00 2001 From: Luc <8822552+luc-github@users.noreply.github.com> Date: Fri, 5 Mar 2021 17:04:39 +0100 Subject: [PATCH 72/82] Bug fixes (#790) * Fix memory leak when answering webcommand (regression issue) Fix wrong error code on web command (regression issue) * Fix sd code not disabled when SD Card feature is disabled * Use proper error codes / messages * Update Grbl.h * Revert "Use proper error codes / messages" This reverts commit ad49cf8cc1cb2064a8f7687cb4c354eba4bc8172. * Updated error code symbols and text - To be generic file system error Co-authored-by: Luc Co-authored-by: bdring --- Grbl_Esp32/src/Error.cpp | 22 ++++++------ Grbl_Esp32/src/Error.h | 20 +++++------ Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Report.cpp | 6 +++- Grbl_Esp32/src/SDCard.cpp | 15 ++++---- Grbl_Esp32/src/Serial.cpp | 4 +++ Grbl_Esp32/src/WebUI/WebServer.cpp | 9 ++--- Grbl_Esp32/src/WebUI/WebSettings.cpp | 54 ++++++++++++++++++++-------- 8 files changed, 85 insertions(+), 47 deletions(-) diff --git a/Grbl_Esp32/src/Error.cpp b/Grbl_Esp32/src/Error.cpp index 34dd1cd4..c18ef200 100644 --- a/Grbl_Esp32/src/Error.cpp +++ b/Grbl_Esp32/src/Error.cpp @@ -61,16 +61,16 @@ std::map ErrorNames = { { Error::GcodeG43DynamicAxisError, "Gcode G43 dynamic axis error" }, { Error::GcodeMaxValueExceeded, "Gcode max value exceeded" }, { Error::PParamMaxExceeded, "P param max exceeded" }, - { Error::SdFailedMount, "SD failed mount" }, - { Error::SdFailedRead, "SD failed read" }, - { Error::SdFailedOpenDir, "SD failed to open directory" }, - { Error::SdDirNotFound, "SD directory not found" }, - { Error::SdFileEmpty, "SD file empty" }, - { Error::SdFileNotFound, "SD file not found" }, - { Error::SdFailedOpenFile, "SD failed to open file" }, - { Error::SdFailedBusy, "SD is busy" }, - { Error::SdFailedDelDir, "SD failed to delete directory" }, - { Error::SdFailedDelFile, "SD failed to delete file" }, + { Error::FsFailedMount, "Failed to mount device" }, + { Error::FsFailedRead, "Failed to read" }, + { Error::FsFailedOpenDir, "Failed to open directory" }, + { Error::FsDirNotFound, "Directory not found" }, + { Error::FsFileEmpty, "File empty" }, + { Error::FsFileNotFound, "File not found" }, + { Error::FsFailedOpenFile, "Failed to open file" }, + { Error::FsFailedBusy, "Device is busy" }, + { Error::FsFailedDelDir, "Failed to delete directory" }, + { Error::FsFailedDelFile, "Failed to delete file" }, { Error::BtFailBegin, "Bluetooth failed to start" }, { Error::WifiFailBegin, "WiFi failed to start" }, { Error::NumberRange, "Number out of range for setting" }, @@ -79,5 +79,5 @@ std::map ErrorNames = { { Error::NvsSetFailed, "Failed to store setting" }, { Error::NvsGetStatsFailed, "Failed to get setting status" }, { Error::AuthenticationFailed, "Authentication failed!" }, - { Error::AnotherInterfaceBusy, "Another interface is busy"}, + { Error::AnotherInterfaceBusy, "Another interface is busy" }, }; diff --git a/Grbl_Esp32/src/Error.h b/Grbl_Esp32/src/Error.h index 80f40a39..14b443eb 100644 --- a/Grbl_Esp32/src/Error.h +++ b/Grbl_Esp32/src/Error.h @@ -64,16 +64,16 @@ enum class Error : uint8_t { GcodeG43DynamicAxisError = 37, GcodeMaxValueExceeded = 38, PParamMaxExceeded = 39, - SdFailedMount = 60, // SD Failed to mount - SdFailedRead = 61, // SD Failed to read file - SdFailedOpenDir = 62, // SD card failed to open directory - SdDirNotFound = 63, // SD Card directory not found - SdFileEmpty = 64, // SD Card directory not found - SdFileNotFound = 65, // SD Card file not found - SdFailedOpenFile = 66, // SD card failed to open file - SdFailedBusy = 67, // SD card is busy - SdFailedDelDir = 68, - SdFailedDelFile = 69, + FsFailedMount = 60, // SD Failed to mount + FsFailedRead = 61, // SD Failed to read file + FsFailedOpenDir = 62, // SD card failed to open directory + FsDirNotFound = 63, // SD Card directory not found + FsFileEmpty = 64, // SD Card directory not found + FsFileNotFound = 65, // SD Card file not found + FsFailedOpenFile = 66, // SD card failed to open file + FsFailedBusy = 67, // SD card is busy + FsFailedDelDir = 68, + FsFailedDelFile = 69, BtFailBegin = 70, // Bluetooth failed to start WifiFailBegin = 71, // WiFi failed to start NumberRange = 80, // Setting number range problem diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index b6d0c438..2172f344 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210228"; +const char* const GRBL_VERSION_BUILD = "20210304"; //#include #include diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp index 892230e8..b4728154 100644 --- a/Grbl_Esp32/src/Report.cpp +++ b/Grbl_Esp32/src/Report.cpp @@ -291,10 +291,14 @@ std::map MessageText = { // NOTE: For interfaces, messages are always placed within brackets. And if silent mode // is installed, the message number codes are less than zero. void report_feedback_message(Message message) { // ok to send to all clients +#if defined (ENABLE_SD_CARD) if (message == Message::SdFileQuit) { grbl_notifyf("SD print canceled", "Reset during SD file at line: %d", sd_get_current_line_number()); grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset during SD file at line: %d", sd_get_current_line_number()); - } else { + + } else +#endif //ENABLE_SD_CARD + { auto it = MessageText.find(message); if (it != MessageText.end()) { grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, it->second); diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp index 32dd95ef..eadac19f 100644 --- a/Grbl_Esp32/src/SDCard.cpp +++ b/Grbl_Esp32/src/SDCard.cpp @@ -18,7 +18,9 @@ along with Grbl. If not, see . */ -#include "SDCard.h" +#include "Config.h" +#ifdef ENABLE_SD_CARD +# include "SDCard.h" File myFile; bool SD_ready_next = false; // Grbl has processed a line and is waiting for another @@ -31,7 +33,7 @@ static char comment[LINE_BUFFER_SIZE]; // Line to be executed. Z /*bool sd_mount() { if(!SD.begin()) { - report_status_message(Error::SdFailedMount, CLIENT_SERIAL); + report_status_message(Error::FsFailedMount, CLIENT_SERIAL); return false; } return true; @@ -41,11 +43,11 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) { //char temp_filename[128]; // to help filter by extension TODO: 128 needs a definition based on something File root = fs.open(dirname); if (!root) { - report_status_message(Error::SdFailedOpenDir, client); + report_status_message(Error::FsFailedOpenDir, client); return; } if (!root.isDirectory()) { - report_status_message(Error::SdDirNotFound, client); + report_status_message(Error::FsDirNotFound, client); return; } File file = root.openNextFile(); @@ -64,7 +66,7 @@ void listDir(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) { boolean openFile(fs::FS& fs, const char* path) { myFile = fs.open(path); if (!myFile) { - //report_status_message(Error::SdFailedRead, CLIENT_SERIAL); + //report_status_message(Error::FsFailedRead, CLIENT_SERIAL); return false; } set_sd_state(SDState::BusyPrinting); @@ -94,7 +96,7 @@ boolean closeFile() { */ boolean readFileLine(char* line, int maxlen) { if (!myFile) { - report_status_message(Error::SdFailedRead, SD_client); + report_status_message(Error::FsFailedRead, SD_client); return false; } sd_current_line_number += 1; @@ -169,3 +171,4 @@ void sd_get_current_filename(char* name) { name[0] = 0; } } +#endif //ENABLE_SD_CARD diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp index 9a4b9e4e..c5d0220b 100644 --- a/Grbl_Esp32/src/Serial.cpp +++ b/Grbl_Esp32/src/Serial.cpp @@ -159,16 +159,20 @@ void serialCheckTask(void* pvParameters) { if (is_realtime_command(data)) { execute_realtime_command(static_cast(data), client); } else { +#if defined(ENABLE_SD_CARD) if (get_sd_state(false) < SDState::Busy) { +#endif //ENABLE_SD_CARD vTaskEnterCritical(&myMutex); client_buffer[client].write(data); vTaskExitCritical(&myMutex); +#if defined(ENABLE_SD_CARD) } else { if (data == '\r' || data == '\n') { grbl_sendf(client, "error %d\r\n", Error::AnotherInterfaceBusy); grbl_msg_sendf(client, MsgLevel::Info, "SD card job running"); } } +#endif //ENABLE_SD_CARD } } // if something available WebUI::COMMANDS::handle(); diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 311c980a..b4a1cef5 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -480,10 +480,11 @@ namespace WebUI { } } if (silent || !espresponse->anyOutput()) { - _webserver->send(err != Error::Ok ? 401 : 200, "text/plain", answer); + _webserver->send(err != Error::Ok ? 500 : 200, "text/plain", answer); } else { espresponse->flush(); } + if(espresponse) delete(espresponse); } else { //execute GCODE if (auth_level == AuthenticationLevel::LEVEL_GUEST) { _webserver->send(401, "text/plain", "Authentication failed!\n"); @@ -491,7 +492,7 @@ namespace WebUI { } //Instead of send several commands one by one by web / send full set and split here String scmd; - const char* res = ""; + bool hasError =false; uint8_t sindex = 0; // TODO Settings - this is very inefficient. get_Splited_Value() is O(n^2) // when it could easily be O(n). Also, it would be just as easy to push @@ -512,10 +513,10 @@ namespace WebUI { scmd += "\n"; } if (!Serial2Socket.push(scmd.c_str())) { - res = "Error"; + hasError = true; } } - _webserver->send(200, "text/plain", res); + _webserver->send(200, "text/plain", hasError?"Error":""); } } diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp index 84e2ec18..f2a312f2 100644 --- a/Grbl_Esp32/src/WebUI/WebSettings.cpp +++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp @@ -302,11 +302,11 @@ namespace WebUI { } if (!SPIFFS.exists(path)) { webPrintln("Error: No such file!"); - return Error::SdFileNotFound; + return Error::FsFileNotFound; } File currentfile = SPIFFS.open(path, FILE_READ); if (!currentfile) { //if file open success - return Error::SdFailedOpenFile; + return Error::FsFailedOpenFile; } //until no line in file Error err; @@ -338,11 +338,11 @@ namespace WebUI { } if (!SPIFFS.exists(path)) { webPrintln("Error: No such file!"); - return Error::SdFileNotFound; + return Error::FsFileNotFound; } File currentfile = SPIFFS.open(path, FILE_READ); if (!currentfile) { - return Error::SdFailedOpenFile; + return Error::FsFailedOpenFile; } while (currentfile.available()) { // String currentline = currentfile.readStringUntil('\n'); @@ -686,16 +686,16 @@ namespace WebUI { if (state != SDState::Idle) { if (state == SDState::NotPresent) { webPrintln("No SD Card"); - return Error::SdFailedMount; + return Error::FsFailedMount; } else { webPrintln("SD Card Busy"); - return Error::SdFailedBusy; + return Error::FsFailedBusy; } } if (!openFile(SD, path.c_str())) { - report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL); + report_status_message(Error::FsFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL); webPrintln(""); - return Error::SdFailedOpenFile; + return Error::FsFailedOpenFile; } return Error::Ok; } @@ -764,18 +764,18 @@ namespace WebUI { File file2del = SD.open(path); if (!file2del) { webPrintln("Cannot stat file!"); - return Error::SdFileNotFound; + return Error::FsFileNotFound; } if (file2del.isDirectory()) { if (!SD.rmdir(path)) { webPrintln("Cannot delete directory! Is directory empty?"); - return Error::SdFailedDelDir; + return Error::FsFailedDelDir; } webPrintln("Directory deleted."); } else { if (!SD.remove(path)) { webPrintln("Cannot delete file!"); - return Error::SdFailedDelFile; + return Error::FsFailedDelFile; } webPrintln("File deleted."); } @@ -788,10 +788,10 @@ namespace WebUI { if (state != SDState::Idle) { if (state == SDState::NotPresent) { webPrintln("No SD Card"); - return Error::SdFailedMount; + return Error::FsFailedMount; } else { webPrintln("SD Card Busy"); - return Error::SdFailedBusy; + return Error::FsFailedBusy; } } webPrintln(""); @@ -806,9 +806,35 @@ namespace WebUI { } #endif + void listDirLocalFS(fs::FS& fs, const char* dirname, uint8_t levels, uint8_t client) { + //char temp_filename[128]; // to help filter by extension TODO: 128 needs a definition based on something + File root = fs.open(dirname); + if (!root) { + //FIXME: need proper error for FS and not usd sd one + report_status_message(Error::FsFailedOpenDir, client); + return; + } + if (!root.isDirectory()) { + //FIXME: need proper error for FS and not usd sd one + report_status_message(Error::FsDirNotFound, client); + return; + } + File file = root.openNextFile(); + while (file) { + if (file.isDirectory()) { + if (levels) { + listDirLocalFS(fs, file.name(), levels - 1, client); + } + } else { + grbl_sendf(CLIENT_ALL, "[FILE:%s|SIZE:%d]\r\n", file.name(), file.size()); + } + file = root.openNextFile(); + } + } + static Error listLocalFiles(char* parameter, AuthenticationLevel auth_level) { // No ESP command webPrintln(""); - listDir(SPIFFS, "/", 10, espresponse->client()); + listDirLocalFS(SPIFFS, "/", 10, espresponse->client()); String ssd = "[Local FS Free:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes() - SPIFFS.usedBytes()); ssd += " Used:" + ESPResponseStream::formatBytes(SPIFFS.usedBytes()); ssd += " Total:" + ESPResponseStream::formatBytes(SPIFFS.totalBytes()); From d4ac1becf6e7a857a85017d231c53916154faf82 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 5 Mar 2021 11:59:56 -0600 Subject: [PATCH 73/82] Get next rmt chan num fix (#793) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Update - moved get_next_RMT_chan_num() to constructor - [Discord discussion](https://discord.com/channels/780079161460916227/786364602223951882/817056437016592384) Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Motors/StandardStepper.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 2172f344..1d3e57e3 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210304"; +const char* const GRBL_VERSION_BUILD = "20210305"; //#include #include diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index 68803a46..3f5a91da 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -40,7 +40,11 @@ namespace Motors { } StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) : - Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) {} + Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) { +#ifdef USE_RMT_STEPS + _rmt_chan_num = get_next_RMT_chan_num(); +#endif + } void StandardStepper::init() { read_settings(); @@ -72,7 +76,6 @@ namespace Motors { rmtItem[1].duration0 = 0; rmtItem[1].duration1 = 0; - _rmt_chan_num = get_next_RMT_chan_num(); if (_rmt_chan_num == RMT_CHANNEL_MAX) { return; } From 11ef4b5dae334a463f4b8cd1af835eadcb71ff25 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 5 Mar 2021 13:05:32 -0600 Subject: [PATCH 74/82] Rc servo updates (#794) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * RcServo Updates - Improved disable. It was not always initially set correctly. - Improved calibration. Now a calibration value greater than 1 moves the motor in a positive direction and a value less than 1 moves it in a negative direction regardless of min/max and direction inverts. * Update Grbl.h Update grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Motors/RcServo.cpp | 23 +++++++++-------------- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 1d3e57e3..5a56918b 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210305"; +const char* const GRBL_VERSION_BUILD = "20210306"; //#include #include diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp index 2eefbfc2..bc0fe8b5 100644 --- a/Grbl_Esp32/src/Motors/RcServo.cpp +++ b/Grbl_Esp32/src/Motors/RcServo.cpp @@ -50,7 +50,7 @@ namespace Motors { ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS); ledcAttachPin(_pwm_pin, _channel_num); _current_pwm_duty = 0; - + _disabled = true; config_message(); startUpdateTask(); } @@ -78,10 +78,6 @@ namespace Motors { // sets the PWM to zero. This allows most servos to be manually moved void RcServo::set_disable(bool disable) { - if (_disabled == disable) { - return; - } - _disabled = disable; if (_disabled) { _write_pwm(0); @@ -107,11 +103,6 @@ namespace Motors { if (_disabled) return; - if (sys.state == State::Alarm) { - set_disable(true); - return; - } - read_settings(); mpos = system_convert_axis_steps_to_mpos(sys_position, _axis_index); // get the axis machine position in mm @@ -127,10 +118,14 @@ namespace Motors { } void RcServo::read_settings() { - _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get(); - _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get(); + if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) { + // swap the pwm values + _pwm_pulse_min = SERVO_MAX_PULSE * (1.0 + (1.0 - rc_servo_cal_min->get())); + _pwm_pulse_max = SERVO_MIN_PULSE * (1.0 + (1.0 - rc_servo_cal_max->get())); - if (bitnum_istrue(dir_invert_mask->get(), _axis_index)) // normal direction - swap(_pwm_pulse_min, _pwm_pulse_max); + } else { + _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get(); + _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get(); + } } } From bf0d0bfa17ebb1e21b45f51688bef6de0a1e7e0c Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 6 Mar 2021 09:22:53 -0600 Subject: [PATCH 75/82] Motor disable fix (#797) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Fixing double invert issue - On StandardStepper the enable invert was being applied twice. One in Motors.cpp and once in StandardStepper.cpp. * Update Grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser --- Grbl_Esp32/src/Motors/StandardStepper.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp index 3f5a91da..ab417919 100644 --- a/Grbl_Esp32/src/Motors/StandardStepper.cpp +++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp @@ -124,7 +124,6 @@ namespace Motors { void StandardStepper::set_direction(bool dir) { digitalWrite(_dir_pin, dir ^ _invert_dir_pin); } void StandardStepper::set_disable(bool disable) { - disable ^= step_enable_invert->get(); digitalWrite(_disable_pin, disable); } } From f04be7e341ad885fde17eefc352441267c454b27 Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 6 Mar 2021 19:00:19 -0600 Subject: [PATCH 76/82] Remove doubled settings (#799) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Update SettingsDefinitions.cpp - Double definition of direction_delay_microseconds & enable_delay_microseconds * Update Grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser --- Grbl_Esp32/src/SettingsDefinitions.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 9d7ad8f6..340ecf1a 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -400,9 +400,6 @@ void make_settings() { step_invert_mask = new AxisMaskSetting(GRBL, WG, "2", "Stepper/StepInvert", DEFAULT_STEPPING_INVERT_MASK, postMotorSetting); stepper_idle_lock_time = new IntSetting(GRBL, WG, "1", "Stepper/IdleTime", DEFAULT_STEPPER_IDLE_LOCK_TIME, 0, 255); pulse_microseconds = new IntSetting(GRBL, WG, "0", "Stepper/Pulse", DEFAULT_STEP_PULSE_MICROSECONDS, 3, 1000); - direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", 0, 0, 1000); - enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", 0, 0, 1000); // microseconds - direction_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Direction/Delay", STEP_PULSE_DELAY, 0, 1000); enable_delay_microseconds = new IntSetting(EXTENDED, WG, NULL, "Stepper/Enable/Delay", DEFAULT_STEP_ENABLE_DELAY, 0, 1000); // microseconds From 5274b4b04748d0cab18564e486ac5b59958cb5cb Mon Sep 17 00:00:00 2001 From: Luc <8822552+luc-github@users.noreply.github.com> Date: Sun, 7 Mar 2021 20:46:43 +0100 Subject: [PATCH 77/82] Clean up -WIP (#801) * Clean up * Remove not necessary Co-authored-by: Luc --- Grbl_Esp32/src/WebUI/WebServer.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index b4a1cef5..1a3f2e99 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -888,7 +888,6 @@ namespace WebUI { path = ""; _webserver->sendHeader("Cache-Control", "no-cache"); _webserver->send(200, "application/json", jsonfile); - _upload_status = UploadStatusType::NONE; } //push error code and message to websocket @@ -1226,8 +1225,8 @@ namespace WebUI { String sstatus = "Ok"; if ((_upload_status == UploadStatusType::FAILED) || (_upload_status == UploadStatusType::FAILED)) { sstatus = "Upload failed"; - _upload_status = UploadStatusType::NONE; } + _upload_status = UploadStatusType::NONE; bool list_files = true; uint64_t totalspace = 0; uint64_t usedspace = 0; @@ -1332,6 +1331,7 @@ namespace WebUI { s += " does not exist on SD Card\"}"; _webserver->send(200, "application/json", s); SD.end(); + set_sd_state(SDState::Idle); return; } if (list_files) { @@ -1408,7 +1408,6 @@ namespace WebUI { jsonfile += "}"; _webserver->sendHeader("Cache-Control", "no-cache"); _webserver->send(200, "application/json", jsonfile); - _upload_status = UploadStatusType::NONE; set_sd_state(SDState::Idle); SD.end(); } From d76a5fe2f2af2d10aa7562bd805862f4cbd2a683 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 8 Mar 2021 11:27:56 -0600 Subject: [PATCH 78/82] External mach def (#807) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Add example of external machine def - This sets some of the important timing parameters. - Removed redundant example to keep example count down * Update Grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser --- Grbl_Esp32/src/Grbl.h | 2 +- ...epstick_XYZ_v1.h => 6_pack_external_XYZ.h} | 92 +++++++++---------- 2 files changed, 46 insertions(+), 48 deletions(-) rename Grbl_Esp32/src/Machines/{6_pack_stepstick_XYZ_v1.h => 6_pack_external_XYZ.h} (57%) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 5a56918b..7c191801 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210306"; +const char* const GRBL_VERSION_BUILD = "20210308"; //#include #include diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_external_XYZ.h similarity index 57% rename from Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h rename to Grbl_Esp32/src/Machines/6_pack_external_XYZ.h index 5bc9da03..435e274c 100644 --- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h +++ b/Grbl_Esp32/src/Machines/6_pack_external_XYZ.h @@ -2,15 +2,15 @@ // clang-format off /* - 6_pack_stepstick_XYZ_v1.h + 6_pack_external_XYZ.h Covers all V1 versions V1p0, V1p1, etc Part of Grbl_ESP32 Pin assignments for the ESP32 I2S 6-axis board - 2018 - Bart Dring - 2020 - Mitch Bradley - 2020 - Michiyasu Odaki + + 2021 - Bart Dring + Grbl_ESP32 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 @@ -21,8 +21,17 @@ GNU General Public License for more details. You should have received a copy of the GNU General Public License along with Grbl_ESP32. If not, see . + + + 6 Pack Jumpers for External Drivers + The only jumper you set is the Vcc on 5V + Stallguard jumpers must not be connected + MS/SPI Do not need to be installed. It is OK to put them oonm the MS side + TMC5160 Is does not matter if this is installed or not on any side. + + */ -#define MACHINE_NAME "6 Pack Controller StepStick XYZ" +#define MACHINE_NAME "6 Pack External XYZ" #define N_AXIS 3 @@ -32,10 +41,6 @@ #define USE_I2S_OUT #define USE_I2S_STEPS //#define DEFAULT_STEPPER ST_I2S_STATIC -// === Default settings -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE - -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set #define I2S_OUT_BCK GPIO_NUM_22 #define I2S_OUT_WS GPIO_NUM_17 @@ -46,19 +51,35 @@ #define X_DISABLE_PIN I2SO(0) #define X_DIRECTION_PIN I2SO(1) #define X_STEP_PIN I2SO(2) -#define X_STEPPER_MS3 I2SO(3) // Motor Socket #2 #define Y_DIRECTION_PIN I2SO(4) #define Y_STEP_PIN I2SO(5) -#define Y_STEPPER_MS3 I2SO(6) #define Y_DISABLE_PIN I2SO(7) // Motor Socket #3 #define Z_DISABLE_PIN I2SO(8) #define Z_DIRECTION_PIN I2SO(9) #define Z_STEP_PIN I2SO(10) -#define Z_STEPPER_MS3 I2SO(11) + +/* +// Motor Socket #4 +#define A_DIRECTION_PIN I2SO(12) +#define A_STEP_PIN I2SO(13) +#define A_DISABLE_PIN I2SO(15) + +// Motor Socket #5 +#define B_DISABLE_PIN I2SO(16) +#define B_DIRECTION_PIN I2SO(17) +#define B_STEP_PIN I2SO(18) + +// Motor Socket #5 +#define C_DIRECTION_PIN I2SO(20) +#define C_STEP_PIN I2SO(21) +#define C_DISABLE_PIN I2SO(23) +*/ + + /* Socket I/O reference @@ -107,39 +128,16 @@ Socket #5 - -// // 4x Input Module in Socket #2 -// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-Switch-Input-module -#define MACRO_BUTTON_0_PIN GPIO_NUM_2 -#define MACRO_BUTTON_1_PIN GPIO_NUM_25 -#define MACRO_BUTTON_2_PIN GPIO_NUM_39 -#define MACRO_BUTTON_3_PIN GPIO_NUM_36 - -// 5V output CNC module in socket #4 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module -#define SPINDLE_TYPE SpindleType::PWM -#define SPINDLE_OUTPUT_PIN GPIO_NUM_14 -#define SPINDLE_ENABLE_PIN GPIO_NUM_13 // optional -#define LASER_OUTPUT_PIN GPIO_NUM_15 // optional -#define LASER_ENABLE_PIN GPIO_NUM_12 - - - - -// // RS485 Modbus In socket #3 -// // https://github.com/bdring/6-Pack_CNC_Controller/wiki/RS485-Modbus-Module -// #define VFD_RS485_TXD_PIN GPIO_NUM_26 -// #define VFD_RS485_RTS_PIN GPIO_NUM_4 -// #define VFD_RS485_RXD_PIN GPIO_NUM_16 - -// Example (4x) 5V Buffer Output on socket #5 -// https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module -#define USER_DIGITAL_PIN_0 I2SO(24) // No PWM -#define USER_DIGITAL_PIN_1 I2SO(25) -#define USER_DIGITAL_PIN_2 I2SO(26) // M7 on M9 Off -#define USER_DIGITAL_PIN_3 I2SO(27) // M8 on M9 Off - // ================= Setting Defaults ========================== -#define DEFAULT_X_STEPS_PER_MM 800 -#define DEFAULT_Y_STEPS_PER_MM 800 -#define DEFAULT_Z_STEPS_PER_MM 800 + +// see wiki https://github.com/bdring/Grbl_Esp32/wiki/External-Stepper-Drivers +#define DEFAULT_STEP_ENABLE_DELAY 5 // how long after enable do we wait for +#define DEFAULT_STEP_PULSE_MICROSECONDS 4 // length of step pulse. Must be greater than I2S_OUT_USEC_PER_PULSE (4) with I2S +#define STEP_PULSE_DELAY 6 // gap between enable and dir changes before step + +#define DEFAULT_STEPPING_INVERT_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_DIRECTION_INVERT_MASK (bit(X_AXIS) | bit(Y_AXIS) | bit(Z_AXIS)) +#define DEFAULT_INVERT_ST_ENABLE false + + + From ef3dc59b8a95aef689d3c20e9f84855f708ac1f3 Mon Sep 17 00:00:00 2001 From: Stefan de Bruijn Date: Thu, 11 Mar 2021 22:41:40 +0100 Subject: [PATCH 79/82] Fix VFD speed change from ISR (#811) * Motion control calls set_rpm frequently from an ISR. Unfortunately the last change added some debug information in there, which can cause the ESP32 to crash in boundary cases. * Update Grbl.h Co-authored-by: Stefan de Bruijn Co-authored-by: bdring --- Grbl_Esp32/src/Grbl.h | 2 +- Grbl_Esp32/src/Spindles/VFDSpindle.cpp | 17 +++++++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 7c191801..e17bbfff 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -22,7 +22,7 @@ // Grbl versioning system const char* const GRBL_VERSION = "1.3a"; -const char* const GRBL_VERSION_BUILD = "20210308"; +const char* const GRBL_VERSION_BUILD = "20210311"; //#include #include diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp index a332d0fd..29c575c7 100644 --- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp +++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp @@ -406,6 +406,11 @@ namespace Spindles { if (_current_state != state) { // already at the desired state. This function gets called a lot. set_mode(state, critical); // critical if we are in a job + + if (rpm != 0 && (rpm < _min_rpm || rpm > _max_rpm)) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed %d outside range:(%d,%d)", rpm, _min_rpm, _max_rpm); + } + set_rpm(rpm); if (state == SpindleState::Disable) { @@ -422,6 +427,10 @@ namespace Spindles { } } else { if (_current_rpm != rpm) { + if (rpm != 0 && (rpm < _min_rpm || rpm > _max_rpm)) { + grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed %d outside range:(%d,%d)", rpm, _min_rpm, _max_rpm); + } + set_rpm(rpm); if (rpm > _current_rpm) { @@ -524,10 +533,11 @@ namespace Spindles { // apply override rpm = rpm * sys.spindle_speed_ovr / 100; // Scale by spindle speed override value (uint8_t percent) - if (rpm < _min_rpm || rpm > _max_rpm) { - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed %d outside range:(%d,%d)", rpm, _min_rpm, _max_rpm); + if (rpm != 0 && (rpm < _min_rpm || rpm > _max_rpm)) { + // NOTE: Don't add a info message here; this method is called from the stepper_pulse_func ISR method, so + // emitting debug information could crash the ESP32. + rpm = constrain(rpm, _min_rpm, _max_rpm); - grbl_msg_sendf(CLIENT_ALL, MsgLevel::Info, "VFD: Requested speed changed to %d", rpm); } // apply limits @@ -554,7 +564,6 @@ namespace Spindles { ModbusCommand rpm_cmd; rpm_cmd.msg[0] = VFD_RS485_ADDR; - set_speed_command(rpm, rpm_cmd); rpm_cmd.critical = (rpm == 0); From 212652ebd257d8d7967c113aa6010b6443ed7c32 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 15 Mar 2021 10:46:27 -0500 Subject: [PATCH 80/82] Update Grbl.h --- Grbl_Esp32/src/Grbl.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 4fef75ad..e17bbfff 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -21,6 +21,7 @@ */ // Grbl versioning system +const char* const GRBL_VERSION = "1.3a"; const char* const GRBL_VERSION_BUILD = "20210311"; //#include From 1f2c8a37574386ed3cc5a79627a0d1a1bb96f9c5 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 16 Mar 2021 10:00:17 -0500 Subject: [PATCH 81/82] Update TMC2209_4x.h --- Grbl_Esp32/src/Machines/TMC2209_4x.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Grbl_Esp32/src/Machines/TMC2209_4x.h b/Grbl_Esp32/src/Machines/TMC2209_4x.h index 004b351b..87a2ced0 100644 --- a/Grbl_Esp32/src/Machines/TMC2209_4x.h +++ b/Grbl_Esp32/src/Machines/TMC2209_4x.h @@ -54,22 +54,22 @@ #define Z_TRINAMIC_DRIVER 2209 #define Z_STEP_PIN GPIO_NUM_2 -#define Z_DIRECTION_PIN GPIO_NUM_4 +#define Z_DIRECTION_PIN GPIO_NUM_14 #define Z_RSENSE TMC2209_RSENSE_DEFAULT #define Z_DRIVER_ADDRESS 2 #define DEFAULT_Z_MICROSTEPS 16 #define A_TRINAMIC_DRIVER 2209 #define A_STEP_PIN GPIO_NUM_16 -#define A_DIRECTION_PIN GPIO_NUM_17 +#define A_DIRECTION_PIN GPIO_NUM_15 #define A_RSENSE TMC2209_RSENSE_DEFAULT #define A_DRIVER_ADDRESS 3 #define DEFAULT_A_MICROSTEPS 16 -#define X_LIMIT_PIN GPIO_NUM_36 -#define Y_LIMIT_PIN GPIO_NUM_39 -#define Z_LIMIT_PIN GPIO_NUM_34 -#define PROBE_PIN GPIO_NUM_35 +#define X_LIMIT_PIN GPIO_NUM_35 +#define Y_LIMIT_PIN GPIO_NUM_34 +#define Z_LIMIT_PIN GPIO_NUM_39 +#define PROBE_PIN GPIO_NUM_36 // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_25 @@ -77,9 +77,9 @@ // https://github.com/bdring/6-Pack_CNC_Controller/wiki/4x-5V-Buffered-Output-Module // https://github.com/bdring/6-Pack_CNC_Controller/wiki/Quad-MOSFET-Module -#define USER_DIGITAL_PIN_0 GPIO_NUM_14 // M62 M63 +#define USER_DIGITAL_PIN_0 GPIO_NUM_4 // M62 M63 #define USER_DIGITAL_PIN_1 GPIO_NUM_13 // M62 M63 -#define USER_DIGITAL_PIN_2 GPIO_NUM_15 // M62 M63 +#define USER_DIGITAL_PIN_2 GPIO_NUM_17 // M62 M63 #define USER_DIGITAL_PIN_3 GPIO_NUM_12 // M62 M63 From 31ac7206d4e451b9a9fada773840c49876592d9e Mon Sep 17 00:00:00 2001 From: Jesse Schoch Date: Wed, 17 Mar 2021 10:10:25 -0700 Subject: [PATCH 82/82] changing to EXTENDED type from GRBL type to prevent sender issues (#821) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Devt (#819) * Fixed various small bugs (#605) * Fixed various small bugs * Fixed potential cast bug * Fixed double reporting of errors Co-authored-by: Stefan de Bruijn * Stallguard tuning (#607) * Devt (#571) * Handles Tranimic drivers errors better - If an unsupported driver is specified, it will give a message and not crash. * Cleaned up unused files Got rid of old unipolar files Got rid of servo axis feature - it is a motor class now Got rid of solenoid pen feature - never really used and it should be a motor class if it is. * Fix ENABLE_AUTHENTICATION (#569) * Fixed authentication code. * Removed another const cast Co-authored-by: Stefan de Bruijn * Fix step leakage with inverted steps (#570) * Fix step leakage with inverted steps * Update build date for merge Co-authored-by: Bart Dring Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring * Update platformio.ini Per PR 583 * Created an enum for mode * Removing some unused machine defs * Added test machine definition * Clean up for PR * Remove test machine def. Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring * Basic testing Complete * Made state variable volatile. * Homing cycle settings (#613) * Initial Tests Complete * Update Grbl.h * Update variables Co-authored-by: Mitch Bradley * fixed dual switches when inverted (#614) * fixed dual switches when inverted * Removed debug message * Cleaning up the machine defs Removed unused #defines. * Store coordinate offsets in NVS (#611) * Store coordinate offsets in NVS * Handle both old Eeprom formats * Implementing fixes (#616) - Stop creating additional tasks when limit_init() gets called again from homing and resets - Explicitly delete an object that was causing a memory loss. * Update Grbl.h * Tweak memory fix and add $H check for $Homing/Cycles * Fix G28.1 and G30.1 * Update Grbl.h * Homing cycle defaults (#624) * Changed to add homing cycle defaults There needs to be a way to set the homing cycle defaults in a machine definition. There will likely be a better way to do this in the future. * Update 10vSpindle.cpp Had wrong error message * Fixed typos and removed obsolete #defines * Probe cleanup (#625) * Cleanup probing code * Update Grbl.h * Update after review * Update error_codes_en_US.csv * More sd_close() to free memory (#622) * Changed buffer sizes to 256 throughout various parts of the program. (#626) This is a patch necessary for F360 personal users, because they decided to add a very lengthy comment... Co-authored-by: Stefan de Bruijn * $sd/show and handle settings in SD files (#629) * $sd/show and handle settings in SD files * Added $LocalFs/Show and fixed $LocalFs/Run output * Infer / at beginning of SD path name The LocalFS path processing code already inserts a / at the beginning of the path is one isn't present. This patch does the same for SD files. * Show $ command responses in WebUI console * Added $Settings/ListChanged AKA $SC This is useful for saving settings in a compact form that leaves defaults unchanged. * $sd/show works in idle or alarm state * Apply idle/alarm checks to SPIFFS files too * Changed sd_close to SD.end() sd_close was a temporary function to check for memory usage * Big BUILD_INFO fix (#632) -- Changes that affect behavior Fixed the bugs with report_build_info() Build info is no longer stored in the fixed "EEPROM" section; instead it is a proper Setting named $Firmware/Build . You can change it in the usual way with $Firmware/Build= $I without the = still works. -- Changes that affect configurability for developers Converted a couple more #defines into enums - SETTINGS_RESTORE_* and BITFLAG_RT_STATUS_* . A side effect of this is that it is no longer possible to configure the behavior of $RST=* by defining SETTINGS_RESTORE_ALL to include only a subset. I think it is a bad idea from a customer support perspective to have the meaning of this command be different for different builds. Changed some of the #define ENABLE_ names to eliminate "EEPROM" -- Changes that are purely cosmetic Clarified descriptions in Config.h, to eliminate spurious/incorrect mentions of "EEPROM" Eliminated all mentions of the name "EEPROM" except the ones that truly mean the EEPROM section, as opposed to generalized non-volatile storage. The contents of SettingsStorage.h and SettingsStorage.cpp, which were really related to coordinate storage in Eeprom, not proper settings, were moved to Eeprom.h and Eeprom.cpp. The SettingsStorage files are gone. Got rid of get_step_pin_mask() and get_direction_pin_mask() because they were just aliases for bit(). That eliminated some junk from the SettingsStorage/Eeprom files. Those files now tightly contain only the residual stuff related to the storage of coordinate data in EEPROM. * Most #defines are gone (#595) * Many more #defines bite the dust * Fixed botch in rt accessory logic * Update Probe.cpp * Update System.cpp * Typo * Fixed WebUI crash (#633) While eliminating a redundant definition of is_realtime_command(), I inadvertently introduced a recursion due to the similarity of the names "is_realtime_command()" and "is_realtime_cmd()". The solution is to eliminate the latter entirely. * Fix i2s probing hang (#608) * Fix I2S stepper hung just after the completion of motor moving * Fix recompile issue Fixed a problem with the recompile not being recompiled even if the files under the Custom folder are changed. * More comment for macOS in debug.ini * Fix the timing of calling I2S out's exclusion function and reset sequence The reset sequence did not seem to be correct, so I changed it. According to the ESP-IDF PR, the correct sequence is as follows: 1)TX module 2)DMA 3)FIFO https://github.com/espressif/esp-idf/commit/c7f33524b469e75937f003d4c06336bf4694a043#diff-27688c6b3c29373d2a2b142b8471981c * Changed the message level for I2S swtiching from warning to debug * Add some comments * Implement stepping through Motors class (#636) * Implement stepping through Motors class WIP for discussion and review - not ready to merge yet * Document Motor methods and variables .. and remove some unused ones and move some that are subclass-specific * Move position_min/max to Limits.cpp ... and coalesced other uses thereof into a unified scheme. * Call motor ->init() explicitly instead of implicitly This makes it possible to inherit constructors without spurious config messages. * Fixed problems with I2S * Changes in class method override syntax per atlaste * Fixed oops * More Motors simplification a) Eliminated can_home() in favor of a return value from set_homing_mode() b) Eliminated axis_name() in favor of reportAxisNameMsg() * Fixes to RcServo and Trinamic - RC Servo was not handling disable ... probably old issue - Display test after config * More tweaks * Define that variable! * Move functions from Motors.cpp to subclasses Created a Servo base class from which RcServo and Dynamixel2 are derived. This gets the servo update task out of Motors. It also eliminates the need for type_id. Now all of the functions that are specific to particular kinds of motors are within their subclasses * Adding Dynamixel to ABC axes. * Removed second #ifndef SPINDLE_TYPE * Fixed potential leak in Report.cpp as reported by @atlaste * Some servo cleanup. Has errors! * min should be max * Removed test rcservo machine definition. * Removed obsolete #defines in machine defs for RcServo cal Co-authored-by: bdring * Cleaned up AMASS code (#635) * Cleaned up AMASS code More #defines gone 74 lines shorter Tested by comparing the result of original AMASS computation code to the new code with values surrounding all of the cutoff frequencies. * I2SOut tick calculation * Sorted out units for stepper pulse periods I tried to make it clear what the units are at different places in the code, and to use argument datatypes that clearly show the value range at different points, instead of relying on implicit type promotion. Hopefully this will make it easier to understand when, where, and why unit conversions occur. * Update Stepper.h * Deleted AMASS Config.h option ... as it is no longer optional * Use less memory (#644) a) closeFile() now does SD.end() to release memory after running a file from SD. b) Several task stacks are smaller c) All tasks now check their free space if DEBUG_REPORT_STACK_FREE is defined. platformio.ini has a commented-out line that can be uncommented to turn that on. d) Similarly, platformio.ini can turn on DEBUG_REPORT_HEAP_SIZE e) Fixed a small leak that occurred when listing local files. With these changes, the heap size tends to hover around 53K, dropping to about 37K when running a file from SD. * Add coolant pin messages to startup (#647) * Add coolant pin messages to startup Help with user support. * Removing incorrect STEPPER_RESET definition * Fix laser mode startup message * cleanup - coolant_init() will behave as before - update build date - return default machine to test_drive * Move CoreXY out of main Grbl (#653) * Created branch * WIP * Update parallel_delta.cpp * Wip * WIP * Wip * Still working on kinematics - Added an interface into the jogging section * WIP * WIP * wip * WIP * WIP * Wip * WIP * WIP * Wip * Update machine defs * Created branch * WIP * Update parallel_delta.cpp * Wip * WIP * Wip * Still working on kinematics - Added an interface into the jogging section * WIP * WIP * wip * WIP * WIP * Wip * WIP * WIP * Wip * Update machine defs * Machine def change. Moved switches to module 1 * WIP * Cleanup before P.R. - Fixed ranges for delta geometry - Added post homing delay option for servos - renamed and removed old machine defs. * Fixing initialization problem when not in USE_KINEMATICS mode * Fixing Git Mess * Publishing Branch - Not ready yet. Issues with Z axis - Need to add midTbot option * WIP - Seems to be fully functional now. - Need to add midTbot option. * Update CoreXY.cpp * I think it is ready for PR - fixed $RST=# - added midTbot geometry factor * Fine tune midtbot definition * Removed more unneeded corexy code. * Fixed doubled #define in machine def file. * Update after review comments * Added $A AKA Alarms/List command (#654) * Added $A AKA Alarms/List command Similar to $E AKA Errors/List $E used to be AKA ErrorCodes/List Also added $Errors/Verbose setting to display full error text instead of the error number. It defaults to true because it works with every sender I have tried so far - cncjs, UGS, and Chrome GCode Sender. If you have problems with some sender you can set it to false. * Added static_assert per atlaste's comment * Added a default and fixed Authentication issue Co-authored-by: bdring * TMC2130 plotter machine servo config update (#657) * TMC2130 plotter machine servo config update based on Slack conversation https://buildlog.slack.com/archives/CBZKZ8LHL/p1604243530253000 * Update Grbl.h * Trinamic reporting (#656) * Enhanced reporting of errors * Change "motor" to "driver" for clarity. * Added better way to show changed Setting values from Mitch * Update build date * Machine Definition Cleanup (#658) - Removed machine definitions to speed up testing. - Moved 6 pack CS/MS3 pins with other axis pins to help them stay in sync with the aixs letters * Spindle delay and Telnet Fix (#676) * Removed early saving of old state Was causing later tests to be wrong * Update Grbl.h * Update TelnetServer.cpp Remove filtering of '\r' character. * ABC Bresenham counter init fix * Rst responses (#679) * Added verification of changes from $RST command When sending $RST=$ you only get these responses. [MSG:WiFi reset done] [MSG:BT reset done] Added the other things that change. [MSG:WiFi reset done] [MSG:BT reset done] [MSG:Settings reset done] [MSG:Postion offsets reset done] * Update ProcessSettings.cpp * Update Grbl.h * Update ProcessSettings.cpp * Fix Spindle State broken in earlier PR * Update Grbl.h * Spindle and laser (#683) * WIP * Updates * Updates - Added Laser/FullPower - Move some stuff from PWM to Laser * WIP * Used the stop function before resetiing pins. * Updates from discussion - Reset_pins is now deinit() - VFD task is now deleted when ... deinit() - Added a Motor/Disable command * Added Mitch's gambit * Cleanup - Finished VFD - Fixed Settings (Thanks Brian!) - changed task cores. * Update VFDSpindle.cpp * Update Laser.cpp * Fixing reset - gpio_reset_pin sets a pullup, which could turn on a device * Changed Spindle messages to CLIENT_ALL * Update Grbl.h * Updates after review * P.R. Cleanup * Most spindle settings cause a new init() * Laser mode (#692) * Update Machine.h * spindles now say if in laser mode * name fix * Updates * Getting rid of crosstalk * Update PWMSpindle.cpp * Reset some values at spindle init() * Update SettingsDefinitions.cpp * Update Grbl.h * Return to test_drive.h * User macro button (#685) * Test Macro Button Idea * Updates * Formating * Changed macro pin reporting to be a single character * Sd Web UI issues (#698) * Updates * returned reportTaskStackSize(uxHighWaterMark); In a #ifdef DEBUG_TASK_STACK guard * Disallow web commands unless idle or alarm state * merging stuff after review * Handle SD busy state in webserver handler (#697) * Handle SD busy state in webserver handler * Update index.html.gz * Fixed reporting * Add case for SD not enabled. * Prevent Web commands except in idle or alarm * Return authentication to the default Co-authored-by: Mitch Bradley Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com> * Update axis squaring checking (#699) * Reverting some spindle changes... CLIENT_ALL caused queue issues * Rate Adjusting Fix * Fix SD card hanging on bad gcode * Fix hang on error 20 from SD/Run (#701) * Fixed strange WCO values on first load (#702) When loading Grbl_Esp32 into a fresh ESP32, the WCOs would often have strange, very large, values. The problem was the code that tries to propagate data from the old "Eeprom" storage format into the new NVS scheme. The old format had a broken checksum computation that made the checksum so weak that if succeeds about half the time on random data. The solution is to get rid of all that old code. The downside is that migration from a build that uses the old format will lose the WCO values. The user will have to reestablish them. Subsequent updates between different versions that both use the new NVS format will propagate WCO values correctly. * Fixes to homing (#706) * Fixes to homing * Update Grbl.h * Clean up after code review. * Trinamic uart (#700) * WIP * WIP * Updates * Update Grbl.h * Removing some test machine definitions * TMC5160 Drivers were not in tests * Fix a few issues with VFDSpindle critical error handling (#705) If a command is critical and fails to receive a response, it should trigger an Alarm. However, because the critical check was only evaluated if the spindle was not already unresponsive, it meant that a critical command failure would be silently ignored if a non-critical command failed before it (putting the VFDSpindle in unresponsive state). Therefore, I've moved the critical check to occur regardless of whether the spindle was already unresponsive. Second, I believe that setting `sys_rt_exec_alarm` is not sufficient to stop the machine and put it into alarm state. Other alarm conditions (such as hard limits) also run an `mc_reset()` to stop motion first. It appears that without this, motion will not be stopped, and in fact, the alarm appears to get cleared if it occurs during motion! * Update per P.R. #704 on main * Update Motors.cpp * Fix undefined probe reporting if inverted. * Settings filtering via regular expressions (#717) * Settings filtering via regular expressions Implements only the most basic - and the most useful - special characters - ^$.* If the search string does not contain a special character, it is interpreted as before. Otherwise the match is either more strict if anchored by ^ or $, or less strict if it contains a . wildcard or a * repetition. * Commentary * Eliminated . metacharacter * Fix SD/List repetition error (#727) * Fix SD/List repetition error The one line change that actually fixes it is Serial.cpp line 162, where the SD state is compared to "not busy" instead of "is idle", thus also handling the "not present" case. In the process, I converted the "const int SDCARD_ ..." to an SDState enum class, thus proving type safety and eliminating yet another untyped uint8_t . * Updates after testing Co-authored-by: bdring * Fixed RcServo Cals * PWM fix and simplification (#722) * PWM fix and simplification This is an alternative solution to the PWM/ISR/float problem. 1. The set_level() argument is the exact value that is passed to the LEDC Write function. It can be interpreted as the numerator of a rational fraction whose denominator is the max PWM value, i.e. the precision, == 1 << _resolution_bits 2. There is a new denominator() method that returns the precision. 3. The sys_pwm_control(mask, duty, synchronize) function is replaced by two functions sys_analog_all_off() and sys_set_analog(io_num, duty). This closely matches the actual usage. The old routine was called from two places, one where the mask was alway 0xFF, the duty was always 0, and synchronize was always false. That is the one that was troublesome from ISR context. The other call always affected a single pin, so the mask formulation with its loop was useless extra baggage. By splitting into two functions, each one becomes much simpler, thus faster and easier to understand. The "synchronize" argument and associated logic moved out to the caller (GCode.cpp), which more closely reflects the behavioral logic. 4. For symmetry, sys_io_control() was similarly split. 5. sys_calc_pwm_precision() was moved into UserOutput.cpp since is it driver specific, not a general system routine. * Update Grbl.h * Delete template.h Co-authored-by: bdring * TMC2209 Stallguard (#748) * TMC2209 Stallguard - Added StallGuard homing support to TMC2209 (UART) - Killed off TMC2208 for now. Too many conflicts with TMC2209. Will return with Diamond motor class hierarchy - Increase StallGuard setting range for TMC2209. Constrianed in each class to actual limits - Added a machine def to test TMC2209 * Update build date * Web cmd modes (#754) * Update System.cpp * WebCommand with configurable modes * Added a few more ESP commands to work in anu state * Update Grbl.h Co-authored-by: Mitch Bradley * Updates from PWM_ISR_Fix branch (#755) - $Message/Level - ISR safe ledcWrite * Core XY fixes (#756) * Updates for CoreXY * Delete fystec_ant.h * Parking delay fix (#770) * Changed delay type - mc_dwell was causing a recursive loop the overflowed the stack - See https://discord.com/channels/780079161460916227/786061602754396160/809288050387189782 * Changed spindle delays from floats to ints in spindle classes - Used local copies, because I did not want to change/break the existing setting. * Cleaning up parking - Added a coolant delay setting - Made an enum class for the dwell types - Got rid of the safety door sepcific delays * Update Grbl.h * Enable per motor fix (#771) * - moved invert option in front of per motor enables. * Added code to prevent motors_set_disable() from setting values that already exist. * Added the enable delay from PR 720 * Adding a defined default for step enable delay * Fixing feed rates with kinematics and arcs. - Kinematics changes the feed rate and the loop that creates arc was re-using the altered rate. This caused a runaway situation. * SD Upload fix by luc (#779) * Configure motors after I/O pins (#742) So machine definitions can change the SPI pins before we talk to any Trinamic drivers. * 1.0.5 compilation fixes (#782) * Fix compilations error due to new enum in esp32 core release 1.0.5 * Update Grbl.h Co-authored-by: Luc * Introduced delays for enable and direction pins (#720) * Added some timing changes to steppers. Basically this should introduce a small (configurable) delay for enable and direction pins. * Updates after testing * Fixed small casting bug * Fixed subtractions as per @Mitch's comments * Added STEP_PULSE_DELAY handling for compatibility with RMT_STEPS. Functionality should be unchanged. * Updates to help merge Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * Vfd and mc_delay issues. Includes VFD sync speed. (#765) * Added some timing changes to steppers. Basically this should introduce a small (configurable) delay for enable and direction pins. * Updates after testing * Fixed small casting bug * Fixed subtractions as per @Mitch's comments * Fixed busy flag in Stepper.cpp * - Changed mc_dwell to double's, because float's have issues with context switches - Changed spinup/spindown of VFD's to something more sensible - Implemented advanced wait for spinup/spindown of VFD's * Changed machine defs * Implemented spindle speed sync. Made spindle safety polling optional; all the chatter appears to give issues somehow, not sure why. * Made safety-polling optional. * Added 'todo' for actual RPM code. Not sure how this should work though. * Added safety check for unchanged RPM. Changed error output channels. * Enabled the RPM code. * Commented out again. * Changed initialization sequence. Implemented Huanyang initialization sequence. * Made some fixes according to our latest insights in the manual * Fixed docs. * Fixed another bug in the comments * Fixed set_speed RPM. * Fixed huanyang message parsing. Fixed comments. * Forgot a fall-through return. * Updated status message after VFD initializes * Changed output RPM test to output frequency test for sync. * Changed mc_dwell to milliseconds and made it an integer. Fixed bug with negative values. Fixed a dwelling bug in the VFD code, which would dwell indefinitely in cases such as safety door. * Fixed a few small issues during the devt merge. Nothing serious. * Not having critical initialization makes sense for the collet wrench lock thing (or just VFD's that are powered up later) in the sense that initialization should eventually happen to get the right values. Moreover, when the VFD enters an unresponsive state, re-initialization happens eventually. * Changed 50 to 5000 for huanyang frequency calc. * Updates after testing Huanyang * Ran clang-format. Cleaned up the code a bit. Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * Bug fixes (#790) * Fix memory leak when answering webcommand (regression issue) Fix wrong error code on web command (regression issue) * Fix sd code not disabled when SD Card feature is disabled * Use proper error codes / messages * Update Grbl.h * Revert "Use proper error codes / messages" This reverts commit ad49cf8cc1cb2064a8f7687cb4c354eba4bc8172. * Updated error code symbols and text - To be generic file system error Co-authored-by: Luc Co-authored-by: bdring * Get next rmt chan num fix (#793) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Update - moved get_next_RMT_chan_num() to constructor - [Discord discussion](https://discord.com/channels/780079161460916227/786364602223951882/817056437016592384) Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser * Rc servo updates (#794) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * RcServo Updates - Improved disable. It was not always initially set correctly. - Improved calibration. Now a calibration value greater than 1 moves the motor in a positive direction and a value less than 1 moves it in a negative direction regardless of min/max and direction inverts. * Update Grbl.h Update grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser * Motor disable fix (#797) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Fixing double invert issue - On StandardStepper the enable invert was being applied twice. One in Motors.cpp and once in StandardStepper.cpp. * Update Grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser * Remove doubled settings (#799) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Update SettingsDefinitions.cpp - Double definition of direction_delay_microseconds & enable_delay_microseconds * Update Grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser * Clean up -WIP (#801) * Clean up * Remove not necessary Co-authored-by: Luc * External mach def (#807) * Initial release with custom machine * initial release * Wrong addition corrected. AmoutOfToolChanges counts up now. * pressing Z probe button once is fine, not a second time. reporting not in sequence to function flow * First working FreeRTOS task with static text. Send "471" every second to all outputs * Z Probe triggered correctly at button pressed. Can read out all values * gitignore * running with state machine, repeatable results in test environment * works, except that function "user_tool_change" starts the tool change and it will interfer with further g code running in parallel. tbd * typo * back to without RTOS.But doesnt move * RTOS but only with "T1 m06". not with g code program * hold is better than door * initial wifi settings * umlaute * gitignore * Fehler bei Limit_Mask * Spindle_Type angepasst * lower debounding after adding capacitor * Revert "lower debounding after adding capacitor" This reverts commit eadbec23596bf6b46ec750a5ae519e8f1893876e. * remove customized gitignore * Revert "remove customized gitignore" This reverts commit ce44131c7afdb53964067073e4a52ab8e3fe8f4b. * reduce debounding period due to adding capacitor * uncomment all tool change source code * Tets Fräse 2.6 ok * Fräse 2.6 * Falscher GPIO Pin für Fräse * test * Revert "test" This reverts commit 1265435786eb327226553c5178f942cd96f08888. * No Bluetooth necessary * OTA update (watch Windows firewall!) * - rename custom machine file name -added "4axis_xyza.txt" to store Grbl_ESP32 config parameters. So programming fits to parameters - added 2 buttons (#1 hold/resume), #2 homing and tool change position * new Z probe button * Z probe corretion * Z probe correction * Z probe correction * Fixed Grbl.h - I think I deleted the GRBL_VERSION line by accident when using the web based conflict editor * Revert "Merge branch 'Devt' of https://github.com/bdring/Grbl_Esp32 into Devt" This reverts commit 361558b6b74f1ab6ae60fa250207081b941f6c20, reversing changes made to 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5. * Revert "Merge remote-tracking branch 'Grbl_Esp32_JH/master' into Devt" This reverts commit 811646f5e77ce838082ee3c89a5ea99aa6ef9fd5, reversing changes made to a61ab51c0bab36195644f7438dc82e84af32a5a9. * Add example of external machine def - This sets some of the important timing parameters. - Removed redundant example to keep example count down * Update Grbl.h Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: Mitch Bradley Co-authored-by: JensHauser * Fix VFD speed change from ISR (#811) * Motion control calls set_rpm frequently from an ISR. Unfortunately the last change added some debug information in there, which can cause the ESP32 to crash in boundary cases. * Update Grbl.h Co-authored-by: Stefan de Bruijn Co-authored-by: bdring * Update Grbl.h Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring Co-authored-by: odaki Co-authored-by: Pete Wildsmith Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com> Co-authored-by: Scott Bezek Co-authored-by: Florian Ragwitz Co-authored-by: Luc Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: JensHauser * changing to EXTENDED type from GRBL type to prevent sender issues when running 1585 Co-authored-by: bdring Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn Co-authored-by: Mitch Bradley Co-authored-by: Bart Dring Co-authored-by: odaki Co-authored-by: Pete Wildsmith Co-authored-by: Luc <8822552+luc-github@users.noreply.github.com> Co-authored-by: Scott Bezek Co-authored-by: Florian Ragwitz Co-authored-by: Luc Co-authored-by: Jens Hauser <44340656+JensHauser@users.noreply.github.com> Co-authored-by: JensHauser Co-authored-by: me --- Grbl_Esp32/src/SettingsDefinitions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp index 340ecf1a..cb83afcc 100644 --- a/Grbl_Esp32/src/SettingsDefinitions.cpp +++ b/Grbl_Esp32/src/SettingsDefinitions.cpp @@ -360,8 +360,8 @@ void make_settings() { spindle_enable_invert = new FlagSetting(GRBL, WG, NULL, "Spindle/Enable/Invert", DEFAULT_INVERT_SPINDLE_ENABLE_PIN, checkSpindleChange); // GRBL Non-numbered settings - startup_line_0 = new StringSetting(GRBL, WG, "N0", "GCode/Line0", "", checkStartupLine); - startup_line_1 = new StringSetting(GRBL, WG, "N1", "GCode/Line1", "", checkStartupLine); + startup_line_0 = new StringSetting(EXTENDED, WG, "N0", "GCode/Line0", "", checkStartupLine); + startup_line_1 = new StringSetting(EXTENDED, WG, "N1", "GCode/Line1", "", checkStartupLine); // GRBL Numbered Settings laser_mode = new FlagSetting(GRBL, WG, "32", "GCode/LaserMode", DEFAULT_LASER_MODE);