From fbf1732efae8c276d4b1d14c26ad6b0705339255 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 1 Sep 2020 16:53:53 -0500 Subject: [PATCH 1/8] 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 --- Grbl_Esp32/src/Grbl.cpp | 7 +- Grbl_Esp32/src/Grbl.h | 8 +- Grbl_Esp32/src/Machines/3_pack_v1.h | 83 ++++ .../src/Machines/esp32_printer_controller.h | 4 - Grbl_Esp32/src/Machines/foo_6axis.h | 7 - Grbl_Esp32/src/Machines/pen_laser.h | 6 - Grbl_Esp32/src/Machines/template.h | 4 - Grbl_Esp32/src/Machines/tmc2130_pen.h | 18 +- Grbl_Esp32/src/Motors/Motor.h | 5 +- Grbl_Esp32/src/Motors/Motors.cpp | 8 +- Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 25 +- Grbl_Esp32/src/ServoAxis.cpp | 382 ------------------ Grbl_Esp32/src/ServoAxis.h | 101 ----- Grbl_Esp32/src/SolenoidPen.cpp | 107 ----- Grbl_Esp32/src/SolenoidPen.h | 68 ---- Grbl_Esp32/src/Stepper.cpp | 29 +- Grbl_Esp32/src/Stepper.h | 3 - Grbl_Esp32/src/WebUI/WebServer.cpp | 16 +- Grbl_Esp32/src/grbl_unipolar.cppold | 214 ---------- Grbl_Esp32/src/grbl_unipolar.hold | 53 --- 20 files changed, 129 insertions(+), 1019 deletions(-) create mode 100644 Grbl_Esp32/src/Machines/3_pack_v1.h delete mode 100644 Grbl_Esp32/src/ServoAxis.cpp delete mode 100644 Grbl_Esp32/src/ServoAxis.h delete mode 100644 Grbl_Esp32/src/SolenoidPen.cpp delete mode 100644 Grbl_Esp32/src/SolenoidPen.h delete mode 100644 Grbl_Esp32/src/grbl_unipolar.cppold delete mode 100644 Grbl_Esp32/src/grbl_unipolar.hold diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp index 04e95d67..3ba79c38 100644 --- a/Grbl_Esp32/src/Grbl.cpp +++ b/Grbl_Esp32/src/Grbl.cpp @@ -45,12 +45,7 @@ void grbl_init() { #ifdef USE_PEN_SERVO servo_init(); #endif -#ifdef USE_SERVO_AXES - init_servos(); -#endif -#ifdef USE_PEN_SOLENOID - solenoid_init(); -#endif + #ifdef USE_MACHINE_INIT machine_init(); // user supplied function for special initialization #endif diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h index 6e686da6..35271cf5 100644 --- a/Grbl_Esp32/src/Grbl.h +++ b/Grbl_Esp32/src/Grbl.h @@ -23,7 +23,7 @@ // Grbl versioning system #define GRBL_VERSION "1.3a" -#define GRBL_VERSION_BUILD "20200823" +#define GRBL_VERSION_BUILD "20200828" //#include #include @@ -85,12 +85,6 @@ # endif #endif -#include "SolenoidPen.h" - -#ifdef USE_SERVO_AXES -# include "ServoAxis.h" -#endif - #ifdef USE_I2S_OUT # include "I2SOut.h" #endif diff --git a/Grbl_Esp32/src/Machines/3_pack_v1.h b/Grbl_Esp32/src/Machines/3_pack_v1.h new file mode 100644 index 00000000..c8066726 --- /dev/null +++ b/Grbl_Esp32/src/Machines/3_pack_v1.h @@ -0,0 +1,83 @@ +#pragma once +// clang-format off + +/* + 3_pack_v1.h + + Covers all V1 versions V1p0, V1p1, etc + + Part of Grbl_ESP32 + Pin assignments for the ESP32 I2S 6-axis board + + 2020 - 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 + (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 "3 Pack Controller V1" + + +#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set + +#define X_STEPPER_MS3 GPIO_NUM_4 // X_CS (works for all drivers on this board) + +#define X_STEP_PIN GPIO_NUM_32 +#define X_DIRECTION_PIN GPIO_NUM_27 + +#define Y_STEP_PIN GPIO_NUM_33 +#define Y_DIRECTION_PIN GPIO_NUM_26 + +#define Z_STEP_PIN GPIO_NUM_25 +#define Z_DIRECTION_PIN GPIO_NUM_17 + +#define STEPPERS_DISABLE_PIN GPIO_NUM_16 + +/* Socket #1 ref +#1 GPIO_NUM_36 // Input Only (X_LIMIT_PIN) +#2 GPIO_NUM_39 // Input Only (Y_LIMIT_PIN) +#4 GPIO_NUM_34 // Input Only (Z_LIMIT_PIN) +#4 GPIO_NUM_35 // Input Only +*/ + +/* Socket #2 ref +#1 GPIO_NUM_2 +#2 GPIO_NUM_21 +#4 GPIO_NUM_22 +#4 N/C +*/ + +/* Socket #3 ref +#1 GPIO_NUM_14 +#2 GPIO_NUM_13 +#4 GPIO_NUM_15 +#4 GPIO_NUM_12 +*/ + +// Socket #1 +#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 + +/* +// Socket #3 5V Output +#define SPINDLE_TYPE SPINDLE_TYPE_LASER +#define SPINDLE_OUTPUT_PIN GPIO_NUM_14 // labeled SpinPWM +#define SPINDLE_ENABLE_PIN GPIO_NUM_13 // labeled SpinEnbl +#define COOLANT_MIST_PIN GPIO_NUM_15 +#define COOLANT_FLOOD_PIN GPIO_NUM_12 +*/ + +// Socket #3 5V Output +#define USER_DIGITAL_PIN_1 GPIO_NUM_14 +#define USER_DIGITAL_PIN_2 GPIO_NUM_13 +#define USER_DIGITAL_PIN_3 GPIO_NUM_15 +#define USER_DIGITAL_PIN_4 GPIO_NUM_12 diff --git a/Grbl_Esp32/src/Machines/esp32_printer_controller.h b/Grbl_Esp32/src/Machines/esp32_printer_controller.h index 8192971c..e41ba8f0 100644 --- a/Grbl_Esp32/src/Machines/esp32_printer_controller.h +++ b/Grbl_Esp32/src/Machines/esp32_printer_controller.h @@ -132,14 +132,10 @@ // === 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 USE_SERVO_AXES //#define SERVO_Z_PIN GPIO_NUM_15 // It cannot be used when JTAG debugging //#define SERVO_Z_RANGE_MIN 0.0 //#define SERVO_Z_RANGE_MAX 5.0 -//#define SERVO_Z_HOMING_TYPE SERVO_HOMING_TARGET // during homing it will instantly move to a target value -//#define SERVO_Z_HOME_POS SERVO_Z_RANGE_MAX // move to max during homing -//#define SERVO_Z_MPOS false // will not use mpos, uses work coordinates // === Homing cycles // The default homing order is Z first (HOMING_CYCLE_0), diff --git a/Grbl_Esp32/src/Machines/foo_6axis.h b/Grbl_Esp32/src/Machines/foo_6axis.h index adaad7c7..7dc1d8ef 100644 --- a/Grbl_Esp32/src/Machines/foo_6axis.h +++ b/Grbl_Esp32/src/Machines/foo_6axis.h @@ -52,20 +52,13 @@ // C is a servo // servos -#define USE_SERVO_AXES #define SERVO_Z_PIN GPIO_NUM_22 #define SERVO_Z_RANGE_MIN 0.0 #define SERVO_Z_RANGE_MAX 5.0 -#define SERVO_Z_HOMING_TYPE SERVO_HOMING_TARGET // during homing it will instantly move to a target value -#define SERVO_Z_HOME_POS SERVO_Z_RANGE_MAX // move to max during homing -#define SERVO_Z_MPOS false // will not use mpos, uses work coordinates #define SERVO_C_PIN GPIO_NUM_2 #define SERVO_C_RANGE_MIN 0.0 #define SERVO_C_RANGE_MAX 5.0 -#define SERVO_C_HOMING_TYPE SERVO_HOMING_TARGET // during homing it will instantly move to a target value -#define SERVO_C_HOME_POS SERVO_C_RANGE_MAX // move to max during homing -#define SERVO_C_MPOS false // will not use mpos, uses work coordinates // limit switches #define X_LIMIT_PIN GPIO_NUM_21 diff --git a/Grbl_Esp32/src/Machines/pen_laser.h b/Grbl_Esp32/src/Machines/pen_laser.h index fbe517e1..3aef4f45 100644 --- a/Grbl_Esp32/src/Machines/pen_laser.h +++ b/Grbl_Esp32/src/Machines/pen_laser.h @@ -52,7 +52,6 @@ #define Y_LIMIT_PIN GPIO_NUM_4 #define USING_SERVO // uncomment to use this feature -//#define USING_SOLENOID // uncomment to use this feature #ifdef USING_SERVO #define Z_SERVO_PIN GPIO_NUM_27 @@ -60,11 +59,6 @@ #define Z_SERVO_RANGE_MAX 10.0 #endif -#ifdef USING_SOLENOID - #define USE_PEN_SOLENOID - #define SOLENOID_PEN_PIN GPIO_NUM_16 -#endif - #define SPINDLE_TYPE SPINDLE_TYPE_NONE // defaults diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h index f795338e..1c0fc3f8 100644 --- a/Grbl_Esp32/src/Machines/template.h +++ b/Grbl_Esp32/src/Machines/template.h @@ -130,14 +130,10 @@ // === 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 USE_SERVO_AXES // #define SERVO_Z_PIN GPIO_NUM_22 // #define SERVO_Z_RANGE_MIN 0.0 // #define SERVO_Z_RANGE_MAX 5.0 -// #define SERVO_Z_HOMING_TYPE SERVO_HOMING_TARGET // during homing it will instantly move to a target value -// #define SERVO_Z_HOME_POS SERVO_Z_RANGE_MAX // move to max during homing -// #define SERVO_Z_MPOS false // will not use mpos, uses work coordinates // === Homing cycles // The default homing order is Z first (HOMING_CYCLE_0), diff --git a/Grbl_Esp32/src/Machines/tmc2130_pen.h b/Grbl_Esp32/src/Machines/tmc2130_pen.h index 182db757..27f4c34c 100644 --- a/Grbl_Esp32/src/Machines/tmc2130_pen.h +++ b/Grbl_Esp32/src/Machines/tmc2130_pen.h @@ -57,22 +57,10 @@ #define STEPPERS_DISABLE_PIN GPIO_NUM_13 - // Define one of these 2 options for spindle or servo -#define USE_SERVO_AXES -//#define USE_SPINDLE - -#ifdef USE_SERVO_AXES - #define SPINDLE_TYPE SPINDLE_TYPE_NONE - - #define Z_SERVO_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control. - #define Z_SERVO_RANGE_MIN 0.0 - #define Z_SERVO_RANGE_MAX 5.0 -#else - - #define SPINDLE_TYPE SPINDLE_TYPE_PWM - #define SPINDLE_OUTPUT_PIN GPIO_NUM_27 -#endif +#define Z_SERVO_PIN GPIO_NUM_27 // comment this out if PWM spindle/laser control. +#define Z_SERVO_RANGE_MIN 0.0 +#define Z_SERVO_RANGE_MAX 5.0 // #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/Motor.h b/Grbl_Esp32/src/Motors/Motor.h index f3bf3629..6e82843e 100644 --- a/Grbl_Esp32/src/Motors/Motor.h +++ b/Grbl_Esp32/src/Motors/Motor.h @@ -52,10 +52,11 @@ namespace Motors { motor_class_id_t type_id; uint8_t is_active = false; + uint8_t has_errors = false; protected: - uint8_t axis_index; // X_AXIS, etc - uint8_t dual_axis_index; // 0 = primary 1=ganged + uint8_t axis_index; // X_AXIS, etc + uint8_t dual_axis_index; // 0 = primary 1=ganged bool _showError; bool _use_mpos = true; diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp index 47ea9318..5a48613b 100644 --- a/Grbl_Esp32/src/Motors/Motors.cpp +++ b/Grbl_Esp32/src/Motors/Motors.cpp @@ -220,8 +220,9 @@ void init_motors() { grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Using StepStick Mode"); - uint8_t ms3_pins[MAX_N_AXIS][2] = { { X_STEPPER_MS3, X2_STEPPER_MS3 }, { Y_STEPPER_MS3, Y2_STEPPER_MS3 }, { Z_STEPPER_MS3, Z2_STEPPER_MS3 }, - { A_STEPPER_MS3, A2_STEPPER_MS3 }, { B_STEPPER_MS3, B2_STEPPER_MS3 }, { C_STEPPER_MS3, C2_STEPPER_MS3 } }; + uint8_t ms3_pins[MAX_N_AXIS][2] = { { X_STEPPER_MS3, X2_STEPPER_MS3 }, { Y_STEPPER_MS3, Y2_STEPPER_MS3 }, + { Z_STEPPER_MS3, Z2_STEPPER_MS3 }, { A_STEPPER_MS3, A2_STEPPER_MS3 }, + { B_STEPPER_MS3, B2_STEPPER_MS3 }, { C_STEPPER_MS3, C2_STEPPER_MS3 } }; for (int axis = 0; axis < N_AXIS; axis++) { for (int gang_index = 0; gang_index < 2; gang_index++) { @@ -333,8 +334,9 @@ void motors_set_disable(bool disable) { // now loop through all the motors to see if they can individually diable for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) { - for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) + for (uint8_t axis = X_AXIS; axis < N_AXIS; axis++) { myMotor[axis][gang_index]->set_disable(disable); + } } } diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp index 42c159c7..a92d079e 100644 --- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp +++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp @@ -44,16 +44,17 @@ namespace Motors { _homing_mode = TRINAMIC_HOMING_MODE; _homing_mask = 0; // no axes homing + set_axis_name(); + if (_driver_part_number == 2130) tmcstepper = new TMC2130Stepper(cs_pin, _r_sense, spi_index); else if (_driver_part_number == 5160) tmcstepper = new TMC5160Stepper(cs_pin, _r_sense, spi_index); else { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Trinamic unsupported p/n:%d", _driver_part_number); + grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_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 return; - } - - set_axis_name(); + } init_step_dir_pins(); // from StandardStepper @@ -70,6 +71,9 @@ namespace Motors { } void TrinamicDriver::init() { + if (has_errors) + return; + SPI.begin(); // this will get called for each motor, but does not seem to hurt anything tmcstepper->begin(); @@ -78,7 +82,7 @@ namespace Motors { set_mode(false); _homing_mask = 0; - is_active = true; // as opposed to NullMotors, this is a real motor + } /* @@ -98,6 +102,8 @@ namespace Motors { } bool TrinamicDriver::test() { + if (has_errors) + return false; switch (tmcstepper->test_connection()) { case 1: grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Trinamic driver test failed. Check connection", _axis_name); @@ -148,6 +154,8 @@ namespace Motors { float hold (as a percentage of run) */ void TrinamicDriver::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; @@ -175,6 +183,8 @@ namespace Motors { Coolstep mode, so it will need to switch to Coolstep when homing */ void TrinamicDriver::set_mode(bool isHoming) { + if (has_errors) + return; if (isHoming) _mode = TRINAMIC_HOMING_MODE; else @@ -216,6 +226,8 @@ 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) + return; uint32_t tstep = tmcstepper->TSTEP(); if (tstep == 0xFFFFF || tstep < 1) // if axis is not moving return @@ -247,7 +259,8 @@ 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) { - //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "%s Axis disable %d", _axis_name, disable); + if (has_errors) + return; digitalWrite(disable_pin, disable); diff --git a/Grbl_Esp32/src/ServoAxis.cpp b/Grbl_Esp32/src/ServoAxis.cpp deleted file mode 100644 index 7f3753c2..00000000 --- a/Grbl_Esp32/src/ServoAxis.cpp +++ /dev/null @@ -1,382 +0,0 @@ -/* - ServoAxis.cpp - Part of Grbl_ESP32 - - copyright (c) 2018 - Bart Dring. This file was intended 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 . - - See ServoAxis.h for more details - -*/ - -#include "Grbl.h" - -#include "ServoAxis.h" - -#ifdef USE_SERVO_AXES - -static TaskHandle_t servosSyncTaskHandle = 0; - -# ifdef SERVO_X_PIN -ServoAxis X_Servo_Axis(X_AXIS, SERVO_X_PIN); -# endif -# ifdef SERVO_Y_PIN -ServoAxis Y_Servo_Axis(Y_AXIS, SERVO_Y_PIN); -# endif -# ifdef SERVO_Z_PIN -ServoAxis Z_Servo_Axis(Z_AXIS, SERVO_Z_PIN); -# endif - -# ifdef SERVO_A_PIN -ServoAxis A_Servo_Axis(A_AXIS, SERVO_A_PIN); -# endif -# ifdef SERVO_B_PIN -ServoAxis B_Servo_Axis(B_AXIS, SERVO_B_PIN); -# endif -# ifdef SERVO_C_PIN -ServoAxis C_Servo_Axis(C_AXIS, SERVO_C_PIN); -# endif - -void init_servos() { - // ======================== X Axis =========================== -# ifdef SERVO_X_PIN - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "X Servo range %4.3f to %4.3f", SERVO_X_RANGE_MIN, SERVO_X_RANGE_MAX); - X_Servo_Axis.init(); - X_Servo_Axis.set_range(SERVO_X_RANGE_MIN, SERVO_X_RANGE_MAX); -# ifdef SERVO_X_HOMING_TYPE - X_Servo_Axis.set_homing_type(SERVO_X_HOMING_TYPE); -# endif -# ifdef SERVO_X_HOME_POS - X_Servo_Axis.set_homing_position(SERVO_X_HOME_POS); -# endif -# ifdef SERVO_X_MPOS // value should be true or false - X_Servo_Axis.set_use_mpos(SERVO_X_MPOS); -# endif -# ifdef SERVO_X_DISABLE_ON_ALARM - set_disable_on_alarm(SERVO_X_DISABLE_ON_ALARM); -# endif -# ifdef SERVO_X_DISABLE_WITH_STEPPERS - set_disable_with_steppers(SERVO_X_DISABLE_WITH_STEPPERS); -# endif -# endif - // ======================== Y Axis =========================== -# ifdef SERVO_Y_PIN - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Y Servo range %4.3f to %4.3f", SERVO_Y_RANGE_MIN, SERVO_Y_RANGE_MAX); - Y_Servo_Axis.init(); - Y_Servo_Axis.set_range(SERVO_Y_RANGE_MIN, SERVO_Y_RANGE_MAX); -# ifdef SERVO_Y_HOMING_TYPE - Y_Servo_Axis.set_homing_type(SERVO_Y_HOMING_TYPE); -# endif -# ifdef SERVO_Y_HOME_POS - Y_Servo_Axis.set_homing_position(SERVO_Y_HOME_POS); -# endif -# ifdef SERVO_Y_MPOS // value should be true or false - Y_Servo_Axis.set_use_mpos(SERVO_Y_MPOS); -# endif -# ifdef SERVO_Y_DISABLE_ON_ALARM - set_disable_on_alarm(SERVO_Y_DISABLE_ON_ALARM); -# endif -# ifdef SERVO_Y_DISABLE_WITH_STEPPERS - set_disable_with_steppers(SERVO_Y_DISABLE_WITH_STEPPERS); -# endif -# endif - // ======================== Z Axis =========================== -# ifdef SERVO_Z_PIN - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Z Servo range %4.3f to %4.3f", SERVO_Z_RANGE_MIN, SERVO_Z_RANGE_MAX); - Z_Servo_Axis.init(); - Z_Servo_Axis.set_range(SERVO_Z_RANGE_MIN, SERVO_Z_RANGE_MAX); -# ifdef SERVO_Z_HOMING_TYPE - Z_Servo_Axis.set_homing_type(SERVO_Z_HOMING_TYPE); -# endif -# ifdef SERVO_Z_HOME_POS - Z_Servo_Axis.set_homing_position(SERVO_Z_HOME_POS); -# endif -# ifdef SERVO_Z_MPOS // value should be true or false - Z_Servo_Axis.set_use_mpos(SERVO_Z_MPOS); -# endif -# ifdef SERVO_Z_DISABLE_ON_ALARM - set_disable_on_alarm(SERVO_Z_DISABLE_ON_ALARM); -# endif -# ifdef SERVO_Z_DISABLE_WITH_STEPPERS - set_disable_with_steppers(SERVO_Z_DISABLE_WITH_STEPPERS); -# endif -# endif - // ======================== A Axis =========================== -# ifdef SERVO_A_PIN - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "A Servo range %4.3f to %4.3f", SERVO_A_RANGE_MIN, SERVO_A_RANGE_MAX); - A_Servo_Axis.init(); - A_Servo_Axis.set_range(SERVO_A_RANGE_MIN, SERVO_A_RANGE_MAX); -# ifdef SERVO_A_HOMING_TYPE - A_Servo_Axis.set_homing_type(SERVO_A_HOMING_TYPE); -# endif -# ifdef SERVO_A_HOME_POS - A_Servo_Axis.set_homing_position(SERVO_A_HOME_POS); -# endif -# ifdef SERVO_A_MPOS // value should be true or false - A_Servo_Axis.set_use_mpos(SERVO_A_MPOS); -# endif -# ifdef SERVO_A_DISABLE_ON_ALARM - set_disable_on_alarm(SERVO_A_DISABLE_ON_ALARM); -# endif -# ifdef SERVO_A_DISABLE_WITH_STEPPERS - set_disable_with_steppers(SERVO_A_DISABLE_WITH_STEPPERS); -# endif -# endif - // ======================== B Axis =========================== -# ifdef SERVO_B_PIN - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "B Servo range %4.3f to %4.3f", SERVO_B_RANGE_MIN, SERVO_B_RANGE_MAX); - B_Servo_Axis.init(); - B_Servo_Axis.set_range(SERVO_B_RANGE_MIN, SERVO_B_RANGE_MAX); -# ifdef SERVO_B_HOMING_TYPE - B_Servo_Axis.set_homing_type(SERVO_B_HOMING_TYPE); -# endif -# ifdef SERVO_B_HOME_POS - B_Servo_Axis.set_homing_position(SERVO_B_HOME_POS); -# endif -# ifdef SERVO_B_MPOS // value should be true or false - B_Servo_Axis.set_use_mpos(SERVO_B_MPOS); -# endif -# ifdef SERVO_B_DISABLE_ON_ALARM - set_disable_on_alarm(SERVO_B_DISABLE_ON_ALARM); -# endif -# ifdef SERVO_B_DISABLE_WITH_STEPPERS - set_disable_with_steppers(SERVO_B_DISABLE_WITH_STEPPERS); -# endif -# endif - // ======================== C Axis =========================== -# ifdef SERVO_C_PIN - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "C Servo range %4.3f to %4.3f", SERVO_C_RANGE_MIN, SERVO_C_RANGE_MAX); - C_Servo_Axis.init(); - C_Servo_Axis.set_range(SERVO_C_RANGE_MIN, SERVO_C_RANGE_MAX); -# ifdef SERVO_C_HOMING_TYPE - C_Servo_Axis.set_homing_type(SERVO_C_HOMING_TYPE); -# endif -# ifdef SERVO_C_HOME_POS - C_Servo_Axis.set_homing_position(SERVO_C_HOME_POS); -# endif -# ifdef SERVO_C_MPOS // value should be true or false - C_Servo_Axis.set_use_mpos(SERVO_C_MPOS); -# endif -# ifdef SERVO_C_DISABLE_ON_ALARM - set_disable_on_alarm(SERVO_C_DISABLE_ON_ALARM); -# endif -# ifdef SERVO_C_DISABLE_WITH_STEPPERS - set_disable_with_steppers(SERVO_C_DISABLE_WITH_STEPPERS); -# endif -# endif - // setup a task that will calculate the determine and set the servo positions - xTaskCreatePinnedToCore(servosSyncTask, // task - "servosSyncTask", // name for task - 4096, // size of task stack - NULL, // parameters - 1, // priority - &servosSyncTaskHandle, - 0 // core - ); -} - -// this is the task -void servosSyncTask(void* pvParameters) { - TickType_t xLastWakeTime; - const TickType_t xServoFrequency = SERVO_TIMER_INT_FREQ; // in ticks (typically ms) - xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. - while (true) { // don't ever return from this or the task dies -# ifdef SERVO_X_PIN - X_Servo_Axis.set_location(); -# endif -# ifdef SERVO_Y_PIN - Y_Servo_Axis.set_location(); -# endif -# ifdef SERVO_Z_PIN - Z_Servo_Axis.set_location(); -# endif -# ifdef SERVO_A_PIN - A_Servo_Axis.set_location(); -# endif -# ifdef SERVO_B_PIN - B_Servo_Axis.set_location(); -# endif -# ifdef SERVO_C_PIN - C_Servo_Axis.set_location(); -# endif - vTaskDelayUntil(&xLastWakeTime, xServoFrequency); - } -} - -// =============================== Class Stuff ================================= // - -ServoAxis::ServoAxis(uint8_t axis, uint8_t pin_num) { // constructor - _axis = axis; - _pin_num = pin_num; - _channel_num = sys_get_next_PWM_chan_num(); - _showError = true; // this will be used to show calibration error only once - _use_mpos = true; // default is to use the machine position rather than work position -} - -void ServoAxis::init() { - _cal_is_valid(); - ledcSetup(_channel_num, _pwm_freq, _pwm_resolution_bits); - ledcAttachPin(_pin_num, _channel_num); - disable(); -} - -void ServoAxis::set_location() { - // These are the pulse lengths for the minimum and maximum positions - // Note: Some machines will have the physical max/min inverted with pulse length max/min due to invert setting $3=... - float servo_pulse_min, servo_pulse_max; - float min_pulse_cal, max_pulse_cal; // calibration values in percent 110% = 1.1 - uint32_t servo_pulse_len; - float servo_pos, mpos, offset; - // skip location if we are in alarm mode - if (_disable_on_alarm && (sys.state == STATE_ALARM)) { - disable(); - return; - } - // track the disable status of the steppers if desired. - if (_disable_with_steppers && get_stepper_disable()) { - disable(); - return; - } - if ((_homing_type == SERVO_HOMING_TARGET) && (sys.state == STATE_HOMING)) { - servo_pos = _homing_position; // go to servos home position - } else { - mpos = system_convert_axis_steps_to_mpos(sys_position, _axis); // get the axis machine position in mm - if (_use_mpos) - servo_pos = mpos; - else { - offset = gc_state.coord_system[_axis] + gc_state.coord_offset[_axis]; // get the current axis work offset - servo_pos = mpos - offset; // determine the current work position - } - } - // 1. Get the pulse ranges of the servos - // 2. Invert if selected in the settings - // 3. Get the calibration values from the settings - // 4. Adjust the calibration offset direction of the cal based on the direction - // 5. Apply the calibrarion - servo_pulse_min = SERVO_MIN_PULSE; - servo_pulse_max = SERVO_MAX_PULSE; - if (bit_istrue(dir_invert_mask->get(), bit(_axis))) // this allows the user to change the direction via settings - swap(servo_pulse_min, servo_pulse_max); - // get the calibration values - if (_cal_is_valid()) { // if calibration settings are OK then apply them - // apply a calibration - // the cals apply differently if the direction is reverse (i.e. longer pulse is lower position) - if (bit_isfalse(dir_invert_mask->get(), bit(_axis))) { // normal direction - min_pulse_cal = 2.0 - (axis_settings[_axis]->steps_per_mm->get() / 100.0); - max_pulse_cal = (axis_settings[_axis]->max_travel->get() / 100.0); - } else { // inverted direction - min_pulse_cal = (axis_settings[_axis]->steps_per_mm->get() / 100.0); - max_pulse_cal = 2.0 - (axis_settings[_axis]->max_travel->get() / -100.0); - } - } else { // settings are not valid so don't apply any calibration - min_pulse_cal = 1.0; - max_pulse_cal = 1.0; - } - // apply the calibrations - servo_pulse_min *= min_pulse_cal; - servo_pulse_max *= max_pulse_cal; - // determine the pulse length - servo_pulse_len = (uint32_t)mapConstrain(servo_pos, _position_min, _position_max, servo_pulse_min, servo_pulse_max); - _write_pwm(servo_pulse_len); -} - -void ServoAxis::_write_pwm(uint32_t duty) { - if (ledcRead(_channel_num) != duty) // only write if it is changing - ledcWrite(_channel_num, duty); -} - -// sets the PWM to zero. This allows most servos to be manually moved -void ServoAxis::disable() { - _write_pwm(0); -} - -// checks to see if calibration values are in an acceptable range -// vebose = true if you want an error sent to serial port -bool ServoAxis::_cal_is_valid() { - bool settingsOK = true; - if ((axis_settings[_axis]->steps_per_mm->get() < SERVO_CAL_MIN) || (axis_settings[_axis]->steps_per_mm->get() > SERVO_CAL_MAX)) { - if (_showError) { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration ($10%d) value error. Reset to 100", _axis); - char reset_val[] = "100"; - axis_settings[_axis]->steps_per_mm->setStringValue(reset_val); - } - settingsOK = false; - } - // Note: Max travel is set positive via $$, but stored as a negative number - auto travel = -axis_settings[_axis]->max_travel->get(); - if ((travel < -SERVO_CAL_MAX) || travel > -SERVO_CAL_MIN) { - if (_showError) { - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Servo calibration ($13%d) value error. Reset to 100", _axis); - char reset_val[] = "-100"; // stored as a negative - axis_settings[_axis]->max_travel->setStringValue(reset_val); - } - settingsOK = false; - } - _showError = false; // to show error once - return settingsOK; -} - -/* - Use this to set the max and min position in mm of the servo - This is used when mapping pulse length to the position -*/ -void ServoAxis::set_range(float min, float max) { - if (min < max) { - _position_min = min; - _position_max = max; - } else - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Error setting range. Min not smaller than max"); -} - -/* - Sets the mode the servo will be in during homing - See servo_axis.h for SERVO_HOMING_xxxxx types -*/ -void ServoAxis::set_homing_type(uint8_t homing_type) { - if (homing_type <= SERVO_HOMING_TARGET) - _homing_type = homing_type; -} - -/* - Use this to set the homing position the servo will be commanded to go if - the current homing mode is SERVO_HOMING_TARGET -*/ -void ServoAxis::set_homing_position(float homing_position) { - _homing_position = homing_position; -} - -/* - Use this to set the disable on alarm feature. If true, then hobby servo PWM - will be disable in Grbl alarm mode (like before homing). Typical hobby servo - can be moved by hand in this mode -*/ -void ServoAxis::set_disable_on_alarm(bool disable_on_alarm) { - _disable_on_alarm = disable_on_alarm; -} - -void ServoAxis::set_disable_with_steppers(bool disable_with_steppers) { - _disable_with_steppers = disable_with_steppers; -} - -/* - If true, servo position will alway be calculated in machine position - Offsets will not be applied -*/ -void ServoAxis::set_use_mpos(bool use_mpos) { - _use_mpos = use_mpos; -} - -#endif diff --git a/Grbl_Esp32/src/ServoAxis.h b/Grbl_Esp32/src/ServoAxis.h deleted file mode 100644 index 8632f564..00000000 --- a/Grbl_Esp32/src/ServoAxis.h +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once - -/* - ServoAxis.h - Part of Grbl_ESP32 - - copyright (c) 2019 - Bart Dring. This file was intended 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 . - - Servo Axis Class - - The Servo axis feature allows you to use a hobby servo on any axis. - This is done using a repeating RTOS task. Grbl continues to calculate - the position of the axis in real time. The task looks at the current position of - the axis and calculates the required PWM value to go to that location. You define the travel - of the servo in millimeters. - - Grbl still uses the acceleration and speed values you have in the settings, so it - will coordinate servo axes with stepper motor axes. This assumes these values are within the - capabilities of the servo - - Usage - - 1. In config.h un-comment #define USE_SERVO_AXES - - 2. In the machine definition file in Machines/, define servo pins and PWM channels like this .... - #define SERVO_Y_PIN GPIO_NUM_14 - - undefine any step and direction pins associated with that axis - - 3. In servo_axis.cpp init_servos() function, configure servos like this .... - X_Servo_Axis.set_range(0.0, 20.0); // millimeter - X_Servo_Axis.set_homing_type(SERVO_HOMING_OFF); - X_Servo_Axis.set_disable_on_alarm(true); - - - The positions can be calibrated using the settings. $10x (resolution) settings adjust the minimum - position and $13x (max travel) settings adjust the maximum position. If the servo is traveling - backwards from what you want, you can use the $3 direction setting to compensate. - -*/ - -#include "Motors/RcServoSettings.h" - -#define SERVO_HOMING_OFF 0 // servo is off during homing -#define SERVO_HOMING_TARGET 1 // servo is send to a location during homing - -extern float my_location; - -void init_servos(); -void servosSyncTask(void* pvParameters); - -class ServoAxis { -public: - ServoAxis(uint8_t axis, uint8_t pin_num); // constructor - void init(); - void set_location(); - void disable(); // sets PWM to 0% duty cycle. Most servos can be manually moved in this state - void set_range(float min, float max); - void set_homing_type(uint8_t homing_type); - void set_homing_position(float homing_position); - void set_disable_on_alarm(bool disable_on_alarm); - void set_disable_with_steppers(bool disable_with_steppers); - void set_use_mpos(bool use_mpos); - -private: - int _axis; // these should be assign in constructor using Grbl X_AXIS type values - int _pin_num; // The GPIO pin being used - int _channel_num; // The PWM channel - bool _showError; - - uint32_t _pwm_freq = SERVO_PULSE_FREQ; - uint32_t _pwm_resolution_bits = SERVO_PULSE_RES_BITS; - float _pulse_min = SERVO_MIN_PULSE; // in pwm counts - float _pulse_max = SERVO_MAX_PULSE; // in pwm counts - float _position_min = SERVO_POSITION_MIN_DEFAULT; // position in millimeters - float _position_max = SERVO_POSITION_MAX_DEFAULT; // position in millimeters - - uint8_t _homing_type = SERVO_HOMING_OFF; - float _homing_position = SERVO_POSITION_MAX_DEFAULT; - bool _disable_on_alarm = true; - bool _disable_with_steppers = false; - bool _use_mpos = true; - - bool _validate_cal_settings(); - void _write_pwm(uint32_t duty); - bool _cal_is_valid(); // checks to see if calibration values are in acceptable range -}; diff --git a/Grbl_Esp32/src/SolenoidPen.cpp b/Grbl_Esp32/src/SolenoidPen.cpp deleted file mode 100644 index 7683b5a9..00000000 --- a/Grbl_Esp32/src/SolenoidPen.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* - SolenoidPen.cpp - Part of Grbl_ESP32 - - copyright (c) 2018 - Bart Dring This file was modified 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" - -#ifdef USE_PEN_SOLENOID - -int8_t solenoid_pwm_chan_num; - -static TaskHandle_t solenoidSyncTaskHandle = 0; - -// used to delay turn on -bool solenoid_pen_enable; -uint16_t solenoide_hold_count; - -void solenoid_init() { - grbl_send(CLIENT_SERIAL, "[MSG:Solenoid Mode]\r\n"); // startup message - //validate_servo_settings(true); // display any calibration errors - solenoid_pen_enable = false; // start delay has not completed yet. - solenoide_hold_count = 0; // initialize - // setup PWM channel - solenoid_pwm_chan_num = sys_get_next_PWM_chan_num(); - ledcSetup(solenoid_pwm_chan_num, SOLENOID_PWM_FREQ, SOLENOID_PWM_RES_BITS); - ledcAttachPin(SOLENOID_PEN_PIN, solenoid_pwm_chan_num); - solenoid_disable(); // start it it off - // setup a task that will calculate the determine and set the servo position - xTaskCreatePinnedToCore(solenoidSyncTask, // task - "solenoidSyncTask", // name for task - 4096, // size of task stack - NULL, // parameters - 1, // priority - &solenoidSyncTaskHandle, - 0 // core - ); -} - -// turn off the PWM (0 duty) -void solenoid_disable() { - ledcWrite(solenoid_pwm_chan_num, 0); -} - -// this is the task -void solenoidSyncTask(void* pvParameters) { - int32_t current_position[N_AXIS]; // copy of current location - float m_pos[N_AXIS]; // machine position in mm - TickType_t xLastWakeTime; - const TickType_t xSolenoidFrequency = SOLENOID_TIMER_INT_FREQ; // in ticks (typically ms) - uint16_t solenoid_delay_counter = 0; - xLastWakeTime = xTaskGetTickCount(); // Initialise the xLastWakeTime variable with the current time. - while (true) { // don't ever return from this or the task dies - if (!solenoid_pen_enable) { - solenoid_delay_counter++; - solenoid_pen_enable = (solenoid_delay_counter > SOLENOID_TURNON_DELAY); - } else { - memcpy(current_position, sys_position, sizeof(sys_position)); // get current position in step - system_convert_array_steps_to_mpos(m_pos, current_position); // convert to millimeters - calc_solenoid(m_pos[Z_AXIS]); // calculate kinematics and move the servos - } - vTaskDelayUntil(&xLastWakeTime, xSolenoidFrequency); - } -} - -// calculate and set the PWM value for the servo -void calc_solenoid(float penZ) { - uint32_t solenoid_pen_pulse_len; - if (!solenoid_pen_enable) // only proceed if startup delay as expired - return; - if (penZ < 0 && (sys.state != STATE_ALARM)) { // alarm also makes it go up - solenoide_hold_count = 0; // reset this count - solenoid_pen_pulse_len = 0; // - } else { - if (solenoide_hold_count < SOLENOID_PULSE_LEN_HOLD) { - solenoid_pen_pulse_len = SOLENOID_PULSE_LEN_UP; - solenoide_hold_count++; - } else - solenoid_pen_pulse_len = SOLENOID_PULSE_LEN_HOLD; - } - // skip setting value if it is unchanged - if (ledcRead(solenoid_pwm_chan_num) == solenoid_pen_pulse_len) - return; - // update the PWM value - // ledcWrite appears to have issues with interrupts, so make this a critical section - portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED; - portENTER_CRITICAL(&myMutex); - ledcWrite(solenoid_pwm_chan_num, solenoid_pen_pulse_len); - portEXIT_CRITICAL(&myMutex); -} - -#endif diff --git a/Grbl_Esp32/src/SolenoidPen.h b/Grbl_Esp32/src/SolenoidPen.h deleted file mode 100644 index 32332c74..00000000 --- a/Grbl_Esp32/src/SolenoidPen.h +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -/* - SolenoidPen.h - Part of Grbl_ESP32 - - copyright (c) 2018 - Bart Dring This file was modified 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 . - - Usage notes: - This is designed to use a solenoid to lift a pen. - When the current Z location is below zero the pen is down - If the Z goes to zero or above the pen goes up. - There are two power levels, the initial pull up strength, then the hold strength - - - Note: There is a still a virtual Z axis that has a finite speed. - If your gcode is commanding long travels in Z, there will be delays - between solenoid states as the Z "travels" to the location that will - change the state. - -*/ - -#ifndef SOLENOID_PWM_FREQ -# define SOLENOID_PWM_FREQ 5000 -#endif - -#ifndef SOLENOID_PWM_RES_BITS -# define SOLENOID_PWM_RES_BITS 8 -#endif - -#ifndef SOLENOID_TURNON_DELAY -# define SOLENOID_TURNON_DELAY (SOLENOID_TIMER_INT_FREQ / 2) -#endif - -#ifndef SOLENOID_PULSE_LEN_UP -# define SOLENOID_PULSE_LEN_UP 255 -#endif - -#ifndef SOLENOID_HOLD_DELAY -# define SOLENOID_HOLD_DELAY (SOLENOID_TIMER_INT_FREQ / 2) // in task counts...after this delay power will change to hold level -#endif - -#ifndef SOLENOID_PULSE_LEN_HOLD -# define SOLENOID_PULSE_LEN_HOLD 80 // solenoid hold level ... typically a lower value to prevent overheating -#endif - -#ifndef SOLENOID_TIMER_INT_FREQ -# define SOLENOID_TIMER_INT_FREQ 50 -#endif - -void solenoid_init(); -void solenoid_disable(); -void solenoidSyncTask(void* pvParameters); -void calc_solenoid(float penZ); diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp index aafcad8a..910a1d06 100644 --- a/Grbl_Esp32/src/Stepper.cpp +++ b/Grbl_Esp32/src/Stepper.cpp @@ -98,10 +98,6 @@ static volatile uint8_t segment_buffer_tail; static uint8_t segment_buffer_head; static uint8_t segment_next_head; -// Step and direction port invert masks. -static uint8_t step_port_invert_mask; -static uint8_t dir_port_invert_mask; - // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though. static volatile uint8_t busy; @@ -420,7 +416,6 @@ static void stepper_pulse_func() { // Generate pulse (at least one pulse) // The pulse resolution is limited by I2S_OUT_USEC_PER_PULSE // - st.step_outbits ^= step_port_invert_mask; // Apply step port invert mask i2s_out_push_sample(pulse_microseconds->get() / I2S_OUT_USEC_PER_PULSE); set_stepper_pins_on(0); // turn all off return; @@ -429,7 +424,6 @@ static void stepper_pulse_func() { #ifdef USE_RMT_STEPS return; #else - st.step_outbits ^= step_port_invert_mask; // Apply step port invert mask // 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 @@ -476,8 +470,6 @@ void st_wake_up() { // Enable stepper drivers. motors_set_disable(false); stepper_idle = false; - // Initialize stepper output bits to ensure first ISR call does not step. - st.step_outbits = step_port_invert_mask; // Initialize step pulse timing from settings. Here to ensure updating after re-writing. #ifdef STEP_PULSE_DELAY // Step pulse delay handling is not require with ESP32...the RMT function does it. @@ -510,8 +502,8 @@ void st_reset() { segment_buffer_head = 0; // empty = tail segment_next_head = 1; busy = false; - st_generate_step_dir_invert_masks(); - st.dir_outbits = dir_port_invert_mask; // Initialize direction bits to default. + st.step_outbits = 0; + st.dir_outbits = dir_invert_mask->get(); // Initialize direction bits to default. // TODO do we need to turn step pins off? } @@ -716,6 +708,7 @@ void st_go_idle() { motors_set_disable(false); set_stepper_pins_on(0); + st.step_outbits = 0; } // Called by planner_recalculate() when the executing block is updated by the new plan. @@ -760,22 +753,6 @@ void st_parking_restore_buffer() { } #endif -// Generates the step and direction port invert masks used in the Stepper Interrupt Driver. -void st_generate_step_dir_invert_masks() { - /* - uint8_t idx; - step_port_invert_mask = 0; - dir_port_invert_mask = 0; - for (idx=0; idxget(),bit(idx))) { step_port_invert_mask |= get_step_pin_mask(idx); } - if (bit_istrue(dir_invert_mask->get(),bit(idx))) { dir_port_invert_mask |= get_direction_pin_mask(idx); } - } - */ - // simpler with ESP32, but let's do it here for easier change management - step_port_invert_mask = step_invert_mask->get(); - dir_port_invert_mask = dir_invert_mask->get(); -} - // Increments the step segment buffer block data ring buffer. static uint8_t st_next_block_index(uint8_t block_index) { block_index++; diff --git a/Grbl_Esp32/src/Stepper.h b/Grbl_Esp32/src/Stepper.h index 55de7f52..78409046 100644 --- a/Grbl_Esp32/src/Stepper.h +++ b/Grbl_Esp32/src/Stepper.h @@ -97,9 +97,6 @@ void st_wake_up(); // Immediately disables steppers void st_go_idle(); -// Generate the step and direction port invert masks. -void st_generate_step_dir_invert_masks(); - // Reset the stepper subsystem variables void st_reset(); diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp index 7cc6d03b..0323526a 100644 --- a/Grbl_Esp32/src/WebUI/WebServer.cpp +++ b/Grbl_Esp32/src/WebUI/WebServer.cpp @@ -592,8 +592,8 @@ namespace WebUI { String sadminPassword = admin_password->get(); String suserPassword = user_password->get(); - if (!((sUser == DEFAULT_ADMIN_LOGIN && sPassword == sadminPassword) || - (sUser == DEFAULT_USER_LOGIN && sPassword == suserPassword)) { + if (!(sUser == DEFAULT_ADMIN_LOGIN && sPassword == sadminPassword) || + (sUser == DEFAULT_USER_LOGIN && sPassword == suserPassword)) { msg_alert_error = true; smsg = "Error: Incorrect password"; code = 401; @@ -607,13 +607,19 @@ namespace WebUI { //change password if (_webserver->hasArg("PASSWORD") && _webserver->hasArg("USER") && _webserver->hasArg("NEWPASSWORD") && (msg_alert_error == false)) { + String newpassword = _webserver->arg("NEWPASSWORD"); - if (COMMANDS::isLocalPasswordValid((char*)newpassword.c_str())) { + + char pwdbuf[MAX_LOCAL_PASSWORD_LENGTH + 1]; + newpassword.toCharArray(pwdbuf, MAX_LOCAL_PASSWORD_LENGTH + 1); + + if (COMMANDS::isLocalPasswordValid(pwdbuf)) { err_t err; + if (sUser == DEFAULT_ADMIN_LOGIN) { - err = admin_password->setStringValue(newpassword); + err = admin_password->setStringValue(pwdbuf); } else { - err = user_password->setStringValue(newpassword); + err = user_password->setStringValue(pwdbuf); } if (err) { msg_alert_error = true; diff --git a/Grbl_Esp32/src/grbl_unipolar.cppold b/Grbl_Esp32/src/grbl_unipolar.cppold deleted file mode 100644 index 6e2f2fa0..00000000 --- a/Grbl_Esp32/src/grbl_unipolar.cppold +++ /dev/null @@ -1,214 +0,0 @@ -/* - unipolar.cpp - Part of Grbl_ESP32 - - copyright (c) 2019 - Bart Dring. This file was intended 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 . - - Unipolar Class - - This class allows you to control a unipolar motor. Unipolar motors have 5 wires. One - is typically tied to a voltage, while the other 4 are switched to ground in a - sequence - - To take a step simply call the step(step, direction) function. - -*/ -#include "grbl.h" - -#ifdef USE_UNIPOLAR - -// assign the I/O pins used for each coil of the motors -#ifdef X_UNIPOLAR - Unipolar X_Unipolar(X_PIN_PHASE_0, X_PIN_PHASE_1, X_PIN_PHASE_2, X_PIN_PHASE_3, true); -#endif - -#ifdef Y_UNIPOLAR - Unipolar Y_Unipolar(Y_PIN_PHASE_0, Y_PIN_PHASE_1, Y_PIN_PHASE_2, Y_PIN_PHASE_3, true); -#endif - -#ifdef Z_UNIPOLAR - Unipolar Z_Unipolar(Z_PIN_PHASE_0, Z_PIN_PHASE_1, Z_PIN_PHASE_2, Z_PIN_PHASE_3, true); -#endif - -void unipolar_init() { -#ifdef X_UNIPOLAR - X_Unipolar.init(); - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "X unipolar"); -#endif -#ifdef Y_UNIPOLAR - Y_Unipolar.init(); - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Y unipolar"); -#endif -#ifdef Z_UNIPOLAR - Z_Unipolar.init(); - grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Z unipolar"); -#endif -} - -void unipolar_step(uint8_t step_mask, uint8_t dir_mask) { -#ifdef X_UNIPOLAR - X_Unipolar.step(step_mask & bit(X_AXIS), dir_mask & bit(X_AXIS)); -#endif -#ifdef Y_UNIPOLAR - Y_Unipolar.step(step_mask & bit(Y_AXIS), dir_mask & bit(Y_AXIS)); -#endif -#ifdef Z_UNIPOLAR - Z_Unipolar.step(step_mask & bit(Z_AXIS), dir_mask & bit(ZX_AXIS)); -#endif -} - -void unipolar_disable(bool disable) { -#ifdef X_UNIPOLAR - X_Unipolar.set_enabled(!disable); -#endif -#ifdef Y_UNIPOLAR - Y_Unipolar.set_enabled(!disable); -#endif -#ifdef Z_UNIPOLAR - Z_Unipolar.set_enabled(!disable); -#endif -} - - -Unipolar::Unipolar(uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3, bool half_step) { // constructor - _pin_phase0 = pin_phase0; - _pin_phase1 = pin_phase1; - _pin_phase2 = pin_phase2; - _pin_phase3 = pin_phase3; - _half_step = half_step; -} - -void Unipolar::init() { - pinMode(_pin_phase0, OUTPUT); - pinMode(_pin_phase1, OUTPUT); - pinMode(_pin_phase2, OUTPUT); - pinMode(_pin_phase3, OUTPUT); - _current_phase = 0; - set_enabled(false); -} - -void Unipolar::set_enabled(bool enabled) { - if (enabled == _enabled) - return; // no change - _enabled = enabled; - if (!enabled) { - digitalWrite(_pin_phase0, 0); - digitalWrite(_pin_phase1, 0); - digitalWrite(_pin_phase2, 0); - digitalWrite(_pin_phase3, 0); - } -} - -/* -To take a step set step to true and set the driection - -step is included so that st.step_outbits can be used to determine if a -step is required on this axis -*/ -void Unipolar::step(bool step, bool dir_forward) { - uint8_t _phase[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // temporary phase values...all start as off - uint8_t phase_max; - if (_half_step) - phase_max = 7; - else - phase_max = 3; - if (!step) - return; // a step is not required on this interrupt - if (!_enabled) - return; // don't do anything, phase is not changed or lost - if (dir_forward) { // count up - if (_current_phase == phase_max) - _current_phase = 0; - else - _current_phase++; - } else { // count down - if (_current_phase == 0) - _current_phase = phase_max; - else - _current_phase--; - } - /* - 8 Step : A – AB – B – BC – C – CD – D – DA - 4 Step : AB – BC – CD – DA (Usual application) - - Step IN4 IN3 IN2 IN1 - A 0 0 0 1 - AB 0 0 1 1 - B 0 0 1 0 - BC 0 1 1 0 - C 0 1 0 0 - CD 1 1 0 0 - D 1 0 0 0 - DA 1 0 0 1 - */ - if (_half_step) { - switch (_current_phase) { - case 0: - _phase[0] = 1; - break; - case 1: - _phase[0] = 1; - _phase[1] = 1; - break; - case 2: - _phase[1] = 1; - break; - case 3: - _phase[1] = 1; - _phase[2] = 1; - break; - case 4: - _phase[2] = 1; - break; - case 5: - _phase[2] = 1; - _phase[3] = 1; - break; - case 6: - _phase[3] = 1; - break; - case 7: - _phase[3] = 1; - _phase[0] = 1; - break; - } - } else { - switch (_current_phase) { - case 0: - _phase[0] = 1; - _phase[1] = 1; - break; - case 1: - _phase[1] = 1; - _phase[2] = 1; - break; - case 2: - _phase[2] = 1; - _phase[3] = 1; - break; - case 3: - _phase[3] = 1; - _phase[0] = 1; - break; - } - } - digitalWrite(_pin_phase0, _phase[0]); - digitalWrite(_pin_phase1, _phase[1]); - digitalWrite(_pin_phase2, _phase[2]); - digitalWrite(_pin_phase3, _phase[3]); -} -#endif diff --git a/Grbl_Esp32/src/grbl_unipolar.hold b/Grbl_Esp32/src/grbl_unipolar.hold deleted file mode 100644 index a7a782c4..00000000 --- a/Grbl_Esp32/src/grbl_unipolar.hold +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -/* - grbl_unipolar.h - Part of Grbl_ESP32 - - copyright (c) 2019 - Bart Dring. This file was intended 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 . - - Unipolar Class - - This class allows you to control a unipolar motor. Unipolar motors have 5 wires. One - is typically tied to a voltage, while the other 4 are switched to ground in a - sequence - - To take a step simply call the step(direction) function. It will take - -*/ - -void unipolar_init(); -void unipolar_step(uint8_t step_mask, uint8_t dir_mask); -void unipolar_disable(bool enable); - -class Unipolar { - public: - Unipolar(uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3, bool half_step); // constructor - void set_enabled(bool enabled); - void step(bool step, bool dir_forward); - void init(); - - private: - uint8_t _current_phase = 0; - bool _enabled = false; - bool _half_step = true; // default is half step, full step - uint8_t _pin_phase0; - uint8_t _pin_phase1; - uint8_t _pin_phase2; - uint8_t _pin_phase3; - -}; From 981ce50993cfc0a59ad83d7a9c4bff8457db5a51 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 11 Sep 2020 16:19:12 -0500 Subject: [PATCH 2/8] Update platformio.ini Per PR 583 --- platformio.ini | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/platformio.ini b/platformio.ini index 326c7073..e600892d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -31,9 +31,7 @@ build_flags = [env] lib_deps = - TMCStepper@>=0.7.0 - arduinoWebSockets - ESP32SSPD + TMCStepper@>=0.7.0,<1.0.0 platform = espressif32 board = esp32dev framework = arduino From 9b38167ed00ae2ee66d9e5eb31c202f2e87857a2 Mon Sep 17 00:00:00 2001 From: bdring Date: Fri, 18 Sep 2020 12:25:33 -0500 Subject: [PATCH 3/8] Removing some unused machine defs --- Grbl_Esp32/src/Machines/3_pack_v1.h | 83 ----- .../src/Machines/esp32_printer_controller.h | 287 ------------------ Grbl_Esp32/src/Machines/foo_6axis.h | 150 --------- 3 files changed, 520 deletions(-) delete mode 100644 Grbl_Esp32/src/Machines/3_pack_v1.h delete mode 100644 Grbl_Esp32/src/Machines/esp32_printer_controller.h delete mode 100644 Grbl_Esp32/src/Machines/foo_6axis.h diff --git a/Grbl_Esp32/src/Machines/3_pack_v1.h b/Grbl_Esp32/src/Machines/3_pack_v1.h deleted file mode 100644 index c8066726..00000000 --- a/Grbl_Esp32/src/Machines/3_pack_v1.h +++ /dev/null @@ -1,83 +0,0 @@ -#pragma once -// clang-format off - -/* - 3_pack_v1.h - - Covers all V1 versions V1p0, V1p1, etc - - Part of Grbl_ESP32 - Pin assignments for the ESP32 I2S 6-axis board - - 2020 - 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 - (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 "3 Pack Controller V1" - - -#define USE_STEPSTICK // makes sure MS1,2,3 !reset and !sleep are set - -#define X_STEPPER_MS3 GPIO_NUM_4 // X_CS (works for all drivers on this board) - -#define X_STEP_PIN GPIO_NUM_32 -#define X_DIRECTION_PIN GPIO_NUM_27 - -#define Y_STEP_PIN GPIO_NUM_33 -#define Y_DIRECTION_PIN GPIO_NUM_26 - -#define Z_STEP_PIN GPIO_NUM_25 -#define Z_DIRECTION_PIN GPIO_NUM_17 - -#define STEPPERS_DISABLE_PIN GPIO_NUM_16 - -/* Socket #1 ref -#1 GPIO_NUM_36 // Input Only (X_LIMIT_PIN) -#2 GPIO_NUM_39 // Input Only (Y_LIMIT_PIN) -#4 GPIO_NUM_34 // Input Only (Z_LIMIT_PIN) -#4 GPIO_NUM_35 // Input Only -*/ - -/* Socket #2 ref -#1 GPIO_NUM_2 -#2 GPIO_NUM_21 -#4 GPIO_NUM_22 -#4 N/C -*/ - -/* Socket #3 ref -#1 GPIO_NUM_14 -#2 GPIO_NUM_13 -#4 GPIO_NUM_15 -#4 GPIO_NUM_12 -*/ - -// Socket #1 -#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 - -/* -// Socket #3 5V Output -#define SPINDLE_TYPE SPINDLE_TYPE_LASER -#define SPINDLE_OUTPUT_PIN GPIO_NUM_14 // labeled SpinPWM -#define SPINDLE_ENABLE_PIN GPIO_NUM_13 // labeled SpinEnbl -#define COOLANT_MIST_PIN GPIO_NUM_15 -#define COOLANT_FLOOD_PIN GPIO_NUM_12 -*/ - -// Socket #3 5V Output -#define USER_DIGITAL_PIN_1 GPIO_NUM_14 -#define USER_DIGITAL_PIN_2 GPIO_NUM_13 -#define USER_DIGITAL_PIN_3 GPIO_NUM_15 -#define USER_DIGITAL_PIN_4 GPIO_NUM_12 diff --git a/Grbl_Esp32/src/Machines/esp32_printer_controller.h b/Grbl_Esp32/src/Machines/esp32_printer_controller.h deleted file mode 100644 index e41ba8f0..00000000 --- a/Grbl_Esp32/src/Machines/esp32_printer_controller.h +++ /dev/null @@ -1,287 +0,0 @@ -#pragma once -// clang-format off - -/* - esp32_printer_controller.h - Part of Grbl_ESP32 - Template for a machine configuration file. - 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 . -*/ - -// 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. - -// 3D printer controller using ESP32 processor -// https://github.com/MitchBradley/Esp32PrinterController - -// === 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 "ESP32_PRINTER_CONTROLLER" - -// 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/esp32_printer_controller.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 3 - - -// == Pin Assignments - -// Step and direction pins; these must be output-capable pins, -// specifically ESP32 GPIO numbers 0..31 -// With the I2S I/O expander enabled, you can specify 128..159 as output pins. -#define X_STEP_PIN I2SO(9) -#define X_DIRECTION_PIN I2SO(7) -#define Y_STEP_PIN I2SO(5) -#define Y_DIRECTION_PIN I2SO(4) -#define Z_STEP_PIN I2SO(2) -#define Z_DIRECTION_PIN I2SO(1) -#define A_STEP_PIN I2SO(12) -#define A_DIRECTION_PIN I2SO(13) - -#define X_LIMIT_PIN GPIO_NUM_34 -//#define Y_LIMIT_PIN GPIO_NUM_35 -//#define Z_LIMIT_PIN GPIO_NUM_32 - -// 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_TYPE SPINDLE_TYPE_NONE -// #define SPINDLE_PWM_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_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_15 // It cannot be used when JTAG debugging -//#define SERVO_Z_RANGE_MIN 0.0 -//#define SERVO_Z_RANGE_MAX 5.0 - -// === 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. - -// 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 - -// === 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. -// 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 -#define DEFAULT_STEP_PULSE_MICROSECONDS I2S_OUT_USEC_PER_PULSE - -#define DEFAULT_HOMING_ENABLE 1 -#define DEFAULT_HOMING_DIR_MASK 0 // move positive dir Z, negative X,Y -#define DEFAULT_HOMING_FEED_RATE 200.0 // mm/min -#define DEFAULT_HOMING_SEEK_RATE 1000.0 // mm/min -#define DEFAULT_HOMING_DEBOUNCE_DELAY 250 // msec (0-65k) -#define DEFAULT_HOMING_PULLOFF 3.0 // mm - -#define DEFAULT_HARD_LIMIT_ENABLE 1 -// === Control Pins - -// The control pins with names that begin with CONTROL_ are -// ignored by default, to avoid noise problems. To make them -// work, you must undefine IGNORE_CONTROL_PINS -#undef IGNORE_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 -#ifndef ENABLE_CONTROL_SW_DEBOUNCE - #define ENABLE_CONTROL_SW_DEBOUNCE -#endif - -#ifdef CONTROL_SW_DEBOUNCE_PERIOD - #undef CONTROL_SW_DEBOUNCE_PERIOD -#endif -#define CONTROL_SW_DEBOUNCE_PERIOD 100 // really long debounce - -#ifdef INVERT_CONTROL_PIN_MASK - #undef INVERT_CONTROL_PIN_MASK -#endif -#define INVERT_CONTROL_PIN_MASK B11111111 - -// -// 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 I2S_OUT_NUM_BITS 16 - -// === 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: -// -#define USE_MACHINE_INIT - -// USE_CUSTOM_HOMING enables the user_defined_homing() 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_KINEMATIC enables the forward_kinematics() function -// that converts motor positions in non-Cartesian coordinate -// systems back to Cartesian form, for status reports. -//#define USE_FWD_KINEMATIC - -// 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/Machines/foo_6axis.h b/Grbl_Esp32/src/Machines/foo_6axis.h deleted file mode 100644 index 7dc1d8ef..00000000 --- a/Grbl_Esp32/src/Machines/foo_6axis.h +++ /dev/null @@ -1,150 +0,0 @@ -#pragma once -// clang-format off - -/* - foo_6axis.h - Part of Grbl_ESP32 - - Pin assignments for a 6-axis system - - 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 "FOO_6X" - -#ifdef N_AXIS - #undef N_AXIS -#endif -#define N_AXIS 6 - -#define SPINDLE_TYPE SPINDLE_TYPE_NONE - -// stepper motors -#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 - -// Z is a servo - -#define A_STEP_PIN GPIO_NUM_27 -#define A_DIRECTION_PIN GPIO_NUM_33 - -#define B_STEP_PIN GPIO_NUM_15 -#define B_DIRECTION_PIN GPIO_NUM_32 - -// C is a servo - -// servos -#define SERVO_Z_PIN GPIO_NUM_22 -#define SERVO_Z_RANGE_MIN 0.0 -#define SERVO_Z_RANGE_MAX 5.0 - -#define SERVO_C_PIN GPIO_NUM_2 -#define SERVO_C_RANGE_MIN 0.0 -#define SERVO_C_RANGE_MAX 5.0 - -// limit switches -#define X_LIMIT_PIN GPIO_NUM_21 -#define Y_LIMIT_PIN GPIO_NUM_17 -#define A_LIMIT_PIN GPIO_NUM_16 -#define B_LIMIT_PIN GPIO_NUM_4 - -// OK to comment out to use pin for other features -#define STEPPERS_DISABLE_PIN GPIO_NUM_13 - -#ifdef HOMING_CYCLE_0 // undefine from config.h - #undef HOMING_CYCLE_0 -#endif -//#define HOMING_CYCLE_0 bit(X_AXIS) -#define HOMING_CYCLE_0 (bit(X_AXIS)|bit(Y_AXIS)) -//#define HOMING_CYCLE_0 (bit(X_AXIS)|bit(Y_AXIS) |bit(A_AXIS)|bit(B_AXIS)) - -#ifdef HOMING_CYCLE_1 // undefine from config.h - #undef HOMING_CYCLE_1 -#endif -//#define HOMING_CYCLE_1 bit(A_AXIS) -#define HOMING_CYCLE_1 (bit(A_AXIS)|bit(B_AXIS)) - -#ifdef HOMING_CYCLE_2 // undefine from config.h - #undef HOMING_CYCLE_2 -#endif -/* -#define HOMING_CYCLE_2 bit(Y_AXIS) - -#ifdef HOMING_CYCLE_3 // undefine from config.h - #undef HOMING_CYCLE_3 -#endif -#define HOMING_CYCLE_3 bit(B_AXIS) -*/ - -#define DEFAULT_STEP_PULSE_MICROSECONDS 3 -#define DEFAULT_STEPPER_IDLE_LOCK_TIME 200 // 200ms - -#define DEFAULT_STEPPING_INVERT_MASK 0 // uint8_t -#define DEFAULT_DIRECTION_INVERT_MASK 2 // uint8_t -#define DEFAULT_INVERT_ST_ENABLE 0 // boolean -#define DEFAULT_INVERT_LIMIT_PINS 1 // boolean -#define DEFAULT_INVERT_PROBE_PIN 0 // boolean - -#define DEFAULT_STATUS_REPORT_MASK 1 - -#define DEFAULT_JUNCTION_DEVIATION 0.01 // mm -#define DEFAULT_ARC_TOLERANCE 0.002 // mm -#define DEFAULT_REPORT_INCHES 0 // false - -#define DEFAULT_SOFT_LIMIT_ENABLE 0 // false -#define DEFAULT_HARD_LIMIT_ENABLE 0 // false - -#define DEFAULT_HOMING_ENABLE 1 -#define DEFAULT_HOMING_DIR_MASK 17 -#define DEFAULT_HOMING_FEED_RATE 200.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 400.0 -#define DEFAULT_Y_STEPS_PER_MM 400.0 -#define DEFAULT_Z_STEPS_PER_MM 100.0 // This is percent in servo mode -#define DEFAULT_A_STEPS_PER_MM 400.0 -#define DEFAULT_B_STEPS_PER_MM 400.0 -#define DEFAULT_C_STEPS_PER_MM 100.0 // This is percent in servo mode - - -#define DEFAULT_X_MAX_RATE 30000.0 // mm/min -#define DEFAULT_Y_MAX_RATE 30000.0 // mm/min -#define DEFAULT_Z_MAX_RATE 8000.0 // mm/min -#define DEFAULT_A_MAX_RATE 30000.0 // mm/min -#define DEFAULT_B_MAX_RATE 30000.0 // mm/min -#define DEFAULT_C_MAX_RATE 8000.0 // mm/min - -#define DEFAULT_X_ACCELERATION 1500.0 // mm/sec^2 -#define DEFAULT_Y_ACCELERATION 1500.0 // mm/sec^2 -#define DEFAULT_Z_ACCELERATION 100.0 // mm/sec^2 -#define DEFAULT_A_ACCELERATION 1500.0 // mm/sec^2 -#define DEFAULT_B_ACCELERATION 1500.0 // mm/sec^2 -#define DEFAULT_C_ACCELERATION 100.0 // mm/sec^2 - -#define DEFAULT_X_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Y_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. -#define DEFAULT_Z_MAX_TRAVEL 100.0 // This is percent in servo mode -#define DEFAULT_A_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. -#define DEFAULT_B_MAX_TRAVEL 250.0 // mm NOTE: Must be a positive value. -#define DEFAULT_C_MAX_TRAVEL 100.0 // This is percent in servo mode - From 050aa19bff3b1ad13df47e65d509397cb23504fa Mon Sep 17 00:00:00 2001 From: bdring Date: Sat, 19 Sep 2020 17:53:54 -0500 Subject: [PATCH 4/8] Fixed various small bugs (#605) (#606) * Fixed various small bugs * Fixed potential cast bug * Fixed double reporting of errors Co-authored-by: Stefan de Bruijn Co-authored-by: Stefan de Bruijn 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 639b6c13452f9810ad704e4ebd4b7861e448abb3 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 21 Sep 2020 08:58:23 -0500 Subject: [PATCH 5/8] Devt (#609) * 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 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 da1d6b83720e8adb83a7efff8820eb5066dc27f3 Mon Sep 17 00:00:00 2001 From: bdring Date: Mon, 21 Sep 2020 20:18:16 -0500 Subject: [PATCH 6/8] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 65a8eab0..7ee0651d 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ # Grbl (CNC Controller) For ESP32 - + ### Project Overview From 286ed2a1046b4471af27fa05c4f38dd0df0af8f3 Mon Sep 17 00:00:00 2001 From: bdring Date: Tue, 22 Sep 2020 17:42:59 -0500 Subject: [PATCH 7/8] 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 8/8] 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)